Navigation Menu

Skip to content

Instantly share code, notes, and snippets.

@aurimasniekis

aurimasniekis/diff_2.8.patch

Created November 16, 2018 10:36
Show Gist options
  • Star 1 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save aurimasniekis/84fc5897492221fda36a69294338fddd to your computer and use it in GitHub Desktop.
Save aurimasniekis/84fc5897492221fda36a69294338fddd to your computer and use it in GitHub Desktop.
Download ZIP
Raw
diff_2.8.patch
Only in Raptor2 Sources/bldc-firmware_2_80: .dep
Only in bldc-cc171422641a50c56452280575f2074f8a88aa45: .gitignore
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/Makefile Raptor2 Sources/bldc-firmware_2_80/Makefile
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/Makefile 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/Makefile 2016-09-19 10:07:06.000000000 +0100
@@ -270,8 +270,8 @@
# Program
upload: build/$(PROJECT).bin
#qstlink2 --cli --erase --write build/$(PROJECT).bin
- openocd -f interface/stlink-v2.cfg -c "set WORKAREASIZE 0x2000" -f target/stm32f4x_stlink.cfg -c "program build/$(PROJECT).elf verify reset"
- #openocd -f board/stm32f4discovery.cfg -c "reset_config trst_only combined" -c "program build/$(PROJECT).elf verify reset exit" # For openocd 0.9
+ #openocd -f interface/stlink-v2.cfg -c "set WORKAREASIZE 0x2000" -f target/stm32f4x_stlink.cfg -c "program build/$(PROJECT).elf verify reset" # Older openocd
+ openocd -f board/stm32f4discovery.cfg -c "reset_config trst_only combined" -c "program build/$(PROJECT).elf verify reset exit" # For openocd 0.9
#program with olimex arm-usb-tiny-h and jtag-swd adapter board. needs openocd>=0.9
upload-olimex: build/$(PROJECT).bin
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/README.md Raptor2 Sources/bldc-firmware_2_80/README.md
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/README.md 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/README.md 2018-08-27 20:57:58.000000000 +0100
@@ -1 +1,12 @@
-This is the source code for my custom BLDC controller. A complete description and tutorial about how to use it can be found here: http://vedder.se/2015/01/vesc-open-source-esc/
+A complete description and tutorial about how to use it can be found here: http://vedder.se/2015/01/vesc-open-source-esc/
+
+This is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+The software is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU General Public License for more details.
+You should have received a copy of the GNU General Public License
+along with this program. If not, see <http://www.gnu.org/licenses
\ No newline at end of file
Only in Raptor2 Sources/bldc-firmware_2_80/appconf: appconf_custom.h
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/appconf/appconf_default.h Raptor2 Sources/bldc-firmware_2_80/appconf/appconf_default.h
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/appconf/appconf_default.h 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/appconf/appconf_default.h 2017-05-31 20:43:38.000000000 +0100
@@ -26,36 +26,39 @@
#define APPCONF_TIMEOUT_MSEC 1000
#endif
#ifndef APPCONF_TIMEOUT_BRAKE_CURRENT
-#define APPCONF_TIMEOUT_BRAKE_CURRENT 0.0
+#define APPCONF_TIMEOUT_BRAKE_CURRENT 0.0
#endif
#ifndef APPCONF_SEND_CAN_STATUS
#define APPCONF_SEND_CAN_STATUS false
#endif
#ifndef APPCONF_SEND_CAN_STATUS_RATE_HZ
-#define APPCONF_SEND_CAN_STATUS_RATE_HZ 500
+#define APPCONF_SEND_CAN_STATUS_RATE_HZ 500
#endif
// The default app is UART in case the UART port is used for
// firmware updates.
#ifndef APPCONF_APP_TO_USE
-#define APPCONF_APP_TO_USE APP_UART
+#define APPCONF_APP_TO_USE APP_PPM_UART
#endif
// PPM app
#ifndef APPCONF_PPM_CTRL_TYPE
-#define APPCONF_PPM_CTRL_TYPE PPM_CTRL_TYPE_NONE
+#define APPCONF_PPM_CTRL_TYPE PPM_CTRL_TYPE_NONE //PPM_CTRL_TYPE_WATT_NOREV_BRAKE
#endif
#ifndef APPCONF_PPM_PID_MAX_ERPM
-#define APPCONF_PPM_PID_MAX_ERPM 15000
+#define APPCONF_PPM_PID_MAX_ERPM 60000
#endif
#ifndef APPCONF_PPM_HYST
-#define APPCONF_PPM_HYST 0.15
+#define APPCONF_PPM_HYST 0.05
#endif
#ifndef APPCONF_PPM_PULSE_START
-#define APPCONF_PPM_PULSE_START 1.0
+#define APPCONF_PPM_PULSE_START 1.04
+#endif
+#ifndef APPCONF_PPM_PULSE_CENTER
+#define APPCONF_PPM_PULSE_CENTER 1.54
#endif
#ifndef APPCONF_PPM_PULSE_END
-#define APPCONF_PPM_PULSE_END 2.0
+#define APPCONF_PPM_PULSE_END 2.04
#endif
#ifndef APPCONF_PPM_MEDIAN_FILTER
#define APPCONF_PPM_MEDIAN_FILTER true
@@ -64,19 +67,77 @@
#define APPCONF_PPM_SAFE_START true
#endif
#ifndef APPCONF_PPM_RPM_LIM_START
-#define APPCONF_PPM_RPM_LIM_START 150000
+#define APPCONF_PPM_RPM_LIM_START 55000
#endif
#ifndef APPCONF_PPM_RPM_LIM_END
-#define APPCONF_PPM_RPM_LIM_END 200000
+#define APPCONF_PPM_RPM_LIM_END 60000
#endif
#ifndef APPCONF_PPM_MULTI_ESC
-#define APPCONF_PPM_MULTI_ESC false
+#define APPCONF_PPM_MULTI_ESC true
#endif
#ifndef APPCONF_PPM_TC
-#define APPCONF_PPM_TC false
+#define APPCONF_PPM_TC true
#endif
#ifndef APPCONF_PPM_TC_MAX_DIFF
-#define APPCONF_PPM_TC_MAX_DIFF 3000.0
+#define APPCONF_PPM_TC_MAX_DIFF 5000.0
+#endif
+#ifndef APPCONF_PPM_TC_OFFSET
+#define APPCONF_PPM_TC_OFFSET 3000.0
+#endif
+#ifndef APPCONF_PPM_MAX_ERPM_FOR_DIR_ACTIVE
+#define APPCONF_PPM_MAX_ERPM_FOR_DIR_ACTIVE true
+#endif
+#ifndef APPCONF_PPM_MAX_ERPM_FOR_DIR
+#define APPCONF_PPM_MAX_ERPM_FOR_DIR 4000.0
+#endif
+
+#ifndef APPCONF_ADJUSTABLE_THROTTLE_ENABLED
+#define APPCONF_ADJUSTABLE_THROTTLE_ENABLED true
+#endif
+#ifndef APPCONF_Y1_THROTTLE
+#define APPCONF_Y1_THROTTLE 0.08
+#endif
+#ifndef APPCONF_Y2_THROTTLE
+#define APPCONF_Y2_THROTTLE 0.225
+#endif
+#ifndef APPCONF_Y3_THROTTLE
+#define APPCONF_Y3_THROTTLE 0.50
+#endif
+#ifndef APPCONF_X1_THROTTLE
+#define APPCONF_X1_THROTTLE 0.25
+#endif
+#ifndef APPCONF_X2_THROTTLE
+#define APPCONF_X2_THROTTLE 0.50
+#endif
+#ifndef APPCONF_X3_THROTTLE
+#define APPCONF_X3_THROTTLE 0.75
+#endif
+
+#ifndef APPCONF_BEZIER_REDUCE_FACTOR
+#define APPCONF_BEZIER_REDUCE_FACTOR 0
+#endif
+
+#ifndef APPCONF_Y1_NEG_THROTTLE
+#define APPCONF_Y1_NEG_THROTTLE 0.125
+#endif
+#ifndef APPCONF_Y2_NEG_THROTTLE
+#define APPCONF_Y2_NEG_THROTTLE 0.30
+#endif
+#ifndef APPCONF_Y3_NEG_THROTTLE
+#define APPCONF_Y3_NEG_THROTTLE 0.60
+#endif
+#ifndef APPCONF_X1_NEG_THROTTLE
+#define APPCONF_X1_NEG_THROTTLE 0.25
+#endif
+#ifndef APPCONF_X2_NEG_THROTTLE
+#define APPCONF_X2_NEG_THROTTLE 0.50
+#endif
+#ifndef APPCONF_X3_NEG_THROTTLE
+#define APPCONF_X3_NEG_THROTTLE 0.75
+#endif
+
+#ifndef APPCONF_BEZIER_REDUCE_NEG_FACTOR
+#define APPCONF_BEZIER_REDUCE_NEG_FACTOR 0
#endif
// ADC app configureation
@@ -84,7 +145,7 @@
#define APPCONF_ADC_CTRL_TYPE ADC_CTRL_TYPE_NONE
#endif
#ifndef APPCONF_ADC_HYST
-#define APPCONF_ADC_HYST 0.15
+#define APPCONF_ADC_HYST 0.15
#endif
#ifndef APPCONF_ADC_VOLTAGE_START
#define APPCONF_ADC_VOLTAGE_START 0.9
@@ -99,25 +160,25 @@
#define APPCONF_ADC_SAFE_START true
#endif
#ifndef APPCONF_ADC_CC_BUTTON_INVERTED
-#define APPCONF_ADC_CC_BUTTON_INVERTED false
+#define APPCONF_ADC_CC_BUTTON_INVERTED false
#endif
#ifndef APPCONF_ADC_REV_BUTTON_INVERTED
-#define APPCONF_ADC_REV_BUTTON_INVERTED false
+#define APPCONF_ADC_REV_BUTTON_INVERTED false
#endif
#ifndef APPCONF_ADC_VOLTAGE_INVERTED
-#define APPCONF_ADC_VOLTAGE_INVERTED false
+#define APPCONF_ADC_VOLTAGE_INVERTED false
#endif
#ifndef APPCONF_ADC_RPM_LIM_START
-#define APPCONF_ADC_RPM_LIM_START 150000.0
+#define APPCONF_ADC_RPM_LIM_START 2.0
#endif
#ifndef APPCONF_ADC_RPM_LIM_END
-#define APPCONF_ADC_RPM_LIM_END 200000.0
+#define APPCONF_ADC_RPM_LIM_END 80.0
#endif
#ifndef APPCONF_ADC_MULTI_ESC
#define APPCONF_ADC_MULTI_ESC false
#endif
#ifndef APPCONF_ADC_TC
-#define APPCONF_ADC_TC false
+#define APPCONF_ADC_TC false
#endif
#ifndef APPCONF_ADC_TC_MAX_DIFF
#define APPCONF_ADC_TC_MAX_DIFF 3000.0
@@ -128,7 +189,7 @@
// UART app
#ifndef APPCONF_UART_BAUDRATE
-#define APPCONF_UART_BAUDRATE 115200
+#define APPCONF_UART_BAUDRATE 9600
#endif
// Nunchuk app
@@ -136,7 +197,7 @@
#define APPCONF_CHUK_CTRL_TYPE CHUK_CTRL_TYPE_CURRENT
#endif
#ifndef APPCONF_CHUK_HYST
-#define APPCONF_CHUK_HYST 0.15
+#define APPCONF_CHUK_HYST 0.15
#endif
#ifndef APPCONF_CHUK_RPM_LIM_START
#define APPCONF_CHUK_RPM_LIM_START 150000.0
@@ -151,16 +212,54 @@
#define APPCONF_CHUK_RAMP_TIME_NEG 0.3
#endif
#ifndef APPCONF_STICK_ERPM_PER_S_IN_CC
-#define APPCONF_STICK_ERPM_PER_S_IN_CC 3000.0
+#define APPCONF_STICK_ERPM_PER_S_IN_CC 3000.0
#endif
#ifndef APPCONF_CHUK_MULTI_ESC
#define APPCONF_CHUK_MULTI_ESC false
#endif
#ifndef APPCONF_CHUK_TC
-#define APPCONF_CHUK_TC false
+#define APPCONF_CHUK_TC false
#endif
#ifndef APPCONF_CHUK_TC_MAX_DIFF
-#define APPCONF_CHUK_TC_MAX_DIFF 3000.0
+#define APPCONF_CHUK_TC_MAX_DIFF 5000.0
+#endif
+#ifndef APPCONF_CHUK_TC_OFFSET
+#define APPCONF_CHUK_TC_OFFSET 3000.0
+#endif
+#ifndef APPCONF_CHUK_BUTTONS_MIRRORED
+#define APPCONF_CHUK_BUTTONS_MIRRORED false
+#endif
+
+// NRF app
+#ifndef APPCONF_NRF_SPEED
+#define APPCONF_NRF_SPEED NRF_SPEED_2M
+#endif
+#ifndef APPCONF_NRF_POWER
+#define APPCONF_NRF_POWER NRF_POWER_0DBM
+#endif
+#ifndef APPCONF_NRF_CRC
+#define APPCONF_NRF_CRC NRF_CRC_1B
+#endif
+#ifndef APPCONF_NRF_RETR_DELAY
+#define APPCONF_NRF_RETR_DELAY NRF_RETR_DELAY_250US
+#endif
+#ifndef APPCONF_NRF_RETRIES
+#define APPCONF_NRF_RETRIES 3
+#endif
+#ifndef APPCONF_NRF_CHANNEL
+#define APPCONF_NRF_CHANNEL 76
+#endif
+#ifndef APPCONF_NRF_ADDR_B0
+#define APPCONF_NRF_ADDR_B0 0xC6
+#endif
+#ifndef APPCONF_NRF_ADDR_B1
+#define APPCONF_NRF_ADDR_B1 0xC7
+#endif
+#ifndef APPCONF_NRF_ADDR_B2
+#define APPCONF_NRF_ADDR_B2 0x0
+#endif
+#ifndef APPCONF_NRF_SEND_CRC_ACK
+#define APPCONF_NRF_SEND_CRC_ACK true
#endif
#endif /* APPCONF_DEFAULT_H_ */
Only in Raptor2 Sources/bldc-firmware_2_80/appconf: appconf_default_master.h
Only in Raptor2 Sources/bldc-firmware_2_80/appconf: appconf_default_slave.h
Only in bldc-cc171422641a50c56452280575f2074f8a88aa45/appconf: appconf_example_ppm.h
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/applications/app.c Raptor2 Sources/bldc-firmware_2_80/applications/app.c
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/applications/app.c 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/applications/app.c 2016-11-16 19:08:14.000000000 +0000
@@ -26,6 +26,7 @@
#include "hal.h"
#include "hw.h"
#include "nrf_driver.h"
+#include "rfhelp.h"
// Private variables
static app_configuration appconf;
@@ -65,6 +66,7 @@
case APP_NRF:
nrf_driver_init();
+ rfhelp_restart();
break;
case APP_CUSTOM:
@@ -92,8 +94,9 @@
*/
void app_set_configuration(app_configuration *conf) {
appconf = *conf;
- app_ppm_configure(&appconf.app_ppm_conf);
+ app_ppm_configure(&appconf.app_ppm_conf, &appconf.app_throttle_conf);
app_adc_configure(&appconf.app_adc_conf);
app_uartcomm_configure(appconf.app_uart_baudrate);
- app_nunchuk_configure(&appconf.app_chuk_conf);
+ app_nunchuk_configure(&appconf.app_chuk_conf, &appconf.app_throttle_conf);
+ rfhelp_update_conf(&appconf.app_nrf_conf);
}
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/applications/app.h Raptor2 Sources/bldc-firmware_2_80/applications/app.h
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/applications/app.h 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/applications/app.h 2016-11-16 23:21:16.000000000 +0000
@@ -34,15 +34,17 @@
// Standard apps
void app_ppm_start(void);
-void app_ppm_configure(ppm_config *conf);
+void app_ppm_configure(ppm_config *conf, throttle_config *throt_config);
void app_adc_start(bool use_rx_tx);
void app_adc_configure(adc_config *conf);
float app_adc_get_decoded_level(void);
float app_adc_get_voltage(void);
+float app_adc_get_decoded_level2(void);
+float app_adc_get_voltage2(void);
void app_uartcomm_start(void);
void app_uartcomm_configure(uint32_t baudrate);
void app_nunchuk_start(void);
-void app_nunchuk_configure(chuk_config *conf);
+void app_nunchuk_configure(chuk_config *conf, throttle_config *throt_config);
float app_nunchuk_get_decoded_chuk(void);
void app_nunchuk_update_output(chuck_data *data);
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/applications/app_adc.c Raptor2 Sources/bldc-firmware_2_80/applications/app_adc.c
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/applications/app_adc.c 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/applications/app_adc.c 2016-09-19 10:07:22.000000000 +0100
@@ -49,6 +49,8 @@
static volatile float ms_without_power = 0;
static volatile float decoded_level = 0.0;
static volatile float read_voltage = 0.0;
+static volatile float decoded_level2 = 0.0;
+static volatile float read_voltage2 = 0.0;
static volatile bool use_rx_tx_as_buttons = false;
void app_adc_configure(adc_config *conf) {
@@ -69,6 +71,15 @@
return read_voltage;
}
+float app_adc_get_decoded_level2(void) {
+ return decoded_level2;
+}
+
+float app_adc_get_voltage2(void) {
+ return read_voltage2;
+}
+
+
static THD_FUNCTION(adc_thread, arg) {
(void)arg;
@@ -132,6 +143,46 @@
decoded_level = pwr;
+ // Read the external ADC pin and convert the value to a voltage.
+#ifdef ADC_IND_EXT2
+ float brake = (float)ADC_Value[ADC_IND_EXT2];
+ brake /= 4095;
+ brake *= V_REG;
+#else
+ float brake = 0.0;
+#endif
+
+ read_voltage2 = brake;
+
+ // Optionally apply a mean value filter
+ if (config.use_filter) {
+ static float filter_buffer2[FILTER_SAMPLES];
+ static int filter_ptr2 = 0;
+
+ filter_buffer2[filter_ptr2++] = brake;
+ if (filter_ptr2 >= FILTER_SAMPLES) {
+ filter_ptr2 = 0;
+ }
+
+ brake = 0.0;
+ for (int i = 0;i < FILTER_SAMPLES;i++) {
+ brake += filter_buffer2[i];
+ }
+ brake /= FILTER_SAMPLES;
+ }
+
+ // Map and truncate the read voltage
+ brake = utils_map(brake, config.voltage_start, config.voltage_end, 0.0, 1.0);
+ utils_truncate_number(&brake, 0.0, 1.0);
+
+ // Optionally invert the read voltage
+ if (config.voltage_inverted) {
+ brake = 1.0 - brake;
+ }
+
+ decoded_level2 = brake;
+
+
// Read the button pins
bool cc_button = false;
bool rev_button = false;
@@ -170,6 +221,13 @@
pwr -= 1.0;
break;
+ case ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_ADC:
+ if( brake > 0.01 ) {
+ pwr = -brake;
+ }
+
+ break;
+
case ADC_CTRL_TYPE_CURRENT_REV_BUTTON:
case ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_BUTTON:
case ADC_CTRL_TYPE_DUTY_REV_BUTTON:
@@ -211,6 +269,7 @@
case ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_CENTER:
case ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_BUTTON:
+ case ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_ADC:
current_mode = true;
if (pwr >= 0.0) {
current = pwr * mcconf->l_current_max;
@@ -232,7 +291,7 @@
}
if (!(ms_without_power < MIN_MS_WITHOUT_POWER && config.safe_start)) {
- mc_interface_set_duty(pwr);
+ mc_interface_set_duty(utils_map(pwr, -1.0, 1.0, -mcconf->l_max_duty, mcconf->l_max_duty));
send_duty = true;
}
break;
@@ -249,6 +308,17 @@
}
pulses_without_power_before = ms_without_power;
mc_interface_set_brake_current(timeout_get_brake_current());
+
+ if (config.multi_esc) {
+ for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+ can_status_msg *msg = comm_can_get_status_msg_index(i);
+
+ if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
+ comm_can_set_current_brake(msg->id, timeout_get_brake_current());
+ }
+ }
+ }
+
continue;
}
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/applications/app_nunchuk.c Raptor2 Sources/bldc-firmware_2_80/applications/app_nunchuk.c
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/applications/app_nunchuk.c 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/applications/app_nunchuk.c 2017-04-13 18:12:14.000000000 +0100
@@ -53,10 +53,71 @@
static volatile chuck_data chuck_d;
static volatile int chuck_error = 0;
static volatile chuk_config config;
+static volatile throttle_config throt_config;
+
static volatile bool output_running = false;
-void app_nunchuk_configure(chuk_config *conf) {
+static float px[5];
+static float py[5];
+static float nx[5];
+static float ny[5];
+
+float calculate_throttle_curve_chuk(float *x, float *y, float bezier_reduce_factor, float t) {
+
+ float directSteps;
+ if (t < x[1]){
+ directSteps = (y[1] / x[1] * t);
+ } else if (t > x[3]) {
+ directSteps = ((y[4] - y[3]) / (x[4] - x[3]) * (t-x[3]) + y[3]);
+ } else if (t > x[2]) {
+ directSteps = ((y[3] - y[2]) / (x[3] - x[2]) * (t-x[2]) + y[2]);
+ } else if (t > x[1]) {
+ directSteps = ((y[2] - y[1]) / (x[2] - x[1]) * (t-x[1]) + y[1]);
+ } else { // (throttle == x[1])
+ directSteps = y[1];
+ };
+
+ float f[5];
+ for (int i = 0; i < 5; i++) f[i] = y[i];
+
+ for (int j = 1; j < 5; j++ )
+ for (int i = 4; i >= j; i--)
+ f[i] = ( (t - x[i-j]) * f[i] - (t - x[i]) * f[i-1]) / (x[i] - x[i-j]);
+
+ float spline = f[4] - ((f[4] - directSteps) * bezier_reduce_factor);
+
+ // safety when stupid values are entered for x and y
+ if (spline > 1.0) return 1.0;
+ if (spline < 0.0) return 0.0;
+
+ return spline;
+}
+
+void app_nunchuk_configure(chuk_config *conf, throttle_config *throttle_conf) {
config = *conf;
+ throt_config = *throttle_conf;
+
+ px[0] = 0.0;
+ px[1] = throt_config.x1_throttle;
+ px[2] = throt_config.x2_throttle;
+ px[3] = throt_config.x3_throttle;
+ px[4] = 1.0;
+ py[0] = 0.0;
+ py[1] = throt_config.y1_throttle;
+ py[2] = throt_config.y2_throttle;
+ py[3] = throt_config.y3_throttle;
+ py[4] = 1.0;
+
+ nx[0] = 0.0;
+ nx[1] = throt_config.x1_neg_throttle;
+ nx[2] = throt_config.x2_neg_throttle;
+ nx[3] = throt_config.x3_neg_throttle;
+ nx[4] = 1.0;
+ ny[0] = 0.0;
+ ny[1] = throt_config.y1_neg_throttle;
+ ny[2] = throt_config.y2_neg_throttle;
+ ny[3] = throt_config.y3_neg_throttle;
+ ny[4] = 1.0;
}
void app_nunchuk_start(void) {
@@ -132,7 +193,7 @@
}
if (is_ok) {
- static uint8_t last_buffer[10];
+ static uint8_t last_buffer[6];
int same = 1;
for (int i = 0;i < 6;i++) {
@@ -191,7 +252,7 @@
continue;
}
- if (chuck_d.bt_z && !was_z && config.ctrl_type == CHUK_CTRL_TYPE_CURRENT &&
+ if ((config.buttons_mirrored ? chuck_d.bt_c : chuck_d.bt_z) && !was_z && (config.ctrl_type == CHUK_CTRL_TYPE_CURRENT || config.ctrl_type == CHUK_CTRL_TYPE_WATT) &&
fabsf(current_now) < MAX_CURR_DIFFERENCE) {
if (is_reverse) {
is_reverse = false;
@@ -200,12 +261,20 @@
}
}
- was_z = chuck_d.bt_z;
+ was_z = config.buttons_mirrored ? chuck_d.bt_c : chuck_d.bt_z;
led_external_set_reversed(is_reverse);
float out_val = app_nunchuk_get_decoded_chuk();
utils_deadband(&out_val, config.hyst, 1.0);
+
+ if (throt_config.adjustable_throttle_enabled && out_val != 0.0){
+ if (out_val > 0.0) {
+ out_val = calculate_throttle_curve_chuk(px, py, throt_config.bezier_reduce_factor, out_val);
+ } else {
+ out_val = -calculate_throttle_curve_chuk(nx, ny, throt_config.bezier_neg_reduce_factor, -out_val);
+ }
+ }
// LEDs
float x_axis = ((float)chuck_d.js_x - 128.0) / 128.0;
@@ -245,7 +314,7 @@
}
rpm_filtered /= RPM_FILTER_SAMPLES;
- if (chuck_d.bt_c) {
+ if (config.buttons_mirrored ? chuck_d.bt_z : chuck_d.bt_c) {
static float pid_rpm = 0.0;
if (!was_pid) {
@@ -277,8 +346,21 @@
}
}
- mc_interface_set_pid_speed(pid_rpm);
-
+ // NEW
+ switch (config.ctrl_type) {
+ case CHUK_CTRL_TYPE_CURRENT:
+ case CHUK_CTRL_TYPE_CURRENT_NOREV:
+ mc_interface_set_pid_speed(pid_rpm);
+ break;
+ case CHUK_CTRL_TYPE_WATT:
+ case CHUK_CTRL_TYPE_WATT_NOREV:
+ mc_interface_set_pid_speed(pid_rpm);
+ break;
+ default:
+ break;
+ }
+ // END NEW
+
// Send the same duty cycle to the other controllers
if (config.multi_esc) {
float duty = mc_interface_get_duty_cycle_now();
@@ -302,12 +384,35 @@
was_pid = false;
float current = 0;
-
- if (out_val >= 0.0) {
- current = out_val * mcconf->l_current_max;
- } else {
- current = out_val * fabsf(mcconf->l_current_min);
+
+ // NEW
+ switch (config.ctrl_type) {
+ case CHUK_CTRL_TYPE_CURRENT:
+ case CHUK_CTRL_TYPE_CURRENT_NOREV:
+ if (out_val >= 0.0) {
+ current = out_val * mcconf->l_current_max;
+ } else {
+ current = out_val * fabsf(mcconf->l_current_min);
+ }
+ break;
+ case CHUK_CTRL_TYPE_WATT:
+ case CHUK_CTRL_TYPE_WATT_NOREV:
+ if (out_val >= 0.0) {
+ if (mcconf->use_max_watt_limit) {
+ current = utils_smallest_of_2(out_val * mcconf->l_current_max,
+ out_val * (mcconf->watts_max / GET_INPUT_VOLTAGE() / mc_interface_get_duty_cycle_for_watt_calculation()));
+ } else {
+ current = utils_smallest_of_2(out_val * mcconf->l_current_max,
+ out_val * mcconf->l_in_current_max / mc_interface_get_duty_cycle_for_watt_calculation());
+ }
+ } else {
+ current = out_val * fabsf(mcconf->l_current_min);
+ }
+ break;
+ default:
+ break;
}
+ // END NEW
// Find lowest RPM and highest current
float rpm_local = mc_interface_get_rpm();
@@ -334,7 +439,7 @@
// Make the current directional
float msg_current = msg->current;
- if (msg->duty < 0.0) {
+ if (msg->duty < 0) {
msg_current = -msg_current;
}
@@ -344,12 +449,17 @@
}
}
}
-
+
// Apply ramping
const float current_range = mcconf->l_current_max + fabsf(mcconf->l_current_min);
const float ramp_time = fabsf(current) > fabsf(prev_current) ? config.ramp_time_pos : config.ramp_time_neg;
-
- if (ramp_time > 0.01) {
+
+ if (ramp_time > 0.01 && current != 0.0) {
+
+ if (prev_current * current < 0.0) {
+ prev_current = 0.0;
+ }
+
const float ramp_step = ((float)OUTPUT_ITERATION_TIME_MS * current_range) / (ramp_time * 1000.0);
float current_goal = prev_current;
@@ -376,38 +486,55 @@
current_goal = goal_tmp2;
}
+ out_val = out_val / current * current_goal;
+
current = current_goal;
}
prev_current = current;
-
+
if (current < 0.0) {
+
mc_interface_set_brake_current(current);
- // Send brake command to all ESCs seen recently on the CAN bus
- for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
- can_status_msg *msg = comm_can_get_status_msg_index(i);
+ if (config.multi_esc) {
+ // Send brake command to all ESCs seen recently on the CAN bus
+ for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+ can_status_msg *msg = comm_can_get_status_msg_index(i);
- if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
- comm_can_set_current_brake(msg->id, current);
+ if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
+ if (config.ctrl_type == CHUK_CTRL_TYPE_WATT || config.ctrl_type == CHUK_CTRL_TYPE_WATT_NOREV) {
+ comm_can_set_brake_servo(msg->id, out_val);
+ } else {
+ comm_can_set_current_brake(msg->id, current);
+ }
+ }
}
}
} else {
+
+ bool use_min_current = false;
// Apply soft RPM limit
- if (rpm_lowest > config.rpm_lim_end && current > 0.0) {
- current = mcconf->cc_min_current;
- } else if (rpm_lowest > config.rpm_lim_start && current > 0.0) {
- current = utils_map(rpm_lowest, config.rpm_lim_start, config.rpm_lim_end, current, mcconf->cc_min_current);
- } else if (rpm_lowest < -config.rpm_lim_end && current < 0.0) {
- current = mcconf->cc_min_current;
- } else if (rpm_lowest < -config.rpm_lim_start && current < 0.0) {
- rpm_lowest = -rpm_lowest;
- current = -current;
- current = utils_map(rpm_lowest, config.rpm_lim_start, config.rpm_lim_end, current, mcconf->cc_min_current);
- current = -current;
+ if (rpm_lowest > config.rpm_lim_end) {
+ if (out_val > 0.0) {
+ use_min_current = true;
+ out_val = 0.000001; // make sure min current ius used
+ }
+ if (current > 0.0) {
+ current = mcconf->cc_min_current;
+ }
+ } else if (rpm_lowest > config.rpm_lim_start) {
+ if (out_val > 0.0) {
+ use_min_current = true;
+ out_val = utils_map(rpm_lowest, config.rpm_lim_start, config.rpm_lim_end, out_val, 0.000001);
+ }
+ if (current > 0.0) {
+ current = utils_highest_of_2(utils_map(rpm_lowest, config.rpm_lim_start, config.rpm_lim_end, current, 0.0), mcconf->cc_min_current);
+ }
}
float current_out = current;
+ float servo_val_out = out_val;
// Traction control
if (config.multi_esc) {
@@ -422,25 +549,39 @@
}
float diff = rpm_tmp - rpm_lowest;
- current_out = utils_map(diff, 0.0, config.tc_max_diff, current, 0.0);
- if (current_out < mcconf->cc_min_current) {
- current_out = 0.0;
+
+ if (diff > config.tc_offset) {
+ current_out = utils_map(diff - config.tc_offset, 0.0, config.tc_max_diff - config.tc_offset, current, 0.0);
+ servo_val_out = utils_map(diff - config.tc_offset, 0.0, config.tc_max_diff - config.tc_offset, out_val, 0.0);
+ } else {
+ current_out = current;
+ servo_val_out = out_val;
}
}
if (is_reverse) {
- comm_can_set_current(msg->id, -current_out);
+ if (config.ctrl_type == CHUK_CTRL_TYPE_WATT || config.ctrl_type == CHUK_CTRL_TYPE_WATT_NOREV) {
+ comm_can_set_servo(msg->id, -servo_val_out, use_min_current);
+ } else {
+ comm_can_set_current(msg->id, -current_out);
+ }
} else {
- comm_can_set_current(msg->id, current_out);
+ if (config.ctrl_type == CHUK_CTRL_TYPE_WATT || config.ctrl_type == CHUK_CTRL_TYPE_WATT_NOREV) {
+ comm_can_set_servo(msg->id, servo_val_out, use_min_current);
+ } else {
+ comm_can_set_current(msg->id, current_out);
+ }
}
}
}
if (config.tc) {
float diff = rpm_local - rpm_lowest;
- current_out = utils_map(diff, 0.0, config.tc_max_diff, current, 0.0);
- if (current_out < mcconf->cc_min_current) {
- current_out = 0.0;
+
+ if (diff > config.tc_offset) {
+ current_out = utils_map(diff - config.tc_offset, 0.0, config.tc_max_diff - config.tc_offset, current, 0.0);
+ } else {
+ current_out = current;
}
}
}
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/applications/app_ppm.c Raptor2 Sources/bldc-firmware_2_80/applications/app_ppm.c
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/applications/app_ppm.c 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/applications/app_ppm.c 2017-04-13 18:12:22.000000000 +0100
@@ -26,6 +26,7 @@
#include "ch.h"
#include "hal.h"
+#include "hw.h"
#include "stm32f4xx_conf.h"
#include "servo_dec.h"
#include "mc_interface.h"
@@ -33,13 +34,15 @@
#include "utils.h"
#include "comm_can.h"
#include <math.h>
+//#include "commands.h"
// Only available if servo output is not active
#if !SERVO_OUT_ENABLE
// Settings
-#define MAX_CAN_AGE 0.1
-#define MIN_PULSES_WITHOUT_POWER 50
+#define MAX_CAN_AGE 0.1
+#define MIN_PULSES_WITHOUT_POWER 50
+#define RPM_FILTER_SAMPLES 8
// Threads
static THD_FUNCTION(ppm_thread, arg);
@@ -52,21 +55,96 @@
// Private variables
static volatile bool is_running = false;
+static volatile float pid_rpm = 0;
+//static volatile int mode_switch_pulses = 0;
static volatile ppm_config config;
+static volatile throttle_config throt_config;
static volatile int pulses_without_power = 0;
+//static volatile int count = 0;
+
+static volatile float filter_buffer[RPM_FILTER_SAMPLES];
+static volatile int filter_ptr = 0;
+static volatile bool has_enough_pid_filter_data = false;
+static volatile float direction_hyst = 0;
+
+static float px[5];
+static float py[5];
+static float nx[5];
+static float ny[5];
+
// Private functions
static void update(void *p);
#endif
-void app_ppm_configure(ppm_config *conf) {
+float calculate_throttle_curve_ppm(float *x, float *y, float bezier_reduce_factor, float t) {
+
+ float directSteps;
+ if (t < x[1]){
+ directSteps = (y[1] / x[1] * t);
+ } else if (t > x[3]) {
+ directSteps = ((y[4] - y[3]) / (x[4] - x[3]) * (t-x[3]) + y[3]);
+ } else if (t > x[2]) {
+ directSteps = ((y[3] - y[2]) / (x[3] - x[2]) * (t-x[2]) + y[2]);
+ } else if (t > x[1]) {
+ directSteps = ((y[2] - y[1]) / (x[2] - x[1]) * (t-x[1]) + y[1]);
+ } else { // (throttle == x[1])
+ directSteps = y[1];
+ };
+
+ float f[5];
+ for (int i = 0; i < 5; i++) f[i] = y[i];
+
+ for (int j = 1; j < 5; j++ )
+ for (int i = 4; i >= j; i--)
+ f[i] = ( (t - x[i-j]) * f[i] - (t - x[i]) * f[i-1]) / (x[i] - x[i-j]);
+
+ float spline = f[4] - ((f[4] - directSteps) * bezier_reduce_factor);
+
+ // safety when stupid values are entered for x and y
+ if (spline > 1.0) return 1.0;
+ if (spline < 0.0) return 0.0;
+
+ return spline;
+}
+
+void app_ppm_configure(ppm_config *conf, throttle_config *throttle_conf) {
#if !SERVO_OUT_ENABLE
config = *conf;
+ throt_config = *throttle_conf;
pulses_without_power = 0;
+
+ has_enough_pid_filter_data = false;
+ filter_ptr = 0;
+ mc_interface_set_cruise_control_status(CRUISE_CONTROL_INACTIVE);
if (is_running) {
- servodec_set_pulse_options(config.pulse_start, config.pulse_end, config.median_filter);
+ servodec_set_pulse_options(config.pulse_start, config.pulse_center, config.pulse_end, config.median_filter);
}
+
+ px[0] = 0.0;
+ px[1] = throt_config.x1_throttle;
+ px[2] = throt_config.x2_throttle;
+ px[3] = throt_config.x3_throttle;
+ px[4] = 1.0;
+ py[0] = 0.0;
+ py[1] = throt_config.y1_throttle;
+ py[2] = throt_config.y2_throttle;
+ py[3] = throt_config.y3_throttle;
+ py[4] = 1.0;
+
+ nx[0] = 0.0;
+ nx[1] = throt_config.x1_neg_throttle;
+ nx[2] = throt_config.x2_neg_throttle;
+ nx[3] = throt_config.x3_neg_throttle;
+ nx[4] = 1.0;
+ ny[0] = 0.0;
+ ny[1] = throt_config.y1_neg_throttle;
+ ny[2] = throt_config.y2_neg_throttle;
+ ny[3] = throt_config.y3_neg_throttle;
+ ny[4] = 1.0;
+
+ direction_hyst = config.max_erpm_for_dir * 0.20;
#else
(void)conf;
#endif
@@ -103,8 +181,9 @@
chRegSetThreadName("APP_PPM");
ppm_tp = chThdGetSelfX();
- servodec_set_pulse_options(config.pulse_start, config.pulse_end, config.median_filter);
+ servodec_set_pulse_options(config.pulse_start, config.pulse_center, config.pulse_end, config.median_filter);
servodec_init(servodec_func);
+
is_running = true;
for(;;) {
@@ -124,26 +203,99 @@
case PPM_CTRL_TYPE_PID_NOREV:
servo_val += 1.0;
servo_val /= 2.0;
+
+ utils_deadband(&servo_val, config.hyst, 1.0);
+
+ servo_val = calculate_throttle_curve_ppm(px, py, throt_config.bezier_reduce_factor, servo_val);
+
+ break;
+ //case PPM_CTRL_TYPE_CURRENT:
+ case PPM_CTRL_TYPE_WATT:
+ utils_deadband(&servo_val, config.hyst, 1.0);
+
+ if (throt_config.adjustable_throttle_enabled && servo_val != 0.0){
+ if (servo_val > 0.0) {
+ servo_val = calculate_throttle_curve_ppm(px, py, throt_config.bezier_reduce_factor, servo_val);
+ } else if (config.max_erpm_for_dir_active == false) {
+ servo_val = -calculate_throttle_curve_ppm(nx, ny, throt_config.bezier_neg_reduce_factor, -servo_val);
+ }
+ }
break;
-
default:
+
+ utils_deadband(&servo_val, config.hyst, 1.0);
+
+ if (throt_config.adjustable_throttle_enabled && servo_val != 0.0){
+ if (servo_val > 0.0) {
+ servo_val = calculate_throttle_curve_ppm(px, py, throt_config.bezier_reduce_factor, servo_val);
+ } else {
+ servo_val = -calculate_throttle_curve_ppm(nx, ny, throt_config.bezier_neg_reduce_factor, -servo_val);
+ }
+ }
+
break;
}
- utils_deadband(&servo_val, config.hyst, 1.0);
-
float current = 0;
+ //float desired_watt = 0;
bool current_mode = false;
bool current_mode_brake = false;
const volatile mc_configuration *mcconf = mc_interface_get_configuration();
bool send_duty = false;
+ //bool send_watt = false;
+ bool send_pid = false;
+
+ // Find lowest RPM and cruise control
+ float rpm_local = mc_interface_get_rpm();
+ float rpm_lowest = rpm_local;
+ float mid_rpm = rpm_local;
+ int motor_count = 1;
+ ppm_cruise cruise_control_status = CRUISE_CONTROL_INACTIVE;
+ if (config.multi_esc) {
+ for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+ can_status_msg *msg = comm_can_get_status_msg_index(i);
+
+ if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
+ // add to middle rpm count
+ mid_rpm += msg->rpm;
+ motor_count += 1;
+ if (fabsf(msg->rpm) < fabsf(rpm_lowest)) {
+ rpm_lowest = msg->rpm;
+ }
+
+ // if any controller (VESC) sends the cruise contol status
+ if (msg->cruise_control_status != CRUISE_CONTROL_INACTIVE) {
+ cruise_control_status = msg->cruise_control_status;
+ }
+ }
+ }
+ }
+
+ // get middle rpm
+ mid_rpm /= motor_count;
+
switch (config.ctrl_type) {
case PPM_CTRL_TYPE_CURRENT:
- case PPM_CTRL_TYPE_CURRENT_NOREV:
current_mode = true;
if (servo_val >= 0.0) {
- current = servo_val * mcconf->l_current_max;
+ // check of can bus send cruise control command
+ if (cruise_control_status != CRUISE_CONTROL_INACTIVE && servo_val == 0.0) {
+ // is rpm in range for cruise control
+ if (rpm_lowest > mcconf->s_pid_min_erpm) {
+ if (pid_rpm == 0) {
+ pid_rpm = rpm_lowest;
+ }
+ current_mode = false;
+ send_pid = true;
+ mc_interface_set_pid_speed_with_cruise_status(rpm_local + pid_rpm - mid_rpm, cruise_control_status);
+ } else {
+ pid_rpm = 0;
+ current = 0.0;
+ }
+ }else{
+ current = servo_val * mcconf->l_current_max;
+ }
} else {
current = servo_val * fabsf(mcconf->l_current_min);
}
@@ -152,21 +304,294 @@
pulses_without_power++;
}
break;
-
+ case PPM_CTRL_TYPE_CURRENT_NOREV:
case PPM_CTRL_TYPE_CURRENT_NOREV_BRAKE:
current_mode = true;
+
if (servo_val >= 0.0) {
- current = servo_val * mcconf->l_current_max;
+ // check of can bus send cruise control command
+ if (cruise_control_status != CRUISE_CONTROL_INACTIVE && servo_val == 0.0) {
+ // is rpm in range for cruise control
+ if (rpm_lowest > mcconf->s_pid_min_erpm) {
+ if (pid_rpm == 0) {
+ pid_rpm = rpm_lowest;
+ }
+ current_mode = false;
+ send_pid = true;
+ mc_interface_set_pid_speed_with_cruise_status(rpm_local + pid_rpm - mid_rpm, cruise_control_status);
+ } else {
+ pid_rpm = 0;
+ current = 0.0;
+ }
+ }else{
+ current = servo_val * mcconf->l_current_max;
+ }
+
} else {
current = fabsf(servo_val * mcconf->l_current_min);
current_mode_brake = true;
}
+ if (servo_val < 0.001) {
+ pulses_without_power++;
+ }
+ break;
+ case PPM_CTRL_TYPE_WATT:
+ current_mode = true;
+
+ if (config.max_erpm_for_dir_active) { // advanced backwards
+ static bool force_brake = true;
+
+ static int8_t did_idle_once = 0;
+
+ // Hysteresis 20 % of actual RPM
+ if (force_brake) {
+ if (rpm_local < config.max_erpm_for_dir - direction_hyst) { // for 2500 it's 2000
+ force_brake = false;
+ did_idle_once = 0;
+ }
+ } else {
+ if (rpm_local > config.max_erpm_for_dir + direction_hyst) { // for 2500 it's 3000
+ force_brake = true;
+ did_idle_once = 0;
+ }
+ }
+
+ if (servo_val >= 0.0) {
+ if (servo_val == 0.0) {
+ // if there was a idle in between then allow going backwards
+ if (did_idle_once == 1 && !force_brake) {
+ did_idle_once = 2;
+ }
+ }else{
+ // accelerated forward or fast enough at least
+ if (rpm_local > -config.max_erpm_for_dir){ // for 2500 it's -2500
+ did_idle_once = 0;
+ }
+ }
+
+ // check of can bus send cruise control command
+ if (cruise_control_status != CRUISE_CONTROL_INACTIVE && servo_val == 0.0) {
+ // is rpm in range for cruise control
+ if (fabsf(rpm_lowest) > mcconf->s_pid_min_erpm) {
+ if (pid_rpm == 0) {
+ pid_rpm = mid_rpm;
+ }
+ current_mode = false;
+ send_pid = true;
+
+ mc_interface_set_pid_speed_with_cruise_status(rpm_local + pid_rpm - mid_rpm, cruise_control_status);
+ } else {
+ pid_rpm = 0;
+ current = 0.0;
+ }
+ }else{
+ if (mcconf->use_max_watt_limit) {
+ current = utils_smallest_of_2(servo_val * mcconf->l_current_max,
+ servo_val * (mcconf->watts_max / GET_INPUT_VOLTAGE() / mc_interface_get_duty_cycle_for_watt_calculation()));
+ } else {
+ current = utils_smallest_of_2(servo_val * mcconf->l_current_max,
+ servo_val * mcconf->l_in_current_max / mc_interface_get_duty_cycle_for_watt_calculation());
+ }
+ }
+ } else {
+
+ // too fast
+ if (force_brake){
+ current_mode_brake = true;
+ }else{
+ // not too fast backwards
+ if (rpm_local > -config.max_erpm_for_dir) { // for 2500 it's -2500
+ // first time that we brake and we are not too fast
+ if (did_idle_once != 2) {
+ did_idle_once = 1;
+ current_mode_brake = true;
+ }
+ // too fast backwards
+ } else {
+ // if brake was active already
+ if (did_idle_once == 1) {
+ current_mode_brake = true;
+ } else {
+ // it's ok to go backwards now braking would be strange now
+ did_idle_once = 2;
+ }
+ }
+ }
+
+ if (current_mode_brake) {
+ // braking
+ if (throt_config.adjustable_throttle_enabled) {
+ // negative calculation
+ servo_val = -calculate_throttle_curve_ppm(nx, ny, throt_config.bezier_neg_reduce_factor, -servo_val);
+ }
+ current = fabsf(servo_val * mcconf->l_current_min);
+ }else {
+ // reverse acceleration
+ if (throt_config.adjustable_throttle_enabled) {
+ // positive calculation
+ servo_val = -calculate_throttle_curve_ppm(px, py, throt_config.bezier_reduce_factor, -servo_val);
+ }
+
+ if (mcconf->use_max_watt_limit) {
+ current = -utils_smallest_of_2(fabsf(servo_val * mcconf->l_current_min),
+ fabsf(servo_val * (mcconf->watts_max / GET_INPUT_VOLTAGE() / mc_interface_get_duty_cycle_for_watt_calculation())));
+ } else {
+ current = -utils_smallest_of_2(fabsf(servo_val * mcconf->l_current_min),
+ fabsf(servo_val * mcconf->l_in_current_max / mc_interface_get_duty_cycle_for_watt_calculation()));
+ }
+ }
+ }
+ } else { // normal backwards
+ if (servo_val >= 0.0) {
+ // check of can bus send cruise control command
+ if (cruise_control_status != CRUISE_CONTROL_INACTIVE && servo_val == 0.0) {
+ // is rpm in range for cruise control
+ if (fabsf(rpm_lowest) > mcconf->s_pid_min_erpm) {
+ if (pid_rpm == 0) {
+ pid_rpm = mid_rpm;
+ }
+ current_mode = false;
+ send_pid = true;
+
+ mc_interface_set_pid_speed_with_cruise_status(rpm_local + pid_rpm - mid_rpm, cruise_control_status);
+ } else {
+ pid_rpm = 0;
+ current = 0.0;
+ }
+ }else{
+ if (mcconf->use_max_watt_limit) {
+ current = utils_smallest_of_2(servo_val * mcconf->l_current_max,
+ servo_val * (mcconf->watts_max / GET_INPUT_VOLTAGE() / mc_interface_get_duty_cycle_for_watt_calculation()));
+ } else {
+ current = utils_smallest_of_2(servo_val * mcconf->l_current_max,
+ servo_val * mcconf->l_in_current_max / mc_interface_get_duty_cycle_for_watt_calculation());
+ }
+ }
+ } else {
+ current = servo_val * fabsf(mcconf->l_current_min);
+ }
+ }
if (servo_val < 0.001) {
pulses_without_power++;
}
break;
+ case PPM_CTRL_TYPE_WATT_NOREV_BRAKE:
+ current_mode = true;
+
+ if (servo_val >= 0.0) {
+ // check of can bus send cruise control command
+ if (cruise_control_status != CRUISE_CONTROL_INACTIVE && servo_val == 0.0) {
+ // is rpm in range for cruise control
+ if (rpm_lowest > mcconf->s_pid_min_erpm) {
+ if (pid_rpm == 0) {
+ pid_rpm = mid_rpm;
+ }
+ current_mode = false;
+ send_pid = true;
+ mc_interface_set_pid_speed_with_cruise_status(rpm_local + pid_rpm - mid_rpm, cruise_control_status);
+
+ } else {
+ pid_rpm = 0;
+ current = 0.0;
+ }
+ }else{
+ if (mcconf->use_max_watt_limit) {
+ current = utils_smallest_of_2(servo_val * mcconf->l_current_max,
+ servo_val * (mcconf->watts_max / GET_INPUT_VOLTAGE() / mc_interface_get_duty_cycle_for_watt_calculation()));
+ } else {
+ current = utils_smallest_of_2(servo_val * mcconf->l_current_max,
+ servo_val * mcconf->l_in_current_max / mc_interface_get_duty_cycle_for_watt_calculation());
+ }
+ }
+ } else {
+
+ current_mode_brake = true;
+
+ // brake as in current mode if not enabled. Calculating via Battery Voltage would brake far too hard
+ current = fabsf(servo_val * mcconf->l_current_min);
+ }
+ if (servo_val < 0.001) {
+ pulses_without_power++;
+ }
+ break;
+ case PPM_CTRL_TYPE_PID_NOACCELERATION:
+ current_mode = true;
+
+ filter_buffer[filter_ptr++] = mid_rpm;
+ if (filter_ptr >= RPM_FILTER_SAMPLES) {
+ filter_ptr = 0;
+ has_enough_pid_filter_data = true;
+ }
+
+ float rpm_filtered = 0.0;
+ // only send when enough values are collected
+ if (has_enough_pid_filter_data) {
+ for (int i = 0; i < RPM_FILTER_SAMPLES; i++) {
+ rpm_filtered += filter_buffer[i];
+ }
+ rpm_filtered /= RPM_FILTER_SAMPLES;
+ }
+
+ if (servo_val >= 0.0) {
+ // check if pid needs to be lowered
+ if (servo_val > 0.0) {
+ // needs to be set first ?
+ if (pid_rpm == 0) {
+ pid_rpm = rpm_filtered;
+ }
+
+ if(rpm_filtered > 1000){
+ float diff = pid_rpm - rpm_filtered;
+ if (diff > 1500) {
+ pid_rpm = pid_rpm - 10;
+ }else if(diff > 500 && rpm_filtered < 1500) {
+ pid_rpm = pid_rpm - 10;
+ }
+ }else{
+ pid_rpm = 0;
+ }
+
+ // if not too slow than set the pid
+ if(pid_rpm > 0 && pid_rpm < config.pid_max_erpm) {
+ current_mode = false;
+
+ send_pid = true;
+ mc_interface_set_pid_speed(rpm_local + pid_rpm - rpm_filtered);
+
+ // overwrite mid_rpm
+ mid_rpm = rpm_filtered;
+ } else {
+ current = 0.0;
+ }
+ }else{
+ current = 0.0;
+ }
+ } else {
+ current = fabsf(servo_val * mcconf->l_current_min);
+ current_mode_brake = true;
+ }
+
+ if (servo_val < 0.001) {
+ pulses_without_power++;
+ }
+ break;
+ case PPM_CTRL_TYPE_CRUISE_CONTROL_SECONDARY_CHANNEL:
+ if (servo_val < -0.3 && config.cruise_left != CRUISE_CONTROL_INACTIVE) {
+ mc_interface_set_cruise_control_status(config.cruise_left);
+ } else if (servo_val > 0.3 && config.cruise_right != CRUISE_CONTROL_INACTIVE) {
+ mc_interface_set_cruise_control_status(config.cruise_right);
+ } else {
+ mc_interface_set_cruise_control_status(CRUISE_CONTROL_INACTIVE);
+ }
+ // Run this loop at 500Hz
+ chThdSleepMilliseconds(2);
+
+ // Reset the timeout
+ //timeout_reset();
+ continue;
+ break;
case PPM_CTRL_TYPE_DUTY:
case PPM_CTRL_TYPE_DUTY_NOREV:
if (fabsf(servo_val) < 0.001) {
@@ -174,7 +599,7 @@
}
if (!(pulses_without_power < MIN_PULSES_WITHOUT_POWER && config.safe_start)) {
- mc_interface_set_duty(servo_val);
+ mc_interface_set_duty(utils_map(servo_val, -1.0, 1.0, -mcconf->l_max_duty, mcconf->l_max_duty));
send_duty = true;
}
break;
@@ -194,6 +619,17 @@
default:
continue;
}
+
+ // switch the mode
+ /*if (servo_val == -1.0){
+ mode_switch_pulses++;
+ }else{
+ if (mode_switch_pulses > 1000){
+ //switch mode
+ //mc_interface_change_speed_mode(); // ???
+ }
+ mode_switch_pulses = 0;
+ }*/
if (pulses_without_power < MIN_PULSES_WITHOUT_POWER && config.safe_start) {
static int pulses_without_power_before = 0;
@@ -205,69 +641,99 @@
continue;
}
- // Find lowest RPM
- float rpm_local = mc_interface_get_rpm();
- float rpm_lowest = rpm_local;
- if (config.multi_esc) {
+ if (send_duty && config.multi_esc) {
+ float duty = mc_interface_get_duty_cycle_now();
+
for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
can_status_msg *msg = comm_can_get_status_msg_index(i);
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
- float rpm_tmp = msg->rpm;
-
- if (fabsf(rpm_tmp) < fabsf(rpm_lowest)) {
- rpm_lowest = rpm_tmp;
- }
+ comm_can_set_duty(msg->id, duty);
}
}
}
- if (send_duty && config.multi_esc) {
- float duty = mc_interface_get_duty_cycle_now();
-
+ if (send_pid && config.multi_esc) {
+
for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
can_status_msg *msg = comm_can_get_status_msg_index(i);
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
- comm_can_set_duty(msg->id, duty);
+ comm_can_set_rpm(msg->id, msg->rpm + pid_rpm - mid_rpm, cruise_control_status);
}
}
}
if (current_mode) {
+
+ pid_rpm = 0; // always reset in current, not that something stupid happens
+
if (current_mode_brake) {
mc_interface_set_brake_current(current);
- // Send brake command to all ESCs seen recently on the CAN bus
- for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
- can_status_msg *msg = comm_can_get_status_msg_index(i);
+ if (config.multi_esc) {
+ // Send brake command to all ESCs seen recently on the CAN bus
+ for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+ can_status_msg *msg = comm_can_get_status_msg_index(i);
- if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
- comm_can_set_current_brake(msg->id, current);
+ if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
+ if (config.ctrl_type == PPM_CTRL_TYPE_WATT_NOREV_BRAKE || config.ctrl_type == PPM_CTRL_TYPE_WATT) {
+ comm_can_set_brake_servo(msg->id, fabsf(servo_val));
+ } else {
+ comm_can_set_current_brake(msg->id, current);
+ }
+ }
}
}
} else {
// Apply soft RPM limit
- if (rpm_lowest > config.rpm_lim_end && current > 0.0) {
- current = mcconf->cc_min_current;
- } else if (rpm_lowest > config.rpm_lim_start && current > 0.0) {
- current = utils_map(rpm_lowest, config.rpm_lim_start, config.rpm_lim_end, current, mcconf->cc_min_current);
- } else if (rpm_lowest < -config.rpm_lim_end && current < 0.0) {
- current = mcconf->cc_min_current;
- } else if (rpm_lowest < -config.rpm_lim_start && current < 0.0) {
- rpm_lowest = -rpm_lowest;
- current = -current;
- current = utils_map(rpm_lowest, config.rpm_lim_start, config.rpm_lim_end, current, mcconf->cc_min_current);
- current = -current;
- rpm_lowest = -rpm_lowest;
- }
+ bool use_min_current = false;
+ if (rpm_lowest > config.rpm_lim_end) {
+ if (servo_val > 0.0) {
+ use_min_current = true;
+ servo_val = 0.000001; // make sure min current ius used
+ }
+ if (current > 0.0) {
+ current = mcconf->cc_min_current;
+ }
+
+ } else if (rpm_lowest > config.rpm_lim_start) {
+ if (servo_val > 0.0) {
+ use_min_current = true;
+ servo_val = utils_map(rpm_lowest, config.rpm_lim_start, config.rpm_lim_end, servo_val, 0.000001);
+ }
+ if (current > 0.0) {
+ current = utils_highest_of_2(utils_map(rpm_lowest, config.rpm_lim_start, config.rpm_lim_end, current, 0.0), mcconf->cc_min_current);
+ }
+ } else if (rpm_lowest < -config.rpm_lim_end) {
+ if (servo_val < 0.0) {
+ use_min_current = true;
+ servo_val = -0.000001; // make sure min current ius used
+ }
+ if (current < 0.0) {
+ current = -mcconf->cc_min_current; // wrong in original
+ }
+
+ } else if (rpm_lowest < -config.rpm_lim_start) {
+ if (servo_val < 0.0) {
+ use_min_current = true;
+ servo_val = -utils_map(-rpm_lowest, config.rpm_lim_start, config.rpm_lim_end, -servo_val, 0.000001);
+ }
+ if (current < 0.0) {
+ current = utils_smallest_of_2(-utils_map(-rpm_lowest, config.rpm_lim_start, config.rpm_lim_end, -current, 0.0), -mcconf->cc_min_current);
+ }
+ }
+
float current_out = current;
+ float servo_val_out = servo_val;
bool is_reverse = false;
- if (current_out < 0.0) {
+ if (servo_val < 0.0) {
is_reverse = true;
current_out = -current_out;
current = -current;
+ servo_val_out = -servo_val_out;
+ servo_val = -servo_val;
rpm_local = -rpm_local;
rpm_lowest = -rpm_lowest;
}
@@ -285,25 +751,38 @@
}
float diff = rpm_tmp - rpm_lowest;
- current_out = utils_map(diff, 0.0, config.tc_max_diff, current, 0.0);
- if (current_out < mcconf->cc_min_current) {
- current_out = 0.0;
+ if (diff > config.tc_offset) {
+ current_out = utils_map(diff - config.tc_offset, 0.0, config.tc_max_diff - config.tc_offset, current, 0.0);
+ servo_val_out = utils_map(diff - config.tc_offset, 0.0, config.tc_max_diff - config.tc_offset, servo_val, 0.0);
+ } else {
+ current_out = current;
+ servo_val_out = servo_val;
}
}
if (is_reverse) {
- comm_can_set_current(msg->id, -current_out);
+ if (config.ctrl_type == PPM_CTRL_TYPE_WATT_NOREV_BRAKE || (config.ctrl_type == PPM_CTRL_TYPE_WATT && config.max_erpm_for_dir_active)) { // if not active you want real brakes
+ comm_can_set_servo(msg->id, -servo_val_out, use_min_current);
+ } else {
+ comm_can_set_current(msg->id, -current_out);
+ }
} else {
- comm_can_set_current(msg->id, current_out);
+ if (config.ctrl_type == PPM_CTRL_TYPE_WATT_NOREV_BRAKE || config.ctrl_type == PPM_CTRL_TYPE_WATT) {
+ comm_can_set_servo(msg->id, servo_val_out, use_min_current);
+ } else {
+ comm_can_set_current(msg->id, current_out);
+ }
}
}
}
if (config.tc) {
+
float diff = rpm_local - rpm_lowest;
- current_out = utils_map(diff, 0.0, config.tc_max_diff, current, 0.0);
- if (current_out < mcconf->cc_min_current) {
- current_out = 0.0;
+ if (diff > config.tc_offset) {
+ current_out = utils_map(diff - config.tc_offset, 0.0, config.tc_max_diff - config.tc_offset, current, 0.0);
+ } else {
+ current_out = current;
}
}
}
@@ -315,7 +794,6 @@
}
}
}
-
}
}
#endif
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/applications/app_sten.c Raptor2 Sources/bldc-firmware_2_80/applications/app_sten.c
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/applications/app_sten.c 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/applications/app_sten.c 2016-09-19 10:07:18.000000000 +0100
@@ -32,6 +32,8 @@
#include "utils.h"
#include "hw.h"
#include "timeout.h"
+#include "comm_can.h"
+
#include <math.h>
// Settings
@@ -147,15 +149,7 @@
}
static void set_output(float output) {
- output /= (1.0 - HYST);
-
- if (output > HYST) {
- output -= HYST;
- } else if (output < -HYST) {
- output += HYST;
- } else {
- output = 0.0;
- }
+ utils_deadband(&output, HYST, 1.0);
const float rpm = mc_interface_get_rpm();
@@ -182,13 +176,11 @@
current_p2 = current_p1;
current_p1 = current;
- if (fabsf(current) < mc_interface_get_configuration()->cc_min_current) {
- current = -mc_interface_get_configuration()->cc_min_current;
- }
-
mc_interface_set_current(current);
+ comm_can_set_current(255, current);
} else {
mc_interface_set_brake_current(output * mc_interface_get_configuration()->l_current_min);
+ comm_can_set_current_brake(255, output * mc_interface_get_configuration()->l_current_min);
}
}
Only in Raptor2 Sources/bldc-firmware_2_80: build
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/comm_can.c Raptor2 Sources/bldc-firmware_2_80/comm_can.c
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/comm_can.c 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/comm_can.c 2017-04-13 17:59:58.000000000 +0100
@@ -23,6 +23,7 @@
*/
#include <string.h>
+#include <math.h>
#include "comm_can.h"
#include "ch.h"
#include "hal.h"
@@ -179,7 +180,19 @@
case CAN_PACKET_SET_RPM:
ind = 0;
- mc_interface_set_pid_speed((float)buffer_get_int32(rxmsg.data8, &ind));
+ mc_interface_set_pid_speed_with_cruise_status((float)buffer_get_int32(rxmsg.data8, &ind), rxmsg.data8[ind++]);
+ timeout_reset();
+ break;
+
+ case CAN_PACKET_SET_SERVO:
+ ind = 0;
+ mc_interface_set_servo((float)buffer_get_int32(rxmsg.data8, &ind) / 1000000.0, rxmsg.data8[ind++]);
+ timeout_reset();
+ break;
+
+ case CAN_PACKET_SET_BRAKE_SERVO:
+ ind = 0;
+ mc_interface_set_brake_servo((float)buffer_get_int32(rxmsg.data8, &ind) / 1000000.0);
timeout_reset();
break;
@@ -256,7 +269,8 @@
stat_tmp->rx_time = chVTGetSystemTime();
stat_tmp->rpm = (float)buffer_get_int32(rxmsg.data8, &ind);
stat_tmp->current = (float)buffer_get_int16(rxmsg.data8, &ind) / 10.0;
- stat_tmp->duty = (float)buffer_get_int16(rxmsg.data8, &ind) / 1000.0;
+ stat_tmp->duty = rxmsg.data8[ind++];
+ stat_tmp->cruise_control_status = rxmsg.data8[ind++];
break;
}
}
@@ -285,7 +299,9 @@
uint8_t buffer[8];
buffer_append_int32(buffer, (int32_t)mc_interface_get_rpm(), &send_index);
buffer_append_int16(buffer, (int16_t)(mc_interface_get_tot_current() * 10.0), &send_index);
- buffer_append_int16(buffer, (int16_t)(mc_interface_get_duty_cycle_now() * 1000.0), &send_index);
+ buffer[send_index++] = (int8_t) floor(mc_interface_get_duty_cycle_now() * 100);
+ buffer[send_index++] = mc_interface_get_cruise_control_status();
+
comm_can_transmit(app_get_configuration()->controller_id | ((uint32_t)CAN_PACKET_STATUS << 8), buffer, send_index);
}
@@ -349,12 +365,12 @@
} else {
unsigned int end_a = 0;
for (unsigned int i = 0;i < len;i += 7) {
- end_a = i;
-
if (i > 255) {
break;
}
+ end_a = i + 7;
+
uint8_t send_len = 7;
send_buffer[0] = i;
@@ -410,6 +426,21 @@
comm_can_transmit(controller_id | ((uint32_t)CAN_PACKET_SET_CURRENT << 8), buffer, send_index);
}
+void comm_can_set_servo(uint8_t controller_id, float servo_val, bool use_min_current) {
+ int32_t send_index = 0;
+ uint8_t buffer[5];
+ buffer_append_int32(buffer, (int32_t)(servo_val * 1000000.0), &send_index);
+ buffer[send_index++] = use_min_current;
+ comm_can_transmit(controller_id | ((uint32_t)CAN_PACKET_SET_SERVO << 8), buffer, send_index);
+}
+
+void comm_can_set_brake_servo(uint8_t controller_id, float servo_val) {
+ int32_t send_index = 0;
+ uint8_t buffer[4];
+ buffer_append_int32(buffer, (int32_t)(servo_val * 1000000.0), &send_index);
+ comm_can_transmit(controller_id | ((uint32_t)CAN_PACKET_SET_BRAKE_SERVO << 8), buffer, send_index);
+}
+
void comm_can_set_current_brake(uint8_t controller_id, float current) {
int32_t send_index = 0;
uint8_t buffer[4];
@@ -417,10 +448,12 @@
comm_can_transmit(controller_id | ((uint32_t)CAN_PACKET_SET_CURRENT_BRAKE << 8), buffer, send_index);
}
-void comm_can_set_rpm(uint8_t controller_id, float rpm) {
+void comm_can_set_rpm(uint8_t controller_id, float rpm, ppm_cruise cruise_status) {
int32_t send_index = 0;
- uint8_t buffer[4];
+ uint8_t buffer[5];
buffer_append_int32(buffer, (int32_t)rpm, &send_index);
+ buffer[send_index++] = cruise_status;
+
comm_can_transmit(controller_id | ((uint32_t)CAN_PACKET_SET_RPM << 8), buffer, send_index);
}
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/comm_can.h Raptor2 Sources/bldc-firmware_2_80/comm_can.h
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/comm_can.h 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/comm_can.h 2017-04-13 18:00:08.000000000 +0100
@@ -29,7 +29,7 @@
// Settings
#define CAN_STATUS_MSG_INT_MS 1
-#define CAN_STATUS_MSGS_TO_STORE 10
+#define CAN_STATUS_MSGS_TO_STORE 3
// Functions
void comm_can_init(void);
@@ -38,7 +38,9 @@
void comm_can_set_duty(uint8_t controller_id, float duty);
void comm_can_set_current(uint8_t controller_id, float current);
void comm_can_set_current_brake(uint8_t controller_id, float current);
-void comm_can_set_rpm(uint8_t controller_id, float rpm);
+void comm_can_set_servo(uint8_t controller_id, float servo_val, bool use_min_current);
+void comm_can_set_brake_servo(uint8_t controller_id, float servo_val);
+void comm_can_set_rpm(uint8_t controller_id, float rpm, ppm_cruise cruise_status);
void comm_can_set_pos(uint8_t controller_id, float pos);
can_status_msg *comm_can_get_status_msg_index(int index);
can_status_msg *comm_can_get_status_msg_id(int id);
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/commands.c Raptor2 Sources/bldc-firmware_2_80/commands.c
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/commands.c 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/commands.c 2017-06-16 22:45:08.000000000 +0100
@@ -25,7 +25,7 @@
#include "commands.h"
#include "ch.h"
#include "hal.h"
-#include "main.h"
+#include "mc_interface.h"
#include "stm32f4xx_conf.h"
#include "servo.h"
#include "servo_simple.h"
@@ -64,10 +64,14 @@
static int8_t detect_hall_table[8];
static int detect_hall_res;
static void(*send_func)(unsigned char *data, unsigned int len) = 0;
+static void(*appdata_func)(unsigned char *data, unsigned int len) = 0;
static disp_pos_mode display_position_mode;
+static uint8_t remote_Mode;
+
void commands_init(void) {
chThdCreateStatic(detect_thread_wa, sizeof(detect_thread_wa), NORMALPRIO, detect_thread, NULL);
+ remote_Mode = 0;
}
/**
@@ -118,6 +122,9 @@
app_configuration appconf;
uint16_t flash_res;
uint32_t new_app_offset;
+ chuck_data chuck_d_tmp;
+
+ bool sendActualSpeedMode = false;
packet_id = data[0];
data++;
@@ -171,7 +178,7 @@
buffer_append_float32(send_buffer, mc_interface_read_reset_avg_input_current(), 1e2, &ind);
buffer_append_float16(send_buffer, mc_interface_get_duty_cycle_now(), 1e3, &ind);
buffer_append_float32(send_buffer, mc_interface_get_rpm(), 1e0, &ind);
- buffer_append_float16(send_buffer, GET_INPUT_VOLTAGE(), 1e1, &ind);
+ buffer_append_float16(send_buffer, GET_INPUT_VOLTAGE(), 1e2, &ind);
buffer_append_float32(send_buffer, mc_interface_get_amp_hours(false), 1e4, &ind);
buffer_append_float32(send_buffer, mc_interface_get_amp_hours_charged(false), 1e4, &ind);
buffer_append_float32(send_buffer, mc_interface_get_watt_hours(false), 1e4, &ind);
@@ -243,6 +250,7 @@
case COMM_SET_MCCONF:
mcconf = *mc_interface_get_configuration();
+ appconf = *app_get_configuration();
ind = 0;
mcconf.pwm_mode = data[ind++];
@@ -251,22 +259,61 @@
mcconf.sensor_mode = data[ind++];
mcconf.l_current_max = buffer_get_float32(data, 1000.0, &ind);
+ if (mcconf.l_current_max > 80.0){
+ mcconf.l_current_max = 80.0;
+ }
+
mcconf.l_current_min = buffer_get_float32(data, 1000.0, &ind);
+ if (mcconf.l_current_min < -80.0){
+ mcconf.l_current_min = -80.0;
+ }
+
mcconf.l_in_current_max = buffer_get_float32(data, 1000.0, &ind);
+ if (mcconf.l_in_current_max > 40.0){
+ mcconf.l_in_current_max = 40.0;
+ }
+
mcconf.l_in_current_min = buffer_get_float32(data, 1000.0, &ind);
+ if (mcconf.l_in_current_min < -12.0){
+ mcconf.l_in_current_min = -12.0;
+ }
+
mcconf.l_abs_current_max = buffer_get_float32(data, 1000.0, &ind);
mcconf.l_min_erpm = buffer_get_float32(data, 1000.0, &ind);
mcconf.l_max_erpm = buffer_get_float32(data, 1000.0, &ind);
mcconf.l_max_erpm_fbrake = buffer_get_float32(data, 1000.0, &ind);
mcconf.l_max_erpm_fbrake_cc = buffer_get_float32(data, 1000.0, &ind);
mcconf.l_min_vin = buffer_get_float32(data, 1000.0, &ind);
+ if (mcconf.l_min_vin < 8.0){
+ mcconf.l_min_vin = 8.0;
+ }
+
mcconf.l_max_vin = buffer_get_float32(data, 1000.0, &ind);
+ if (mcconf.l_max_vin > 57.0){
+ mcconf.l_max_vin = 57.0;
+ }
+
mcconf.l_battery_cut_start = buffer_get_float32(data, 1000.0, &ind);
+ if (mcconf.l_battery_cut_start < 31.0){
+ mcconf.l_battery_cut_start = 31.0;
+ }
+
mcconf.l_battery_cut_end = buffer_get_float32(data, 1000.0, &ind);
+ if (mcconf.l_battery_cut_end < 29.0){
+ mcconf.l_battery_cut_end = 29.0;
+ }
mcconf.l_slow_abs_current = data[ind++];
mcconf.l_rpm_lim_neg_torque = data[ind++];
mcconf.l_temp_fet_start = buffer_get_float32(data, 1000.0, &ind);
+ if (mcconf.l_temp_fet_start > 80.0){
+ mcconf.l_temp_fet_start = 80.0;
+ }
+
mcconf.l_temp_fet_end = buffer_get_float32(data, 1000.0, &ind);
+ if (mcconf.l_temp_fet_end > 100.0){
+ mcconf.l_temp_fet_end = 100.0;
+ }
+
mcconf.l_temp_motor_start = buffer_get_float32(data, 1000.0, &ind);
mcconf.l_temp_motor_end = buffer_get_float32(data, 1000.0, &ind);
mcconf.l_min_duty = buffer_get_float32(data, 1000000.0, &ind);
@@ -280,17 +327,32 @@
mcconf.sl_min_erpm = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.sl_min_erpm_cycle_int_limit = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.sl_max_fullbreak_current_dir_change = (float)buffer_get_int32(data, &ind) / 1000.0;
- mcconf.sl_cycle_int_limit = (float)buffer_get_int32(data, &ind) / 1000.0;
+ if (!appconf.send_can_status || mcconf.l_temp_motor_end == 101.0) {
+ mcconf.sl_cycle_int_limit = (float)buffer_get_int32(data, &ind) / 1000.0;
+ }else{
+ ind += 4;
+ }
mcconf.sl_phase_advance_at_br = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.sl_cycle_int_rpm_br = (float)buffer_get_int32(data, &ind) / 1000.0;
- mcconf.sl_bemf_coupling_k = (float)buffer_get_int32(data, &ind) / 1000.0;
+ if (!appconf.send_can_status || mcconf.l_temp_motor_end == 101.0) {
+ mcconf.sl_bemf_coupling_k = (float)buffer_get_int32(data, &ind) / 1000.0;
+ }else{
+ ind += 4;
+ }
- memcpy(mcconf.hall_table, data + ind, 8);
+ if (!appconf.send_can_status || mcconf.l_temp_motor_end == 101.0) {
+ memcpy(mcconf.hall_table, data + ind, 8);
+ }
ind += 8;
+
mcconf.hall_sl_erpm = (float)buffer_get_int32(data, &ind) / 1000.0;
- mcconf.foc_current_kp = buffer_get_float32(data, 1e5, &ind);
- mcconf.foc_current_ki = buffer_get_float32(data, 1e5, &ind);
+ if (!appconf.send_can_status || mcconf.l_temp_motor_end == 101.0) {
+ mcconf.foc_current_kp = buffer_get_float32(data, 1e5, &ind);
+ mcconf.foc_current_ki = buffer_get_float32(data, 1e5, &ind);
+ } else {
+ ind += 8;
+ }
mcconf.foc_f_sw = buffer_get_float32(data, 1e3, &ind);
mcconf.foc_dt_us = buffer_get_float32(data, 1e6, &ind);
mcconf.foc_encoder_inverted = data[ind++];
@@ -299,10 +361,15 @@
mcconf.foc_sensor_mode = data[ind++];
mcconf.foc_pll_kp = buffer_get_float32(data, 1e3, &ind);
mcconf.foc_pll_ki = buffer_get_float32(data, 1e3, &ind);
- mcconf.foc_motor_l = buffer_get_float32(data, 1e8, &ind);
- mcconf.foc_motor_r = buffer_get_float32(data, 1e5, &ind);
- mcconf.foc_motor_flux_linkage = buffer_get_float32(data, 1e5, &ind);
- mcconf.foc_observer_gain = buffer_get_float32(data, 1e0, &ind);
+ if (!appconf.send_can_status || mcconf.l_temp_motor_end == 101.0) {
+ mcconf.foc_motor_l = buffer_get_float32(data, 1e8, &ind);
+ mcconf.foc_motor_r = buffer_get_float32(data, 1e5, &ind);
+ mcconf.foc_motor_flux_linkage = buffer_get_float32(data, 1e5, &ind);
+ mcconf.foc_observer_gain = buffer_get_float32(data, 1e0, &ind);
+ }else{
+ ind += 16;
+ }
+
mcconf.foc_duty_dowmramp_kp = buffer_get_float32(data, 1e3, &ind);
mcconf.foc_duty_dowmramp_ki = buffer_get_float32(data, 1e3, &ind);
mcconf.foc_openloop_rpm = buffer_get_float32(data, 1e3, &ind);
@@ -310,33 +377,64 @@
mcconf.foc_sl_openloop_time = buffer_get_float32(data, 1e3, &ind);
mcconf.foc_sl_d_current_duty = buffer_get_float32(data, 1e3, &ind);
mcconf.foc_sl_d_current_factor = buffer_get_float32(data, 1e3, &ind);
+
+ if (!appconf.send_can_status || mcconf.l_temp_motor_end == 101.0) {
+ memcpy(mcconf.foc_hall_table, data + ind, 8);
+ }
+ ind += 8;
+
+ mcconf.foc_sl_erpm = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.s_pid_kp = (float)buffer_get_int32(data, &ind) / 1000000.0;
mcconf.s_pid_ki = (float)buffer_get_int32(data, &ind) / 1000000.0;
mcconf.s_pid_kd = (float)buffer_get_int32(data, &ind) / 1000000.0;
mcconf.s_pid_min_erpm = (float)buffer_get_int32(data, &ind) / 1000.0;
-
+
mcconf.p_pid_kp = (float)buffer_get_int32(data, &ind) / 1000000.0;
mcconf.p_pid_ki = (float)buffer_get_int32(data, &ind) / 1000000.0;
mcconf.p_pid_kd = (float)buffer_get_int32(data, &ind) / 1000000.0;
+ mcconf.p_pid_ang_div = (float)buffer_get_int32(data, &ind) / 100000.0;
mcconf.cc_startup_boost_duty = (float)buffer_get_int32(data, &ind) / 1000000.0;
mcconf.cc_min_current = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.cc_gain = (float)buffer_get_int32(data, &ind) / 1000000.0;
- mcconf.cc_ramp_step_max = (float)buffer_get_int32(data, &ind) / 1000000.0;
+ mcconf.cc_ramp_step_max = buffer_get_float32(data, 1e6, &ind);
mcconf.m_fault_stop_time_ms = buffer_get_int32(data, &ind);
mcconf.m_duty_ramp_step = (float)buffer_get_float32(data, 1000000.0, &ind);
mcconf.m_duty_ramp_step_rpm_lim = (float)buffer_get_float32(data, 1000000.0, &ind);
mcconf.m_current_backoff_gain = (float)buffer_get_float32(data, 1000000.0, &ind);
mcconf.m_encoder_counts = buffer_get_uint32(data, &ind);
+ mcconf.m_sensor_port_mode = data[ind++];
+
+ // new config
+ mcconf.s_pid_breaking_enabled = data[ind++];
+ mcconf.use_max_watt_limit = data[ind++];
+ mcconf.watts_max = (float)buffer_get_int16(data, &ind);
+ // new config end
conf_general_store_mc_configuration(&mcconf);
mc_interface_set_configuration(&mcconf);
-
-#if ENCODER_ENABLE
- encoder_set_counts(mcconf.m_encoder_counts);
-#endif
+
+ if (!appconf.send_can_status) {
+
+ ind = 0;
+ // send to all others
+ uint8_t send_buffer_can[PACKET_MAX_PL_LEN];
+
+ send_buffer_can[ind++] = packet_id;
+ for(unsigned int i = 0; i < len; i++){
+ send_buffer_can[ind++] = data[i];
+ }
+
+ for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+ can_status_msg *msg = comm_can_get_status_msg_index(i);
+
+ if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < 1) {
+ comm_can_send_buffer(msg->id, send_buffer_can, len + 1, false);
+ }
+ }
+ }
ind = 0;
send_buffer[ind++] = packet_id;
@@ -414,15 +512,19 @@
buffer_append_float32(send_buffer, mcconf.foc_sl_openloop_time, 1e3, &ind);
buffer_append_float32(send_buffer, mcconf.foc_sl_d_current_duty, 1e3, &ind);
buffer_append_float32(send_buffer, mcconf.foc_sl_d_current_factor, 1e3, &ind);
+ memcpy(send_buffer + ind, mcconf.foc_hall_table, 8);
+ ind += 8;
+ buffer_append_int32(send_buffer, (int32_t)(mcconf.foc_sl_erpm * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.s_pid_kp * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.s_pid_ki * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.s_pid_kd * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.s_pid_min_erpm * 1000.0), &ind);
-
+
buffer_append_int32(send_buffer, (int32_t)(mcconf.p_pid_kp * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.p_pid_ki * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.p_pid_kd * 1000000.0), &ind);
+ buffer_append_float32(send_buffer, mcconf.p_pid_ang_div, 1e5, &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.cc_startup_boost_duty * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.cc_min_current * 1000.0), &ind);
@@ -434,6 +536,13 @@
buffer_append_float32(send_buffer, mcconf.m_duty_ramp_step_rpm_lim, 1000000.0, &ind);
buffer_append_float32(send_buffer, mcconf.m_current_backoff_gain, 1000000.0, &ind);
buffer_append_uint32(send_buffer, mcconf.m_encoder_counts, &ind);
+ send_buffer[ind++] = mcconf.m_sensor_port_mode;
+
+ // new config
+ send_buffer[ind++] = mcconf.s_pid_breaking_enabled;
+ send_buffer[ind++] = mcconf.use_max_watt_limit;
+ buffer_append_int16(send_buffer, (int16_t)(mcconf.watts_max), &ind);
+ // new config end
commands_send_packet(send_buffer, ind);
break;
@@ -462,7 +571,7 @@
appconf.app_ppm_conf.multi_esc = data[ind++];
appconf.app_ppm_conf.tc = data[ind++];
appconf.app_ppm_conf.tc_max_diff = (float)buffer_get_int32(data, &ind) / 1000.0;
-
+
appconf.app_adc_conf.ctrl_type = data[ind++];
appconf.app_adc_conf.hyst = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_adc_conf.voltage_start = (float)buffer_get_int32(data, &ind) / 1000.0;
@@ -491,7 +600,45 @@
appconf.app_chuk_conf.multi_esc = data[ind++];
appconf.app_chuk_conf.tc = data[ind++];
appconf.app_chuk_conf.tc_max_diff = buffer_get_float32(data, 1000.0, &ind);
-
+
+ appconf.app_nrf_conf.speed = data[ind++];
+ appconf.app_nrf_conf.power = data[ind++];
+ appconf.app_nrf_conf.crc_type = data[ind++];
+ appconf.app_nrf_conf.retry_delay = data[ind++];
+ appconf.app_nrf_conf.retries = data[ind++];
+ appconf.app_nrf_conf.channel = data[ind++];
+ memcpy(appconf.app_nrf_conf.address, data + ind, 3);
+ ind += 3;
+ appconf.app_nrf_conf.send_crc_ack = data[ind++];
+
+ // new config
+ appconf.app_ppm_conf.pulse_center = (float)buffer_get_int32(data, &ind) / 1000.0;
+ appconf.app_ppm_conf.tc_offset = (float)buffer_get_int32(data, &ind) / 1000.0;
+ appconf.app_ppm_conf.cruise_left = data[ind++];
+ appconf.app_ppm_conf.cruise_right = data[ind++];
+ appconf.app_ppm_conf.max_erpm_for_dir_active = data[ind++];
+ appconf.app_ppm_conf.max_erpm_for_dir = buffer_get_float32(data, 1000.0, &ind);
+
+ appconf.app_chuk_conf.tc_offset = (float)buffer_get_int32(data, &ind) / 1000.0;
+ appconf.app_chuk_conf.buttons_mirrored = data[ind++];
+
+ appconf.app_throttle_conf.adjustable_throttle_enabled = data[ind++];
+ appconf.app_throttle_conf.y1_throttle = (float)buffer_get_int16(data, &ind) / 1000.0;
+ appconf.app_throttle_conf.y2_throttle = (float)buffer_get_int16(data, &ind) / 1000.0;
+ appconf.app_throttle_conf.y3_throttle = (float)buffer_get_int16(data, &ind) / 1000.0;
+ appconf.app_throttle_conf.x1_throttle = (float)buffer_get_int16(data, &ind) / 1000.0;
+ appconf.app_throttle_conf.x2_throttle = (float)buffer_get_int16(data, &ind) / 1000.0;
+ appconf.app_throttle_conf.x3_throttle = (float)buffer_get_int16(data, &ind) / 1000.0;
+ appconf.app_throttle_conf.bezier_reduce_factor = (float)buffer_get_int16(data, &ind) / 100.0;
+ appconf.app_throttle_conf.y1_neg_throttle = (float)buffer_get_int16(data, &ind) / 1000.0;
+ appconf.app_throttle_conf.y2_neg_throttle = (float)buffer_get_int16(data, &ind) / 1000.0;
+ appconf.app_throttle_conf.y3_neg_throttle = (float)buffer_get_int16(data, &ind) / 1000.0;
+ appconf.app_throttle_conf.x1_neg_throttle = (float)buffer_get_int16(data, &ind) / 1000.0;
+ appconf.app_throttle_conf.x2_neg_throttle = (float)buffer_get_int16(data, &ind) / 1000.0;
+ appconf.app_throttle_conf.x3_neg_throttle = (float)buffer_get_int16(data, &ind) / 1000.0;
+ appconf.app_throttle_conf.bezier_neg_reduce_factor = (float)buffer_get_int16(data, &ind) / 100.0;
+ // new config end
+
conf_general_store_app_configuration(&appconf);
app_set_configuration(&appconf);
timeout_configure(appconf.timeout_msec, appconf.timeout_brake_current);
@@ -531,7 +678,7 @@
send_buffer[ind++] = appconf.app_ppm_conf.multi_esc;
send_buffer[ind++] = appconf.app_ppm_conf.tc;
buffer_append_int32(send_buffer, (int32_t)(appconf.app_ppm_conf.tc_max_diff * 1000.0), &ind);
-
+
send_buffer[ind++] = appconf.app_adc_conf.ctrl_type;
buffer_append_int32(send_buffer, (int32_t)(appconf.app_adc_conf.hyst * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(appconf.app_adc_conf.voltage_start * 1000.0), &ind);
@@ -560,7 +707,45 @@
send_buffer[ind++] = appconf.app_chuk_conf.multi_esc;
send_buffer[ind++] = appconf.app_chuk_conf.tc;
buffer_append_int32(send_buffer, (int32_t)(appconf.app_chuk_conf.tc_max_diff * 1000.0), &ind);
-
+
+ send_buffer[ind++] = appconf.app_nrf_conf.speed;
+ send_buffer[ind++] = appconf.app_nrf_conf.power;
+ send_buffer[ind++] = appconf.app_nrf_conf.crc_type;
+ send_buffer[ind++] = appconf.app_nrf_conf.retry_delay;
+ send_buffer[ind++] = appconf.app_nrf_conf.retries;
+ send_buffer[ind++] = appconf.app_nrf_conf.channel;
+ memcpy(send_buffer + ind, appconf.app_nrf_conf.address, 3);
+ ind += 3;
+ send_buffer[ind++] = appconf.app_nrf_conf.send_crc_ack;
+
+ // new config
+ buffer_append_int32(send_buffer, (int32_t)(appconf.app_ppm_conf.pulse_center * 1000.0), &ind);
+ buffer_append_int32(send_buffer, (int32_t)(appconf.app_ppm_conf.tc_offset * 1000.0), &ind);
+ send_buffer[ind++] = appconf.app_ppm_conf.cruise_left;
+ send_buffer[ind++] = appconf.app_ppm_conf.cruise_right;
+ send_buffer[ind++] = appconf.app_ppm_conf.max_erpm_for_dir_active;
+ buffer_append_float32(send_buffer, appconf.app_ppm_conf.max_erpm_for_dir, 1000.0, &ind);
+
+ buffer_append_int32(send_buffer, (int32_t)(appconf.app_chuk_conf.tc_offset * 1000.0), &ind);
+ send_buffer[ind++] = appconf.app_chuk_conf.buttons_mirrored;
+
+ send_buffer[ind++] = appconf.app_throttle_conf.adjustable_throttle_enabled;
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.y1_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.y2_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.y3_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.x1_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.x2_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.x3_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.bezier_reduce_factor * 100.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.y1_neg_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.y2_neg_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.y3_neg_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.x1_neg_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.x2_neg_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.x3_neg_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.bezier_neg_reduce_factor * 100.0), &ind);
+ // end new config
+
commands_send_packet(send_buffer, ind);
break;
@@ -569,7 +754,7 @@
at_start = data[ind++];
sample_len = buffer_get_uint16(data, &ind);
decimation = data[ind++];
- main_sample_print_data(at_start, sample_len, decimation);
+ mc_interface_sample_print_data(at_start, sample_len, decimation);
break;
case COMM_TERMINAL_CMD:
@@ -633,39 +818,74 @@
break;
case COMM_DETECT_ENCODER: {
-#if ENCODER_ENABLE
- mcconf = *mc_interface_get_configuration();
- mcconf_old = mcconf;
+ if (encoder_is_configured()) {
+ mcconf = *mc_interface_get_configuration();
+ mcconf_old = mcconf;
- ind = 0;
- float current = buffer_get_float32(data, 1e3, &ind);
+ ind = 0;
+ float current = buffer_get_float32(data, 1e3, &ind);
- mcconf.motor_type = MOTOR_TYPE_FOC;
- mcconf.foc_f_sw = 10000.0;
- mcconf.foc_current_kp = 0.01;
- mcconf.foc_current_ki = 10.0;
- mc_interface_set_configuration(&mcconf);
+ mcconf.motor_type = MOTOR_TYPE_FOC;
+ mcconf.foc_f_sw = 10000.0;
+ mcconf.foc_current_kp = 0.01;
+ mcconf.foc_current_ki = 10.0;
+ mc_interface_set_configuration(&mcconf);
+
+ float offset = 0.0;
+ float ratio = 0.0;
+ bool inverted = false;
+ mcpwm_foc_encoder_detect(current, false, &offset, &ratio, &inverted);
+ mc_interface_set_configuration(&mcconf_old);
+
+ ind = 0;
+ send_buffer[ind++] = COMM_DETECT_ENCODER;
+ buffer_append_float32(send_buffer, offset, 1e6, &ind);
+ buffer_append_float32(send_buffer, ratio, 1e6, &ind);
+ send_buffer[ind++] = inverted;
+ commands_send_packet(send_buffer, ind);
+ } else {
+ ind = 0;
+ send_buffer[ind++] = COMM_DETECT_ENCODER;
+ buffer_append_float32(send_buffer, 1001.0, 1e6, &ind);
+ buffer_append_float32(send_buffer, 0.0, 1e6, &ind);
+ send_buffer[ind++] = false;
+ commands_send_packet(send_buffer, ind);
+ }
+ }
+ break;
- float offset = 0.0;
- float ratio = 0.0;
- bool inverted = false;
- mcpwm_foc_encoder_detect(current, false, &offset, &ratio, &inverted);
- mc_interface_set_configuration(&mcconf_old);
+ case COMM_DETECT_HALL_FOC: {
+ mcconf = *mc_interface_get_configuration();
- ind = 0;
- send_buffer[ind++] = COMM_DETECT_ENCODER;
- buffer_append_float32(send_buffer, offset, 1e6, &ind);
- buffer_append_float32(send_buffer, ratio, 1e6, &ind);
- send_buffer[ind++] = inverted;
- commands_send_packet(send_buffer, ind);
-#else
- ind = 0;
- send_buffer[ind++] = COMM_DETECT_ENCODER;
- buffer_append_float32(send_buffer, 1001.0, 1e6, &ind);
- buffer_append_float32(send_buffer, 0.0, 1e6, &ind);
- send_buffer[ind++] = false;
- commands_send_packet(send_buffer, ind);
-#endif
+ if (mcconf.m_sensor_port_mode == SENSOR_PORT_MODE_HALL) {
+ mcconf_old = mcconf;
+ ind = 0;
+ float current = buffer_get_float32(data, 1e3, &ind);
+
+ mcconf.motor_type = MOTOR_TYPE_FOC;
+ mcconf.foc_f_sw = 10000.0;
+ mcconf.foc_current_kp = 0.01;
+ mcconf.foc_current_ki = 10.0;
+ mc_interface_set_configuration(&mcconf);
+
+ uint8_t hall_tab[8];
+ bool res = mcpwm_foc_hall_detect(current, hall_tab);
+ mc_interface_set_configuration(&mcconf_old);
+
+ ind = 0;
+ send_buffer[ind++] = COMM_DETECT_HALL_FOC;
+ memcpy(send_buffer + ind, hall_tab, 8);
+ ind += 8;
+ send_buffer[ind++] = res ? 0 : 1;
+
+ commands_send_packet(send_buffer, ind);
+ } else {
+ ind = 0;
+ send_buffer[ind++] = COMM_DETECT_HALL_FOC;
+ memset(send_buffer, 255, 8);
+ ind += 8;
+ send_buffer[ind++] = 0;
+ }
}
break;
@@ -692,6 +912,8 @@
send_buffer[ind++] = COMM_GET_DECODED_ADC;
buffer_append_int32(send_buffer, (int32_t)(app_adc_get_decoded_level() * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(app_adc_get_voltage() * 1000000.0), &ind);
+ buffer_append_int32(send_buffer, (int32_t)(app_adc_get_decoded_level2() * 1000000.0), &ind);
+ buffer_append_int32(send_buffer, (int32_t)(app_adc_get_voltage2() * 1000000.0), &ind);
commands_send_packet(send_buffer, ind);
break;
@@ -706,9 +928,535 @@
comm_can_send_buffer(data[0], data + 1, len - 1, false);
break;
+ case COMM_SET_CHUCK_DATA:
+ ind = 0;
+ chuck_d_tmp.js_x = data[ind++];
+ chuck_d_tmp.js_y = data[ind++];
+ chuck_d_tmp.bt_c = data[ind++];
+ chuck_d_tmp.bt_z = data[ind++];
+ chuck_d_tmp.acc_x = buffer_get_int16(data, &ind);
+ chuck_d_tmp.acc_y = buffer_get_int16(data, &ind);
+ chuck_d_tmp.acc_z = buffer_get_int16(data, &ind);
+ app_nunchuk_update_output(&chuck_d_tmp);
+ break;
+
+ case COMM_CUSTOM_APP_DATA:
+ if (appdata_func) {
+ appdata_func(data, len);
+ }
+ break;
+
+ case COMM_SET_SPEED_MODE:
+ ind = 0;
+ uint8_t new_remote_Mode = data[ind++];
+ uint8_t new_speed_mode_int = data[ind++];
+ bool use_max_watt = data[ind++];
+ float watts_max = buffer_get_float16(data, 1, &ind);
+ float cur_mot_max = buffer_get_float16(data, 10, &ind);
+ float cur_bat_max = buffer_get_float16(data, 10, &ind);
+ float cur_mot_min = buffer_get_float16(data, 10, &ind);
+ float cur_bat_min = buffer_get_float16(data, 10, &ind);
+ float max_erpm = buffer_get_float16(data, 0.1, &ind);
+
+ bool useSpecialThrottleCurve = data[ind++];
+ float newY1 = 0.0, newY2 = 0.0, newY3 = 0.0, newX1 = 0.0, newX2 = 0.0, newX3 = 0.0, newBezier_pos = 0.0, newY1_neg = 0.0, newY2_neg = 0.0, newY3_neg = 0.0, newX1_neg = 0.0, newX2_neg = 0.0, newX3_neg = 0.0, newBezier_neg = 0.0;
+ if (useSpecialThrottleCurve) {
+ newY1 = buffer_get_float16(data, 1000, &ind);
+ newY2 = buffer_get_float16(data, 1000, &ind);
+ newY3 = buffer_get_float16(data, 1000, &ind);
+ newX1 = buffer_get_float16(data, 1000, &ind);
+ newX2 = buffer_get_float16(data, 1000, &ind);
+ newX3 = buffer_get_float16(data, 1000, &ind);
+ newBezier_pos = buffer_get_float16(data, 100, &ind);
+ newY1_neg = buffer_get_float16(data, 1000, &ind);
+ newY2_neg = buffer_get_float16(data, 1000, &ind);
+ newY3_neg = buffer_get_float16(data, 1000, &ind);
+ newX1_neg = buffer_get_float16(data, 1000, &ind);
+ newX2_neg = buffer_get_float16(data, 1000, &ind);
+ newX3_neg = buffer_get_float16(data, 1000, &ind);
+ newBezier_neg = buffer_get_float16(data, 100, &ind);
+ }
+
+ bool useSpecialPIDBrakingEnabled = data[ind++];
+ bool pidBrakingEnabled = false;
+ if (useSpecialPIDBrakingEnabled) {
+ pidBrakingEnabled = data[ind++];
+ }
+
+ bool useSpecialSpeedPID = data[ind++];
+ float speed_pid_kp = 0.0, speed_pid_ki = 0.0, speed_pid_kd = 0.0;
+ if (useSpecialSpeedPID) {
+ speed_pid_kp = buffer_get_float32(data, 1000000.0, &ind);
+ speed_pid_ki = buffer_get_float32(data, 1000000.0, &ind);
+ speed_pid_kd = buffer_get_float32(data, 1000000.0, &ind);
+ }
+
+ uint8_t front_controller_first = data[ind++];
+ uint8_t front_controller_second = data[ind++];
+ bool front_use_max_watt = false;
+ float front_watts_max = 0.0, front_cur_mot_max = 0.0, front_cur_bat_max = 0.0, front_cur_mot_min = 0.0, front_cur_bat_min = 0.0;
+ if (front_controller_first != 9 || front_controller_second != 9) {
+ front_use_max_watt = data[ind++];
+ front_watts_max = buffer_get_float16(data, 1, &ind);
+ front_cur_mot_max = buffer_get_float16(data, 10, &ind);
+ front_cur_bat_max = buffer_get_float16(data, 10, &ind);
+ front_cur_mot_min = buffer_get_float16(data, 10, &ind);
+ front_cur_bat_min = buffer_get_float16(data, 10, &ind);
+ }
+
+ appconf = *app_get_configuration();
+
+ if (appconf.app_to_use == APP_PPM_UART
+ || appconf.app_to_use == APP_PPM
+ || appconf.app_to_use == APP_NONE
+ || appconf.app_to_use == APP_UART
+ || appconf.app_to_use == APP_NUNCHUK
+ || appconf.app_to_use == APP_NRF) {
+
+ app_configuration saved_appconf;
+
+ conf_general_read_app_configuration(&saved_appconf);
+
+ mcconf = *mc_interface_get_configuration();
+
+ mc_configuration saved_mcconf;
+ // gett base settings for motor
+ conf_general_read_mc_configuration(&saved_mcconf);
+
+ // reset app ppm
+ appconf.app_ppm_conf.ctrl_type = saved_appconf.app_ppm_conf.ctrl_type;
+ appconf.app_ppm_conf.pid_max_erpm = saved_appconf.app_ppm_conf.pid_max_erpm;
+ appconf.app_ppm_conf.rpm_lim_end = saved_appconf.app_ppm_conf.rpm_lim_end;
+ appconf.app_ppm_conf.rpm_lim_start = saved_appconf.app_ppm_conf.rpm_lim_start;
+
+ // reset app chuk
+ appconf.app_chuk_conf.ctrl_type = saved_appconf.app_chuk_conf.ctrl_type;
+ appconf.app_chuk_conf.rpm_lim_end = saved_appconf.app_chuk_conf.rpm_lim_end;
+ appconf.app_chuk_conf.rpm_lim_start = saved_appconf.app_chuk_conf.rpm_lim_start;
+
+ // reset throttle curve
+ appconf.app_throttle_conf.adjustable_throttle_enabled = saved_appconf.app_throttle_conf.adjustable_throttle_enabled;
+ appconf.app_throttle_conf.y1_throttle = saved_appconf.app_throttle_conf.y1_throttle;
+ appconf.app_throttle_conf.y2_throttle = saved_appconf.app_throttle_conf.y2_throttle;
+ appconf.app_throttle_conf.y3_throttle = saved_appconf.app_throttle_conf.y3_throttle;
+ appconf.app_throttle_conf.x1_throttle = saved_appconf.app_throttle_conf.x1_throttle;
+ appconf.app_throttle_conf.x2_throttle = saved_appconf.app_throttle_conf.x2_throttle;
+ appconf.app_throttle_conf.x3_throttle = saved_appconf.app_throttle_conf.x3_throttle;
+ appconf.app_throttle_conf.bezier_reduce_factor = saved_appconf.app_throttle_conf.bezier_reduce_factor;
+ appconf.app_throttle_conf.y1_neg_throttle = saved_appconf.app_throttle_conf.y1_neg_throttle;
+ appconf.app_throttle_conf.y2_neg_throttle = saved_appconf.app_throttle_conf.y2_neg_throttle;
+ appconf.app_throttle_conf.y3_neg_throttle = saved_appconf.app_throttle_conf.y3_neg_throttle;
+ appconf.app_throttle_conf.x1_neg_throttle = saved_appconf.app_throttle_conf.x1_neg_throttle;
+ appconf.app_throttle_conf.x2_neg_throttle = saved_appconf.app_throttle_conf.x2_neg_throttle;
+ appconf.app_throttle_conf.x3_neg_throttle = saved_appconf.app_throttle_conf.x3_neg_throttle;
+ appconf.app_throttle_conf.bezier_neg_reduce_factor = saved_appconf.app_throttle_conf.bezier_neg_reduce_factor;
+
+ // reset motor
+ mcconf.l_current_max = saved_mcconf.l_current_max;
+ mcconf.l_current_min = saved_mcconf.l_current_min;
+ mcconf.l_in_current_max = saved_mcconf.l_in_current_max;
+ mcconf.l_in_current_min = saved_mcconf.l_in_current_min;
+
+ mcconf.use_max_watt_limit = saved_mcconf.use_max_watt_limit;
+ mcconf.watts_max = saved_mcconf.watts_max;
+
+ mcconf.s_pid_breaking_enabled = saved_mcconf.s_pid_breaking_enabled;
+
+ mcconf.s_pid_kp = saved_mcconf.s_pid_kp;
+ mcconf.s_pid_ki = saved_mcconf.s_pid_ki;
+ mcconf.s_pid_kd = saved_mcconf.s_pid_kd;
+
+ if (new_remote_Mode == 0){
+ remote_Mode = new_remote_Mode;
+ } else {
+ if (appconf.app_to_use == APP_NONE || appconf.app_to_use == APP_UART) {
+ if (appconf.send_can_status) {
+ remote_Mode = new_remote_Mode;
+ }
+ }
+
+ if (appconf.app_to_use == APP_PPM_UART || appconf.app_to_use == APP_PPM) {
+ switch (new_speed_mode_int) {
+ case PPM_CTRL_TYPE_PID_NOACCELERATION:
+ case PPM_CTRL_TYPE_NONE:
+ case PPM_CTRL_TYPE_CURRENT:
+ case PPM_CTRL_TYPE_CURRENT_NOREV:
+ case PPM_CTRL_TYPE_CURRENT_NOREV_BRAKE:
+ case PPM_CTRL_TYPE_DUTY:
+ case PPM_CTRL_TYPE_DUTY_NOREV:
+ case PPM_CTRL_TYPE_PID:
+ case PPM_CTRL_TYPE_PID_NOREV:
+ case PPM_CTRL_TYPE_WATT_NOREV_BRAKE:
+ case PPM_CTRL_TYPE_WATT:
+ if (!appconf.send_can_status) {
+ appconf.app_ppm_conf.ctrl_type = new_speed_mode_int;
+
+ if (max_erpm > 0 && max_erpm <= 100000.0) {
+ appconf.app_ppm_conf.pid_max_erpm = max_erpm;
+ appconf.app_ppm_conf.rpm_lim_end = max_erpm;
+ appconf.app_ppm_conf.rpm_lim_start = max_erpm - ((int16_t)(max_erpm / 10)); // 10% off
+ }
+ }
+
+ remote_Mode = new_remote_Mode;
+ break;
+ default:
+ //only the basic settings which are defined by the bldc-tool
+ remote_Mode = 0;
+ break;
+ }
+ }
+
+ if (appconf.app_to_use == APP_NUNCHUK || appconf.app_to_use == APP_NRF) {
+ switch (new_speed_mode_int) {
+ case CHUK_CTRL_TYPE_NONE:
+ case CHUK_CTRL_TYPE_CURRENT:
+ case CHUK_CTRL_TYPE_CURRENT_NOREV:
+ case CHUK_CTRL_TYPE_WATT:
+ case CHUK_CTRL_TYPE_WATT_NOREV:
+ if (!appconf.send_can_status) {
+ appconf.app_chuk_conf.ctrl_type = new_speed_mode_int;
+
+ if (max_erpm > 0 && max_erpm <= 100000.0) {
+ appconf.app_chuk_conf.rpm_lim_end = max_erpm;
+ appconf.app_chuk_conf.rpm_lim_start = max_erpm - ((int16_t)(max_erpm / 10)); // 10% off
+ }
+
+ }
+
+ remote_Mode = new_remote_Mode;
+ break;
+ default:
+ //only the basic settings which are defined by the bldc-tool
+ remote_Mode = 0;
+ break;
+ }
+ }
+
+ if(remote_Mode != 0){
+ if (!appconf.send_can_status) {
+ if (useSpecialThrottleCurve) {
+ appconf.app_throttle_conf.adjustable_throttle_enabled = true;
+ appconf.app_throttle_conf.y1_throttle = newY1;
+ appconf.app_throttle_conf.y2_throttle = newY2;
+ appconf.app_throttle_conf.y3_throttle = newY3;
+ appconf.app_throttle_conf.x1_throttle = newX1;
+ appconf.app_throttle_conf.x2_throttle = newX2;
+ appconf.app_throttle_conf.x3_throttle = newX3;
+ appconf.app_throttle_conf.bezier_reduce_factor = newBezier_pos;
+ appconf.app_throttle_conf.y1_neg_throttle = newY1_neg;
+ appconf.app_throttle_conf.y2_neg_throttle = newY2_neg;
+ appconf.app_throttle_conf.y3_neg_throttle = newY3_neg;
+ appconf.app_throttle_conf.x1_neg_throttle = newX1_neg;
+ appconf.app_throttle_conf.x2_neg_throttle = newX2_neg;
+ appconf.app_throttle_conf.x3_neg_throttle = newX3_neg;
+ appconf.app_throttle_conf.bezier_neg_reduce_factor = newBezier_neg;
+ }
+ }
+
+ if (useSpecialPIDBrakingEnabled) {
+ mcconf.s_pid_breaking_enabled = pidBrakingEnabled;
+ }
+
+ if (useSpecialSpeedPID) {
+ mcconf.s_pid_kp = speed_pid_kp;
+ mcconf.s_pid_ki = speed_pid_ki;
+ mcconf.s_pid_kd = speed_pid_kd;
+ }
+
+ if ((front_controller_first != 9 && appconf.controller_id == front_controller_first)
+ || (front_controller_second != 9 && appconf.controller_id == front_controller_second)) { // or last position
+
+ mcconf.use_max_watt_limit = front_use_max_watt;
+ mcconf.watts_max = front_watts_max;
+
+ if (front_cur_mot_max >= mcconf.cc_min_current && front_cur_mot_max <= 80) mcconf.l_current_max = front_cur_mot_max;
+
+ if (front_cur_bat_max >= mcconf.cc_min_current && front_cur_bat_max <= 40) mcconf.l_in_current_max = front_cur_bat_max;
+
+ if (front_cur_mot_min <= -mcconf.cc_min_current && front_cur_mot_min >= -80) mcconf.l_current_min = front_cur_mot_min;
+
+ if (front_cur_bat_min <= -mcconf.cc_min_current && front_cur_bat_min >= -12) mcconf.l_in_current_min = front_cur_bat_min;
+
+ } else { // normal or rear setup
+ mcconf.use_max_watt_limit = use_max_watt;
+ mcconf.watts_max = watts_max;
+
+ if (cur_mot_max >= mcconf.cc_min_current && cur_mot_max <= 80) mcconf.l_current_max = cur_mot_max;
+
+ if (cur_bat_max >= mcconf.cc_min_current && cur_bat_max <= 40) mcconf.l_in_current_max = cur_bat_max;
+
+ if (cur_mot_min <= -mcconf.cc_min_current && cur_mot_min >= -80) mcconf.l_current_min = cur_mot_min;
+
+ if (cur_bat_min <= -mcconf.cc_min_current && cur_bat_min >= -12) mcconf.l_in_current_min = cur_bat_min;
+ }
+ }
+ }
+
+ if (!appconf.send_can_status) {
+ sendActualSpeedMode = true;
+
+ ind = 0;
+ // send to all others
+ uint8_t send_buffer_can[PACKET_MAX_PL_LEN];
+
+ send_buffer_can[ind++] = packet_id;
+ send_buffer_can[ind++] = remote_Mode;
+
+ for(unsigned int i = 1; i < len; i++){
+ send_buffer_can[ind++] = data[i];
+ }
+
+ for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+ can_status_msg *msg = comm_can_get_status_msg_index(i);
+
+ if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < 1) {
+ comm_can_send_buffer(msg->id, send_buffer_can, len + 1, false);
+ }
+ }
+ }
+
+ mc_interface_set_configuration(&mcconf);
+ app_set_configuration(&appconf);
+
+ timeout_configure(appconf.timeout_msec, appconf.timeout_brake_current);
+
+ timeout_reset();
+ }
+
+ break;
+ case COMM_CHANGE_SPEED_MODE:
+/*
+ ind = 0;
+
+ app_configuration saved_appconf2;
+
+ conf_general_read_app_configuration(&saved_appconf2);
+
+ appconf = *app_get_configuration();
+
+ appconf.app_ppm_conf.ctrl_type = saved_appconf2.app_ppm_conf.ctrl_type;
+
+ mcconf = *mc_interface_get_configuration();
+
+ mc_configuration saved_mcconf2;
+ // gett base settings for motor
+ conf_general_read_mc_configuration(&saved_mcconf2);
+
+ mcconf.l_current_max = saved_mcconf2.l_current_max;
+ mcconf.l_in_current_max = saved_mcconf2.l_in_current_max;
+
+ if (appconf.app_to_use == APP_PPM_UART || appconf.app_to_use == APP_PPM) {
+ if (appconf.app_ppm_conf.ctrl_type == PPM_CTRL_TYPE_WATT_NOREV_BRAKE) {
+ appconf.app_ppm_conf.ctrl_type = PPM_CTRL_TYPE_PID_NOACCELERATION;
+ float watt = 250 / 0.9;
+ } else if (appconf.app_ppm_conf.ctrl_type == PPM_CTRL_TYPE_PID_NOACCELERATION) {
+ appconf.app_ppm_conf.ctrl_type = PPM_CTRL_TYPE_WATT_NOREV_BRAKE;
+ *//*} else if (new_speed_mode_int == PPM_CTRL_TYPE_CURRENT_NOREV_BRAKE) {
+ appconf.app_ppm_conf.ctrl_type = new_speed_mode_int;
+
+ if (current_motor >= mcconf.cc_min_current && current_motor <= mcconf.l_current_max){
+ mcconf.l_current_max = current_motor;
+ }
+
+ if (current_battery >= mcconf.cc_min_current && current_battery <= mcconf.l_in_current_max){
+ mcconf.l_in_current_max = current_battery;
+ }*/
+/*
+ } else if (appconf.app_ppm_conf.ctrl_type == PPM_CTRL_TYPE_WATT_NOREV_BRAKE) {
+ appconf.app_ppm_conf.ctrl_type = PPM_CTRL_TYPE_CURRENT_NOREV_BRAKE;
+ //currents will be the default values
+ } else{
+ //only the basic settings which are defined by the bldc-tool
+ }
+ }
+
+ mc_interface_set_configuration(&mcconf);
+ app_set_configuration(&appconf);
+ timeout_configure(appconf.timeout_msec, appconf.timeout_brake_current);
+
+ timeout_reset();
+ sendActualSpeedMode = true;
+*/
+ break;
+ case COMM_GET_SPEED_MODE:
+ sendActualSpeedMode = true;
+ break;
+ case COMM_SET_CURRENT_CONF_AS_DEFAULT:
+
+ mcconf = *mc_interface_get_configuration();
+
+ appconf = *app_get_configuration();
+
+ if (!appconf.send_can_status) {
+ sendActualSpeedMode = true;
+
+ ind = 0;
+ // send to all others
+ uint8_t send_buffer_can[PACKET_MAX_PL_LEN];
+
+ send_buffer_can[ind++] = packet_id;
+
+ for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+ can_status_msg *msg = comm_can_get_status_msg_index(i);
+
+ if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < 1) {
+ comm_can_send_buffer(msg->id, send_buffer_can, len + 1, false);
+ }
+ }
+ }
+
+ conf_general_store_mc_configuration(&mcconf);
+ mc_interface_set_configuration(&mcconf);
+
+ conf_general_store_app_configuration(&appconf);
+ app_set_configuration(&appconf);
+
+ timeout_configure(appconf.timeout_msec, appconf.timeout_brake_current);
+
+ remote_Mode = 0;
+ break;
+ case COMM_SET_MOTOR_TYPE:
+
+ appconf = *app_get_configuration();
+
+ bool change_allowed = false;
+
+ ind = 0;
+ uint8_t new_motor_type = data[ind++];
+
+ if (new_motor_type == MOTOR_TYPE_FOC || new_motor_type == MOTOR_TYPE_BLDC) {
+
+ mcconf = *mc_interface_get_configuration();
+
+ if (mcconf.motor_type != new_motor_type) {
+
+ mc_configuration default_mcconf;
+ //get default settings
+ conf_general_get_default_mc_configuration(&default_mcconf);
+
+ change_allowed = true;
+
+ /*if (new_motor_type == MOTOR_TYPE_FOC &&
+ (mcconf.foc_motor_l != default_mcconf.foc_motor_l
+ || mcconf.foc_motor_r != default_mcconf.foc_motor_r
+ || mcconf.foc_motor_flux_linkage != default_mcconf.foc_motor_flux_linkage)) {
+ change_allowed = true;
+ }
+
+ if (new_motor_type == MOTOR_TYPE_BLDC &&
+ (mcconf.sl_cycle_int_limit != default_mcconf.sl_cycle_int_limit
+ || mcconf.sl_bemf_coupling_k != default_mcconf.sl_bemf_coupling_k)) {
+ change_allowed = true;
+ }*/
+
+
+ if (change_allowed) {
+ if (!appconf.send_can_status) {
+ ind = 0;
+ // send to all others
+ uint8_t send_buffer_can[PACKET_MAX_PL_LEN];
+
+ send_buffer_can[ind++] = packet_id;
+ send_buffer_can[ind++] = new_motor_type;
+
+ for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+ can_status_msg *msg = comm_can_get_status_msg_index(i);
+
+ if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < 1) {
+ comm_can_send_buffer(msg->id, send_buffer_can, len + 1, false);
+ }
+ }
+ }
+
+ //change motor config
+ mcconf.motor_type = new_motor_type;
+
+ //write motor config
+ //conf_general_store_mc_configuration(&saved_mcconf);
+ mc_interface_set_configuration(&mcconf);
+
+ if (!appconf.send_can_status) {
+ int32_t ind_motor_type = 0;
+ uint8_t send_buffer_motor_type[PACKET_MAX_PL_LEN];
+
+ send_buffer_motor_type[ind_motor_type++] = packet_id;
+ send_buffer_motor_type[ind_motor_type++] = new_motor_type;
+ commands_send_packet(send_buffer_motor_type, ind_motor_type);
+ }
+ }
+ }
+ }
+
+ //if (change_allowed == false && !appconf.send_can_status) {
+ if (!appconf.send_can_status) {
+ sendActualSpeedMode = true;
+ }
+
+ break;
default:
break;
}
+
+ if (sendActualSpeedMode) {
+ appconf = *app_get_configuration();
+ mcconf = *mc_interface_get_configuration();
+
+ ind = 0;
+ send_buffer[ind++] = COMM_GET_SPEED_MODE;
+ send_buffer[ind++] = remote_Mode;
+ if(appconf.app_to_use == APP_NUNCHUK || appconf.app_to_use == APP_NRF){
+ send_buffer[ind++] = appconf.app_chuk_conf.ctrl_type;
+ }else{
+ send_buffer[ind++] = appconf.app_ppm_conf.ctrl_type;
+ }
+
+ send_buffer[ind++] = mcconf.use_max_watt_limit;
+ buffer_append_int16(send_buffer, (int16_t) (mcconf.watts_max), &ind);
+
+ buffer_append_int16(send_buffer, (int16_t) (mcconf.l_current_max * 10), &ind);
+ buffer_append_int16(send_buffer, (int16_t) (mcconf.l_in_current_max * 10), &ind);
+ buffer_append_int16(send_buffer, (int16_t) (mcconf.l_current_min * 10), &ind);
+ buffer_append_int16(send_buffer, (int16_t) (mcconf.l_in_current_min * 10), &ind);
+
+ if(appconf.app_to_use == APP_NUNCHUK || appconf.app_to_use == APP_NRF){
+ buffer_append_int16(send_buffer, (int16_t) (appconf.app_chuk_conf.rpm_lim_end / 10), &ind);
+ }else{
+ buffer_append_int16(send_buffer, (int16_t) (appconf.app_ppm_conf.rpm_lim_end / 10), &ind);
+ }
+
+ buffer_append_int16(send_buffer, (int16_t) (mcconf.l_battery_cut_start * 100), &ind);
+ buffer_append_int16(send_buffer, (int16_t) (mcconf.l_battery_cut_end * 100), &ind);
+ buffer_append_int16(send_buffer, (int16_t) (mcconf.l_temp_fet_start * 100), &ind);
+ buffer_append_int16(send_buffer, (int16_t) (mcconf.l_temp_fet_end * 100), &ind);
+
+ send_buffer[ind++] = appconf.app_to_use;
+
+ send_buffer[ind++] = appconf.app_throttle_conf.adjustable_throttle_enabled;
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.y1_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.y2_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.y3_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.x1_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.x2_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.x3_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.bezier_reduce_factor * 100.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.y1_neg_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.y2_neg_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.y3_neg_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.x1_neg_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.x2_neg_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.x3_neg_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.bezier_neg_reduce_factor * 100.0), &ind);
+
+ send_buffer[ind++] = mcconf.s_pid_breaking_enabled;
+ buffer_append_int32(send_buffer, (int32_t)(mcconf.s_pid_kp * 1000000.0), &ind);
+ buffer_append_int32(send_buffer, (int32_t)(mcconf.s_pid_ki * 1000000.0), &ind);
+ buffer_append_int32(send_buffer, (int32_t)(mcconf.s_pid_kd * 1000000.0), &ind);
+
+ send_buffer[ind++] = mcconf.motor_type;
+
+ commands_send_packet(send_buffer, ind);
+ }
}
void commands_printf(char* format, ...) {
@@ -721,7 +1469,7 @@
len = vsnprintf(print_buffer+1, 254, format, arg);
va_end (arg);
- if(len>0) {
+ if(len > 0) {
commands_send_packet((unsigned char*)print_buffer, (len<254)? len+1: 255);
}
}
@@ -770,6 +1518,20 @@
return display_position_mode;
}
+void commands_set_app_data_handler(void(*func)(unsigned char *data, unsigned int len)) {
+ appdata_func = func;
+}
+
+void commands_send_app_data(unsigned char *data, unsigned int len) {
+ int32_t index = 0;
+
+ send_buffer[index++] = COMM_CUSTOM_APP_DATA;
+ memcpy(send_buffer + index, data, len);
+ index += len;
+
+ commands_send_packet(send_buffer, index);
+}
+
static THD_FUNCTION(detect_thread, arg) {
(void)arg;
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/commands.h Raptor2 Sources/bldc-firmware_2_80/commands.h
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/commands.h 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/commands.h 2016-09-19 10:06:42.000000000 +0100
@@ -37,5 +37,7 @@
void commands_send_rotor_pos(float rotor_pos);
void commands_send_experiment_samples(float *samples, int len);
disp_pos_mode commands_get_disp_pos_mode(void);
+void commands_set_app_data_handler(void(*func)(unsigned char *data, unsigned int len));
+void commands_send_app_data(unsigned char *data, unsigned int len);
#endif /* COMMANDS_H_ */
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/conf_general.c Raptor2 Sources/bldc-firmware_2_80/conf_general.c
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/conf_general.c 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/conf_general.c 2017-04-15 03:04:20.000000000 +0100
@@ -47,7 +47,7 @@
memset(VirtAddVarTab, 0, sizeof(VirtAddVarTab));
int ind = 0;
- for (unsigned int i = 0;i < (sizeof(app_configuration) / 2);i++) {
+ for (unsigned int i = 0;i < (sizeof(mc_configuration) / 2);i++) {
VirtAddVarTab[ind++] = EEPROM_BASE_MCCONF + i;
}
@@ -89,7 +89,7 @@
conf->app_ppm_conf.multi_esc = APPCONF_PPM_MULTI_ESC;
conf->app_ppm_conf.tc = APPCONF_PPM_TC;
conf->app_ppm_conf.tc_max_diff = APPCONF_PPM_TC_MAX_DIFF;
-
+
conf->app_adc_conf.ctrl_type = APPCONF_ADC_CTRL_TYPE;
conf->app_adc_conf.hyst = APPCONF_ADC_HYST;
conf->app_adc_conf.voltage_start = APPCONF_ADC_VOLTAGE_START;
@@ -118,6 +118,45 @@
conf->app_chuk_conf.multi_esc = APPCONF_CHUK_MULTI_ESC;
conf->app_chuk_conf.tc = APPCONF_CHUK_TC;
conf->app_chuk_conf.tc_max_diff = APPCONF_CHUK_TC_MAX_DIFF;
+
+ conf->app_nrf_conf.speed = APPCONF_NRF_SPEED;
+ conf->app_nrf_conf.power = APPCONF_NRF_POWER;
+ conf->app_nrf_conf.crc_type = APPCONF_NRF_CRC;
+ conf->app_nrf_conf.retry_delay = APPCONF_NRF_RETR_DELAY;
+ conf->app_nrf_conf.retries = APPCONF_NRF_RETRIES;
+ conf->app_nrf_conf.channel = APPCONF_NRF_CHANNEL;
+ conf->app_nrf_conf.address[0] = APPCONF_NRF_ADDR_B0;
+ conf->app_nrf_conf.address[1] = APPCONF_NRF_ADDR_B1;
+ conf->app_nrf_conf.address[2] = APPCONF_NRF_ADDR_B2;
+ conf->app_nrf_conf.send_crc_ack = APPCONF_NRF_SEND_CRC_ACK;
+
+ //new config
+ conf->app_ppm_conf.pulse_center = APPCONF_PPM_PULSE_CENTER;
+ conf->app_ppm_conf.tc_offset = APPCONF_PPM_TC_OFFSET;
+ conf->app_ppm_conf.cruise_left = CRUISE_CONTROL_MOTOR_SETTINGS;
+ conf->app_ppm_conf.cruise_right = CRUISE_CONTROL_MOTOR_SETTINGS;
+ conf->app_ppm_conf.max_erpm_for_dir_active = APPCONF_PPM_MAX_ERPM_FOR_DIR_ACTIVE;
+ conf->app_ppm_conf.max_erpm_for_dir = APPCONF_PPM_MAX_ERPM_FOR_DIR;
+
+ conf->app_chuk_conf.tc_offset = APPCONF_CHUK_TC_OFFSET;
+ conf->app_chuk_conf.buttons_mirrored = APPCONF_CHUK_BUTTONS_MIRRORED;
+
+ conf->app_throttle_conf.adjustable_throttle_enabled = APPCONF_ADJUSTABLE_THROTTLE_ENABLED;
+ conf->app_throttle_conf.y1_throttle = APPCONF_Y1_THROTTLE;
+ conf->app_throttle_conf.y2_throttle = APPCONF_Y2_THROTTLE;
+ conf->app_throttle_conf.y3_throttle = APPCONF_Y3_THROTTLE;
+ conf->app_throttle_conf.x1_throttle = APPCONF_X1_THROTTLE;
+ conf->app_throttle_conf.x2_throttle = APPCONF_X2_THROTTLE;
+ conf->app_throttle_conf.x3_throttle = APPCONF_X3_THROTTLE;
+ conf->app_throttle_conf.bezier_reduce_factor = APPCONF_BEZIER_REDUCE_FACTOR;
+ conf->app_throttle_conf.y1_neg_throttle = APPCONF_Y1_NEG_THROTTLE;
+ conf->app_throttle_conf.y2_neg_throttle = APPCONF_Y2_NEG_THROTTLE;
+ conf->app_throttle_conf.y3_neg_throttle = APPCONF_Y3_NEG_THROTTLE;
+ conf->app_throttle_conf.x1_neg_throttle = APPCONF_X1_NEG_THROTTLE;
+ conf->app_throttle_conf.x2_neg_throttle = APPCONF_X2_NEG_THROTTLE;
+ conf->app_throttle_conf.x3_neg_throttle = APPCONF_X3_NEG_THROTTLE;
+ conf->app_throttle_conf.bezier_neg_reduce_factor = APPCONF_BEZIER_REDUCE_NEG_FACTOR;
+ //new config end
}
/**
@@ -168,6 +207,16 @@
conf->sl_cycle_int_rpm_br = MCCONF_SL_CYCLE_INT_BR;
conf->sl_bemf_coupling_k = MCCONF_SL_BEMF_COUPLING_K;
+ conf->hall_table[0] = MCCONF_HALL_TAB_0;
+ conf->hall_table[1] = MCCONF_HALL_TAB_1;
+ conf->hall_table[2] = MCCONF_HALL_TAB_2;
+ conf->hall_table[3] = MCCONF_HALL_TAB_3;
+ conf->hall_table[4] = MCCONF_HALL_TAB_4;
+ conf->hall_table[5] = MCCONF_HALL_TAB_5;
+ conf->hall_table[6] = MCCONF_HALL_TAB_6;
+ conf->hall_table[7] = MCCONF_HALL_TAB_7;
+ conf->hall_sl_erpm = MCCONF_HALL_ERPM;
+
conf->foc_current_kp = MCCONF_FOC_CURRENT_KP;
conf->foc_current_ki = MCCONF_FOC_CURRENT_KI;
conf->foc_f_sw = MCCONF_FOC_F_SW;
@@ -189,25 +238,26 @@
conf->foc_sl_openloop_time = MCCONF_FOC_SL_OPENLOOP_TIME;
conf->foc_sl_d_current_duty = MCCONF_FOC_SL_D_CURRENT_DUTY;
conf->foc_sl_d_current_factor = MCCONF_FOC_SL_D_CURRENT_FACTOR;
-
- conf->hall_table[0] = MCCONF_HALL_TAB_0;
- conf->hall_table[1] = MCCONF_HALL_TAB_1;
- conf->hall_table[2] = MCCONF_HALL_TAB_2;
- conf->hall_table[3] = MCCONF_HALL_TAB_3;
- conf->hall_table[4] = MCCONF_HALL_TAB_4;
- conf->hall_table[5] = MCCONF_HALL_TAB_5;
- conf->hall_table[6] = MCCONF_HALL_TAB_6;
- conf->hall_table[7] = MCCONF_HALL_TAB_7;
- conf->hall_sl_erpm = MCCONF_HALL_ERPM;
+ conf->foc_hall_table[0] = MCCONF_FOC_HALL_TAB_0;
+ conf->foc_hall_table[1] = MCCONF_FOC_HALL_TAB_1;
+ conf->foc_hall_table[2] = MCCONF_FOC_HALL_TAB_2;
+ conf->foc_hall_table[3] = MCCONF_FOC_HALL_TAB_3;
+ conf->foc_hall_table[4] = MCCONF_FOC_HALL_TAB_4;
+ conf->foc_hall_table[5] = MCCONF_FOC_HALL_TAB_5;
+ conf->foc_hall_table[6] = MCCONF_FOC_HALL_TAB_6;
+ conf->foc_hall_table[7] = MCCONF_FOC_HALL_TAB_7;
+ conf->foc_sl_erpm = MCCONF_FOC_SL_ERPM;
conf->s_pid_kp = MCCONF_S_PID_KP;
conf->s_pid_ki = MCCONF_S_PID_KI;
conf->s_pid_kd = MCCONF_S_PID_KD;
conf->s_pid_min_erpm = MCCONF_S_PID_MIN_RPM;
+ conf->s_pid_breaking_enabled = MCCONF_S_PID_BREAKING_ENABLED;
conf->p_pid_kp = MCCONF_P_PID_KP;
conf->p_pid_ki = MCCONF_P_PID_KI;
conf->p_pid_kd = MCCONF_P_PID_KD;
+ conf->p_pid_ang_div = MCCONF_P_PID_ANG_DIV;
conf->cc_startup_boost_duty = MCCONF_CC_STARTUP_BOOST_DUTY;
conf->cc_min_current = MCCONF_CC_MIN_CURRENT;
@@ -219,6 +269,10 @@
conf->m_duty_ramp_step_rpm_lim = MCCONF_M_RAMP_STEP_RPM_LIM;
conf->m_current_backoff_gain = MCCONF_M_CURRENT_BACKOFF_GAIN;
conf->m_encoder_counts = MCCONF_M_ENCODER_COUNTS;
+ conf->m_sensor_port_mode = MCCONF_M_SENSOR_PORT_MODE;
+
+ conf->use_max_watt_limit = MCCONF_USE_MAX_WATT_LIMIT;
+ conf->watts_max = MCCONF_WATT_MAX;
}
/**
@@ -242,6 +296,32 @@
}
}
+ if(is_ok == false){
+ chThdSleepMilliseconds(10);
+ for (unsigned int i = 0;i < (sizeof(app_configuration) / 2);i++) {
+ if (EE_ReadVariable(EEPROM_BASE_APPCONF + i, &var) == 0) {
+ conf_addr[2 * i] = (var >> 8) & 0xFF;
+ conf_addr[2 * i + 1] = var & 0xFF;
+ } else {
+ is_ok = false;
+ break;
+ }
+ }
+ }
+
+ if(is_ok == false){
+ chThdSleepMilliseconds(10);
+ for (unsigned int i = 0;i < (sizeof(app_configuration) / 2);i++) {
+ if (EE_ReadVariable(EEPROM_BASE_APPCONF + i, &var) == 0) {
+ conf_addr[2 * i] = (var >> 8) & 0xFF;
+ conf_addr[2 * i + 1] = var & 0xFF;
+ } else {
+ is_ok = false;
+ break;
+ }
+ }
+ }
+
// Set the default configuration
if (!is_ok) {
conf_general_get_default_app_configuration(conf);
@@ -277,6 +357,34 @@
break;
}
}
+
+ // try one more time
+ if(is_ok == false){
+ chThdSleepMilliseconds(10);
+ for (unsigned int i = 0;i < (sizeof(app_configuration) / 2);i++) {
+ var = (conf_addr[2 * i] << 8) & 0xFF00;
+ var |= conf_addr[2 * i + 1] & 0xFF;
+
+ if (EE_WriteVariable(EEPROM_BASE_APPCONF + i, var) != FLASH_COMPLETE) {
+ is_ok = false;
+ break;
+ }
+ }
+ }
+
+ // try again one more time
+ if(is_ok == false){
+ chThdSleepMilliseconds(10);
+ for (unsigned int i = 0;i < (sizeof(app_configuration) / 2);i++) {
+ var = (conf_addr[2 * i] << 8) & 0xFF00;
+ var |= conf_addr[2 * i + 1] & 0xFF;
+
+ if (EE_WriteVariable(EEPROM_BASE_APPCONF + i, var) != FLASH_COMPLETE) {
+ is_ok = false;
+ break;
+ }
+ }
+ }
RCC_APB1PeriphClockCmd(RCC_APB1Periph_WWDG, ENABLE);
utils_sys_unlock_cnt();
@@ -305,6 +413,34 @@
}
}
+ // try one more time
+ if(is_ok == false){
+ chThdSleepMilliseconds(10);
+ for (unsigned int i = 0;i < (sizeof(mc_configuration) / 2);i++) {
+ if (EE_ReadVariable(EEPROM_BASE_MCCONF + i, &var) == 0) {
+ conf_addr[2 * i] = (var >> 8) & 0xFF;
+ conf_addr[2 * i + 1] = var & 0xFF;
+ } else {
+ is_ok = false;
+ break;
+ }
+ }
+ }
+
+ // try another one more time
+ if(is_ok == false){
+ chThdSleepMilliseconds(10);
+ for (unsigned int i = 0;i < (sizeof(mc_configuration) / 2);i++) {
+ if (EE_ReadVariable(EEPROM_BASE_MCCONF + i, &var) == 0) {
+ conf_addr[2 * i] = (var >> 8) & 0xFF;
+ conf_addr[2 * i + 1] = var & 0xFF;
+ } else {
+ is_ok = false;
+ break;
+ }
+ }
+ }
+
if (!is_ok) {
conf_general_get_default_mc_configuration(conf);
}
@@ -339,6 +475,34 @@
break;
}
}
+
+ // try one more time
+ if(is_ok == false){
+ chThdSleepMilliseconds(10);
+ for (unsigned int i = 0;i < (sizeof(mc_configuration) / 2);i++) {
+ var = (conf_addr[2 * i] << 8) & 0xFF00;
+ var |= conf_addr[2 * i + 1] & 0xFF;
+
+ if (EE_WriteVariable(EEPROM_BASE_MCCONF + i, var) != FLASH_COMPLETE) {
+ is_ok = false;
+ break;
+ }
+ }
+ }
+
+ // try again one more time
+ if(is_ok == false){
+ chThdSleepMilliseconds(10);
+ for (unsigned int i = 0;i < (sizeof(mc_configuration) / 2);i++) {
+ var = (conf_addr[2 * i] << 8) & 0xFF00;
+ var |= conf_addr[2 * i + 1] & 0xFF;
+
+ if (EE_WriteVariable(EEPROM_BASE_MCCONF + i, var) != FLASH_COMPLETE) {
+ is_ok = false;
+ break;
+ }
+ }
+ }
RCC_APB1PeriphClockCmd(RCC_APB1Periph_WWDG, ENABLE);
utils_sys_unlock_cnt();
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/conf_general.h Raptor2 Sources/bldc-firmware_2_80/conf_general.h
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/conf_general.h 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/conf_general.h 2017-04-17 04:06:52.000000000 +0100
@@ -27,18 +27,18 @@
// Firmware version
#define FW_VERSION_MAJOR 2
-#define FW_VERSION_MINOR 8
+#define FW_VERSION_MINOR 80
#include "datatypes.h"
/*
* Settings
*/
-#define AUTO_PRINT_FAULTS 0
#define SYSTEM_CORE_CLOCK 168000000
// Settings and parameters to override
//#define VIN_R1 33000.0
+#define VIN_R1 39000.0
//#define VIN_R2 2200.0
//#define CURRENT_AMP_GAIN 10.0
//#define CURRENT_SHUNT_RES 0.0005
@@ -57,7 +57,7 @@
//#define HW_VERSION_46 // Also for 4.7
//#define HW_VERSION_48
//#define HW_VERSION_49
-#define HW_VERSION_410 // Also for 4.11
+#define HW_VERSION_410 // Also for 4.11 and 4.12
//#define HW_VERSION_R2
//#define HW_VERSION_VICTOR_R1A
#endif
@@ -65,15 +65,15 @@
/*
* Select default user motor configuration
*/
-//#define MCCONF_DEFAULT_USER "mcconf_outrunner2.h"
//#define MCCONF_DEFAULT_USER "mcconf_sten.h"
-//#define MCCONF_DEFAULT_USER "mcconf_foc_erwin.h"
-//#define MCCONF_DEFAULT_USER "mcconf_foc_scorpion.h"
+//#define MCCONF_DEFAULT_USER "mcconf_sp_540kv.h"
+//#define MCCONF_DEFAULT_USER "mcconf_castle_2028.h"
/*
* Select default user app configuration
*/
//#define APPCONF_DEFAULT_USER "appconf_example_ppm.h"
+//#define APPCONF_DEFAULT_USER "appconf_custom.h"
/*
* Select which custom application to use. To configure the default applications and
@@ -83,13 +83,6 @@
//#define USE_APP_STEN
/*
- * Use encoder
- */
-#ifndef ENCODER_ENABLE
-#define ENCODER_ENABLE 0
-#endif
-
-/*
* Enable CAN-bus
*/
#define CAN_ENABLE 1
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/datatypes.h Raptor2 Sources/bldc-firmware_2_80/datatypes.h
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/datatypes.h 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/datatypes.h 2017-04-13 18:02:30.000000000 +0100
@@ -38,6 +38,13 @@
} mc_state;
typedef enum {
+ CRUISE_CONTROL_MOTOR_SETTINGS = 0,
+ CRUISE_CONTROL_BRAKING_DISABLED,
+ CRUISE_CONTROL_BRAKING_ENABLED,
+ CRUISE_CONTROL_INACTIVE
+} ppm_cruise;
+
+typedef enum {
PWM_MODE_NONSYNCHRONOUS_HISW = 0, // This mode is not recommended
PWM_MODE_SYNCHRONOUS, // The recommended and most tested mode
PWM_MODE_BIPOLAR // Some glitches occasionally, can kill MOSFETs
@@ -56,7 +63,8 @@
typedef enum {
FOC_SENSOR_MODE_SENSORLESS = 0,
- FOC_SENSOR_MODE_ENCODER
+ FOC_SENSOR_MODE_ENCODER,
+ FOC_SENSOR_MODE_HALL
} mc_foc_sensor_mode;
typedef enum {
@@ -89,10 +97,17 @@
DISP_POS_MODE_INDUCTANCE,
DISP_POS_MODE_OBSERVER,
DISP_POS_MODE_ENCODER,
- DISP_POS_MODE_ENCODER_POS_ERROR,
+ DISP_POS_MODE_PID_POS,
+ DISP_POS_MODE_PID_POS_ERROR,
DISP_POS_MODE_ENCODER_OBSERVER_ERROR
} disp_pos_mode;
+typedef enum {
+ SENSOR_PORT_MODE_HALL = 0,
+ SENSOR_PORT_MODE_ABI,
+ SENSOR_PORT_MODE_AS5047_SPI
+} sensor_port_mode;
+
typedef struct {
float cycle_int_limit;
float cycle_int_limit_running;
@@ -115,6 +130,8 @@
float l_in_current_max;
float l_in_current_min;
float l_abs_current_max;
+ bool use_max_watt_limit;
+ float watts_max;
float l_min_erpm;
float l_max_erpm;
float l_max_erpm_fbrake;
@@ -169,15 +186,19 @@
float foc_sl_d_current_duty;
float foc_sl_d_current_factor;
mc_foc_sensor_mode foc_sensor_mode;
+ uint8_t foc_hall_table[8];
+ float foc_sl_erpm;
// Speed PID
float s_pid_kp;
float s_pid_ki;
float s_pid_kd;
float s_pid_min_erpm;
+ bool s_pid_breaking_enabled;
// Pos PID
float p_pid_kp;
float p_pid_ki;
float p_pid_kd;
+ float p_pid_ang_div;
// Current controller
float cc_startup_boost_duty;
float cc_min_current;
@@ -189,6 +210,7 @@
float m_duty_ramp_step_rpm_lim;
float m_current_backoff_gain;
uint32_t m_encoder_counts;
+ sensor_port_mode m_sensor_port_mode;
} mc_configuration;
// Applications to use
@@ -213,7 +235,11 @@
PPM_CTRL_TYPE_DUTY,
PPM_CTRL_TYPE_DUTY_NOREV,
PPM_CTRL_TYPE_PID,
- PPM_CTRL_TYPE_PID_NOREV
+ PPM_CTRL_TYPE_PID_NOREV,
+ PPM_CTRL_TYPE_WATT_NOREV_BRAKE,
+ PPM_CTRL_TYPE_PID_NOACCELERATION,
+ PPM_CTRL_TYPE_CRUISE_CONTROL_SECONDARY_CHANNEL,
+ PPM_CTRL_TYPE_WATT
} ppm_control_type;
typedef struct {
@@ -229,6 +255,12 @@
bool multi_esc;
bool tc;
float tc_max_diff;
+ float pulse_center;
+ float tc_offset;
+ ppm_cruise cruise_left;
+ ppm_cruise cruise_right;
+ bool max_erpm_for_dir_active;
+ float max_erpm_for_dir;
} ppm_config;
// ADC control types
@@ -239,6 +271,7 @@
ADC_CTRL_TYPE_CURRENT_REV_BUTTON,
ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_CENTER,
ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_BUTTON,
+ ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_ADC,
ADC_CTRL_TYPE_DUTY,
ADC_CTRL_TYPE_DUTY_REV_CENTER,
ADC_CTRL_TYPE_DUTY_REV_BUTTON
@@ -266,7 +299,9 @@
typedef enum {
CHUK_CTRL_TYPE_NONE = 0,
CHUK_CTRL_TYPE_CURRENT,
- CHUK_CTRL_TYPE_CURRENT_NOREV
+ CHUK_CTRL_TYPE_CURRENT_NOREV,
+ CHUK_CTRL_TYPE_WATT,
+ CHUK_CTRL_TYPE_WATT_NOREV
} chuk_control_type;
typedef struct {
@@ -280,8 +315,84 @@
bool multi_esc;
bool tc;
float tc_max_diff;
+ float tc_offset;
+ bool buttons_mirrored;
} chuk_config;
+// NRF Datatypes
+typedef enum {
+ NRF_SPEED_250K = 0,
+ NRF_SPEED_1M,
+ NRF_SPEED_2M
+} NRF_SPEED;
+
+typedef enum {
+ NRF_POWER_M18DBM = 0,
+ NRF_POWER_M12DBM,
+ NRF_POWER_M6DBM,
+ NRF_POWER_0DBM
+} NRF_POWER;
+
+typedef enum {
+ NRF_AW_3 = 0,
+ NRF_AW_4,
+ NRF_AW_5
+} NRF_AW;
+
+typedef enum {
+ NRF_CRC_DISABLED = 0,
+ NRF_CRC_1B,
+ NRF_CRC_2B
+} NRF_CRC;
+
+typedef enum {
+ NRF_RETR_DELAY_250US = 0,
+ NRF_RETR_DELAY_500US,
+ NRF_RETR_DELAY_750US,
+ NRF_RETR_DELAY_1000US,
+ NRF_RETR_DELAY_1250US,
+ NRF_RETR_DELAY_1500US,
+ NRF_RETR_DELAY_1750US,
+ NRF_RETR_DELAY_2000US,
+ NRF_RETR_DELAY_2250US,
+ NRF_RETR_DELAY_2500US,
+ NRF_RETR_DELAY_2750US,
+ NRF_RETR_DELAY_3000US,
+ NRF_RETR_DELAY_3250US,
+ NRF_RETR_DELAY_3500US,
+ NRF_RETR_DELAY_3750US,
+ NRF_RETR_DELAY_4000US
+} NRF_RETR_DELAY;
+
+typedef struct {
+ NRF_SPEED speed;
+ NRF_POWER power;
+ NRF_CRC crc_type;
+ NRF_RETR_DELAY retry_delay;
+ unsigned char retries;
+ unsigned char channel;
+ unsigned char address[3];
+ bool send_crc_ack;
+} nrf_config;
+
+typedef struct {
+ bool adjustable_throttle_enabled;
+ float y1_throttle;
+ float y2_throttle;
+ float y3_throttle;
+ float x1_throttle;
+ float x2_throttle;
+ float x3_throttle;
+ float bezier_reduce_factor;
+ float y1_neg_throttle;
+ float y2_neg_throttle;
+ float y3_neg_throttle;
+ float x1_neg_throttle;
+ float x2_neg_throttle;
+ float x3_neg_throttle;
+ float bezier_neg_reduce_factor;
+} throttle_config;
+
typedef struct {
// Settings
uint8_t controller_id;
@@ -304,6 +415,11 @@
// Nunchuk application settings
chuk_config app_chuk_conf;
+
+ // NRF application settings
+ nrf_config app_nrf_conf;
+
+ throttle_config app_throttle_conf;
} app_configuration;
// Communication commands
@@ -335,12 +451,20 @@
COMM_DETECT_MOTOR_R_L,
COMM_DETECT_MOTOR_FLUX_LINKAGE,
COMM_DETECT_ENCODER,
+ COMM_DETECT_HALL_FOC,
COMM_REBOOT,
COMM_ALIVE,
COMM_GET_DECODED_PPM,
COMM_GET_DECODED_ADC,
COMM_GET_DECODED_CHUK,
- COMM_FORWARD_CAN
+ COMM_FORWARD_CAN,
+ COMM_SET_CHUCK_DATA,
+ COMM_CUSTOM_APP_DATA,
+ COMM_SET_SPEED_MODE,
+ COMM_CHANGE_SPEED_MODE,
+ COMM_GET_SPEED_MODE,
+ COMM_SET_CURRENT_CONF_AS_DEFAULT,
+ COMM_SET_MOTOR_TYPE
} COMM_PACKET_ID;
// CAN commands
@@ -354,7 +478,9 @@
CAN_PACKET_FILL_RX_BUFFER_LONG,
CAN_PACKET_PROCESS_RX_BUFFER,
CAN_PACKET_PROCESS_SHORT_BUFFER,
- CAN_PACKET_STATUS
+ CAN_PACKET_STATUS,
+ CAN_PACKET_SET_SERVO,
+ CAN_PACKET_SET_BRAKE_SERVO
} CAN_PACKET_ID;
// Logged fault data
@@ -401,7 +527,8 @@
systime_t rx_time;
float rpm;
float current;
- float duty;
+ int8_t duty;
+ ppm_cruise cruise_control_status;
} can_status_msg;
typedef struct {
@@ -429,20 +556,20 @@
float temp_mos2;
float temp_mos3;
float temp_mos4;
- float temp_mos5;
- float temp_mos6;
- float temp_pcb;
- float current_motor;
- float current_in;
- float rpm;
- float duty_now;
- float amp_hours;
- float amp_hours_charged;
- float watt_hours;
- float watt_hours_charged;
- int tachometer;
- int tachometer_abs;
- mc_fault_code fault_code;
+ float temp_mos5;
+ float temp_mos6;
+ float temp_pcb;
+ float current_motor;
+ float current_in;
+ float rpm;
+ float duty_now;
+ float amp_hours;
+ float amp_hours_charged;
+ float watt_hours;
+ float watt_hours_charged;
+ int tachometer;
+ int tachometer_abs;
+ mc_fault_code fault_code;
} mc_values;
#endif /* DATATYPES_H_ */
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/eeprom.h Raptor2 Sources/bldc-firmware_2_80/eeprom.h
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/eeprom.h 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/eeprom.h 2017-04-13 18:03:20.000000000 +0100
@@ -69,7 +69,7 @@
#define PAGE_FULL ((uint8_t)0x80)
/* Variables' number */
-#define NB_OF_VAR ((uint8_t)160)
+#define NB_OF_VAR ((uint8_t)255)
/* Exported types ------------------------------------------------------------*/
/* Exported macro ------------------------------------------------------------*/
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/encoder.c Raptor2 Sources/bldc-firmware_2_80/encoder.c
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/encoder.c 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/encoder.c 2016-09-19 10:06:24.000000000 +0100
@@ -1,5 +1,5 @@
/*
- Copyright 2012-2015 Benjamin Vedder benjamin@vedder.se
+ Copyright 2012-2016 Benjamin Vedder benjamin@vedder.se
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
@@ -15,26 +15,64 @@
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
-/*
- * encoder.c
- *
- * Created on: 7 mar 2015
- * Author: benjamin
- */
-
#include "encoder.h"
#include "ch.h"
#include "hal.h"
#include "stm32f4xx_conf.h"
#include "hw.h"
+#include "utils.h"
+
+// Defines
+#define AS5047P_READ_ANGLECOM (0x3FFF | 0x4000 | 0x8000) // This is just ones
+#define AS5047_SAMPLE_RATE_HZ 20000
+
+#define SPI_SW_MISO_GPIO HW_HALL_ENC_GPIO2
+#define SPI_SW_MISO_PIN HW_HALL_ENC_PIN2
+#define SPI_SW_SCK_GPIO HW_HALL_ENC_GPIO1
+#define SPI_SW_SCK_PIN HW_HALL_ENC_PIN1
+#define SPI_SW_CS_GPIO HW_HALL_ENC_GPIO3
+#define SPI_SW_CS_PIN HW_HALL_ENC_PIN3
+
+// Private types
+typedef enum {
+ ENCODER_MODE_NONE = 0,
+ ENCODER_MODE_ABI,
+ ENCODER_MODE_AS5047P_SPI
+} encoder_mode;
// Private variables
-static bool index_found;
+static bool index_found = false;
static uint32_t enc_counts = 10000;
+static encoder_mode mode = ENCODER_MODE_NONE;
+static float last_enc_angle = 0.0;
+
+// Private functions
+uint16_t spi_exchange(uint16_t x);
+static void spi_transfer(uint16_t *in_buf, const uint16_t *out_buf, int length);
+static void spi_begin(void);
+static void spi_end(void);
+static void spi_delay(void);
+
+void encoder_deinit(void) {
+ nvicDisableVector(HW_ENC_EXTI_CH);
+ nvicDisableVector(HW_ENC_TIM_ISR_CH);
+
+ TIM_DeInit(HW_ENC_TIM);
+
+ palSetPadMode(SPI_SW_MISO_GPIO, SPI_SW_MISO_PIN, PAL_MODE_INPUT_PULLUP);
+ palSetPadMode(SPI_SW_SCK_GPIO, SPI_SW_SCK_PIN, PAL_MODE_INPUT_PULLUP);
+ palSetPadMode(SPI_SW_CS_GPIO, SPI_SW_CS_PIN, PAL_MODE_INPUT_PULLUP);
-void encoder_init(uint32_t counts) {
+ palSetPadMode(HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1, PAL_MODE_INPUT_PULLUP);
+ palSetPadMode(HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2, PAL_MODE_INPUT_PULLUP);
+
+ index_found = false;
+ mode = ENCODER_MODE_NONE;
+ last_enc_angle = 0.0;
+}
+
+void encoder_init_abi(uint32_t counts) {
EXTI_InitTypeDef EXTI_InitStructure;
- NVIC_InitTypeDef NVIC_InitStructure;
// Initialize variables
index_found = false;
@@ -61,7 +99,7 @@
TIM_ICPolarity_Rising);
TIM_SetAutoreload(HW_ENC_TIM, enc_counts - 1);
- TIM_Cmd (HW_ENC_TIM, ENABLE);
+ TIM_Cmd(HW_ENC_TIM, ENABLE);
// Interrupt on index pulse
@@ -76,15 +114,62 @@
EXTI_Init(&EXTI_InitStructure);
// Enable and set EXTI Line Interrupt to the highest priority
- NVIC_InitStructure.NVIC_IRQChannel = HW_ENC_EXTI_CH;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x00;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x00;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- NVIC_Init(&NVIC_InitStructure);
+ nvicEnableVector(HW_ENC_EXTI_CH, 0);
+
+ mode = ENCODER_MODE_ABI;
+}
+
+void encoder_init_as5047p_spi(void) {
+ TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
+
+ palSetPadMode(SPI_SW_MISO_GPIO, SPI_SW_MISO_PIN, PAL_MODE_INPUT);
+ palSetPadMode(SPI_SW_SCK_GPIO, SPI_SW_SCK_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
+ palSetPadMode(SPI_SW_CS_GPIO, SPI_SW_CS_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
+
+ // Enable timer clock
+ HW_ENC_TIM_CLK_EN();
+
+ // Time Base configuration
+ TIM_TimeBaseStructure.TIM_Prescaler = 0;
+ TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
+ TIM_TimeBaseStructure.TIM_Period = ((168000000 / 2 / AS5047_SAMPLE_RATE_HZ) - 1);
+ TIM_TimeBaseStructure.TIM_ClockDivision = 0;
+ TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
+ TIM_TimeBaseInit(HW_ENC_TIM, &TIM_TimeBaseStructure);
+
+ // Enable overflow interrupt
+ TIM_ITConfig(HW_ENC_TIM, TIM_IT_Update, ENABLE);
+
+ // Enable timer
+ TIM_Cmd(HW_ENC_TIM, ENABLE);
+
+ nvicEnableVector(HW_ENC_TIM_ISR_CH, 6);
+
+ mode = ENCODER_MODE_AS5047P_SPI;
+ index_found = true;
+}
+
+bool encoder_is_configured(void) {
+ return mode != ENCODER_MODE_NONE;
}
float encoder_read_deg(void) {
- return ((float)HW_ENC_TIM->CNT * 360.0) / (float)enc_counts;
+ static float angle = 0.0;
+
+ switch (mode) {
+ case ENCODER_MODE_ABI:
+ angle = ((float)HW_ENC_TIM->CNT * 360.0) / (float)enc_counts;
+ break;
+
+ case ENCODER_MODE_AS5047P_SPI:
+ angle = last_enc_angle;
+ break;
+
+ default:
+ break;
+ }
+
+ return angle;
}
/**
@@ -96,6 +181,20 @@
}
/**
+ * Timer interrupt
+ */
+void encoder_tim_isr(void) {
+ uint16_t pos;
+
+ spi_begin();
+ spi_transfer(&pos, 0, 1);
+ spi_end();
+
+ pos &= 0x3FFF;
+ last_enc_angle = ((float)pos * 360.0) / 16384.0;
+}
+
+/**
* Set the number of encoder counts.
*
* @param counts
@@ -118,3 +217,53 @@
bool encoder_index_found(void) {
return index_found;
}
+
+// Software SPI
+uint16_t spi_exchange(uint16_t x) {
+ uint16_t rx;
+ spi_transfer(&rx, &x, 1);
+ return rx;
+}
+
+static void spi_transfer(uint16_t *in_buf, const uint16_t *out_buf, int length) {
+ for (int i = 0;i < length;i++) {
+ uint16_t send = out_buf ? out_buf[i] : 0xFFFF;
+ uint16_t recieve = 0;
+
+ for (int bit = 0;bit < 16;bit++) {
+ //palWritePad(HW_SPI_PORT_MOSI, HW_SPI_PIN_MOSI, send >> 15);
+ send <<= 1;
+
+ spi_delay();
+ palSetPad(SPI_SW_SCK_GPIO, SPI_SW_SCK_PIN);
+ spi_delay();
+
+ recieve <<= 1;
+ if (palReadPad(SPI_SW_MISO_GPIO, SPI_SW_MISO_PIN)) {
+ recieve |= 1;
+ }
+
+ palClearPad(SPI_SW_SCK_GPIO, SPI_SW_SCK_PIN);
+ spi_delay();
+ }
+
+ if (in_buf) {
+ in_buf[i] = recieve;
+ }
+ }
+}
+
+static void spi_begin(void) {
+ palClearPad(SPI_SW_CS_GPIO, SPI_SW_CS_PIN);
+}
+
+static void spi_end(void) {
+ palSetPad(SPI_SW_CS_GPIO, SPI_SW_CS_PIN);
+}
+
+static void spi_delay(void) {
+ __NOP();
+ __NOP();
+ __NOP();
+ __NOP();
+}
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/encoder.h Raptor2 Sources/bldc-firmware_2_80/encoder.h
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/encoder.h 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/encoder.h 2016-09-19 10:06:22.000000000 +0100
@@ -28,9 +28,13 @@
#include "conf_general.h"
// Functions
-void encoder_init(uint32_t counts);
+void encoder_deinit(void);
+void encoder_init_abi(uint32_t counts);
+void encoder_init_as5047p_spi(void);
+bool encoder_is_configured(void);
float encoder_read_deg(void);
void encoder_reset(void);
+void encoder_tim_isr(void);
void encoder_set_counts(uint32_t counts);
bool encoder_index_found(void);
Only in Raptor2 Sources/bldc-firmware_2_80: gpl.txt
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/halconf.h Raptor2 Sources/bldc-firmware_2_80/halconf.h
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/halconf.h 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/halconf.h 2016-09-19 10:06:16.000000000 +0100
@@ -146,7 +146,7 @@
* @brief Enables the SPI subsystem.
*/
#if !defined(HAL_USE_SPI) || defined(__DOXYGEN__)
-#define HAL_USE_SPI FALSE
+#define HAL_USE_SPI TRUE
#endif
/**
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/hwconf/hw_40.h Raptor2 Sources/bldc-firmware_2_80/hwconf/hw_40.h
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/hwconf/hw_40.h 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/hwconf/hw_40.h 2016-08-04 23:47:30.000000000 +0100
@@ -146,6 +146,8 @@
#define HW_ENC_EXTI_CH EXTI9_5_IRQn
#define HW_ENC_EXTI_LINE EXTI_Line8
#define HW_ENC_EXTI_ISR_VEC EXTI9_5_IRQHandler
+#define HW_ENC_TIM_ISR_CH TIM4_IRQn
+#define HW_ENC_TIM_ISR_VEC TIM4_IRQHandler
// NRF pins
#define NRF_PORT_CSN HW_ICU_GPIO
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/hwconf/hw_410.h Raptor2 Sources/bldc-firmware_2_80/hwconf/hw_410.h
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/hwconf/hw_410.h 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/hwconf/hw_410.h 2016-08-04 23:47:30.000000000 +0100
@@ -148,6 +148,8 @@
#define HW_ENC_EXTI_CH EXTI15_10_IRQn
#define HW_ENC_EXTI_LINE EXTI_Line11
#define HW_ENC_EXTI_ISR_VEC EXTI15_10_IRQHandler
+#define HW_ENC_TIM_ISR_CH TIM4_IRQn
+#define HW_ENC_TIM_ISR_VEC TIM4_IRQHandler
// NRF pins
#define NRF_PORT_CSN GPIOA
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/hwconf/hw_45.h Raptor2 Sources/bldc-firmware_2_80/hwconf/hw_45.h
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/hwconf/hw_45.h 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/hwconf/hw_45.h 2016-08-04 23:47:30.000000000 +0100
@@ -146,6 +146,8 @@
#define HW_ENC_EXTI_CH EXTI15_10_IRQn
#define HW_ENC_EXTI_LINE EXTI_Line11
#define HW_ENC_EXTI_ISR_VEC EXTI15_10_IRQHandler
+#define HW_ENC_TIM_ISR_CH TIM4_IRQn
+#define HW_ENC_TIM_ISR_VEC TIM4_IRQHandler
// NRF pins
#define NRF_PORT_CSN HW_ICU_GPIO
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/hwconf/hw_46.h Raptor2 Sources/bldc-firmware_2_80/hwconf/hw_46.h
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/hwconf/hw_46.h 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/hwconf/hw_46.h 2016-08-04 23:47:30.000000000 +0100
@@ -154,6 +154,8 @@
#define HW_ENC_EXTI_CH EXTI15_10_IRQn
#define HW_ENC_EXTI_LINE EXTI_Line11
#define HW_ENC_EXTI_ISR_VEC EXTI15_10_IRQHandler
+#define HW_ENC_TIM_ISR_CH TIM4_IRQn
+#define HW_ENC_TIM_ISR_VEC TIM4_IRQHandler
// NRF pins
#define NRF_PORT_CSN HW_ICU_GPIO
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/hwconf/hw_48.h Raptor2 Sources/bldc-firmware_2_80/hwconf/hw_48.h
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/hwconf/hw_48.h 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/hwconf/hw_48.h 2016-08-04 23:47:30.000000000 +0100
@@ -148,6 +148,8 @@
#define HW_ENC_EXTI_CH EXTI15_10_IRQn
#define HW_ENC_EXTI_LINE EXTI_Line11
#define HW_ENC_EXTI_ISR_VEC EXTI15_10_IRQHandler
+#define HW_ENC_TIM_ISR_CH TIM4_IRQn
+#define HW_ENC_TIM_ISR_VEC TIM4_IRQHandler
// NRF pins
#define NRF_PORT_CSN GPIOB
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/hwconf/hw_49.h Raptor2 Sources/bldc-firmware_2_80/hwconf/hw_49.h
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/hwconf/hw_49.h 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/hwconf/hw_49.h 2016-08-04 23:47:30.000000000 +0100
@@ -148,6 +148,8 @@
#define HW_ENC_EXTI_CH EXTI15_10_IRQn
#define HW_ENC_EXTI_LINE EXTI_Line11
#define HW_ENC_EXTI_ISR_VEC EXTI15_10_IRQHandler
+#define HW_ENC_TIM_ISR_CH TIM4_IRQn
+#define HW_ENC_TIM_ISR_VEC TIM4_IRQHandler
// NRF pins
#define NRF_PORT_CSN GPIOB
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/hwconf/hw_r2.h Raptor2 Sources/bldc-firmware_2_80/hwconf/hw_r2.h
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/hwconf/hw_r2.h 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/hwconf/hw_r2.h 2016-08-04 23:47:30.000000000 +0100
@@ -156,6 +156,8 @@
#define HW_ENC_EXTI_CH EXTI9_5_IRQn
#define HW_ENC_EXTI_LINE EXTI_Line8
#define HW_ENC_EXTI_ISR_VEC EXTI9_5_IRQHandler
+#define HW_ENC_TIM_ISR_CH TIM3_IRQn
+#define HW_ENC_TIM_ISR_VEC TIM3_IRQHandler
// NRF pins
#define NRF_PORT_CSN HW_ICU_GPIO
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/hwconf/hw_victor_r1a.h Raptor2 Sources/bldc-firmware_2_80/hwconf/hw_victor_r1a.h
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/hwconf/hw_victor_r1a.h 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/hwconf/hw_victor_r1a.h 2016-08-04 23:47:30.000000000 +0100
@@ -146,6 +146,8 @@
#define HW_ENC_EXTI_CH EXTI15_10_IRQn
#define HW_ENC_EXTI_LINE EXTI_Line11
#define HW_ENC_EXTI_ISR_VEC EXTI15_10_IRQHandler
+#define HW_ENC_TIM_ISR_CH TIM4_IRQn
+#define HW_ENC_TIM_ISR_VEC TIM4_IRQHandler
// NRF pins
#define NRF_PORT_CSN HW_ICU_GPIO
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/irq_handlers.c Raptor2 Sources/bldc-firmware_2_80/irq_handlers.c
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/irq_handlers.c 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/irq_handlers.c 2016-09-19 10:06:12.000000000 +0100
@@ -19,7 +19,6 @@
#include "hal.h"
#include "stm32f4xx_conf.h"
#include "isr_vector_table.h"
-#include "main.h"
#include "mc_interface.h"
#include "servo.h"
#include "hw.h"
@@ -49,3 +48,12 @@
EXTI_ClearITPendingBit(HW_ENC_EXTI_LINE);
}
}
+
+CH_IRQ_HANDLER(HW_ENC_TIM_ISR_VEC) {
+ if (TIM_GetITStatus(HW_ENC_TIM, TIM_IT_Update) != RESET) {
+ encoder_tim_isr();
+
+ // Clear the IT pending bit
+ TIM_ClearITPendingBit(HW_ENC_TIM, TIM_IT_Update);
+ }
+}
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/main.c Raptor2 Sources/bldc-firmware_2_80/main.c
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/main.c 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/main.c 2016-09-19 10:05:50.000000000 +0100
@@ -24,7 +24,6 @@
#include <string.h>
#include <stdlib.h>
-#include "main.h"
#include "mc_interface.h"
#include "mcpwm.h"
#include "mcpwm_foc.h"
@@ -68,55 +67,17 @@
*
*/
-/*
- * Notes:
- *
- * Disable USB VBUS sensing:
- * ChibiOS-RT-master/os/hal/platforms/STM32/OTGv1/usb_lld.c
- *
- * change
- * otgp->GCCFG = GCCFG_VBUSASEN | GCCFG_VBUSBSEN | GCCFG_PWRDWN;
- * to
- * otgp->GCCFG = GCCFG_NOVBUSSENS | GCCFG_PWRDWN;
- *
- * This should be handled automatically with the latest version of
- * ChibiOS since I have added an option to the makefile.
- *
- */
-
// Private variables
-#define ADC_SAMPLE_MAX_LEN 2000
-static volatile int16_t curr0_samples[ADC_SAMPLE_MAX_LEN];
-static volatile int16_t curr1_samples[ADC_SAMPLE_MAX_LEN];
-static volatile int16_t ph1_samples[ADC_SAMPLE_MAX_LEN];
-static volatile int16_t ph2_samples[ADC_SAMPLE_MAX_LEN];
-static volatile int16_t ph3_samples[ADC_SAMPLE_MAX_LEN];
-static volatile int16_t vzero_samples[ADC_SAMPLE_MAX_LEN];
-static volatile uint8_t status_samples[ADC_SAMPLE_MAX_LEN];
-static volatile int16_t curr_fir_samples[ADC_SAMPLE_MAX_LEN];
-static volatile int16_t f_sw_samples[ADC_SAMPLE_MAX_LEN];
-
-static volatile int sample_len = 1000;
-static volatile int sample_int = 1;
-static volatile int sample_ready = 1;
-static volatile int sample_now = 0;
-static volatile int sample_at_start = 0;
-static volatile int start_comm = 0;
-static volatile float main_last_adc_duration = 0.0;
+
static THD_WORKING_AREA(periodic_thread_wa, 1024);
-static THD_WORKING_AREA(sample_send_thread_wa, 1024);
static THD_WORKING_AREA(timer_thread_wa, 128);
-static thread_t *sample_send_tp;
-
static THD_FUNCTION(periodic_thread, arg) {
(void)arg;
chRegSetThreadName("Main periodic");
- int fault_print = 0;
-
for(;;) {
if (mc_interface_get_state() == MC_STATE_RUNNING) {
ledpwm_set_intensity(LED_GREEN, 1.0);
@@ -126,12 +87,6 @@
mc_fault_code fault = mc_interface_get_fault();
if (fault != FAULT_CODE_NONE) {
- if (!fault_print && AUTO_PRINT_FAULTS) {
- fault_print = 1;
- commands_printf("%s\n", mc_interface_fault_to_string(
- mc_interface_get_fault()));
- }
-
for (int i = 0;i < (int)fault;i++) {
ledpwm_set_intensity(LED_RED, 1.0);
chThdSleepMilliseconds(250);
@@ -142,7 +97,6 @@
chThdSleepMilliseconds(500);
} else {
ledpwm_set_intensity(LED_RED, 0.0);
- fault_print = 0;
}
if (mc_interface_get_state() == MC_STATE_DETECTING) {
@@ -156,8 +110,12 @@
commands_send_rotor_pos(encoder_read_deg());
break;
- case DISP_POS_MODE_ENCODER_POS_ERROR:
- commands_send_rotor_pos(utils_angle_difference(mc_interface_get_pos_set(), encoder_read_deg()));
+ case DISP_POS_MODE_PID_POS:
+ commands_send_rotor_pos(mc_interface_get_pid_pos_now());
+ break;
+
+ case DISP_POS_MODE_PID_POS_ERROR:
+ commands_send_rotor_pos(utils_angle_difference(mc_interface_get_pid_pos_set(), mc_interface_get_pid_pos_now()));
break;
default:
@@ -183,43 +141,6 @@
}
}
-static THD_FUNCTION(sample_send_thread, arg) {
- (void)arg;
-
- chRegSetThreadName("Main sample");
-
- sample_send_tp = chThdGetSelfX();
-
- for(;;) {
- chEvtWaitAny((eventmask_t) 1);
-
- for (int i = 0;i < sample_len;i++) {
- uint8_t buffer[20];
- int index = 0;
-
- buffer[index++] = curr0_samples[i] >> 8;
- buffer[index++] = curr0_samples[i];
- buffer[index++] = curr1_samples[i] >> 8;
- buffer[index++] = curr1_samples[i];
- buffer[index++] = ph1_samples[i] >> 8;
- buffer[index++] = ph1_samples[i];
- buffer[index++] = ph2_samples[i] >> 8;
- buffer[index++] = ph2_samples[i];
- buffer[index++] = ph3_samples[i] >> 8;
- buffer[index++] = ph3_samples[i];
- buffer[index++] = vzero_samples[i] >> 8;
- buffer[index++] = vzero_samples[i];
- buffer[index++] = status_samples[i];
- buffer[index++] = curr_fir_samples[i] >> 8;
- buffer[index++] = curr_fir_samples[i];
- buffer[index++] = f_sw_samples[i] >> 8;
- buffer[index++] = f_sw_samples[i];
-
- commands_send_samples(buffer, index);
- }
- }
-}
-
static THD_FUNCTION(timer_thread, arg) {
(void)arg;
@@ -231,85 +152,6 @@
}
}
-/*
- * Called every time new ADC values are available. Note that
- * the ADC is initialized from mcpwm.c
- */
-void main_dma_adc_handler(void) {
- ledpwm_update_pwm();
-
- if (sample_at_start && (mc_interface_get_state() == MC_STATE_RUNNING ||
- start_comm != mcpwm_get_comm_step())) {
- sample_now = 0;
- sample_ready = 0;
- sample_at_start = 0;
- }
-
- static int a = 0;
- if (!sample_ready) {
- a++;
- if (a >= sample_int) {
- a = 0;
-
- if (mc_interface_get_state() == MC_STATE_DETECTING) {
- curr0_samples[sample_now] = (int16_t)mcpwm_detect_currents[mcpwm_get_comm_step() - 1];
- curr1_samples[sample_now] = (int16_t)mcpwm_detect_currents_diff[mcpwm_get_comm_step() - 1];
-
- ph1_samples[sample_now] = (int16_t)mcpwm_detect_voltages[0];
- ph2_samples[sample_now] = (int16_t)mcpwm_detect_voltages[1];
- ph3_samples[sample_now] = (int16_t)mcpwm_detect_voltages[2];
- } else {
- curr0_samples[sample_now] = ADC_curr_norm_value[0];
- curr1_samples[sample_now] = ADC_curr_norm_value[1];
-
- ph1_samples[sample_now] = ADC_V_L1 - mcpwm_vzero;
- ph2_samples[sample_now] = ADC_V_L2 - mcpwm_vzero;
- ph3_samples[sample_now] = ADC_V_L3 - mcpwm_vzero;
- }
-
- vzero_samples[sample_now] = mcpwm_vzero;
-
- curr_fir_samples[sample_now] = (int16_t)(mc_interface_get_tot_current() * 100.0);
- f_sw_samples[sample_now] = (int16_t)(mc_interface_get_switching_frequency_now() / 10.0);
-
- status_samples[sample_now] = mcpwm_get_comm_step() | (mcpwm_read_hall_phase() << 3);
-
- sample_now++;
-
- if (sample_now == sample_len) {
- sample_ready = 1;
- sample_now = 0;
- chSysLockFromISR();
- chEvtSignalI(sample_send_tp, (eventmask_t) 1);
- chSysUnlockFromISR();
- }
-
- main_last_adc_duration = mcpwm_get_last_adc_isr_duration();
- }
- }
-}
-
-float main_get_last_adc_isr_duration(void) {
- return main_last_adc_duration;
-}
-
-void main_sample_print_data(bool at_start, uint16_t len, uint8_t decimation) {
- if (len > ADC_SAMPLE_MAX_LEN) {
- len = ADC_SAMPLE_MAX_LEN;
- }
-
- sample_len = len;
- sample_int = decimation;
-
- if (at_start) {
- sample_at_start = 1;
- start_comm = mcpwm_get_comm_step();
- } else {
- sample_now = 0;
- sample_ready = 0;
- }
-}
-
int main(void) {
halInit();
chSysInit();
@@ -325,11 +167,6 @@
mc_configuration mcconf;
conf_general_read_mc_configuration(&mcconf);
-
-#if ENCODER_ENABLE
- encoder_init(mcconf.m_encoder_counts);
-#endif
-
mc_interface_init(&mcconf);
commands_init();
@@ -361,14 +198,13 @@
// Threads
chThdCreateStatic(periodic_thread_wa, sizeof(periodic_thread_wa), NORMALPRIO, periodic_thread, NULL);
- chThdCreateStatic(sample_send_thread_wa, sizeof(sample_send_thread_wa), NORMALPRIO - 1, sample_send_thread, NULL);
chThdCreateStatic(timer_thread_wa, sizeof(timer_thread_wa), NORMALPRIO, timer_thread, NULL);
for(;;) {
chThdSleepMilliseconds(10);
-#if ENCODER_ENABLE
-// comm_can_set_pos(0, encoder_read_deg());
-#endif
+ if (encoder_is_configured()) {
+ // comm_can_set_pos(0, encoder_read_deg());
+ }
}
}
Only in bldc-cc171422641a50c56452280575f2074f8a88aa45: main.h
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/mc_interface.c Raptor2 Sources/bldc-firmware_2_80/mc_interface.c
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/mc_interface.c 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/mc_interface.c 2017-06-16 22:00:36.000000000 +0100
@@ -25,13 +25,15 @@
#include "mc_interface.h"
#include "mcpwm.h"
#include "mcpwm_foc.h"
-#include "main.h"
+#include "ledpwm.h"
#include "stm32f4xx_conf.h"
#include "hw.h"
#include "terminal.h"
#include "utils.h"
#include "ch.h"
#include "hal.h"
+#include "commands.h"
+#include "encoder.h"
#include <math.h>
// Global variables
@@ -39,21 +41,43 @@
volatile int ADC_curr_norm_value[3];
// Private variables
-static volatile mc_configuration conf;
-static mc_fault_code fault_now;
-static int ignore_iterations;
-static volatile unsigned int cycles_running;
-static volatile bool lock_enabled;
-static volatile bool lock_override_once;
-static volatile float motor_current_sum;
-static volatile float input_current_sum;
-static volatile float motor_current_iterations;
-static volatile float input_current_iterations;
-static volatile float amp_seconds;
-static volatile float amp_seconds_charged;
-static volatile float watt_seconds;
-static volatile float watt_seconds_charged;
-static volatile float position_set;
+static volatile mc_configuration m_conf;
+static volatile mc_configuration m_conf_original;
+static mc_fault_code m_fault_now;
+static int m_ignore_iterations;
+static volatile unsigned int m_cycles_running;
+static volatile bool m_lock_enabled;
+static volatile bool m_lock_override_once;
+static volatile float m_motor_current_sum;
+static volatile float m_input_current_sum;
+static volatile float m_motor_current_iterations;
+static volatile float m_input_current_iterations;
+static volatile float m_amp_seconds;
+static volatile float m_amp_seconds_charged;
+static volatile float m_watt_seconds;
+static volatile float m_watt_seconds_charged;
+static volatile float m_position_set;
+// new
+static volatile ppm_cruise cruise_control_status;
+
+// Sampling variables
+#define ADC_SAMPLE_MAX_LEN 2000
+static volatile int16_t m_curr0_samples[ADC_SAMPLE_MAX_LEN];
+static volatile int16_t m_curr1_samples[ADC_SAMPLE_MAX_LEN];
+static volatile int16_t m_ph1_samples[ADC_SAMPLE_MAX_LEN];
+static volatile int16_t m_ph2_samples[ADC_SAMPLE_MAX_LEN];
+static volatile int16_t m_ph3_samples[ADC_SAMPLE_MAX_LEN];
+static volatile int16_t m_vzero_samples[ADC_SAMPLE_MAX_LEN];
+static volatile uint8_t m_status_samples[ADC_SAMPLE_MAX_LEN];
+static volatile int16_t m_curr_fir_samples[ADC_SAMPLE_MAX_LEN];
+static volatile int16_t m_f_sw_samples[ADC_SAMPLE_MAX_LEN];
+static volatile int m_sample_len;
+static volatile int m_sample_int;
+static volatile int m_sample_ready;
+static volatile int m_sample_now;
+static volatile int m_sample_at_start;
+static volatile int m_start_comm;
+static volatile float m_last_adc_duration_sample;
// Private functions
static void update_override_limits(volatile mc_configuration *conf);
@@ -64,36 +88,66 @@
// Threads
static THD_WORKING_AREA(timer_thread_wa, 1024);
static THD_FUNCTION(timer_thread, arg);
+static THD_WORKING_AREA(sample_send_thread_wa, 1024);
+static THD_FUNCTION(sample_send_thread, arg);
+static thread_t *sample_send_tp;
void mc_interface_init(mc_configuration *configuration) {
- conf = *configuration;
- fault_now = FAULT_CODE_NONE;
- ignore_iterations = 0;
- cycles_running = 0;
- lock_enabled = false;
- lock_override_once = false;
- motor_current_sum = 0.0;
- input_current_sum = 0.0;
- motor_current_iterations = 0.0;
- input_current_iterations = 0.0;
- amp_seconds = 0.0;
- amp_seconds_charged = 0.0;
- watt_seconds = 0.0;
- watt_seconds_charged = 0.0;
- position_set = 0.0;
+ m_conf = *configuration;
+ m_conf_original = *configuration;
+ m_fault_now = FAULT_CODE_NONE;
+ m_ignore_iterations = 0;
+ m_cycles_running = 0;
+ m_lock_enabled = false;
+ m_lock_override_once = false;
+ m_motor_current_sum = 0.0;
+ m_input_current_sum = 0.0;
+ m_motor_current_iterations = 0.0;
+ m_input_current_iterations = 0.0;
+ m_amp_seconds = 0.0;
+ m_amp_seconds_charged = 0.0;
+ m_watt_seconds = 0.0;
+ m_watt_seconds_charged = 0.0;
+ m_position_set = 0.0;
+ m_last_adc_duration_sample = 0.0;
+ cruise_control_status = CRUISE_CONTROL_INACTIVE;
+
+ m_sample_len = 1000;
+ m_sample_int = 1;
+ m_sample_ready = 1;
+ m_sample_now = 0;
+ m_sample_at_start = 0;
+ m_start_comm = 0;
// Start threads
chThdCreateStatic(timer_thread_wa, sizeof(timer_thread_wa), NORMALPRIO, timer_thread, NULL);
+ chThdCreateStatic(sample_send_thread_wa, sizeof(sample_send_thread_wa), NORMALPRIO - 1, sample_send_thread, NULL);
+
+ // Initialize encoder
+#if !WS2811_ENABLE
+ switch (m_conf.m_sensor_port_mode) {
+ case SENSOR_PORT_MODE_ABI:
+ encoder_init_abi(m_conf.m_encoder_counts);
+ break;
+
+ case SENSOR_PORT_MODE_AS5047_SPI:
+ encoder_init_as5047p_spi();
+ break;
+
+ default:
+ break;
+ }
+#endif
// Initialize selected implementation
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
- mcpwm_init(&conf);
+ mcpwm_init(&m_conf);
break;
case MOTOR_TYPE_FOC:
- mcpwm_foc_init(&conf);
+ mcpwm_foc_init(&m_conf);
break;
default:
@@ -102,34 +156,56 @@
}
const volatile mc_configuration* mc_interface_get_configuration(void) {
- return &conf;
+ return &m_conf;
}
void mc_interface_set_configuration(mc_configuration *configuration) {
- if (conf.motor_type == MOTOR_TYPE_FOC
+#if !WS2811_ENABLE
+ if (m_conf.m_sensor_port_mode != configuration->m_sensor_port_mode) {
+ encoder_deinit();
+ switch (configuration->m_sensor_port_mode) {
+ case SENSOR_PORT_MODE_ABI:
+ encoder_init_abi(configuration->m_encoder_counts);
+ break;
+
+ case SENSOR_PORT_MODE_AS5047_SPI:
+ encoder_init_as5047p_spi();
+ break;
+
+ default:
+ break;
+ }
+ }
+
+ if (configuration->m_sensor_port_mode == SENSOR_PORT_MODE_ABI) {
+ encoder_set_counts(configuration->m_encoder_counts);
+ }
+#endif
+
+ if (m_conf.motor_type == MOTOR_TYPE_FOC
&& configuration->motor_type != MOTOR_TYPE_FOC) {
mcpwm_foc_deinit();
- conf = *configuration;
- mcpwm_init(&conf);
- } else if (conf.motor_type != MOTOR_TYPE_FOC
+ m_conf = *configuration;
+ mcpwm_init(&m_conf);
+ } else if (m_conf.motor_type != MOTOR_TYPE_FOC
&& configuration->motor_type == MOTOR_TYPE_FOC) {
mcpwm_deinit();
- conf = *configuration;
- mcpwm_foc_init(&conf);
+ m_conf = *configuration;
+ mcpwm_foc_init(&m_conf);
} else {
- conf = *configuration;
+ m_conf = *configuration;
}
- update_override_limits(&conf);
+ update_override_limits(&m_conf);
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
- mcpwm_set_configuration(&conf);
+ mcpwm_set_configuration(&m_conf);
break;
case MOTOR_TYPE_FOC:
- mcpwm_foc_set_configuration(&conf);
+ mcpwm_foc_set_configuration(&m_conf);
break;
default:
@@ -139,7 +215,7 @@
bool mc_interface_dccal_done(void) {
bool ret = false;
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
ret = mcpwm_is_dccal_done();
@@ -170,25 +246,25 @@
* Lock the control by disabling all control commands.
*/
void mc_interface_lock(void) {
- lock_enabled = true;
+ m_lock_enabled = true;
}
/**
* Unlock all control commands.
*/
void mc_interface_unlock(void) {
- lock_enabled = false;
+ m_lock_enabled = false;
}
/**
* Allow just one motor control command in the locked state.
*/
void mc_interface_lock_override_once(void) {
- lock_override_once = true;
+ m_lock_override_once = true;
}
mc_fault_code mc_interface_get_fault(void) {
- return fault_now;
+ return m_fault_now;
}
const char* mc_interface_fault_to_string(mc_fault_code fault) {
@@ -206,7 +282,7 @@
mc_state mc_interface_get_state(void) {
mc_state ret = MC_STATE_OFF;
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
ret = mcpwm_get_state();
@@ -228,7 +304,7 @@
return;
}
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
mcpwm_set_duty(dutyCycle);
@@ -248,7 +324,7 @@
return;
}
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
mcpwm_set_duty_noramp(dutyCycle);
@@ -268,7 +344,7 @@
return;
}
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
mcpwm_set_pid_speed(rpm);
@@ -283,14 +359,43 @@
}
}
+void mc_interface_set_pid_speed_with_cruise_status(float rpm, ppm_cruise cruise_status) {
+ if (mc_interface_try_input()) {
+ return;
+ }
+
+ switch (m_conf.motor_type) {
+ case MOTOR_TYPE_BLDC:
+ case MOTOR_TYPE_DC:
+ mcpwm_set_pid_speed_with_cruise_status(rpm, cruise_status);
+ break;
+
+ case MOTOR_TYPE_FOC:
+ mcpwm_foc_set_pid_speed_with_cruise_status(rpm, cruise_status);
+ break;
+
+ default:
+ break;
+ }
+}
+
+ppm_cruise mc_interface_get_cruise_control_status(void){
+ return cruise_control_status;
+}
+
+// true = active false = inactive
+void mc_interface_set_cruise_control_status(ppm_cruise status){
+ cruise_control_status = status;
+}
+
void mc_interface_set_pid_pos(float pos) {
if (mc_interface_try_input()) {
return;
}
- position_set = pos;
+ m_position_set = pos;
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
mcpwm_set_pid_pos(pos);
@@ -310,7 +415,7 @@
return;
}
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
mcpwm_set_current(current);
@@ -330,7 +435,75 @@
return;
}
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
+ case MOTOR_TYPE_BLDC:
+ case MOTOR_TYPE_DC:
+ mcpwm_set_brake_current(current);
+ break;
+
+ case MOTOR_TYPE_FOC:
+ mcpwm_foc_set_brake_current(current);
+ break;
+
+ default:
+ break;
+ }
+}
+
+void mc_interface_set_servo(float servo_val, bool use_min_current) {
+ if (mc_interface_try_input()) {
+ return;
+ }
+
+ float current = 0.0;
+
+ if (servo_val > 0.0){
+ if (m_conf.use_max_watt_limit) {
+ current = utils_smallest_of_2(servo_val * m_conf.l_current_max,
+ servo_val * (m_conf.watts_max / GET_INPUT_VOLTAGE() / mc_interface_get_duty_cycle_for_watt_calculation()));
+ } else {
+ current = utils_smallest_of_2(servo_val * m_conf.l_current_max,
+ servo_val * m_conf.l_in_current_max / mc_interface_get_duty_cycle_for_watt_calculation());
+ }
+ if (use_min_current) {
+ current = utils_highest_of_2(current, m_conf.cc_min_current);
+ }
+ } else if (servo_val < 0.0){
+ if (m_conf.use_max_watt_limit) {
+ current = -utils_smallest_of_2(fabsf(servo_val * m_conf.l_current_min),
+ fabsf(servo_val * (m_conf.watts_max / GET_INPUT_VOLTAGE() / mc_interface_get_duty_cycle_for_watt_calculation())));
+ } else {
+ current = -utils_smallest_of_2(fabsf(servo_val * m_conf.l_current_min),
+ fabsf(servo_val * m_conf.l_in_current_max / mc_interface_get_duty_cycle_for_watt_calculation()));
+ }
+ if (use_min_current) {
+ current = utils_smallest_of_2(current, -m_conf.cc_min_current);
+ }
+ }
+
+ switch (m_conf.motor_type) {
+ case MOTOR_TYPE_BLDC:
+ case MOTOR_TYPE_DC:
+ mcpwm_set_current(current);
+ break;
+
+ case MOTOR_TYPE_FOC:
+ mcpwm_foc_set_current(current);
+ break;
+
+ default:
+ break;
+ }
+}
+
+void mc_interface_set_brake_servo(float servo_val) {
+ if (mc_interface_try_input()) {
+ return;
+ }
+
+ float current = servo_val * fabsf(m_conf.l_current_min);
+
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
mcpwm_set_brake_current(current);
@@ -356,13 +529,34 @@
mc_interface_set_current(0.0);
}
+float mc_interface_get_duty_cycle_for_watt_calculation(void) {
+ float actual_duty;
+
+ if (m_conf.motor_type == MOTOR_TYPE_FOC) {
+ //float battery_current = mcpwm_foc_get_tot_current_in_filtered();
+ //float motor_current = mcpwm_foc_get_tot_current_filtered();
+
+ //if (fabsf(battery_current) > 1.0 && fabsf(motor_current) > 1.0) {
+ // actual_duty = fabsf(battery_current / motor_current);
+ //} else {
+ actual_duty = fabsf(mcpwm_foc_get_duty_cycle_now()) * 0.86602540378;
+ //}
+ } else {
+ actual_duty = fabsf(mc_interface_get_duty_cycle_now());
+ }
+ if (actual_duty < m_conf.l_min_duty) {
+ return m_conf.l_min_duty;
+ }
+ return actual_duty;
+}
+
/**
* Stop the motor and use braking.
*/
float mc_interface_get_duty_cycle_set(void) {
float ret = 0.0;
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
ret = mcpwm_get_duty_cycle_set();
@@ -382,7 +576,7 @@
float mc_interface_get_duty_cycle_now(void) {
float ret = 0.0;
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
ret = mcpwm_get_duty_cycle_now();
@@ -399,17 +593,17 @@
return ret;
}
-float mc_interface_get_switching_frequency_now(void) {
+float mc_interface_get_sampling_frequency_now(void) {
float ret = 0.0;
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
ret = mcpwm_get_switching_frequency_now();
break;
case MOTOR_TYPE_FOC:
- ret = mcpwm_foc_get_switching_frequency_now();
+ ret = mcpwm_foc_get_switching_frequency_now() / 2.0;
break;
default:
@@ -422,7 +616,7 @@
float mc_interface_get_rpm(void) {
float ret = 0.0;
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
ret = mcpwm_get_rpm();
@@ -449,10 +643,10 @@
* The amount of amp hours drawn.
*/
float mc_interface_get_amp_hours(bool reset) {
- float val = amp_seconds / 3600;
+ float val = m_amp_seconds / 3600;
if (reset) {
- amp_seconds = 0.0;
+ m_amp_seconds = 0.0;
}
return val;
@@ -468,10 +662,10 @@
* The amount of amp hours fed back.
*/
float mc_interface_get_amp_hours_charged(bool reset) {
- float val = amp_seconds_charged / 3600;
+ float val = m_amp_seconds_charged / 3600;
if (reset) {
- amp_seconds_charged = 0.0;
+ m_amp_seconds_charged = 0.0;
}
return val;
@@ -487,10 +681,10 @@
* The amount of watt hours drawn.
*/
float mc_interface_get_watt_hours(bool reset) {
- float val = watt_seconds / 3600;
+ float val = m_watt_seconds / 3600;
if (reset) {
- amp_seconds = 0.0;
+ m_amp_seconds = 0.0;
}
return val;
@@ -506,10 +700,10 @@
* The amount of watt hours fed back.
*/
float mc_interface_get_watt_hours_charged(bool reset) {
- float val = watt_seconds_charged / 3600;
+ float val = m_watt_seconds_charged / 3600;
if (reset) {
- watt_seconds_charged = 0.0;
+ m_watt_seconds_charged = 0.0;
}
return val;
@@ -518,7 +712,7 @@
float mc_interface_get_tot_current(void) {
float ret = 0.0;
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
ret = mcpwm_get_tot_current();
@@ -538,7 +732,7 @@
float mc_interface_get_tot_current_filtered(void) {
float ret = 0.0;
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
ret = mcpwm_get_tot_current_filtered();
@@ -558,7 +752,7 @@
float mc_interface_get_tot_current_directional(void) {
float ret = 0.0;
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
ret = mcpwm_get_tot_current_directional();
@@ -578,7 +772,7 @@
float mc_interface_get_tot_current_directional_filtered(void) {
float ret = 0.0;
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
ret = mcpwm_get_tot_current_directional_filtered();
@@ -598,7 +792,7 @@
float mc_interface_get_tot_current_in(void) {
float ret = 0.0;
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
ret = mcpwm_get_tot_current_in();
@@ -618,7 +812,7 @@
float mc_interface_get_tot_current_in_filtered(void) {
float ret = 0.0;
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
ret = mcpwm_get_tot_current_in_filtered();
@@ -638,7 +832,7 @@
int mc_interface_get_tachometer_value(bool reset) {
int ret = 0;
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
ret = mcpwm_get_tachometer_value(reset);
@@ -658,7 +852,7 @@
int mc_interface_get_tachometer_abs_value(bool reset) {
int ret = 0;
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
ret = mcpwm_get_tachometer_abs_value(reset);
@@ -678,7 +872,7 @@
float mc_interface_get_last_inj_adc_isr_duration(void) {
float ret = 0.0;
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
ret = mcpwm_get_last_inj_adc_isr_duration();
@@ -696,21 +890,62 @@
}
float mc_interface_read_reset_avg_motor_current(void) {
- float res = motor_current_sum / motor_current_iterations;
- motor_current_sum = 0;
- motor_current_iterations = 0;
+ float res = m_motor_current_sum / m_motor_current_iterations;
+ m_motor_current_sum = 0;
+ m_motor_current_iterations = 0;
return res;
}
float mc_interface_read_reset_avg_input_current(void) {
- float res = input_current_sum / input_current_iterations;
- input_current_sum = 0;
- input_current_iterations = 0;
+ float res = m_input_current_sum / m_input_current_iterations;
+ m_input_current_sum = 0;
+ m_input_current_iterations = 0;
return res;
}
-float mc_interface_get_pos_set(void) {
- return position_set;
+float mc_interface_get_pid_pos_set(void) {
+ return m_position_set;
+}
+
+float mc_interface_get_pid_pos_now(void) {
+ float ret = 0.0;
+
+ switch (m_conf.motor_type) {
+ case MOTOR_TYPE_BLDC:
+ case MOTOR_TYPE_DC:
+ ret = encoder_read_deg();
+ break;
+
+ case MOTOR_TYPE_FOC:
+ ret = mcpwm_foc_get_pid_pos_now();
+ break;
+
+ default:
+ break;
+ }
+
+ return ret;
+}
+
+float mc_interface_get_last_sample_adc_isr_duration(void) {
+ return m_last_adc_duration_sample;
+}
+
+void mc_interface_sample_print_data(bool at_start, uint16_t len, uint8_t decimation) {
+ if (len > ADC_SAMPLE_MAX_LEN) {
+ len = ADC_SAMPLE_MAX_LEN;
+ }
+
+ m_sample_len = len;
+ m_sample_int = decimation;
+
+ if (at_start) {
+ m_sample_at_start = 1;
+ m_start_comm = mcpwm_get_comm_step();
+ } else {
+ m_sample_now = 0;
+ m_sample_ready = 0;
+ }
}
// MC implementation functions
@@ -726,16 +961,16 @@
// TODO: Remove this later
if (mc_interface_get_state() == MC_STATE_DETECTING) {
mcpwm_stop_pwm();
- ignore_iterations = MCPWM_DETECT_STOP_TIME;
+ m_ignore_iterations = MCPWM_DETECT_STOP_TIME;
}
- int retval = ignore_iterations;
+ int retval = m_ignore_iterations;
- if (!ignore_iterations && lock_enabled) {
- if (!lock_override_once) {
+ if (!m_ignore_iterations && m_lock_enabled) {
+ if (!m_lock_override_once) {
retval = 1;
} else {
- lock_override_once = false;
+ m_lock_override_once = false;
}
}
@@ -743,7 +978,7 @@
}
void mc_interface_fault_stop(mc_fault_code fault) {
- if (mc_interface_dccal_done() && fault_now == FAULT_CODE_NONE) {
+ if (mc_interface_dccal_done() && m_fault_now == FAULT_CODE_NONE) {
// Sent to terminal fault logger so that all faults and their conditions
// can be printed for debugging.
chSysLock();
@@ -754,13 +989,18 @@
fault_data fdata;
fdata.fault = fault;
- fdata.current = mc_interface_get_tot_current();
- fdata.current_filtered = mc_interface_get_tot_current_filtered();
+ if (m_conf.motor_type == MOTOR_TYPE_FOC) {
+ fdata.current = mcpwm_foc_get_abs_motor_current();
+ fdata.current_filtered = mcpwm_foc_get_abs_motor_current_filtered();
+ }else{
+ fdata.current = mc_interface_get_tot_current();
+ fdata.current_filtered = mc_interface_get_tot_current_filtered();
+ }
fdata.voltage = GET_INPUT_VOLTAGE();
fdata.duty = mc_interface_get_duty_cycle_now();
fdata.rpm = mc_interface_get_rpm();
fdata.tacho = mc_interface_get_tachometer_value(false);
- fdata.cycles_running = cycles_running;
+ fdata.cycles_running = m_cycles_running;
fdata.tim_val_samp = val_samp;
fdata.tim_current_samp = current_samp;
fdata.tim_top = tim_top;
@@ -769,9 +1009,9 @@
terminal_add_fault_data(&fdata);
}
- ignore_iterations = conf.m_fault_stop_time_ms;
+ m_ignore_iterations = m_conf.m_fault_stop_time_ms;
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
mcpwm_stop_pwm();
@@ -785,20 +1025,22 @@
break;
}
- fault_now = fault;
+ m_fault_now = fault;
}
void mc_interface_mc_timer_isr(void) {
+ ledpwm_update_pwm(); // LED PWM Driver update
+
const float input_voltage = GET_INPUT_VOLTAGE();
// Check for faults that should stop the motor
static int wrong_voltage_iterations = 0;
- if (input_voltage < conf.l_min_vin ||
- input_voltage > conf.l_max_vin) {
+ if (input_voltage < m_conf.l_min_vin ||
+ input_voltage > m_conf.l_max_vin) {
wrong_voltage_iterations++;
if ((wrong_voltage_iterations >= 8)) {
- mc_interface_fault_stop(input_voltage < conf.l_min_vin ?
+ mc_interface_fault_stop(input_voltage < m_conf.l_min_vin ?
FAULT_CODE_UNDER_VOLTAGE : FAULT_CODE_OVER_VOLTAGE);
}
} else {
@@ -806,70 +1048,119 @@
}
if (mc_interface_get_state() == MC_STATE_RUNNING) {
- cycles_running++;
+ m_cycles_running++;
} else {
- cycles_running = 0;
+ m_cycles_running = 0;
}
- main_dma_adc_handler();
-
if (pwn_done_func) {
pwn_done_func();
}
const float current = mc_interface_get_tot_current_filtered();
const float current_in = mc_interface_get_tot_current_in_filtered();
- motor_current_sum += current;
- input_current_sum += current_in;
- motor_current_iterations++;
- input_current_iterations++;
+ m_motor_current_sum += current;
+ m_input_current_sum += current_in;
+ m_motor_current_iterations++;
+ m_input_current_iterations++;
float abs_current = mc_interface_get_tot_current();
float abs_current_filtered = current;
- if (conf.motor_type == MOTOR_TYPE_FOC) {
+ if (m_conf.motor_type == MOTOR_TYPE_FOC) {
// TODO: Make this more general
abs_current = mcpwm_foc_get_abs_motor_current();
abs_current_filtered = mcpwm_foc_get_abs_motor_current_filtered();
}
// Current fault code
- if (conf.l_slow_abs_current) {
- if (fabsf(abs_current_filtered) > conf.l_abs_current_max) {
+ if (m_conf.l_slow_abs_current) {
+ if (fabsf(abs_current_filtered) > m_conf.l_abs_current_max) {
mc_interface_fault_stop(FAULT_CODE_ABS_OVER_CURRENT);
}
} else {
- if (fabsf(abs_current) > conf.l_abs_current_max) {
+ if (fabsf(abs_current) > m_conf.l_abs_current_max) {
mc_interface_fault_stop(FAULT_CODE_ABS_OVER_CURRENT);
}
}
// Watt and ah counters
- const float f_sw = mc_interface_get_switching_frequency_now();
+ const float f_samp = mc_interface_get_sampling_frequency_now();
if (fabsf(current) > 1.0) {
// Some extra filtering
static float curr_diff_sum = 0.0;
static float curr_diff_samples = 0;
- curr_diff_sum += current_in / f_sw;
- curr_diff_samples += 1.0 / f_sw;
+ curr_diff_sum += current_in / f_samp;
+ curr_diff_samples += 1.0 / f_samp;
if (curr_diff_samples >= 0.01) {
if (curr_diff_sum > 0.0) {
- amp_seconds += curr_diff_sum;
- watt_seconds += curr_diff_sum * input_voltage;
+ m_amp_seconds += curr_diff_sum;
+ m_watt_seconds += curr_diff_sum * input_voltage;
} else {
- amp_seconds_charged -= curr_diff_sum;
- watt_seconds_charged -= curr_diff_sum * input_voltage;
+ m_amp_seconds_charged -= curr_diff_sum;
+ m_watt_seconds_charged -= curr_diff_sum * input_voltage;
}
curr_diff_samples = 0.0;
curr_diff_sum = 0.0;
}
}
+
+ // Sample collection
+ if (m_sample_at_start && (mc_interface_get_state() == MC_STATE_RUNNING ||
+ m_start_comm != mcpwm_get_comm_step())) {
+ m_sample_now = 0;
+ m_sample_ready = 0;
+ m_sample_at_start = 0;
+ }
+
+ static int a = 0;
+ if (!m_sample_ready) {
+ a++;
+ if (a >= m_sample_int) {
+ a = 0;
+
+ if (mc_interface_get_state() == MC_STATE_DETECTING) {
+ m_curr0_samples[m_sample_now] = (int16_t)mcpwm_detect_currents[mcpwm_get_comm_step() - 1];
+ m_curr1_samples[m_sample_now] = (int16_t)mcpwm_detect_currents_diff[mcpwm_get_comm_step() - 1];
+
+ m_ph1_samples[m_sample_now] = (int16_t)mcpwm_detect_voltages[0];
+ m_ph2_samples[m_sample_now] = (int16_t)mcpwm_detect_voltages[1];
+ m_ph3_samples[m_sample_now] = (int16_t)mcpwm_detect_voltages[2];
+ } else {
+ m_curr0_samples[m_sample_now] = ADC_curr_norm_value[0];
+ m_curr1_samples[m_sample_now] = ADC_curr_norm_value[1];
+
+ m_ph1_samples[m_sample_now] = ADC_V_L1 - mcpwm_vzero;
+ m_ph2_samples[m_sample_now] = ADC_V_L2 - mcpwm_vzero;
+ m_ph3_samples[m_sample_now] = ADC_V_L3 - mcpwm_vzero;
+ }
+
+ m_vzero_samples[m_sample_now] = mcpwm_vzero;
+
+ m_curr_fir_samples[m_sample_now] = (int16_t)(mc_interface_get_tot_current() * 100.0);
+ m_f_sw_samples[m_sample_now] = (int16_t)(f_samp / 10.0);
+
+ m_status_samples[m_sample_now] = mcpwm_get_comm_step() | (mcpwm_read_hall_phase() << 3);
+
+ m_sample_now++;
+
+ if (m_sample_now == m_sample_len) {
+ m_sample_ready = 1;
+ m_sample_now = 0;
+ chSysLockFromISR();
+ chEvtSignalI(sample_send_tp, (eventmask_t) 1);
+ chSysUnlockFromISR();
+ }
+
+ m_last_adc_duration_sample = mcpwm_get_last_adc_isr_duration();
+ }
+ }
}
void mc_interface_adc_inj_int_handler(void) {
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
mcpwm_adc_inj_int_handler();
@@ -891,7 +1182,7 @@
* The configaration to update.
*/
static void update_override_limits(volatile mc_configuration *conf) {
- const float temp = NTC_TEMP(ADC_IND_TEMP_MOS1);
+ const float temp = NTC_TEMP(ADC_IND_TEMP_MOS2);
const float v_in = GET_INPUT_VOLTAGE();
// Temperature
@@ -944,16 +1235,53 @@
}
// Decrease fault iterations
- if (ignore_iterations > 0) {
- ignore_iterations--;
+ if (m_ignore_iterations > 0) {
+ m_ignore_iterations--;
} else {
if (!IS_DRV_FAULT()) {
- fault_now = FAULT_CODE_NONE;
+ m_fault_now = FAULT_CODE_NONE;
}
}
- update_override_limits(&conf);
+ update_override_limits(&m_conf);
chThdSleepMilliseconds(1);
}
}
+
+static THD_FUNCTION(sample_send_thread, arg) {
+ (void)arg;
+
+ chRegSetThreadName("SampleSender");
+
+ sample_send_tp = chThdGetSelfX();
+
+ for(;;) {
+ chEvtWaitAny((eventmask_t) 1);
+
+ for (int i = 0;i < m_sample_len;i++) {
+ uint8_t buffer[20];
+ int index = 0;
+
+ buffer[index++] = m_curr0_samples[i] >> 8;
+ buffer[index++] = m_curr0_samples[i];
+ buffer[index++] = m_curr1_samples[i] >> 8;
+ buffer[index++] = m_curr1_samples[i];
+ buffer[index++] = m_ph1_samples[i] >> 8;
+ buffer[index++] = m_ph1_samples[i];
+ buffer[index++] = m_ph2_samples[i] >> 8;
+ buffer[index++] = m_ph2_samples[i];
+ buffer[index++] = m_ph3_samples[i] >> 8;
+ buffer[index++] = m_ph3_samples[i];
+ buffer[index++] = m_vzero_samples[i] >> 8;
+ buffer[index++] = m_vzero_samples[i];
+ buffer[index++] = m_status_samples[i];
+ buffer[index++] = m_curr_fir_samples[i] >> 8;
+ buffer[index++] = m_curr_fir_samples[i];
+ buffer[index++] = m_f_sw_samples[i] >> 8;
+ buffer[index++] = m_f_sw_samples[i];
+
+ commands_send_samples(buffer, index);
+ }
+ }
+}
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/mc_interface.h Raptor2 Sources/bldc-firmware_2_80/mc_interface.h
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/mc_interface.h 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/mc_interface.h 2017-04-13 18:05:06.000000000 +0100
@@ -41,14 +41,20 @@
void mc_interface_set_duty(float dutyCycle);
void mc_interface_set_duty_noramp(float dutyCycle);
void mc_interface_set_pid_speed(float rpm);
+void mc_interface_set_pid_speed_with_cruise_status(float rpm, ppm_cruise cruise_status);
+ppm_cruise mc_interface_get_cruise_control_status(void);
+void mc_interface_set_cruise_control_status(ppm_cruise status); // 1 = active 0 = inactive
void mc_interface_set_pid_pos(float pos);
void mc_interface_set_current(float current);
void mc_interface_set_brake_current(float current);
+void mc_interface_set_servo(float servo_val, bool use_min_current);
+void mc_interface_set_brake_servo(float servo_val);
void mc_interface_brake_now(void);
void mc_interface_release_motor(void);
+float mc_interface_get_duty_cycle_for_watt_calculation(void);
float mc_interface_get_duty_cycle_set(void);
float mc_interface_get_duty_cycle_now(void);
-float mc_interface_get_switching_frequency_now(void);
+float mc_interface_get_sampling_frequency_now(void);
float mc_interface_get_rpm(void);
float mc_interface_get_amp_hours(bool reset);
float mc_interface_get_amp_hours_charged(bool reset);
@@ -65,7 +71,10 @@
float mc_interface_get_last_inj_adc_isr_duration(void);
float mc_interface_read_reset_avg_motor_current(void);
float mc_interface_read_reset_avg_input_current(void);
-float mc_interface_get_pos_set(void);
+float mc_interface_get_pid_pos_set(void);
+float mc_interface_get_pid_pos_now(void);
+float mc_interface_get_last_sample_adc_isr_duration(void);
+void mc_interface_sample_print_data(bool at_start, uint16_t len, uint8_t decimation);
// MC implementation functions
void mc_interface_fault_stop(mc_fault_code fault);
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/mcconf/mcconf_default.h Raptor2 Sources/bldc-firmware_2_80/mcconf/mcconf_default.h
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/mcconf/mcconf_default.h 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/mcconf/mcconf_default.h 2018-08-28 19:44:30.000000000 +0100
@@ -20,16 +20,16 @@
// Default settings
#ifndef MCCONF_DEFAULT_MOTOR_TYPE
-#define MCCONF_DEFAULT_MOTOR_TYPE MOTOR_TYPE_BLDC
+#define MCCONF_DEFAULT_MOTOR_TYPE MOTOR_TYPE_FOC
#endif
#ifndef MCCONF_PWM_MODE
-#define MCCONF_PWM_MODE PWM_MODE_SYNCHRONOUS // Default PWM mode
+#define MCCONF_PWM_MODE PWM_MODE_SYNCHRONOUS // Default PWM mode
#endif
#ifndef MCCONF_SENSOR_MODE
-#define MCCONF_SENSOR_MODE SENSOR_MODE_SENSORLESS // Sensor mode
+#define MCCONF_SENSOR_MODE SENSOR_MODE_HYBRID // Sensor mode
#endif
#ifndef MCCONF_COMM_MODE
-#define MCCONF_COMM_MODE COMM_MODE_INTEGRATE // The commutation mode to use
+#define MCCONF_COMM_MODE COMM_MODE_INTEGRATE // The commutation mode to use
#endif
// Limits
@@ -37,13 +37,13 @@
#define MCCONF_L_CURRENT_MAX 60.0 // Current limit in Amperes (Upper)
#endif
#ifndef MCCONF_L_CURRENT_MIN
-#define MCCONF_L_CURRENT_MIN -60.0 // Current limit in Amperes (Lower)
+#define MCCONF_L_CURRENT_MIN -50.0 // Current limit in Amperes (Lower)
#endif
#ifndef MCCONF_L_IN_CURRENT_MAX
-#define MCCONF_L_IN_CURRENT_MAX 60.0 // Input current limit in Amperes (Upper)
+#define MCCONF_L_IN_CURRENT_MAX 25.0 // Input current limit in Amperes (Upper)
#endif
#ifndef MCCONF_L_IN_CURRENT_MIN
-#define MCCONF_L_IN_CURRENT_MIN -20.0 // Input current limit in Amperes (Lower)
+#define MCCONF_L_IN_CURRENT_MIN -10.0 // Input current limit in Amperes (Lower)
#endif
#ifndef MCCONF_L_MAX_ABS_CURRENT
#define MCCONF_L_MAX_ABS_CURRENT 130.0 // The maximum absolute current above which a fault is generated
@@ -52,37 +52,37 @@
#define MCCONF_L_MIN_VOLTAGE 8.0 // Minimum input voltage
#endif
#ifndef MCCONF_L_MAX_VOLTAGE
-#define MCCONF_L_MAX_VOLTAGE 50.0 // Maximum input voltage
+#define MCCONF_L_MAX_VOLTAGE 57.0 // Maximum input voltage
#endif
#ifndef MCCONF_L_BATTERY_CUT_START
-#define MCCONF_L_BATTERY_CUT_START 10.0 // Start limiting the positive current at this voltage
+#define MCCONF_L_BATTERY_CUT_START 33.5 // Start limiting the positive current at this voltage
#endif
#ifndef MCCONF_L_BATTERY_CUT_END
-#define MCCONF_L_BATTERY_CUT_END 8.0 // Limit the positive current completely at this voltage
+#define MCCONF_L_BATTERY_CUT_END 29.0 // Limit the positive current completely at this voltage
#endif
#ifndef MCCONF_L_RPM_MAX
-#define MCCONF_L_RPM_MAX 100000.0 // The motor speed limit (Upper)
+#define MCCONF_L_RPM_MAX 60000.0 // The motor speed limit (Upper)
#endif
#ifndef MCCONF_L_RPM_MIN
-#define MCCONF_L_RPM_MIN -100000.0 // The motor speed limit (Lower)
+#define MCCONF_L_RPM_MIN -60000.0 // The motor speed limit (Lower)
#endif
#ifndef MCCONF_L_SLOW_ABS_OVERCURRENT
-#define MCCONF_L_SLOW_ABS_OVERCURRENT true // Use the filtered (and hence slower) current for the overcurrent fault detection
+#define MCCONF_L_SLOW_ABS_OVERCURRENT true // Use the filtered (and hence slower) current for the overcurrent fault detection
#endif
#ifndef MCCONF_L_MIN_DUTY
-#define MCCONF_L_MIN_DUTY 0.005 // Minimum duty cycle
+#define MCCONF_L_MIN_DUTY 0.005 // Minimum duty cycle
#endif
#ifndef MCCONF_L_MAX_DUTY
-#define MCCONF_L_MAX_DUTY 0.95 // Maximum duty cycle
+#define MCCONF_L_MAX_DUTY 0.95 // Maximum duty cycle
#endif
#ifndef MCCONF_L_RPM_LIMIT_NEG_TORQUE
-#define MCCONF_L_RPM_LIMIT_NEG_TORQUE true // Use negative torque to limit the RPM
+#define MCCONF_L_RPM_LIMIT_NEG_TORQUE false // Use negative torque to limit the RPM
#endif
#ifndef MCCONF_L_CURR_MAX_RPM_FBRAKE
-#define MCCONF_L_CURR_MAX_RPM_FBRAKE 300 // Maximum electrical RPM to use full brake at
+#define MCCONF_L_CURR_MAX_RPM_FBRAKE 300 // Maximum electrical RPM to use full brake at
#endif
#ifndef MCCONF_L_CURR_MAX_RPM_FBRAKE_CC
-#define MCCONF_L_CURR_MAX_RPM_FBRAKE_CC 1500 // Maximum electrical RPM to use full brake at with current control
+#define MCCONF_L_CURR_MAX_RPM_FBRAKE_CC 1500 // Maximum electrical RPM to use full brake at with current control
#endif
#ifndef MCCONF_L_LIM_TEMP_FET_START
#define MCCONF_L_LIM_TEMP_FET_START 80.0 // MOSFET temperature where current limiting should begin
@@ -91,7 +91,7 @@
#define MCCONF_L_LIM_TEMP_FET_END 100.0 // MOSFET temperature where everything should be shut off
#endif
#ifndef MCCONF_L_LIM_TEMP_MOTOR_START
-#define MCCONF_L_LIM_TEMP_MOTOR_START 80.0 // MOTOR temperature where current limiting should begin
+#define MCCONF_L_LIM_TEMP_MOTOR_START 80.0 // MOTOR temperature where current limiting should begin
#endif
#ifndef MCCONF_L_LIM_TEMP_MOTOR_END
#define MCCONF_L_LIM_TEMP_MOTOR_END 100.0 // MOTOR temperature where everything should be shut off
@@ -99,58 +99,64 @@
// Speed PID parameters
#ifndef MCCONF_S_PID_KP
-#define MCCONF_S_PID_KP 0.0001 // Proportional gain
+#define MCCONF_S_PID_KP 0.003 // Proportional gain
#endif
#ifndef MCCONF_S_PID_KI
-#define MCCONF_S_PID_KI 0.015 // Integral gain
+#define MCCONF_S_PID_KI 0.003 // Integral gain
#endif
#ifndef MCCONF_S_PID_KD
-#define MCCONF_S_PID_KD 0.0 // Derivative gain
+#define MCCONF_S_PID_KD 0.0 // Derivative gain
#endif
#ifndef MCCONF_S_PID_MIN_RPM
#define MCCONF_S_PID_MIN_RPM 900.0 // Minimum allowed RPM
#endif
+#ifndef MCCONF_S_PID_BREAKING_ENABLED
+#define MCCONF_S_PID_BREAKING_ENABLED true // Break at PID speed mode (cruiso control)
+#endif
// Position PID parameters
#ifndef MCCONF_P_PID_KP
-#define MCCONF_P_PID_KP 0.03 // Proportional gain
+#define MCCONF_P_PID_KP 0.03 // Proportional gain
#endif
#ifndef MCCONF_P_PID_KI
-#define MCCONF_P_PID_KI 0.0 // Integral gain
+#define MCCONF_P_PID_KI 0.0 // Integral gain
#endif
#ifndef MCCONF_P_PID_KD
-#define MCCONF_P_PID_KD 0.0004 // Derivative gain
+#define MCCONF_P_PID_KD 0.0004 // Derivative gain
+#endif
+#ifndef MCCONF_P_PID_ANG_DIV
+#define MCCONF_P_PID_ANG_DIV 1.0 // Divide angle by this value
#endif
// Current control parameters
#ifndef MCCONF_CC_GAIN
-#define MCCONF_CC_GAIN 0.0046 // Current controller error gain
+#define MCCONF_CC_GAIN 0.0046 // Current controller error gain
#endif
#ifndef MCCONF_CC_MIN_CURRENT
-#define MCCONF_CC_MIN_CURRENT 1.0 // Minimum allowed current
+#define MCCONF_CC_MIN_CURRENT 0.5 // Minimum allowed current
#endif
#ifndef MCCONF_CC_STARTUP_BOOST_DUTY
-#define MCCONF_CC_STARTUP_BOOST_DUTY 0.01 // The lowest duty cycle to use in current control mode (has to be > MCPWM_MIN_DUTY_CYCLE)
+#define MCCONF_CC_STARTUP_BOOST_DUTY 0.01 // The lowest duty cycle to use in current control mode (has to be > MCPWM_MIN_DUTY_CYCLE)
#endif
#ifndef MCCONF_CC_RAMP_STEP
-#define MCCONF_CC_RAMP_STEP 0.04 // Maximum duty cycle ramping step in CC mode
+#define MCCONF_CC_RAMP_STEP 0.04 // Maximum duty cycle ramping step in CC mode
#endif
// BLDC
#ifndef MCCONF_SL_MIN_RPM
-#define MCCONF_SL_MIN_RPM 150 // Auto-commutate below this RPM
+#define MCCONF_SL_MIN_RPM 150 // Auto-commutate below this RPM
#endif
#ifndef MCCONF_SL_MIN_ERPM_CYCLE_INT_LIMIT
#define MCCONF_SL_MIN_ERPM_CYCLE_INT_LIMIT 1100.0 // Minimum RPM to calculate the BEMF coupling from
#endif
#ifndef MCCONF_SL_CYCLE_INT_LIMIT
-#define MCCONF_SL_CYCLE_INT_LIMIT 62.0 // Flux integrator limit 0 ERPM
+#define MCCONF_SL_CYCLE_INT_LIMIT 100.0 // Flux integrator limit 0 ERPM
#endif
#ifndef MCCONF_SL_BEMF_COUPLING_K
#define MCCONF_SL_BEMF_COUPLING_K 600.0 // Input voltage to bemf coupling constant
#endif
#ifndef MCCONF_SL_PHASE_ADVANCE_AT_BR
-#define MCCONF_SL_PHASE_ADVANCE_AT_BR 0.8 // Flux integrator limit percentage at MCPWM_CYCLE_INT_START_RPM_BR ERPM
+#define MCCONF_SL_PHASE_ADVANCE_AT_BR 0.8 // Flux integrator limit percentage at MCPWM_CYCLE_INT_START_RPM_BR ERPM
#endif
#ifndef MCCONF_SL_CYCLE_INT_BR
#define MCCONF_SL_CYCLE_INT_BR 80000.0 // RPM border between the START and LOW interval
@@ -159,18 +165,47 @@
#define MCCONF_SL_MAX_FB_CURR_DIR_CHANGE 10.0 // Maximum current during full brake during which a direction change is allowed
#endif
+// BLDC hall sensor table
+#ifndef MCCONF_HALL_TAB_0
+#define MCCONF_HALL_TAB_0 -1
+#endif
+#ifndef MCCONF_HALL_TAB_1
+#define MCCONF_HALL_TAB_1 1
+#endif
+#ifndef MCCONF_HALL_TAB_2
+#define MCCONF_HALL_TAB_2 3
+#endif
+#ifndef MCCONF_HALL_TAB_3
+#define MCCONF_HALL_TAB_3 2
+#endif
+#ifndef MCCONF_HALL_TAB_4
+#define MCCONF_HALL_TAB_4 5
+#endif
+#ifndef MCCONF_HALL_TAB_5
+#define MCCONF_HALL_TAB_5 6
+#endif
+#ifndef MCCONF_HALL_TAB_6
+#define MCCONF_HALL_TAB_6 4
+#endif
+#ifndef MCCONF_HALL_TAB_7
+#define MCCONF_HALL_TAB_7 -1
+#endif
+#ifndef MCCONF_HALL_ERPM
+#define MCCONF_HALL_ERPM 5000.0 // ERPM above which sensorless commutation is used in hybrid mode
+#endif
+
// FOC
#ifndef MCCONF_FOC_CURRENT_KP
-#define MCCONF_FOC_CURRENT_KP 0.03
+#define MCCONF_FOC_CURRENT_KP 0.0215 //0.0255
#endif
#ifndef MCCONF_FOC_CURRENT_KI
-#define MCCONF_FOC_CURRENT_KI 50.0
+#define MCCONF_FOC_CURRENT_KI 47.80 //41.00
#endif
#ifndef MCCONF_FOC_F_SW
-#define MCCONF_FOC_F_SW 25000.0
+#define MCCONF_FOC_F_SW 20000.0
#endif
#ifndef MCCONF_FOC_DT_US
-#define MCCONF_FOC_DT_US 0.15 // Microseconds for dead time compensation
+#define MCCONF_FOC_DT_US 0.08 // Microseconds for dead time compensation
#endif
#ifndef MCCONF_FOC_ENCODER_INVERTED
#define MCCONF_FOC_ENCODER_INVERTED false
@@ -182,25 +217,25 @@
#define MCCONF_FOC_ENCODER_RATIO 7.0
#endif
#ifndef MCCONF_FOC_SENSOR_MODE
-#define MCCONF_FOC_SENSOR_MODE FOC_SENSOR_MODE_SENSORLESS
+#define MCCONF_FOC_SENSOR_MODE FOC_SENSOR_MODE_HALL
#endif
#ifndef MCCONF_FOC_PLL_KP
-#define MCCONF_FOC_PLL_KP 40.0
+#define MCCONF_FOC_PLL_KP 2000.0
#endif
#ifndef MCCONF_FOC_PLL_KI
-#define MCCONF_FOC_PLL_KI 40000.0
+#define MCCONF_FOC_PLL_KI 20000.0
#endif
#ifndef MCCONF_FOC_MOTOR_L
-#define MCCONF_FOC_MOTOR_L 0.000007
+#define MCCONF_FOC_MOTOR_L 0.0000215 //0.0000255
#endif
#ifndef MCCONF_FOC_MOTOR_R
-#define MCCONF_FOC_MOTOR_R 0.015
+#define MCCONF_FOC_MOTOR_R 0.04780 //0.04100
#endif
#ifndef MCCONF_FOC_MOTOR_FLUX_LINKAGE
-#define MCCONF_FOC_MOTOR_FLUX_LINKAGE 0.00245
+#define MCCONF_FOC_MOTOR_FLUX_LINKAGE 0.006278//0.004363
#endif
#ifndef MCCONF_FOC_OBSERVER_GAIN
-#define MCCONF_FOC_OBSERVER_GAIN 9e7 // Can be something like 600 / L
+#define MCCONF_FOC_OBSERVER_GAIN 4.651e7 //3.922e7 // Can be something like 600 / L
#endif
#ifndef MCCONF_FOC_DUTY_DOWNRAMP_KP
#define MCCONF_FOC_DUTY_DOWNRAMP_KP 10.0 // PI controller for duty control when decreasing the duty
@@ -209,48 +244,46 @@
#define MCCONF_FOC_DUTY_DOWNRAMP_KI 200.0 // PI controller for duty control when decreasing the duty
#endif
#ifndef MCCONF_FOC_OPENLOOP_RPM
-#define MCCONF_FOC_OPENLOOP_RPM 350.0 // Openloop RPM (sensorless low speed or when finding index pulse)
+#define MCCONF_FOC_OPENLOOP_RPM 1200.0 // Openloop RPM (sensorless low speed or when finding index pulse)
#endif
#ifndef MCCONF_FOC_SL_OPENLOOP_HYST
-#define MCCONF_FOC_SL_OPENLOOP_HYST 0.5 // Time below min RPM to activate openloop (s)
+#define MCCONF_FOC_SL_OPENLOOP_HYST 0.5 // Time below min RPM to activate openloop (s)
#endif
#ifndef MCCONF_FOC_SL_OPENLOOP_TIME
-#define MCCONF_FOC_SL_OPENLOOP_TIME 0.5 // Time to remain in openloop (s)
+#define MCCONF_FOC_SL_OPENLOOP_TIME 0.5 // Time to remain in openloop (s)
#endif
#ifndef MCCONF_FOC_SL_D_CURRENT_DUTY
-#define MCCONF_FOC_SL_D_CURRENT_DUTY 0.0 // Inject d-axis current below this duty cycle in sensorless more
+#define MCCONF_FOC_SL_D_CURRENT_DUTY 0.0 // Inject d-axis current below this duty cycle in sensorless more
#endif
#ifndef MCCONF_FOC_SL_D_CURRENT_FACTOR
-#define MCCONF_FOC_SL_D_CURRENT_FACTOR 0.0 // Maximum q-axis current factor
+#define MCCONF_FOC_SL_D_CURRENT_FACTOR 0.0 // Maximum q-axis current factor
#endif
-
-// Default hall sensor table
-#ifndef MCCONF_HALL_TAB_0
-#define MCCONF_HALL_TAB_0 -1
+#ifndef MCCONF_FOC_HALL_TAB_0
+#define MCCONF_FOC_HALL_TAB_0 255
#endif
-#ifndef MCCONF_HALL_TAB_1
-#define MCCONF_HALL_TAB_1 1
+#ifndef MCCONF_FOC_HALL_TAB_1
+#define MCCONF_FOC_HALL_TAB_1 255
#endif
-#ifndef MCCONF_HALL_TAB_2
-#define MCCONF_HALL_TAB_2 3
+#ifndef MCCONF_FOC_HALL_TAB_2
+#define MCCONF_FOC_HALL_TAB_2 255
#endif
-#ifndef MCCONF_HALL_TAB_3
-#define MCCONF_HALL_TAB_3 2
+#ifndef MCCONF_FOC_HALL_TAB_3
+#define MCCONF_FOC_HALL_TAB_3 255
#endif
-#ifndef MCCONF_HALL_TAB_4
-#define MCCONF_HALL_TAB_4 5
+#ifndef MCCONF_FOC_HALL_TAB_4
+#define MCCONF_FOC_HALL_TAB_4 255
#endif
-#ifndef MCCONF_HALL_TAB_5
-#define MCCONF_HALL_TAB_5 6
+#ifndef MCCONF_FOC_HALL_TAB_5
+#define MCCONF_FOC_HALL_TAB_5 255
#endif
-#ifndef MCCONF_HALL_TAB_6
-#define MCCONF_HALL_TAB_6 4
+#ifndef MCCONF_FOC_HALL_TAB_6
+#define MCCONF_FOC_HALL_TAB_6 255
#endif
-#ifndef MCCONF_HALL_TAB_7
-#define MCCONF_HALL_TAB_7 -1
+#ifndef MCCONF_FOC_HALL_TAB_7
+#define MCCONF_FOC_HALL_TAB_7 255
#endif
-#ifndef MCCONF_HALL_ERPM
-#define MCCONF_HALL_ERPM 2000.0 // ERPM above which sensorless commutation is used in hybrid mode
+#ifndef MCCONF_FOC_SL_ERPM
+#define MCCONF_FOC_SL_ERPM 5000.0 // ERPM above which only the observer is used
#endif
// Misc
@@ -258,16 +291,25 @@
#define MCCONF_M_FAULT_STOP_TIME 3000 // Ignore commands for this duration in msec when faults occur
#endif
#ifndef MCCONF_M_RAMP_STEP
-#define MCCONF_M_RAMP_STEP 0.02 // Duty cycle ramping step (1000 times/sec) at maximum duty cycle
+#define MCCONF_M_RAMP_STEP 0.02 // Duty cycle ramping step (1000 times/sec) at maximum duty cycle
#endif
#ifndef MCCONF_M_RAMP_STEP_RPM_LIM
#define MCCONF_M_RAMP_STEP_RPM_LIM 0.0005 // Ramping step when limiting the RPM
#endif
#ifndef MCCONF_M_CURRENT_BACKOFF_GAIN
-#define MCCONF_M_CURRENT_BACKOFF_GAIN 0.5 // The error gain of the current limiting algorithm
+#define MCCONF_M_CURRENT_BACKOFF_GAIN 0.5 // The error gain of the current limiting algorithm
#endif
#ifndef MCCONF_M_ENCODER_COUNTS
#define MCCONF_M_ENCODER_COUNTS 8192 // The number of encoder counts
#endif
+#ifndef MCCONF_M_SENSOR_PORT_MODE
+#define MCCONF_M_SENSOR_PORT_MODE SENSOR_PORT_MODE_HALL // The mode of the hall_encoder port
+#endif
+#ifndef MCCONF_USE_MAX_WATT_LIMIT
+#define MCCONF_USE_MAX_WATT_LIMIT false
+#endif
+#ifndef MCCONF_WATT_MAX
+#define MCCONF_WATT_MAX 1000.0 // Input current limit in Amperes (Upper)
+#endif
#endif /* MCCONF_DEFAULT_H_ */
Only in Raptor2 Sources/bldc-firmware_2_80/mcconf: mcconf_default.h~
Only in bldc-cc171422641a50c56452280575f2074f8a88aa45/mcconf: mcconf_foc_erwin.h
Only in bldc-cc171422641a50c56452280575f2074f8a88aa45/mcconf: mcconf_foc_scorpion.h
Only in bldc-cc171422641a50c56452280575f2074f8a88aa45/mcconf: mcconf_outrunner2.h
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/mcconf/mcconf_sten.h Raptor2 Sources/bldc-firmware_2_80/mcconf/mcconf_sten.h
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/mcconf/mcconf_sten.h 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/mcconf/mcconf_sten.h 2016-09-23 23:02:14.000000000 +0100
@@ -26,43 +26,54 @@
#define MCCONF_STEN_H_
/*
+ * HW: 4.10
+ */
+
+/*
* Parameters
*/
+#define MCCONF_DEFAULT_MOTOR_TYPE MOTOR_TYPE_BLDC
#define MCCONF_L_CURRENT_MAX 35.0 // Current limit in Amperes (Upper)
#define MCCONF_L_CURRENT_MIN -30.0 // Current limit in Amperes (Lower)
-#define MCCONF_L_MAX_ABS_CURRENT 100.0 // The maximum absolute current above which a fault is generated
+#define MCCONF_L_MAX_ABS_CURRENT 130.0 // The maximum absolute current above which a fault is generated
#define MCCONF_L_SLOW_ABS_OVERCURRENT 1 // Use the filtered (and hence slower) current for the overcurrent fault detection
-#define MCCONF_L_IN_CURRENT_MAX 25.0 // Input current limit in Amperes (Upper)
-#define MCCONF_L_IN_CURRENT_MIN -20.0 // Input current limit in Amperes (Lower)
+#define MCCONF_L_IN_CURRENT_MAX 25.0 // Input current limit in Amperes (Upper)
+#define MCCONF_L_IN_CURRENT_MIN -20.0 // Input current limit in Amperes (Lower)
#define MCCONF_L_RPM_MAX 80000.0 // The motor speed limit (Upper)
#define MCCONF_L_RPM_MIN -80000.0 // The motor speed limit (Lower)
-#define MCCONF_L_MIN_VOLTAGE 20.0 // Minimum input voltage
-#define MCCONF_L_MAX_VOLTAGE 50.0 // Maximum input voltage
-#define MCCONF_CC_STARTUP_BOOST_DUTY 0.02 // The lowest duty cycle to use in current control mode (has to be > MCPWM_MIN_DUTY_CYCLE)
+#define MCCONF_L_MIN_VOLTAGE 10.0 // Minimum input voltage
+#define MCCONF_L_MAX_VOLTAGE 57.0 // Maximum input voltage
+#define MCCONF_CC_STARTUP_BOOST_DUTY 0.03 // The lowest duty cycle to use in current control mode (has to be > MCPWM_MIN_DUTY_CYCLE)
#define MCCONF_L_RPM_LIMIT_NEG_TORQUE 0 // Use negative torque to limit the RPM
#define MCCONF_L_CURR_MAX_RPM_FBRAKE 1500 // Maximum electrical RPM to use full brake at
+#define MCCONF_L_BATTERY_CUT_START 41.0 // Start limiting the positive current at this voltage
+#define MCCONF_L_BATTERY_CUT_END 39.0 // Limit the positive current completely at this voltage
+
// Sensorless settings
#define MCCONF_SENSOR_MODE SENSOR_MODE_SENSORLESS // Sensor mode
#define MCCONF_SL_MIN_RPM 250 // Auto-commutate below this RPM
-#define MCCONF_SL_MIN_ERPM_CYCLE_INT_LIMIT 500.0 // Minimum RPM to calculate the BEMF coupling from
+#define MCCONF_SL_MIN_ERPM_CYCLE_INT_LIMIT 1100.0 // Minimum RPM to calculate the BEMF coupling from
#define MCCONF_SL_CYCLE_INT_LIMIT 80.0 // Flux integrator limit 0 ERPM
#define MCCONF_SL_PHASE_ADVANCE_AT_BR 0.8 // Flux integrator limit percentage at MCPWM_CYCLE_INT_START_RPM_BR ERPM
#define MCCONF_SL_BEMF_COUPLING_K 750.0 // Input voltage to bemf coupling constant
+// FOC settings
+#define MCCONF_FOC_CURRENT_KP 0.03
+#define MCCONF_FOC_CURRENT_KI 50.0
+#define MCCONF_FOC_F_SW 20000.0
+#define MCCONF_FOC_MOTOR_L 0.000007
+#define MCCONF_FOC_MOTOR_R 0.015
+#define MCCONF_FOC_MOTOR_FLUX_LINKAGE 0.00245
+#define MCCONF_FOC_OBSERVER_GAIN 9e7
+#define MCCONF_FOC_OPENLOOP_RPM 600.0
+#define MCCONF_FOC_SL_OPENLOOP_HYST 0.5
+#define MCCONF_FOC_SL_OPENLOOP_TIME 0.5
+
// Speed PID parameters
#define MCCONF_S_PID_KP 0.0001 // Proportional gain
#define MCCONF_S_PID_KI 0.002 // Integral gain
#define MCCONF_S_PID_KD 0.0 // Derivative gain
-#define MCCONF_S_PID_MIN_RPM 1200.0 // Minimum allowed RPM
-
-// Position PID parameters
-#define MCCONF_P_PID_KP 0.0001 // Proportional gain
-#define MCCONF_P_PID_KI 0.002 // Integral gain
-#define MCCONF_P_PID_KD 0.0 // Derivative gain
-
-// Current control parameters
-#define MCCONF_CC_GAIN 0.0046 // Current controller error gain
-#define MCCONF_CC_MIN_CURRENT 0.05 // Minimum allowed current
-
+#define MCCONF_S_PID_MIN_RPM 1200.0 // Minimum allowed RPM
+#define MCCONF_S_PID_BREAKING_ENABLED 1 // enable braking at pid speed mode (cruise control)
#endif /* MCCONF_STEN_H_ */
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/mcpwm.c Raptor2 Sources/bldc-firmware_2_80/mcpwm.c
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/mcpwm.c 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/mcpwm.c 2017-04-13 18:06:02.000000000 +0100
@@ -56,6 +56,7 @@
static volatile float dutycycle_now;
static volatile float rpm_now;
static volatile float speed_pid_set_rpm;
+static volatile ppm_cruise speed_pid_cruise_control_type;
static volatile float pos_pid_set_pos;
static volatile float current_set;
static volatile int tachometer;
@@ -90,6 +91,8 @@
static volatile float last_pwm_cycles_sums[6];
static volatile bool dccal_done;
static volatile bool sensorless_now;
+static volatile float sensor_hyst_min;
+static volatile float sensor_hyst_max;
static volatile int hall_detect_table[8][7];
// KV FIR filter
@@ -174,6 +177,7 @@
dutycycle_set = 0.0;
dutycycle_now = 0.0;
speed_pid_set_rpm = 0.0;
+ speed_pid_cruise_control_type = CRUISE_CONTROL_MOTOR_SETTINGS;
pos_pid_set_pos = 0.0;
current_set = 0.0;
tachometer = 0;
@@ -494,7 +498,7 @@
* The commutations corresponding to the hall sensor states in the forward direction-
*/
void mcpwm_init_hall_table(int8_t *table) {
- const int fwd_to_rev[7] = {-1,4,3,2,1,6,5};
+ const int fwd_to_rev[7] = {-1,1,6,5,4,3,2};
for (int i = 0;i < 8;i++) {
hall_to_phase_table[8 + i] = table[i];
@@ -505,14 +509,16 @@
continue;
}
- ind_now += 2;
- if (ind_now > 6) {
- ind_now -= 6;
- }
- ind_now = fwd_to_rev[ind_now];
-
- hall_to_phase_table[i] = ind_now;
+ hall_to_phase_table[i] = fwd_to_rev[ind_now];
+ }
+
+ // init the sensor hyst
+ float sensor_hyst = conf->hall_sl_erpm * 0.10;
+ if(sensor_hyst > 500.0){
+ sensor_hyst = 500.0;
}
+ sensor_hyst_min = conf->hall_sl_erpm - sensor_hyst;
+ sensor_hyst_max = conf->hall_sl_erpm + sensor_hyst;
}
static void do_dc_cal(void) {
@@ -571,8 +577,31 @@
* The electrical RPM goal value to use.
*/
void mcpwm_set_pid_speed(float rpm) {
+ if (fabsf(rpm) <conf->s_pid_min_erpm) {
+ mcpwm_set_duty(0.0);
+ return;
+ }
control_mode = CONTROL_MODE_SPEED;
speed_pid_set_rpm = rpm;
+ speed_pid_cruise_control_type = CRUISE_CONTROL_MOTOR_SETTINGS;
+
+ if (state != MC_STATE_RUNNING) {
+ set_duty_cycle_hl(conf->l_min_duty);
+ }
+}
+
+void mcpwm_set_pid_speed_with_cruise_status(float rpm, ppm_cruise cruise_status) {
+ if (fabsf(rpm) <conf->s_pid_min_erpm) {
+ mcpwm_set_duty(0.0);
+ return;
+ }
+ control_mode = CONTROL_MODE_SPEED;
+ speed_pid_set_rpm = rpm;
+ speed_pid_cruise_control_type = cruise_status;
+
+ if (state != MC_STATE_RUNNING) {
+ set_duty_cycle_hl(conf->l_min_duty);
+ }
}
/**
@@ -1064,23 +1093,23 @@
}
static void run_pid_control_speed(void) {
- static float i_term = 0;
- static float prev_error = 0;
+ static float i_term = 0.0;
+ static float prev_error = 0.0;
float p_term;
float d_term;
// PID is off. Return.
if (control_mode != CONTROL_MODE_SPEED) {
- i_term = dutycycle_now;
- prev_error = 0;
+ i_term = 0.0;
+ prev_error = 0.0;
return;
}
- // Too low RPM set. Stop and return.
- if (fabsf(speed_pid_set_rpm) < conf->s_pid_min_erpm) {
- i_term = dutycycle_now;
+ // Too low RPM set. Reset state and return.
+ if (fabsf(speed_pid_set_rpm) <conf->s_pid_min_erpm) {
+ i_term = 0.0;
prev_error = 0;
- mcpwm_set_duty(0.0);
+ current_set = 0.0;
return;
}
@@ -1104,21 +1133,18 @@
// Calculate output
float output = p_term + i_term + d_term;
- // Make sure that at least minimum output is used
- if (fabsf(output) < conf->l_min_duty) {
- output = SIGN(output) * conf->l_min_duty;
- }
-
- // Do not output in reverse direction to oppose too high rpm
- if (speed_pid_set_rpm > 0.0 && output < 0.0) {
- output = conf->l_min_duty;
- i_term = 0.0;
- } else if (speed_pid_set_rpm < 0.0 && output > 0.0) {
- output = -conf->l_min_duty;
- i_term = 0.0;
+ if ((speed_pid_cruise_control_type == CRUISE_CONTROL_MOTOR_SETTINGS && conf->s_pid_breaking_enabled) || speed_pid_cruise_control_type == CRUISE_CONTROL_BRAKING_ENABLED) {
+ utils_truncate_number(&output, -1.0, 1.0);
+ } else {
+ if(speed_pid_set_rpm < 0.0){
+ utils_truncate_number(&output, -1.0, 0.0);
+ }else{
+ utils_truncate_number(&output, 0.0, 1.0);
+ }
}
-
- set_duty_cycle_hl(output);
+
+ current_set = output * conf->lo_current_max;
+
}
static void run_pid_control_pos(float dt) {
@@ -1784,7 +1810,7 @@
float dutycycle_now_tmp = dutycycle_now;
- if (control_mode == CONTROL_MODE_CURRENT || control_mode == CONTROL_MODE_POS) {
+ if (control_mode == CONTROL_MODE_CURRENT || control_mode == CONTROL_MODE_POS || control_mode == CONTROL_MODE_SPEED) {
// Compute error
const float error = current_set - (direction ? current_nofilter : -current_nofilter);
float step = error * conf->cc_gain * voltage_scale;
@@ -1922,7 +1948,7 @@
mc_interface_mc_timer_isr();
- if (ENCODER_ENABLE) {
+ if (encoder_is_configured()) {
run_pid_control_pos(1.0 / switching_frequency_now);
}
@@ -2051,7 +2077,7 @@
int mcpwm_get_hall_detect_result(int8_t *table) {
if (WS2811_ENABLE) {
return -2;
- } else if (ENCODER_ENABLE) {
+ } else if (conf->m_sensor_port_mode != SENSOR_PORT_MODE_HALL) {
return -3;
}
@@ -2336,6 +2362,7 @@
}
}
+/*
static void update_sensor_mode(void) {
if (conf->sensor_mode == SENSOR_MODE_SENSORLESS ||
(conf->sensor_mode == SENSOR_MODE_HYBRID &&
@@ -2345,6 +2372,26 @@
sensorless_now = false;
}
}
+*/
+
+static void update_sensor_mode(void) {
+ if (conf->sensor_mode == SENSOR_MODE_SENSORLESS ) {
+ sensorless_now = true;
+ } else if (conf->sensor_mode == SENSOR_MODE_HYBRID) {
+ // Hysteresis 5 % of total speed
+ if (sensorless_now) {
+ if (fabsf(rpm_now) < sensor_hyst_min) {
+ sensorless_now = false;
+ }
+ } else {
+ if (fabsf(rpm_now) > sensor_hyst_max) {
+ sensorless_now = true;
+ }
+ }
+ }else{
+ sensorless_now = false;
+ }
+}
static void commutate(int steps) {
last_pwm_cycles_sum = pwm_cycles_sum;
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/mcpwm.h Raptor2 Sources/bldc-firmware_2_80/mcpwm.h
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/mcpwm.h 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/mcpwm.h 2017-04-13 18:06:14.000000000 +0100
@@ -35,6 +35,7 @@
void mcpwm_set_duty(float dutyCycle);
void mcpwm_set_duty_noramp(float dutyCycle);
void mcpwm_set_pid_speed(float rpm);
+void mcpwm_set_pid_speed_with_cruise_status(float rpm, ppm_cruise cruise_status);
void mcpwm_set_pid_pos(float pos);
void mcpwm_set_current(float current);
void mcpwm_set_brake_current(float current);
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/mcpwm_foc.c Raptor2 Sources/bldc-firmware_2_80/mcpwm_foc.c
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/mcpwm_foc.c 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/mcpwm_foc.c 2017-06-05 00:12:34.000000000 +0100
@@ -51,6 +51,7 @@
float i_abs;
float i_abs_filter;
float i_bus;
+ float i_bus_filter;
float v_bus;
float v_alpha;
float v_beta;
@@ -89,8 +90,9 @@
static volatile float m_iq_set;
static volatile bool m_dccal_done;
static volatile bool m_output_on;
-static volatile float m_pos_pid_set_pos;
+static volatile float m_pos_pid_set;
static volatile float m_speed_pid_set_rpm;
+static volatile ppm_cruise m_speed_pid_cruise_control_type;
static volatile float m_phase_now_observer;
static volatile float m_phase_now_observer_override;
static volatile bool m_phase_observer_override;
@@ -101,9 +103,10 @@
static volatile float m_pll_phase;
static volatile float m_pll_speed;
static volatile mc_sample_t m_samples;
-static volatile int m_tachometer;
-static volatile int m_tachometer_abs;
+static volatile float m_tachometer;
+static volatile float m_tachometer_abs;
static volatile float last_inj_adc_isr_duration;
+static volatile float m_pos_pid_now;
// Private functions
static void do_dc_cal(void);
@@ -114,10 +117,13 @@
static void control_current(volatile motor_state_t *state_m, float dt);
static void svm(float alpha, float beta, uint32_t PWMHalfPeriod,
uint32_t* tAout, uint32_t* tBout, uint32_t* tCout);
-static void run_pid_control_pos(float dt);
+static void run_pid_control_pos(float angle_now, float angle_set, float dt);
static void run_pid_control_speed(float dt);
static void stop_pwm_hw(void);
static void start_pwm_hw(void);
+static int read_hall(void);
+static float correct_encoder(float obs_angle, float enc_angle, float speed);
+static float correct_hall(float angle, float speed, float dt);
// Threads
static THD_WORKING_AREA(timer_thread_wa, 2048);
@@ -199,8 +205,9 @@
m_id_set = 0.0;
m_iq_set = 0.0;
m_output_on = false;
- m_pos_pid_set_pos = 0.0;
+ m_pos_pid_set = 0.0;
m_speed_pid_set_rpm = 0.0;
+ m_speed_pid_cruise_control_type = CRUISE_CONTROL_MOTOR_SETTINGS;
m_phase_now_observer = 0.0;
m_phase_now_observer_override = 0.0;
m_phase_observer_override = false;
@@ -210,9 +217,10 @@
m_observer_x2 = 0.0;
m_pll_phase = 0.0;
m_pll_speed = 0.0;
- m_tachometer = 0;
- m_tachometer_abs = 0;
+ m_tachometer = 0.0;
+ m_tachometer_abs = 0.0;
last_inj_adc_isr_duration = 0;
+ m_pos_pid_now = 0.0;
memset((void*)&m_motor_state, 0, sizeof(motor_state_t));
memset((void*)&m_samples, 0, sizeof(mc_sample_t));
@@ -512,10 +520,30 @@
* The electrical RPM goal value to use.
*/
void mcpwm_foc_set_pid_speed(float rpm) {
+ // Too low RPM set. Reset state and return.
+ if (fabsf(rpm) < m_conf->s_pid_min_erpm) {
+ mcpwm_foc_set_duty(0.0);
+ return;
+ }
+ m_control_mode = CONTROL_MODE_SPEED;
+ m_speed_pid_set_rpm = rpm;
+ m_speed_pid_cruise_control_type = CRUISE_CONTROL_MOTOR_SETTINGS;
+
+ if (m_state != MC_STATE_RUNNING) {
+ m_state = MC_STATE_RUNNING;
+ }
+}
+
+void mcpwm_foc_set_pid_speed_with_cruise_status(float rpm, ppm_cruise cruise_status) {
+ if (fabsf(rpm) < m_conf->s_pid_min_erpm) {
+ mcpwm_foc_set_duty(0.0);
+ return;
+ }
m_control_mode = CONTROL_MODE_SPEED;
m_speed_pid_set_rpm = rpm;
+ m_speed_pid_cruise_control_type = cruise_status;
- if (m_state != MC_STATE_RUNNING && fabsf(rpm) > m_conf->s_pid_min_erpm) {
+ if (m_state != MC_STATE_RUNNING) {
m_state = MC_STATE_RUNNING;
}
}
@@ -529,7 +557,7 @@
*/
void mcpwm_foc_set_pid_pos(float pos) {
m_control_mode = CONTROL_MODE_POS;
- m_pos_pid_set_pos = pos;
+ m_pos_pid_set = pos;
if (m_state != MC_STATE_RUNNING) {
m_state = MC_STATE_RUNNING;
@@ -577,7 +605,7 @@
return;
}
- utils_truncate_number(&current, m_conf->lo_current_min, m_conf->lo_current_max);
+ utils_truncate_number(&current, m_conf->lo_current_min, fabsf(m_conf->lo_current_min));
m_control_mode = CONTROL_MODE_CURRENT_BRAKE;
m_iq_set = current;
@@ -595,6 +623,14 @@
return m_motor_state.duty_now;
}
+float mcpwm_foc_get_pid_pos_set(void) {
+ return m_pos_pid_set;
+}
+
+float mcpwm_foc_get_pid_pos_now(void) {
+ return m_pos_pid_now;
+}
+
/**
* Get the current switching frequency.
*
@@ -702,7 +738,7 @@
* The filtered input current.
*/
float mcpwm_foc_get_tot_current_in_filtered(void) {
- return m_motor_state.i_bus; // TODO: Calculate filtered current?
+ return m_motor_state.i_bus_filter; // TODO: Calculate filtered current?
}
/**
@@ -717,10 +753,10 @@
* be this number divided by (3 * MOTOR_POLE_NUMBER).
*/
int mcpwm_foc_get_tachometer_value(bool reset) {
- int val = m_tachometer;
+ int val = (int) m_tachometer;
if (reset) {
- m_tachometer = 0;
+ m_tachometer = 0.0;
}
return val;
@@ -737,10 +773,10 @@
* be this number divided by (3 * MOTOR_POLE_NUMBER).
*/
int mcpwm_foc_get_tachometer_abs_value(bool reset) {
- int val = m_tachometer_abs;
+ int val = (int) m_tachometer_abs;
if (reset) {
- m_tachometer_abs = 0;
+ m_tachometer_abs = 0.0;
}
return val;
@@ -806,6 +842,8 @@
* The detected direction.
*/
void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *ratio, bool *inverted) {
+ mc_interface_lock();
+
m_phase_override = true;
m_id_set = current;
m_iq_set = 0.0;
@@ -815,7 +853,7 @@
// Disable timeout
systime_t tout = timeout_get_timeout_msec();
float tout_c = timeout_get_brake_current();
- timeout_configure(60000, 0.0);
+ timeout_configure(600000, 0.0);
// Save configuration
float offset_old = m_conf->foc_encoder_offset;
@@ -858,34 +896,68 @@
// Inverted and ratio
chThdSleepMilliseconds(1000);
+ const int it_rat = 20;
float s_sum = 0.0;
float c_sum = 0.0;
- for (int i = 0; i < 10; i++) {
+ float first = m_phase_now_encoder;
+
+ for (int i = 0; i < it_rat; i++) {
float phase_old = m_phase_now_encoder;
float phase_ovr_tmp = m_phase_now_override;
- for (float i = phase_ovr_tmp; i < phase_ovr_tmp + 0.8 * M_PI;
+ for (float i = phase_ovr_tmp; i < phase_ovr_tmp + (2.0 / 3.0) * M_PI;
i += (2.0 * M_PI) / 500.0) {
m_phase_now_override = i;
- chThdSleepMilliseconds(2);
+ chThdSleepMilliseconds(1);
+ }
+ utils_norm_angle_rad((float*)&m_phase_now_override);
+ chThdSleepMilliseconds(300);
+ float diff = utils_angle_difference_rad(m_phase_now_encoder, phase_old);
+
+ float s, c;
+ sincosf(diff, &s, &c);
+ s_sum += s;
+ c_sum += c;
+
+ if (print) {
+ commands_printf("%.2f", (double)(diff * 180.0 / M_PI));
+ }
+
+ if (i > 3 && fabsf(utils_angle_difference_rad(m_phase_now_encoder, first)) < fabsf(diff / 2.0)) {
+ break;
+ }
+ }
+
+ first = m_phase_now_encoder;
+
+ for (int i = 0; i < it_rat; i++) {
+ float phase_old = m_phase_now_encoder;
+ float phase_ovr_tmp = m_phase_now_override;
+ for (float i = phase_ovr_tmp; i > phase_ovr_tmp - (2.0 / 3.0) * M_PI;
+ i -= (2.0 * M_PI) / 500.0) {
+ m_phase_now_override = i;
+ chThdSleepMilliseconds(1);
}
- chThdSleepMilliseconds(1000);
- float diff = utils_angle_difference(
- m_phase_now_encoder * (180.0 / M_PI),
- phase_old * (180.0 / M_PI));
+ utils_norm_angle_rad((float*)&m_phase_now_override);
+ chThdSleepMilliseconds(300);
+ float diff = utils_angle_difference_rad(phase_old, m_phase_now_encoder);
float s, c;
- sincosf(diff * M_PI / 180.0, &s, &c);
+ sincosf(diff, &s, &c);
s_sum += s;
c_sum += c;
if (print) {
- commands_printf("%.2f", (double)diff);
+ commands_printf("%.2f", (double)(diff * 180.0 / M_PI));
+ }
+
+ if (i > 3 && fabsf(utils_angle_difference_rad(m_phase_now_encoder, first)) < fabsf(diff / 2.0)) {
+ break;
}
}
float diff = atan2f(s_sum, c_sum) * 180.0 / M_PI;
*inverted = diff < 0.0;
- *ratio = roundf((0.8 * 180.0) /
+ *ratio = roundf(((2.0 / 3.0) * 180.0) /
fabsf(diff));
m_conf->foc_encoder_inverted = *inverted;
@@ -906,33 +978,37 @@
commands_printf("Enc: %.2f", (double)encoder_read_deg());
}
+ const int it_ofs = m_conf->foc_encoder_ratio * 3.0;
s_sum = 0.0;
c_sum = 0.0;
- for (int i = 0;i < 5;i++) {
- m_phase_now_override = (float)i * M_PI / 5.0;
- chThdSleepMilliseconds(1000);
+ for (int i = 0;i < it_ofs;i++) {
+ m_phase_now_override = ((float)i * 2.0 * M_PI * m_conf->foc_encoder_ratio) / ((float)it_ofs);
+ chThdSleepMilliseconds(500);
+
+ float diff = utils_angle_difference_rad(m_phase_now_encoder, m_phase_now_override);
float s, c;
- sincosf(m_phase_now_encoder - m_phase_now_override, &s, &c);
+ sincosf(diff, &s, &c);
s_sum += s;
c_sum += c;
if (print) {
- commands_printf("%.2f", (double)((m_phase_now_encoder - m_phase_now_override) * 180.0 / M_PI));
+ commands_printf("%.2f", (double)(diff * 180.0 / M_PI));
}
}
- for (int i = 3;i >= -1;i--) {
- m_phase_now_override = (float)i * M_PI / 5.0;
- chThdSleepMilliseconds(1000);
+ for (int i = it_ofs;i > 0;i--) {
+ m_phase_now_override = ((float)i * 2.0 * M_PI * m_conf->foc_encoder_ratio) / ((float)it_ofs);
+ chThdSleepMilliseconds(500);
+ float diff = utils_angle_difference_rad(m_phase_now_encoder, m_phase_now_override);
float s, c;
- sincosf(m_phase_now_encoder - m_phase_now_override, &s, &c);
+ sincosf(diff, &s, &c);
s_sum += s;
c_sum += c;
if (print) {
- commands_printf("%.2f", (double)((m_phase_now_encoder - m_phase_now_override) * 180.0 / M_PI));
+ commands_printf("%.2f", (double)(diff * 180.0 / M_PI));
}
}
@@ -962,6 +1038,8 @@
// Enable timeout
timeout_configure(tout, tout_c);
+
+ mc_interface_unlock();
}
/**
@@ -1117,7 +1195,7 @@
float res_tmp = 0.0;
float i_last = 0.0;
- for (float i = 2.0;i < 35.0;i *= 1.5) {
+ for (float i = 2.0;i < (m_conf->l_current_max / 2.0);i *= 1.5) {
res_tmp = mcpwm_foc_measure_resistance(i, 20);
if (i > (0.5 / res_tmp)) {
@@ -1127,7 +1205,7 @@
}
if (i_last < 0.01) {
- i_last = 35.0;
+ i_last = (m_conf->l_current_max / 2.0);
}
*res = mcpwm_foc_measure_resistance(i_last, 500);
@@ -1137,7 +1215,7 @@
TIMER_UPDATE_SAMP_TOP(top - 2, 5, top);
float duty_last = 0.0;
- for (float i = 0.02;i < 0.9;i *= 1.5) {
+ for (float i = 0.02;i < 0.5;i *= 1.5) {
float i_tmp;
mcpwm_foc_measure_inductance(i, 20, &i_tmp);
@@ -1159,6 +1237,101 @@
return true;
}
+/**
+ * Run the motor in open loop and figure out at which angles the hall sensors are.
+ *
+ * @param current
+ * Current to use.
+ *
+ * @param hall_table
+ * Table to store the result to.
+ *
+ * @return
+ * true: Success
+ * false: Something went wrong
+ */
+bool mcpwm_foc_hall_detect(float current, uint8_t *hall_table) {
+ mc_interface_lock();
+
+ m_phase_override = true;
+ m_id_set = current;
+ m_iq_set = 0.0;
+ m_control_mode = CONTROL_MODE_CURRENT;
+ m_state = MC_STATE_RUNNING;
+
+ // Disable timeout
+ systime_t tout = timeout_get_timeout_msec();
+ float tout_c = timeout_get_brake_current();
+ timeout_configure(60000, 0.0);
+
+ // Lock the motor
+ m_phase_now_override = 0;
+ chThdSleepMilliseconds(1000);
+
+ float sin_hall[8];
+ float cos_hall[8];
+ int hall_iterations[8];
+ memset(sin_hall, 0, sizeof(sin_hall));
+ memset(cos_hall, 0, sizeof(cos_hall));
+ memset(hall_iterations, 0, sizeof(hall_iterations));
+
+ // Forwards
+ for (int i = 0;i < 3;i++) {
+ for (int i = 0;i < 360;i++) {
+ m_phase_now_override = (float)i * M_PI / 180.0;
+ chThdSleepMilliseconds(5);
+
+ int hall = read_hall();
+ float s, c;
+ sincosf(m_phase_now_override, &s, &c);
+ sin_hall[hall] += s;
+ cos_hall[hall] += c;
+ hall_iterations[hall]++;
+ }
+ }
+
+ // Reverse
+ for (int i = 0;i < 3;i++) {
+ for (int i = 360;i >= 0;i--) {
+ m_phase_now_override = (float)i * M_PI / 180.0;
+ chThdSleepMilliseconds(5);
+
+ int hall = read_hall();
+ float s, c;
+ sincosf(m_phase_now_override, &s, &c);
+ sin_hall[hall] += s;
+ cos_hall[hall] += c;
+ hall_iterations[hall]++;
+ }
+ }
+
+ m_id_set = 0.0;
+ m_iq_set = 0.0;
+ m_phase_override = false;
+ m_control_mode = CONTROL_MODE_NONE;
+ m_state = MC_STATE_OFF;
+ stop_pwm_hw();
+
+ // Enable timeout
+ timeout_configure(tout, tout_c);
+
+ int fails = 0;
+ for(int i = 0;i < 8;i++) {
+ if (hall_iterations[i] > 30) {
+ float ang = atan2f(sin_hall[i], cos_hall[i]) * 180.0 / M_PI;
+ utils_norm_angle(&ang);
+ hall_table[i] = (uint8_t)(ang * 200.0 / 360.0);
+ } else {
+ hall_table[i] = 255;
+ fails++;
+ }
+ }
+
+ mc_interface_unlock();
+
+ return fails == 2;
+}
+
void mcpwm_foc_print_state(void) {
commands_printf("Mod d: %.2f", (double)m_motor_state.mod_d);
commands_printf("Mod q: %.2f", (double)m_motor_state.mod_q);
@@ -1259,16 +1432,18 @@
m_motor_state.v_bus = GET_INPUT_VOLTAGE();
-#if ENCODER_ENABLE
- float phase_tmp = encoder_read_deg();
- if (m_conf->foc_encoder_inverted) {
- phase_tmp = 360.0 - phase_tmp;
- }
- phase_tmp *= m_conf->foc_encoder_ratio;
- phase_tmp -= m_conf->foc_encoder_offset;
- utils_norm_angle((float*)&phase_tmp);
- m_phase_now_encoder = phase_tmp * (M_PI / 180.0);
-#endif
+ float enc_ang = 0;
+ if (encoder_is_configured()) {
+ enc_ang = encoder_read_deg();
+ float phase_tmp = enc_ang;
+ if (m_conf->foc_encoder_inverted) {
+ phase_tmp = 360.0 - phase_tmp;
+ }
+ phase_tmp *= m_conf->foc_encoder_ratio;
+ phase_tmp -= m_conf->foc_encoder_offset;
+ utils_norm_angle((float*)&phase_tmp);
+ m_phase_now_encoder = phase_tmp * (M_PI / 180.0);
+ }
static float phase_before = 0.0;
const float phase_diff = utils_angle_difference_rad(m_motor_state.phase, phase_before);
@@ -1299,12 +1474,19 @@
// observer has lost tracking. Use duty cycle control with the lowest duty cycle
// to get as smooth braking as possible.
if (m_control_mode == CONTROL_MODE_CURRENT_BRAKE
- && m_conf->foc_sensor_mode == FOC_SENSOR_MODE_SENSORLESS
+// && (m_conf->foc_sensor_mode != FOC_SENSOR_MODE_ENCODER) // Don't use this with encoderss
&& fabsf(duty_filtered) < 0.03) {
control_duty = true;
duty_set = 0.0;
}
+ // Brake when set ERPM is below min ERPM
+ if (m_control_mode == CONTROL_MODE_SPEED &&
+ fabsf(m_speed_pid_set_rpm) < m_conf->s_pid_min_erpm) {
+ control_duty = true;
+ duty_set = 0.0;
+ }
+
if (control_duty) {
// Duty cycle control
static float duty_i_term = 0.0;
@@ -1361,7 +1543,7 @@
switch (m_conf->foc_sensor_mode) {
case FOC_SENSOR_MODE_ENCODER:
if (encoder_index_found()) {
- m_motor_state.phase = m_phase_now_encoder;
+ m_motor_state.phase = correct_encoder(m_phase_now_observer, m_phase_now_encoder, m_pll_speed);
} else {
// Rotate the motor in open loop if the index isn't found.
m_motor_state.phase = m_phase_now_encoder_no_index;
@@ -1371,6 +1553,14 @@
id_set_tmp = 0.0;
}
break;
+ case FOC_SENSOR_MODE_HALL:
+ m_phase_now_observer = correct_hall(m_phase_now_observer, m_pll_speed, dt);
+ m_motor_state.phase = m_phase_now_observer;
+
+ if (!m_phase_override) {
+ id_set_tmp = 0.0;
+ }
+ break;
case FOC_SENSOR_MODE_SENSORLESS:
if (m_phase_observer_override) {
m_motor_state.phase = m_phase_now_observer_override;
@@ -1400,7 +1590,12 @@
// Apply current limits
const float mod_q = m_motor_state.mod_q;
utils_truncate_number(&iq_set_tmp, m_conf->lo_current_min, m_conf->lo_current_max);
- utils_saturate_vector_2d(&id_set_tmp, &iq_set_tmp, m_conf->lo_current_max);
+ if (iq_set_tmp < 0.0) {
+ utils_saturate_vector_2d(&id_set_tmp, &iq_set_tmp, m_conf->lo_current_min);
+ } else {
+
+ utils_saturate_vector_2d(&id_set_tmp, &iq_set_tmp, m_conf->lo_current_max);
+ }
if (mod_q > 0.001) {
utils_truncate_number(&iq_set_tmp, m_conf->lo_in_current_min / mod_q, m_conf->lo_in_current_max / mod_q);
} else if (mod_q < -0.001) {
@@ -1411,7 +1606,6 @@
m_motor_state.iq_target = iq_set_tmp;
control_current(&m_motor_state, dt);
- run_pid_control_pos(dt);
} else {
// Track back emf
float Va = ADC_VOLTS(ADC_IND_SENS1) * ((VIN_R1 + VIN_R2) / VIN_R2);
@@ -1440,6 +1634,7 @@
m_motor_state.id_filter = 0.0;
m_motor_state.iq_filter = 0.0;
m_motor_state.i_bus = 0.0;
+ m_motor_state.i_bus_filter = 0.0;
m_motor_state.i_abs = 0.0;
m_motor_state.i_abs_filter = 0.0;
@@ -1449,8 +1644,16 @@
&m_observer_x2, &m_phase_now_observer);
switch (m_conf->foc_sensor_mode) {
- case FOC_SENSOR_MODE_ENCODER: m_motor_state.phase = m_phase_now_encoder; break;
- case FOC_SENSOR_MODE_SENSORLESS: m_motor_state.phase = m_phase_now_observer; break;
+ case FOC_SENSOR_MODE_ENCODER:
+ m_motor_state.phase = correct_encoder(m_phase_now_observer, m_phase_now_encoder, m_pll_speed);
+ break;
+ case FOC_SENSOR_MODE_HALL:
+ m_phase_now_observer = correct_hall(m_phase_now_observer, m_pll_speed, dt);
+ m_motor_state.phase = m_phase_now_observer;
+ break;
+ case FOC_SENSOR_MODE_SENSORLESS:
+ m_motor_state.phase = m_phase_now_observer;
+ break;
}
}
@@ -1464,13 +1667,38 @@
// Update tachometer
static float phase_last = 0.0;
- int diff = (int)((utils_angle_difference_rad(m_motor_state.phase, phase_last) * (180.0 / M_PI)) / 60.0);
- if (diff != 0) {
+ // int diff = (int)((utils_angle_difference_rad(m_motor_state.phase, phase_last) * (180.0 / M_PI)) / 60.0);
+ float diff = (utils_angle_difference_rad(m_motor_state.phase, phase_last) * (180.0 / M_PI)) / 60.0;
+ if ((int) diff != 0) {
m_tachometer += diff;
- m_tachometer_abs += abs(diff);
+ m_tachometer_abs += fabsf(diff);
phase_last = m_motor_state.phase;
}
+ // Track position control angle
+ // TODO: Have another look at this.
+ float angle_now = 0.0;
+ if (encoder_is_configured()) {
+ angle_now = enc_ang;
+ } else {
+ angle_now = m_motor_state.phase * (180.0 / M_PI);
+ }
+
+ if (m_conf->p_pid_ang_div > 0.98 && m_conf->p_pid_ang_div < 1.02) {
+ m_pos_pid_now = angle_now;
+ } else {
+ static float angle_last = 0.0;
+ float diff_f = utils_angle_difference(angle_now, angle_last);
+ angle_last = angle_now;
+ m_pos_pid_now += diff_f / m_conf->p_pid_ang_div;
+ utils_norm_angle((float*)&m_pos_pid_now);
+ }
+
+ // Run position control
+ if (m_state == MC_STATE_RUNNING) {
+ run_pid_control_pos(m_pos_pid_now, m_pos_pid_set, dt);
+ }
+
// MCIF handler
mc_interface_mc_timer_isr();
@@ -1490,8 +1718,12 @@
return;
}
+ float openloop_rpm = utils_map(fabsf(m_motor_state.iq_target),
+ 0.0, m_conf->lo_current_max,
+ 0.0, m_conf->foc_openloop_rpm);
+
const float dt = 0.001;
- const float min_rads = (m_conf->foc_openloop_rpm * 2.0 * M_PI) / 60.0;
+ const float min_rads = (openloop_rpm * 2.0 * M_PI) / 60.0;
static float min_rpm_hyst_timer = 0.0;
static float min_rpm_timer = 0.0;
@@ -1514,7 +1746,7 @@
}
// Don't use this in brake mode.
- if (m_control_mode == CONTROL_MODE_CURRENT_BRAKE) {
+ if (m_control_mode == CONTROL_MODE_CURRENT_BRAKE || fabsf(m_motor_state.duty_now) < 0.001) {
min_rpm_hyst_timer = 0.0;
m_phase_observer_override = false;
}
@@ -1671,19 +1903,13 @@
// TODO: Have a look at this?
state_m->i_bus = state_m->mod_d * state_m->id + state_m->mod_q * state_m->iq;
+ UTILS_LP_FAST(state_m->i_bus_filter, state_m->i_bus, MCPWM_FOC_I_FILTER_CONST);
state_m->i_abs = sqrtf(state_m->id * state_m->id + state_m->iq * state_m->iq);
state_m->i_abs_filter = sqrtf(state_m->id_filter * state_m->id_filter + state_m->iq_filter * state_m->iq_filter);
float mod_alpha = c * state_m->mod_d - s * state_m->mod_q;
float mod_beta = c * state_m->mod_q + s * state_m->mod_d;
- state_m->v_alpha = mod_alpha * (2.0 / 3.0) * state_m->v_bus;
- state_m->v_beta = mod_beta * (2.0 / 3.0) * state_m->v_bus;
-
- // Set output (HW Dependent)
- uint32_t duty1, duty2, duty3, top;
- top = TIM1->ARR;
-
// Deadtime compensation
const float i_alpha_set = c * state_m->id_target - s * state_m->iq_target;
const float i_beta_set = c * state_m->iq_target + s * state_m->id_target;
@@ -1692,10 +1918,20 @@
const float ic_set = -0.5 * i_alpha_set - SQRT3_BY_2 * i_beta_set;
const float mod_alpha_set_sgn = (2.0 / 3.0) * SIGN(ia_set) - (1.0 / 3.0) * SIGN(ib_set) - (1.0 / 3.0) * SIGN(ic_set);
const float mod_beta_set_sgn = ONE_BY_SQRT3 * SIGN(ib_set) - ONE_BY_SQRT3 * SIGN(ic_set);
+ const float mod_comp_fact = m_conf->foc_dt_us * 1e-6 * m_conf->foc_f_sw;
+ const float mod_alpha_comp = mod_alpha_set_sgn * mod_comp_fact;
+ const float mod_beta_comp = mod_beta_set_sgn * mod_comp_fact;
+
+// mod_alpha += mod_alpha_comp;
+// mod_beta += mod_beta_comp;
+
+ // Apply compensation here so that 0 duty cyle has no glitches.
+ state_m->v_alpha = (mod_alpha - mod_alpha_comp) * (2.0 / 3.0) * state_m->v_bus;
+ state_m->v_beta = (mod_beta - mod_beta_comp) * (2.0 / 3.0) * state_m->v_bus;
- mod_alpha += mod_alpha_set_sgn * m_conf->foc_dt_us * 1e-6 * m_conf->foc_f_sw;
- mod_beta += mod_beta_set_sgn * m_conf->foc_dt_us * 1e-6 * m_conf->foc_f_sw;
-
+ // Set output (HW Dependent)
+ uint32_t duty1, duty2, duty3, top;
+ top = TIM1->ARR;
svm(-mod_alpha, -mod_beta, top, &duty1, &duty2, &duty3);
TIMER_UPDATE_DUTY(duty1, duty2, duty3);
@@ -1834,7 +2070,7 @@
*tCout = tC;
}
-static void run_pid_control_pos(float dt) {
+static void run_pid_control_pos(float angle_now, float angle_set, float dt) {
static float i_term = 0;
static float prev_error = 0;
float p_term;
@@ -1847,19 +2083,30 @@
return;
}
- // Compute error
- float angle = encoder_read_deg();
- float error = utils_angle_difference(m_pos_pid_set_pos, angle);
+ // Compute parameters
+ float error = utils_angle_difference(angle_set, angle_now);
- if (m_conf->foc_encoder_inverted) {
- error = -error;
+ if (encoder_is_configured()) {
+ if (m_conf->foc_encoder_inverted) {
+ error = -error;
+ }
}
-
- // Compute parameters
p_term = error * m_conf->p_pid_kp;
i_term += error * (m_conf->p_pid_ki * dt);
- d_term = (error - prev_error) * (m_conf->p_pid_kd / dt);
+
+ // Average DT for the D term when the error does not change. This likely
+ // happens at low speed when the position resolution is low and several
+ // control iterations run without position updates.
+ // TODO: Are there problems with this approach?
+ static float dt_int = 0.0;
+ dt_int += dt;
+ if (error == prev_error) {
+ d_term = 0.0;
+ } else {
+ d_term = (error - prev_error) * (m_conf->p_pid_kd / dt_int);
+ dt_int = 0.0;
+ }
// I-term wind-up protection
utils_truncate_number(&i_term, -1.0, 1.0);
@@ -1871,11 +2118,15 @@
float output = p_term + i_term + d_term;
utils_truncate_number(&output, -1.0, 1.0);
- if (encoder_index_found()) {
- m_iq_set = output * m_conf->lo_current_max;
+ if (encoder_is_configured()) {
+ if (encoder_index_found()) {
+ m_iq_set = output * m_conf->lo_current_max;
+ } else {
+ // Rotate the motor with 40 % power until the encoder index is found.
+ m_iq_set = 0.4 * m_conf->lo_current_max;
+ }
} else {
- // Rotate the motor with 40 % power until the encoder index is found.
- m_iq_set = 0.4 * m_conf->lo_current_max;
+ m_iq_set = output * m_conf->lo_current_max;
}
}
@@ -1892,15 +2143,11 @@
return;
}
- // Too low RPM set. Stop and return.
+ // Too low RPM set. Reset state and return.
if (fabsf(m_speed_pid_set_rpm) < m_conf->s_pid_min_erpm) {
i_term = 0.0;
prev_error = 0;
m_iq_set = 0.0;
- m_state = MC_STATE_OFF;
- if (m_output_on) {
- stop_pwm_hw();
- }
return;
}
@@ -1923,8 +2170,17 @@
// Calculate output
float output = p_term + i_term + d_term;
- utils_truncate_number(&output, -1.0, 1.0);
+ if ((m_speed_pid_cruise_control_type == CRUISE_CONTROL_MOTOR_SETTINGS && m_conf->s_pid_breaking_enabled) || m_speed_pid_cruise_control_type == CRUISE_CONTROL_BRAKING_ENABLED) {
+ utils_truncate_number(&output, -1.0, 1.0);
+ } else {
+ if(m_speed_pid_set_rpm < 0.0){
+ utils_truncate_number(&output, -1.0, 0.0);
+ }else{
+ utils_truncate_number(&output, 0.0, 1.0);
+ }
+ }
+
m_iq_set = output * m_conf->lo_current_max;
}
@@ -1963,3 +2219,113 @@
m_output_on = true;
}
+
+static int read_hall(void) {
+ return READ_HALL1() | (READ_HALL2() << 1) | (READ_HALL3() << 2);
+}
+
+static float correct_encoder(float obs_angle, float enc_angle, float speed) {
+ float rpm_abs = fabsf(speed / ((2.0 * M_PI) / 60.0));
+ static bool using_encoder = true;
+
+ // Hysteresis 5 % of total speed
+ float hyst = m_conf->foc_sl_erpm * 0.05;
+ if (using_encoder) {
+ if (rpm_abs > (m_conf->foc_sl_erpm + hyst)) {
+ using_encoder = false;
+ }
+ } else {
+ if (rpm_abs < (m_conf->foc_sl_erpm - hyst)) {
+ using_encoder = true;
+ }
+ }
+
+ return using_encoder ? enc_angle : obs_angle;
+}
+
+static float correct_hall(float angle, float speed, float dt) {
+ static int ang_hall_int_prev = -1;
+ float rpm_abs = fabsf(speed / ((2.0 * M_PI) / 60.0));
+ static bool using_hall = true;
+
+ // Hysteresis 10 % of total speed
+ float hyst = m_conf->foc_sl_erpm * 0.10;
+ if (using_hall) {
+ if (rpm_abs > (m_conf->foc_sl_erpm + hyst)) {
+ using_hall = false;
+ }
+ } else {
+ if (rpm_abs < (m_conf->foc_sl_erpm - hyst)) {
+ using_hall = true;
+ }
+ }
+
+ if (using_hall) {
+ int ang_hall_int = m_conf->foc_hall_table[read_hall()];
+
+ // Only override the observer if the hall sensor value is valid.
+ if (ang_hall_int < 201) {
+ static float ang_hall = 0.0;
+ float ang_hall_now = (((float)ang_hall_int / 200.0) * 360.0) * M_PI / 180.0;
+
+ if (ang_hall_int_prev < 0) {
+ // Previous angle not valid
+ ang_hall_int_prev = ang_hall_int;
+
+ if (ang_hall_int_prev == -2) {
+ // Before was sensorless, initialize with the provided angle
+ ang_hall = angle;
+ } else {
+ // A boot or error has occurred. Use center of hall sensor angle.
+ ang_hall = ((ang_hall_int / 200.0) * 360.0) * M_PI / 180.0;
+ }
+ } else if (ang_hall_int != ang_hall_int_prev) {
+ // A transition was just made. The angle is in the middle of the new and old angle.
+ int ang_avg = abs(ang_hall_int - ang_hall_int_prev);
+ if (ang_avg < 100) {
+ ang_avg = (ang_hall_int + ang_hall_int_prev) / 2;
+ } else if (ang_avg != 100) {
+ ang_avg = (ang_hall_int + ang_hall_int_prev) / 2 + 100;
+ }
+ ang_avg %= 200;
+ ang_hall = (((float)ang_avg / 200.0) * 360.0) * M_PI / 180.0;
+ }
+
+ ang_hall_int_prev = ang_hall_int;
+
+ if (rpm_abs < 100) {
+ // Don't interpolate on very low speed, just use the closest hall sensor
+ ang_hall = ang_hall_now;
+ } else {
+ // Interpolate
+ float diff = utils_angle_difference_rad(ang_hall, ang_hall_now);
+ if (fabsf(diff) < ((2.0 * M_PI) / 12.0)) {
+ // Do interpolation
+ ang_hall += speed * dt;
+ } else {
+ // We are too far away with the interpolation
+ ang_hall -= diff / 100.0;
+ }
+ }
+
+ utils_norm_angle_rad(&ang_hall);
+
+ angle = ang_hall;
+ } else {
+ // Invalid hall reading. Don't update angle.
+ ang_hall_int_prev = -1;
+
+ // Also allow open loop in order to behave like normal sensorless
+ // operation. Then the motor works even if the hall sensor cable
+ // gets disconnected (when the sensor spacing is 120 degrees).
+ if (m_phase_observer_override && m_state == MC_STATE_RUNNING) {
+ angle = m_phase_now_observer_override;
+ }
+ }
+ } else {
+ // We are running sensorless.
+ ang_hall_int_prev = -2;
+ }
+
+ return angle;
+}
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/mcpwm_foc.h Raptor2 Sources/bldc-firmware_2_80/mcpwm_foc.h
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/mcpwm_foc.h 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/mcpwm_foc.h 2017-04-13 18:06:44.000000000 +0100
@@ -39,11 +39,14 @@
void mcpwm_foc_set_duty(float dutyCycle);
void mcpwm_foc_set_duty_noramp(float dutyCycle);
void mcpwm_foc_set_pid_speed(float rpm);
+void mcpwm_foc_set_pid_speed_with_cruise_status(float rpm, ppm_cruise cruise_status);
void mcpwm_foc_set_pid_pos(float pos);
void mcpwm_foc_set_current(float current);
void mcpwm_foc_set_brake_current(float current);
float mcpwm_foc_get_duty_cycle_set(void);
float mcpwm_foc_get_duty_cycle_now(void);
+float mcpwm_foc_get_pid_pos_set(void);
+float mcpwm_foc_get_pid_pos_now(void);
float mcpwm_foc_get_switching_frequency_now(void);
float mcpwm_foc_get_rpm(void);
float mcpwm_foc_get_tot_current(void);
@@ -65,6 +68,7 @@
float mcpwm_foc_measure_resistance(float current, int samples);
float mcpwm_foc_measure_inductance(float duty, int samples, float *curr);
bool mcpwm_foc_measure_res_ind(float *res, float *ind);
+bool mcpwm_foc_hall_detect(float current, uint8_t *hall_table);
void mcpwm_foc_print_state(void);
float mcpwm_foc_get_last_inj_adc_isr_duration(void);
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/mcuconf.h Raptor2 Sources/bldc-firmware_2_80/mcuconf.h
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/mcuconf.h 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/mcuconf.h 2016-09-19 10:05:44.000000000 +0100
@@ -248,7 +248,7 @@
/*
* SPI driver system settings.
*/
-#define STM32_SPI_USE_SPI1 FALSE
+#define STM32_SPI_USE_SPI1 TRUE
#define STM32_SPI_USE_SPI2 FALSE
#define STM32_SPI_USE_SPI3 FALSE
#define STM32_SPI_SPI1_RX_DMA_STREAM STM32_DMA_STREAM_ID(2, 0)
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/nrf/nrf_driver.c Raptor2 Sources/bldc-firmware_2_80/nrf/nrf_driver.c
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/nrf/nrf_driver.c 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/nrf/nrf_driver.c 2016-08-04 23:47:30.000000000 +0100
@@ -34,44 +34,64 @@
#include "packet.h"
// Settings
-#define MAX_PL_LEN 25
-#define RX_BUFFER_SIZE PACKET_MAX_PL_LEN
+#define MAX_PL_LEN 25
+#define RX_BUFFER_SIZE PACKET_MAX_PL_LEN
+
+#define ALIVE_INTERVAL 50 // Send alive packets at this rate
+#define NRF_RESTART_TIMEOUT 500 // Restart the NRF if nothing has been received or acked for this time
// Variables
static THD_WORKING_AREA(rx_thread_wa, 2048);
static THD_WORKING_AREA(tx_thread_wa, 512);
static mote_state mstate;
static uint8_t rx_buffer[RX_BUFFER_SIZE];
+static int nosend_cnt;
+static int nrf_restart_rx_time;
+static int nrf_restart_tx_time;
// Functions
static THD_FUNCTION(rx_thread, arg);
static THD_FUNCTION(tx_thread, arg);
+static int rf_tx_wrapper(char *data, int len);
void nrf_driver_init(void) {
- rf_init();
rfhelp_init();
- // Set RF address
- const char addr[5] = {0xC4, 0xC5, 0xC6, 0xC7, app_get_configuration()->controller_id};
- rfhelp_set_rx_addr(0, addr, 5);
- rfhelp_set_tx_addr(addr, 5);
+ nosend_cnt = 0;
+ nrf_restart_rx_time = 0;
+ nrf_restart_tx_time = 0;
chThdCreateStatic(rx_thread_wa, sizeof(rx_thread_wa), NORMALPRIO - 1, rx_thread, NULL);
chThdCreateStatic(tx_thread_wa, sizeof(tx_thread_wa), NORMALPRIO - 1, tx_thread, NULL);
}
+static int rf_tx_wrapper(char *data, int len) {
+ int res = rfhelp_send_data_crc(data, len);
+
+ if (res == 0) {
+ nrf_restart_tx_time = NRF_RESTART_TIMEOUT;
+ }
+
+ return res;
+}
+
static THD_FUNCTION(tx_thread, arg) {
(void)arg;
chRegSetThreadName("Nrf TX");
for(;;) {
- uint8_t pl[6];
- int32_t index = 0;
- pl[index++] = MOTE_PACKET_ALIVE;
- rfhelp_send_data_crc((char*)pl, index);
+ nosend_cnt++;
+
+ if (nosend_cnt >= ALIVE_INTERVAL) {
+ uint8_t pl[2];
+ int32_t index = 0;
+ pl[index++] = MOTE_PACKET_ALIVE;
+ rf_tx_wrapper((char*)pl, index);
+ nosend_cnt = 0;
+ }
- chThdSleepMilliseconds(50);
+ chThdSleepMilliseconds(1);
}
}
@@ -81,8 +101,6 @@
chRegSetThreadName("Nrf RX");
- int reset_cnt = 0;
-
for(;;) {
uint8_t buf[32];
int len;
@@ -90,6 +108,7 @@
for(;;) {
int res = rfhelp_read_rx_data_crc((char*)buf, &len, &pipe);
+
chuck_data cdata;
int32_t ind = 0;
int buttons;
@@ -98,6 +117,8 @@
if (res >= 0) {
MOTE_PACKET packet = buf[0];
+ nrf_restart_rx_time = NRF_RESTART_TIMEOUT;
+
switch (packet) {
case MOTE_PACKET_BATT_LEVEL:
// TODO!
@@ -146,10 +167,15 @@
uint8_t crc_high = buf[ind++];
uint8_t crc_low = buf[ind++];
+ memcpy(rx_buffer + rxbuf_len - (len - ind), buf + ind, len - ind);
+
if (crc16(rx_buffer, rxbuf_len)
== ((unsigned short) crc_high << 8
| (unsigned short) crc_low)) {
+ // Wait a bit in case retries are still made
+ chThdSleepMilliseconds(2);
+
commands_set_send_func(nrf_driver_send_buffer);
commands_process_packet(rx_buffer, rxbuf_len);
}
@@ -157,6 +183,9 @@
break;
case MOTE_PACKET_PROCESS_SHORT_BUFFER:
+ // Wait a bit in case retries are still made
+ chThdSleepMilliseconds(2);
+
commands_set_send_func(nrf_driver_send_buffer);
commands_process_packet(buf + 1, len - 1);
break;
@@ -177,11 +206,15 @@
chThdSleepMilliseconds(5);
- // Restart the nrf every 500ms just in case
- reset_cnt++;
- if (reset_cnt > 100) {
- reset_cnt = 0;
+ // Restart the nrf if nothing has been received for a while
+ if (nrf_restart_rx_time > 0 && nrf_restart_tx_time > 0) {
+ nrf_restart_rx_time -= 5;
+ nrf_restart_tx_time -= 5;
+ } else {
+ rfhelp_power_up();
rfhelp_restart();
+ nrf_restart_rx_time = NRF_RESTART_TIMEOUT;
+ nrf_restart_tx_time = NRF_RESTART_TIMEOUT;
}
}
}
@@ -194,44 +227,49 @@
send_buffer[ind++] = MOTE_PACKET_PROCESS_SHORT_BUFFER;
memcpy(send_buffer + ind, data, len);
ind += len;
- rfhelp_send_data_crc((char*)send_buffer, ind);
+ rf_tx_wrapper((char*)send_buffer, ind);
+ nosend_cnt = 0;
} else {
unsigned int end_a = 0;
- for (unsigned int i = 0;i < len;i += (MAX_PL_LEN - 2)) {
- end_a = i;
+ unsigned int len2 = len - (MAX_PL_LEN - 5);
+ for (unsigned int i = 0;i < len2;i += (MAX_PL_LEN - 2)) {
if (i > 255) {
break;
}
+ end_a = i + (MAX_PL_LEN - 2);
+
uint8_t send_len = (MAX_PL_LEN - 2);
send_buffer[0] = MOTE_PACKET_FILL_RX_BUFFER;
send_buffer[1] = i;
- if ((i + (MAX_PL_LEN - 2)) <= len) {
+ if ((i + (MAX_PL_LEN - 2)) <= len2) {
memcpy(send_buffer + 2, data + i, send_len);
} else {
- send_len = len - i;
+ send_len = len2 - i;
memcpy(send_buffer + 2, data + i, send_len);
}
- rfhelp_send_data_crc((char*)send_buffer, send_len + 2);
+ rf_tx_wrapper((char*)send_buffer, send_len + 2);
+ nosend_cnt = 0;
}
- for (unsigned int i = end_a;i < len;i += (MAX_PL_LEN - 3)) {
+ for (unsigned int i = end_a;i < len2;i += (MAX_PL_LEN - 3)) {
uint8_t send_len = (MAX_PL_LEN - 3);
send_buffer[0] = MOTE_PACKET_FILL_RX_BUFFER_LONG;
send_buffer[1] = i >> 8;
send_buffer[2] = i & 0xFF;
- if ((i + (MAX_PL_LEN - 3)) <= len) {
+ if ((i + (MAX_PL_LEN - 3)) <= len2) {
memcpy(send_buffer + 3, data + i, send_len);
} else {
- send_len = len - i;
+ send_len = len2 - i;
memcpy(send_buffer + 3, data + i, send_len);
}
- rfhelp_send_data_crc((char*)send_buffer, send_len + 3);
+ rf_tx_wrapper((char*)send_buffer, send_len + 3);
+ nosend_cnt = 0;
}
uint32_t ind = 0;
@@ -241,7 +279,10 @@
unsigned short crc = crc16(data, len);
send_buffer[ind++] = (uint8_t)(crc >> 8);
send_buffer[ind++] = (uint8_t)(crc & 0xFF);
+ memcpy(send_buffer + 5, data + len2, len - len2);
+ ind += len - len2;
- rfhelp_send_data_crc((char*)send_buffer, ind);
+ rf_tx_wrapper((char*)send_buffer, ind);
+ nosend_cnt = 0;
}
}
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/nrf/rf.c Raptor2 Sources/bldc-firmware_2_80/nrf/rf.c
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/nrf/rf.c 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/nrf/rf.c 2016-08-04 23:47:30.000000000 +0100
@@ -22,32 +22,6 @@
void rf_init(void) {
spi_sw_init();
-
- rf_power_down();
-
- // Set default register values (TODO for the rest)
- rf_write_reg_byte(NRF_REG_EN_RXADDR, 0);
- rf_write_reg_byte(NRF_REG_DYNPD, 0);
-
- rf_set_crc_type(NRF_CRC_1B);
- rf_set_retr_retries(2);
- rf_set_retr_delay(NRF_RETR_DELAY_500US);
- rf_set_power(NRF_POWER_0DBM);
- rf_set_speed(NRF_SPEED_250K);
- rf_set_address_width(NRF_AW_5);
- rf_set_frequency(2400 + 0x20);
- rf_enable_features(NRF_FEATURE_DPL | NRF_FEATURE_DYN_ACK);
-
- rf_enable_pipe_autoack(NRF_MASK_PIPE0);
- rf_enable_pipe_address(NRF_MASK_PIPE0);
- rf_enable_pipe_dlp(NRF_MASK_PIPE0);
-
- // Note: The address should be set by the application.
-
- rf_power_up();
- rf_mode_rx();
- rf_flush_all();
- rf_clear_irq();
}
void rf_set_speed(NRF_SPEED speed) {
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/nrf/rf.h Raptor2 Sources/bldc-firmware_2_80/nrf/rf.h
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/nrf/rf.h 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/nrf/rf.h 2016-08-04 23:47:30.000000000 +0100
@@ -20,51 +20,7 @@
#define RF_H_
#include <stdint.h>
-
-// Data types
-typedef enum {
- NRF_SPEED_250K = 0,
- NRF_SPEED_1M,
- NRF_SPEED_2M
-} NRF_SPEED;
-
-typedef enum {
- NRF_POWER_M18DBM = 0,
- NRF_POWER_M12DBM,
- NRF_POWER_M6DBM,
- NRF_POWER_0DBM
-} NRF_POWER;
-
-typedef enum {
- NRF_AW_3 = 0,
- NRF_AW_4,
- NRF_AW_5
-} NRF_AW;
-
-typedef enum {
- NRF_CRC_DISABLED = 0,
- NRF_CRC_1B,
- NRF_CRC_2B
-} NRF_CRC;
-
-typedef enum {
- NRF_RETR_DELAY_250US = 0,
- NRF_RETR_DELAY_500US,
- NRF_RETR_DELAY_750US,
- NRF_RETR_DELAY_1000US,
- NRF_RETR_DELAY_1250US,
- NRF_RETR_DELAY_1500US,
- NRF_RETR_DELAY_1750US,
- NRF_RETR_DELAY_2000US,
- NRF_RETR_DELAY_2250US,
- NRF_RETR_DELAY_2500US,
- NRF_RETR_DELAY_2750US,
- NRF_RETR_DELAY_3000US,
- NRF_RETR_DELAY_3250US,
- NRF_RETR_DELAY_3500US,
- NRF_RETR_DELAY_3750US,
- NRF_RETR_DELAY_4000US
-} NRF_RETR_DELAY;
+#include <datatypes.h>
// Status register masks
#define NRF_STATUS_TX_FULL (1<<0)
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/nrf/rfhelp.c Raptor2 Sources/bldc-firmware_2_80/nrf/rfhelp.c
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/nrf/rfhelp.c 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/nrf/rfhelp.c 2016-08-04 23:47:30.000000000 +0100
@@ -21,7 +21,6 @@
#include "hal.h"
#include "crc.h"
#include <string.h>
-#include <stdbool.h>
// Variables
static mutex_t rf_mutex;
@@ -30,25 +29,41 @@
static bool rx_addr_set[6];
static int address_length;
static bool tx_pipe0_addr_eq;
+static nrf_config nrf_conf;
+static bool init_done = false;
void rfhelp_init(void) {
chMtxObjectInit(&rf_mutex);
+ rf_init();
// address_length = rf_get_address_width();
- address_length = 5; // We assume length 5
+ address_length = 3; // We assume length 3
// This should not happen
if (address_length > 5 || address_length < 3) {
- address_length = 5;
+ address_length = 3;
}
for (int i = 0;i < 6;i++) {
rf_read_reg(NRF_REG_RX_ADDR_P0, rx_addr[i], address_length);
rx_addr_set[i] = false;
}
- rf_read_reg(NRF_REG_TX_ADDR, tx_addr, address_length);
+ rf_read_reg(NRF_REG_TX_ADDR, tx_addr, address_length);
tx_pipe0_addr_eq = memcmp(rx_addr[0], tx_addr, address_length) == 0;
+
+ // TODO: fill nrf_conf with values from the nrf chip. For now we assume
+ // that nrf_conf is already set when rfhelp_restart is called.
+
+ init_done = true;
+}
+
+void rfhelp_update_conf(nrf_config *conf) {
+ nrf_conf = *conf;
+
+ if (init_done) {
+ rfhelp_restart();
+ }
}
/**
@@ -57,13 +72,38 @@
void rfhelp_restart(void) {
chMtxLock(&rf_mutex);
- rf_init();
+ rf_power_down();
+
+ // Set default register values.
+ // TODO: make this file consistent with multiple
+ // rx_addr and tx_addr, and the rest in general.
+ rf_write_reg_byte(NRF_REG_EN_RXADDR, 0);
+ rf_write_reg_byte(NRF_REG_DYNPD, 0);
+
+ rf_set_crc_type(nrf_conf.crc_type);
+ rf_set_retr_retries(nrf_conf.retries);
+ rf_set_retr_delay(nrf_conf.retry_delay);
+ rf_set_power(nrf_conf.power);
+ rf_set_speed(nrf_conf.speed);
+ rf_set_address_width(NRF_AW_3); // Always use 3 byte address
+ rf_set_frequency(2400 + (unsigned int)nrf_conf.channel);
+ rf_enable_features(NRF_FEATURE_DPL | NRF_FEATURE_DYN_ACK);
+
+ rf_enable_pipe_autoack(NRF_MASK_PIPE0);
+ rf_enable_pipe_address(NRF_MASK_PIPE0);
+ rf_enable_pipe_dlp(NRF_MASK_PIPE0);
+
+ memcpy(tx_addr, nrf_conf.address, 3);
+ memcpy(rx_addr[0], nrf_conf.address, 3);
+ tx_pipe0_addr_eq = true;
+
rf_set_tx_addr(tx_addr, address_length);
- for (int i = 0;i < 6;i++) {
- if (rx_addr_set[i]) {
- rf_set_rx_addr(i, rx_addr[i], address_length);
- }
- }
+ rf_set_rx_addr(0, rx_addr[0], address_length);
+
+ rf_power_up();
+ rf_mode_rx();
+ rf_flush_all();
+ rf_clear_irq();
chMtxUnlock(&rf_mutex);
}
@@ -82,7 +122,7 @@
* -1: Max RT.
* -2: Timeout
*/
-int rfhelp_send_data(char *data, int len) {
+int rfhelp_send_data(char *data, int len, bool ack) {
int timeout = 60;
int retval = -1;
@@ -92,13 +132,17 @@
rf_clear_irq();
rf_flush_all();
- rf_write_tx_payload(data, len);
-
// Pipe0-address and tx-address must be equal for ack to work.
- if (!tx_pipe0_addr_eq) {
+ if (!tx_pipe0_addr_eq && ack) {
rf_set_rx_addr(0, tx_addr, address_length);
}
+ if (ack) {
+ rf_write_tx_payload(data, len);
+ } else {
+ rf_write_tx_payload_no_ack(data, len);
+ }
+
for(;;) {
int s = rf_status();
@@ -119,7 +163,7 @@
}
// Restore pipe0 address
- if (!tx_pipe0_addr_eq) {
+ if (!tx_pipe0_addr_eq && ack) {
rf_set_rx_addr(0, rx_addr[0], address_length);
}
@@ -154,7 +198,7 @@
buffer[len] = (char)(crc >> 8);
buffer[len + 1] = (char)(crc & 0xFF);
- return rfhelp_send_data(buffer, len + 2);
+ return rfhelp_send_data(buffer, len + 2, nrf_conf.send_crc_ack);
}
/**
@@ -191,7 +235,7 @@
if (*len <= 32 && *len >= 0) {
rf_read_rx_payload(data, *len);
rf_clear_rx_irq();
- rf_flush_rx();
+// rf_flush_rx();
s = rf_status();
if (NRF_STATUS_GET_RX_P_NO(s) == 7) {
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/nrf/rfhelp.h Raptor2 Sources/bldc-firmware_2_80/nrf/rfhelp.h
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/nrf/rfhelp.h 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/nrf/rfhelp.h 2016-08-04 23:47:30.000000000 +0100
@@ -18,10 +18,14 @@
#ifndef RFHELP_H_
#define RFHELP_H_
+#include <stdbool.h>
+#include "datatypes.h"
+
// Functions
void rfhelp_init(void);
+void rfhelp_update_conf(nrf_config *conf);
void rfhelp_restart(void);
-int rfhelp_send_data(char *data, int len);
+int rfhelp_send_data(char *data, int len, bool ack);
int rfhelp_send_data_crc(char *data, int len);
int rfhelp_read_rx_data(char *data, int *len, int *pipe);
int rfhelp_read_rx_data_crc(char *data, int *len, int *pipe);
Only in Raptor2 Sources/bldc-firmware_2_80: oh5la4w3.ag1
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/packet.h Raptor2 Sources/bldc-firmware_2_80/packet.h
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/packet.h 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/packet.h 2017-04-13 18:06:50.000000000 +0100
@@ -28,7 +28,7 @@
#include <stdint.h>
// Settings
-#define PACKET_RX_TIMEOUT 2
+#define PACKET_RX_TIMEOUT 10
#define PACKET_HANDLERS 2
#define PACKET_MAX_PL_LEN 1024
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/servo_dec.c Raptor2 Sources/bldc-firmware_2_80/servo_dec.c
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/servo_dec.c 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/servo_dec.c 2016-10-24 21:35:06.000000000 +0100
@@ -39,6 +39,7 @@
static volatile systime_t last_update_time;
static volatile float servo_pos[SERVO_NUM];
static volatile float pulse_start = 1.0;
+static volatile float pulse_center = 1.5;
static volatile float pulse_end = 2.0;
static volatile float last_len_received[SERVO_NUM];
static volatile bool use_median_filter = false;
@@ -68,8 +69,16 @@
}
if (len >= 0.0) {
+ float len_to_center = len + pulse_start - pulse_center;
+
if (use_median_filter) {
- float c = (len * 2.0 - len_set) / len_set;
+ float c;
+ if (len_to_center >= 0.0) {
+ c = (1.0 / (pulse_end - pulse_center)) * len_to_center;
+ } else {
+ c = (1.0 / (pulse_center - pulse_start)) * len_to_center;
+ }
+ //float c = (len * 2.0 - len_set) / len_set;
static float c1 = 0.5;
static float c2 = 0.5;
float med = utils_middle_of_3(c, c1, c2);
@@ -79,7 +88,12 @@
servo_pos[0] = med;
} else {
- servo_pos[0] = (len * 2.0 - len_set) / len_set;
+ if (len_to_center >= 0.0) {
+ servo_pos[0] = (1 / (pulse_end - pulse_center)) * len_to_center;
+ } else {
+ servo_pos[0] = (1 / (pulse_center - pulse_start)) * len_to_center;
+ }
+ //servo_pos[0] = (len * 2.0 - len_set) / len_set;
}
last_update_time = chVTGetSystemTime();
@@ -135,8 +149,9 @@
* @param end
* he amount of milliseconds the pulse ends at (default is 2.0)
*/
-void servodec_set_pulse_options(float start, float end, bool median_filter) {
+void servodec_set_pulse_options(float start, float center, float end, bool median_filter) {
pulse_start = start;
+ pulse_center = center;
pulse_end = end;
use_median_filter = median_filter;
}
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/servo_dec.h Raptor2 Sources/bldc-firmware_2_80/servo_dec.h
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/servo_dec.h 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/servo_dec.h 2016-10-24 16:46:32.000000000 +0100
@@ -30,7 +30,7 @@
// Functions
void servodec_init(void (*d_func)(void));
-void servodec_set_pulse_options(float start, float end, bool median_filter);
+void servodec_set_pulse_options(float start, float center, float end, bool median_filter);
float servodec_get_servo(int servo_num);
uint32_t servodec_get_time_since_update(void);
float servodec_get_last_pulse_len(int servo_num);
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/terminal.c Raptor2 Sources/bldc-firmware_2_80/terminal.c
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/terminal.c 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/terminal.c 2017-04-13 18:07:08.000000000 +0100
@@ -29,11 +29,12 @@
#include "mcpwm_foc.h"
#include "mc_interface.h"
#include "commands.h"
-#include "main.h"
+#include "app.h"
#include "hw.h"
#include "comm_can.h"
#include "utils.h"
#include "timeout.h"
+#include "encoder.h"
#include <string.h>
#include <stdio.h>
@@ -73,7 +74,7 @@
} else if (strcmp(argv[0], "last_adc_duration") == 0) {
commands_printf("Latest ADC duration: %.4f ms", (double)(mcpwm_get_last_adc_isr_duration() * 1000.0));
commands_printf("Latest injected ADC duration: %.4f ms", (double)(mc_interface_get_last_inj_adc_isr_duration() * 1000.0));
- commands_printf("Latest main ADC duration: %.4f ms\n", (double)(main_get_last_adc_isr_duration() * 1000.0));
+ commands_printf("Latest sample ADC duration: %.4f ms\n", (double)(mc_interface_get_last_sample_adc_isr_duration() * 1000.0));
} else if (strcmp(argv[0], "kv") == 0) {
commands_printf("Calculated KV: %.2f rpm/volt\n", (double)mcpwm_get_kv_filtered());
} else if (strcmp(argv[0], "mem") == 0) {
@@ -98,6 +99,27 @@
commands_printf("");
} else if (strcmp(argv[0], "fault") == 0) {
commands_printf("%s\n", mc_interface_fault_to_string(mc_interface_get_fault()));
+ } else if (strcmp(argv[0], "faulta") == 0) {
+ if (fault_vec_write == 0) {
+ commands_printf("No faults");
+ } else {
+ commands_printf("Fault: %s\nID: %i\nCurrent: %.1f\nCurrent filtered: %.1f\nVoltage: %.2f\nDuty: %.2f\nRPM: %.1f\nTacho: %d\nCycles running: %d\nTIM duty: %d\nTIM val samp: %d\nTIM current samp: %d\nTIM top: %d\nComm step: %d\nTemperature: %.2f",
+ mc_interface_fault_to_string(fault_vec[fault_vec_write - 1].fault),
+ app_get_configuration()->controller_id,
+ (double)fault_vec[fault_vec_write - 1].current,
+ (double)fault_vec[fault_vec_write - 1].current_filtered,
+ (double)fault_vec[fault_vec_write - 1].voltage,
+ (double)fault_vec[fault_vec_write - 1].duty,
+ (double)fault_vec[fault_vec_write - 1].rpm,
+ fault_vec[fault_vec_write - 1].tacho,
+ fault_vec[fault_vec_write - 1].cycles_running,
+ (int)((float)fault_vec[fault_vec_write - 1].tim_top * fault_vec[fault_vec_write - 1].duty),
+ fault_vec[fault_vec_write - 1].tim_val_samp,
+ fault_vec[fault_vec_write - 1].tim_current_samp,
+ fault_vec[fault_vec_write - 1].tim_top,
+ fault_vec[fault_vec_write - 1].comm_step,
+ (double)fault_vec[fault_vec_write - 1].temperature);
+ }
} else if (strcmp(argv[0], "faults") == 0) {
if (fault_vec_write == 0) {
commands_printf("No faults registered since startup\n");
@@ -211,36 +233,66 @@
commands_printf("ID : %i", msg->id);
commands_printf("RX Time : %i", msg->rx_time);
commands_printf("Age (milliseconds) : %.2f", (double)(UTILS_AGE_S(msg->rx_time) * 1000.0));
- commands_printf("RPM : %.2f", (double)msg->rpm);
+ commands_printf("RPM : %.2f", (double)(msg->rpm));
commands_printf("Current : %.2f", (double)msg->current);
- commands_printf("Duty : %.2f\n", (double)msg->duty);
+ commands_printf("Duty : %i", msg->duty);
+ commands_printf("Cruise Control : %i\n", msg->cruise_control_status);
+ }
+ }
+ } else if (strcmp(argv[0], "can_member") == 0) {
+ char controllerIds[100] = "CAN:\n";
+ for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+ can_status_msg *msg = comm_can_get_status_msg_index(i);
+
+ if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < 1.0) {
+ char idArray[5];
+ sprintf(idArray, "%i\n", msg->id);
+ strcat(controllerIds, idArray);
}
}
- } else if (strcmp(argv[0], "foc_encoder_detect") == 0) {
+ commands_printf(controllerIds);
+ } else if (strcmp(argv[0], "can_erpms") == 0) {
+ char controllerIds[100] = "CAN ERPMs:\n";
+
+ char rpmArrayMaster[15];
+ sprintf(rpmArrayMaster, "M %6.2f\n", (double)mc_interface_get_rpm());
+ strcat(controllerIds, rpmArrayMaster);
+
+ for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+ can_status_msg *msg = comm_can_get_status_msg_index(i);
+
+ if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < 1.0) {
+ char rpmArray[15];
+ sprintf(rpmArray, "%i %6.2f\n", msg->id, (double)(msg->rpm));
+ strcat(controllerIds, rpmArray);
+ }
+ }
+ commands_printf(controllerIds);
+ }else if (strcmp(argv[0], "foc_encoder_detect") == 0) {
if (argc == 2) {
float current = -1.0;
sscanf(argv[1], "%f", &current);
if (current > 0.0 && current <= mcconf.l_current_max) {
-#if ENCODER_ENABLE
- mc_motor_type type_old = mcconf.motor_type;
- mcconf.motor_type = MOTOR_TYPE_FOC;
- mc_interface_set_configuration(&mcconf);
-
- float offset = 0.0;
- float ratio = 0.0;
- bool inverted = false;
- mcpwm_foc_encoder_detect(current, true, &offset, &ratio, &inverted);
-
- mcconf.motor_type = type_old;
- mc_interface_set_configuration(&mcconf);
-
- commands_printf("Offset : %.2f", (double)offset);
- commands_printf("Ratio : %.2f", (double)ratio);
- commands_printf("Inverted : %s\n", inverted ? "true" : "false");
-#else
- commands_printf("Encoder not enabled.\n");
-#endif
+ if (encoder_is_configured()) {
+ mc_motor_type type_old = mcconf.motor_type;
+ mcconf.motor_type = MOTOR_TYPE_FOC;
+ mc_interface_set_configuration(&mcconf);
+
+ float offset = 0.0;
+ float ratio = 0.0;
+ bool inverted = false;
+ mcpwm_foc_encoder_detect(current, true, &offset, &ratio, &inverted);
+
+ mcconf.motor_type = type_old;
+ mc_interface_set_configuration(&mcconf);
+
+ commands_printf("Offset : %.2f", (double)offset);
+ commands_printf("Ratio : %.2f", (double)ratio);
+ commands_printf("Inverted : %s\n", inverted ? "true" : "false");
+ } else {
+ commands_printf("Encoder not enabled.\n");
+ }
} else {
commands_printf("Invalid argument(s).\n");
}
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/utils.c Raptor2 Sources/bldc-firmware_2_80/utils.c
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/utils.c 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/utils.c 2017-04-13 18:07:16.000000000 +0100
@@ -133,22 +133,28 @@
* The difference between the angles
*/
float utils_angle_difference(float angle1, float angle2) {
- utils_norm_angle(&angle1);
- utils_norm_angle(&angle2);
+// utils_norm_angle(&angle1);
+// utils_norm_angle(&angle2);
+//
+// if (fabsf(angle1 - angle2) > 180.0) {
+// if (angle1 < angle2) {
+// angle1 += 360.0;
+// } else {
+// angle2 += 360.0;
+// }
+// }
+//
+// return angle1 - angle2;
- if (fabsf(angle1 - angle2) > 180.0) {
- if (angle1 < angle2) {
- angle1 += 360.0;
- } else {
- angle2 += 360.0;
- }
- }
-
- return angle1 - angle2;
+ // Faster in most cases
+ float difference = angle1 - angle2;
+ while (difference < -180.0) difference += 2.0 * 180.0;
+ while (difference > 180.0) difference -= 2.0 * 180.0;
+ return difference;
}
/**
- * Get the difference between two angles. Will always be between -180 and +180 degrees.
+ * Get the difference between two angles. Will always be between -pi and +pi radians.
* @param angle1
* The first angle in radians
* @param angle2
@@ -221,6 +227,73 @@
}
/**
+ * Get the smallest value of two values
+ *
+ * @param a
+ * First value
+ *
+ * @param b
+ * Second value
+ *
+ * @return
+ * The smallest value
+ */
+float utils_smallest_of_2(float a, float b) {
+ if (b < a) {
+ return b;
+ }
+ return a;
+}
+
+/**
+ * Get the smallest value of three values
+ *
+ * @param a
+ * First value
+ *
+ * @param b
+ * Second value
+ *
+ * @param c
+ * Third value
+ *
+ * @return
+ * The smallest value
+ */
+float utils_smallest_of_3(float a, float b, float c) {
+ if (b < a) {
+ if (c < b) {
+ return c;
+ }
+ return b;
+ } else {
+ if (c < a) {
+ return c;
+ }
+ return a;
+ }
+}
+
+/**
+ * Get the smallest value of two values
+ *
+ * @param a
+ * First value
+ *
+ * @param b
+ * Second value
+ *
+ * @return
+ * The smallest value
+ */
+float utils_highest_of_2(float a, float b) {
+ if (b > a) {
+ return b;
+ }
+ return a;
+}
+
+/**
* Get the middle value of three values
*
* @param a
diff -ru bldc-cc171422641a50c56452280575f2074f8a88aa45/utils.h Raptor2 Sources/bldc-firmware_2_80/utils.h
--- bldc-cc171422641a50c56452280575f2074f8a88aa45/utils.h 2015-12-29 01:37:09.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/utils.h 2017-04-13 18:07:22.000000000 +0100
@@ -39,6 +39,9 @@
float utils_angle_difference_rad(float angle1, float angle2);
float utils_avg_angles_rad_fast(float *angles, float *weights, int angles_num);
float utils_middle_of_3(float a, float b, float c);
+float utils_smallest_of_2(float a, float b);
+float utils_smallest_of_3(float a, float b, float c);
+float utils_highest_of_2(float a, float b);
int utils_middle_of_3_int(int a, int b, int c);
float utils_fast_inv_sqrt(float x);
float utils_fast_atan2(float y, float x);
Raw
diff_2.9.patch
Only in Raptor2 Sources/bldc-firmware_2_80: .dep
Only in bldc-90986c31defe50830b4795a388ee5b2687dc2516: .gitignore
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/Makefile Raptor2 Sources/bldc-firmware_2_80/Makefile
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/Makefile 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/Makefile 2016-09-19 10:07:06.000000000 +0100
@@ -270,8 +270,8 @@
# Program
upload: build/$(PROJECT).bin
#qstlink2 --cli --erase --write build/$(PROJECT).bin
- openocd -f interface/stlink-v2.cfg -c "set WORKAREASIZE 0x2000" -f target/stm32f4x_stlink.cfg -c "program build/$(PROJECT).elf verify reset"
- #openocd -f board/stm32f4discovery.cfg -c "reset_config trst_only combined" -c "program build/$(PROJECT).elf verify reset exit" # For openocd 0.9
+ #openocd -f interface/stlink-v2.cfg -c "set WORKAREASIZE 0x2000" -f target/stm32f4x_stlink.cfg -c "program build/$(PROJECT).elf verify reset" # Older openocd
+ openocd -f board/stm32f4discovery.cfg -c "reset_config trst_only combined" -c "program build/$(PROJECT).elf verify reset exit" # For openocd 0.9
#program with olimex arm-usb-tiny-h and jtag-swd adapter board. needs openocd>=0.9
upload-olimex: build/$(PROJECT).bin
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/README.md Raptor2 Sources/bldc-firmware_2_80/README.md
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/README.md 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/README.md 2018-08-27 20:57:58.000000000 +0100
@@ -1 +1,12 @@
-This is the source code for my custom BLDC controller. A complete description and tutorial about how to use it can be found here: http://vedder.se/2015/01/vesc-open-source-esc/
+A complete description and tutorial about how to use it can be found here: http://vedder.se/2015/01/vesc-open-source-esc/
+
+This is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+The software is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU General Public License for more details.
+You should have received a copy of the GNU General Public License
+along with this program. If not, see <http://www.gnu.org/licenses
\ No newline at end of file
Only in Raptor2 Sources/bldc-firmware_2_80/appconf: appconf_custom.h
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/appconf/appconf_default.h Raptor2 Sources/bldc-firmware_2_80/appconf/appconf_default.h
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/appconf/appconf_default.h 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/appconf/appconf_default.h 2017-05-31 20:43:38.000000000 +0100
@@ -26,36 +26,39 @@
#define APPCONF_TIMEOUT_MSEC 1000
#endif
#ifndef APPCONF_TIMEOUT_BRAKE_CURRENT
-#define APPCONF_TIMEOUT_BRAKE_CURRENT 0.0
+#define APPCONF_TIMEOUT_BRAKE_CURRENT 0.0
#endif
#ifndef APPCONF_SEND_CAN_STATUS
#define APPCONF_SEND_CAN_STATUS false
#endif
#ifndef APPCONF_SEND_CAN_STATUS_RATE_HZ
-#define APPCONF_SEND_CAN_STATUS_RATE_HZ 500
+#define APPCONF_SEND_CAN_STATUS_RATE_HZ 500
#endif
// The default app is UART in case the UART port is used for
// firmware updates.
#ifndef APPCONF_APP_TO_USE
-#define APPCONF_APP_TO_USE APP_UART
+#define APPCONF_APP_TO_USE APP_PPM_UART
#endif
// PPM app
#ifndef APPCONF_PPM_CTRL_TYPE
-#define APPCONF_PPM_CTRL_TYPE PPM_CTRL_TYPE_NONE
+#define APPCONF_PPM_CTRL_TYPE PPM_CTRL_TYPE_NONE //PPM_CTRL_TYPE_WATT_NOREV_BRAKE
#endif
#ifndef APPCONF_PPM_PID_MAX_ERPM
-#define APPCONF_PPM_PID_MAX_ERPM 15000
+#define APPCONF_PPM_PID_MAX_ERPM 60000
#endif
#ifndef APPCONF_PPM_HYST
-#define APPCONF_PPM_HYST 0.15
+#define APPCONF_PPM_HYST 0.05
#endif
#ifndef APPCONF_PPM_PULSE_START
-#define APPCONF_PPM_PULSE_START 1.0
+#define APPCONF_PPM_PULSE_START 1.04
+#endif
+#ifndef APPCONF_PPM_PULSE_CENTER
+#define APPCONF_PPM_PULSE_CENTER 1.54
#endif
#ifndef APPCONF_PPM_PULSE_END
-#define APPCONF_PPM_PULSE_END 2.0
+#define APPCONF_PPM_PULSE_END 2.04
#endif
#ifndef APPCONF_PPM_MEDIAN_FILTER
#define APPCONF_PPM_MEDIAN_FILTER true
@@ -64,19 +67,77 @@
#define APPCONF_PPM_SAFE_START true
#endif
#ifndef APPCONF_PPM_RPM_LIM_START
-#define APPCONF_PPM_RPM_LIM_START 150000
+#define APPCONF_PPM_RPM_LIM_START 55000
#endif
#ifndef APPCONF_PPM_RPM_LIM_END
-#define APPCONF_PPM_RPM_LIM_END 200000
+#define APPCONF_PPM_RPM_LIM_END 60000
#endif
#ifndef APPCONF_PPM_MULTI_ESC
-#define APPCONF_PPM_MULTI_ESC false
+#define APPCONF_PPM_MULTI_ESC true
#endif
#ifndef APPCONF_PPM_TC
-#define APPCONF_PPM_TC false
+#define APPCONF_PPM_TC true
#endif
#ifndef APPCONF_PPM_TC_MAX_DIFF
-#define APPCONF_PPM_TC_MAX_DIFF 3000.0
+#define APPCONF_PPM_TC_MAX_DIFF 5000.0
+#endif
+#ifndef APPCONF_PPM_TC_OFFSET
+#define APPCONF_PPM_TC_OFFSET 3000.0
+#endif
+#ifndef APPCONF_PPM_MAX_ERPM_FOR_DIR_ACTIVE
+#define APPCONF_PPM_MAX_ERPM_FOR_DIR_ACTIVE true
+#endif
+#ifndef APPCONF_PPM_MAX_ERPM_FOR_DIR
+#define APPCONF_PPM_MAX_ERPM_FOR_DIR 4000.0
+#endif
+
+#ifndef APPCONF_ADJUSTABLE_THROTTLE_ENABLED
+#define APPCONF_ADJUSTABLE_THROTTLE_ENABLED true
+#endif
+#ifndef APPCONF_Y1_THROTTLE
+#define APPCONF_Y1_THROTTLE 0.08
+#endif
+#ifndef APPCONF_Y2_THROTTLE
+#define APPCONF_Y2_THROTTLE 0.225
+#endif
+#ifndef APPCONF_Y3_THROTTLE
+#define APPCONF_Y3_THROTTLE 0.50
+#endif
+#ifndef APPCONF_X1_THROTTLE
+#define APPCONF_X1_THROTTLE 0.25
+#endif
+#ifndef APPCONF_X2_THROTTLE
+#define APPCONF_X2_THROTTLE 0.50
+#endif
+#ifndef APPCONF_X3_THROTTLE
+#define APPCONF_X3_THROTTLE 0.75
+#endif
+
+#ifndef APPCONF_BEZIER_REDUCE_FACTOR
+#define APPCONF_BEZIER_REDUCE_FACTOR 0
+#endif
+
+#ifndef APPCONF_Y1_NEG_THROTTLE
+#define APPCONF_Y1_NEG_THROTTLE 0.125
+#endif
+#ifndef APPCONF_Y2_NEG_THROTTLE
+#define APPCONF_Y2_NEG_THROTTLE 0.30
+#endif
+#ifndef APPCONF_Y3_NEG_THROTTLE
+#define APPCONF_Y3_NEG_THROTTLE 0.60
+#endif
+#ifndef APPCONF_X1_NEG_THROTTLE
+#define APPCONF_X1_NEG_THROTTLE 0.25
+#endif
+#ifndef APPCONF_X2_NEG_THROTTLE
+#define APPCONF_X2_NEG_THROTTLE 0.50
+#endif
+#ifndef APPCONF_X3_NEG_THROTTLE
+#define APPCONF_X3_NEG_THROTTLE 0.75
+#endif
+
+#ifndef APPCONF_BEZIER_REDUCE_NEG_FACTOR
+#define APPCONF_BEZIER_REDUCE_NEG_FACTOR 0
#endif
// ADC app configureation
@@ -84,7 +145,7 @@
#define APPCONF_ADC_CTRL_TYPE ADC_CTRL_TYPE_NONE
#endif
#ifndef APPCONF_ADC_HYST
-#define APPCONF_ADC_HYST 0.15
+#define APPCONF_ADC_HYST 0.15
#endif
#ifndef APPCONF_ADC_VOLTAGE_START
#define APPCONF_ADC_VOLTAGE_START 0.9
@@ -99,25 +160,25 @@
#define APPCONF_ADC_SAFE_START true
#endif
#ifndef APPCONF_ADC_CC_BUTTON_INVERTED
-#define APPCONF_ADC_CC_BUTTON_INVERTED false
+#define APPCONF_ADC_CC_BUTTON_INVERTED false
#endif
#ifndef APPCONF_ADC_REV_BUTTON_INVERTED
-#define APPCONF_ADC_REV_BUTTON_INVERTED false
+#define APPCONF_ADC_REV_BUTTON_INVERTED false
#endif
#ifndef APPCONF_ADC_VOLTAGE_INVERTED
-#define APPCONF_ADC_VOLTAGE_INVERTED false
+#define APPCONF_ADC_VOLTAGE_INVERTED false
#endif
#ifndef APPCONF_ADC_RPM_LIM_START
-#define APPCONF_ADC_RPM_LIM_START 150000.0
+#define APPCONF_ADC_RPM_LIM_START 2.0
#endif
#ifndef APPCONF_ADC_RPM_LIM_END
-#define APPCONF_ADC_RPM_LIM_END 200000.0
+#define APPCONF_ADC_RPM_LIM_END 80.0
#endif
#ifndef APPCONF_ADC_MULTI_ESC
#define APPCONF_ADC_MULTI_ESC false
#endif
#ifndef APPCONF_ADC_TC
-#define APPCONF_ADC_TC false
+#define APPCONF_ADC_TC false
#endif
#ifndef APPCONF_ADC_TC_MAX_DIFF
#define APPCONF_ADC_TC_MAX_DIFF 3000.0
@@ -128,7 +189,7 @@
// UART app
#ifndef APPCONF_UART_BAUDRATE
-#define APPCONF_UART_BAUDRATE 115200
+#define APPCONF_UART_BAUDRATE 9600
#endif
// Nunchuk app
@@ -136,7 +197,7 @@
#define APPCONF_CHUK_CTRL_TYPE CHUK_CTRL_TYPE_CURRENT
#endif
#ifndef APPCONF_CHUK_HYST
-#define APPCONF_CHUK_HYST 0.15
+#define APPCONF_CHUK_HYST 0.15
#endif
#ifndef APPCONF_CHUK_RPM_LIM_START
#define APPCONF_CHUK_RPM_LIM_START 150000.0
@@ -151,16 +212,54 @@
#define APPCONF_CHUK_RAMP_TIME_NEG 0.3
#endif
#ifndef APPCONF_STICK_ERPM_PER_S_IN_CC
-#define APPCONF_STICK_ERPM_PER_S_IN_CC 3000.0
+#define APPCONF_STICK_ERPM_PER_S_IN_CC 3000.0
#endif
#ifndef APPCONF_CHUK_MULTI_ESC
#define APPCONF_CHUK_MULTI_ESC false
#endif
#ifndef APPCONF_CHUK_TC
-#define APPCONF_CHUK_TC false
+#define APPCONF_CHUK_TC false
#endif
#ifndef APPCONF_CHUK_TC_MAX_DIFF
-#define APPCONF_CHUK_TC_MAX_DIFF 3000.0
+#define APPCONF_CHUK_TC_MAX_DIFF 5000.0
+#endif
+#ifndef APPCONF_CHUK_TC_OFFSET
+#define APPCONF_CHUK_TC_OFFSET 3000.0
+#endif
+#ifndef APPCONF_CHUK_BUTTONS_MIRRORED
+#define APPCONF_CHUK_BUTTONS_MIRRORED false
+#endif
+
+// NRF app
+#ifndef APPCONF_NRF_SPEED
+#define APPCONF_NRF_SPEED NRF_SPEED_2M
+#endif
+#ifndef APPCONF_NRF_POWER
+#define APPCONF_NRF_POWER NRF_POWER_0DBM
+#endif
+#ifndef APPCONF_NRF_CRC
+#define APPCONF_NRF_CRC NRF_CRC_1B
+#endif
+#ifndef APPCONF_NRF_RETR_DELAY
+#define APPCONF_NRF_RETR_DELAY NRF_RETR_DELAY_250US
+#endif
+#ifndef APPCONF_NRF_RETRIES
+#define APPCONF_NRF_RETRIES 3
+#endif
+#ifndef APPCONF_NRF_CHANNEL
+#define APPCONF_NRF_CHANNEL 76
+#endif
+#ifndef APPCONF_NRF_ADDR_B0
+#define APPCONF_NRF_ADDR_B0 0xC6
+#endif
+#ifndef APPCONF_NRF_ADDR_B1
+#define APPCONF_NRF_ADDR_B1 0xC7
+#endif
+#ifndef APPCONF_NRF_ADDR_B2
+#define APPCONF_NRF_ADDR_B2 0x0
+#endif
+#ifndef APPCONF_NRF_SEND_CRC_ACK
+#define APPCONF_NRF_SEND_CRC_ACK true
#endif
#endif /* APPCONF_DEFAULT_H_ */
Only in Raptor2 Sources/bldc-firmware_2_80/appconf: appconf_default_master.h
Only in Raptor2 Sources/bldc-firmware_2_80/appconf: appconf_default_slave.h
Only in bldc-90986c31defe50830b4795a388ee5b2687dc2516/appconf: appconf_example_ppm.h
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/applications/app.c Raptor2 Sources/bldc-firmware_2_80/applications/app.c
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/applications/app.c 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/applications/app.c 2016-11-16 19:08:14.000000000 +0000
@@ -26,6 +26,7 @@
#include "hal.h"
#include "hw.h"
#include "nrf_driver.h"
+#include "rfhelp.h"
// Private variables
static app_configuration appconf;
@@ -65,6 +66,7 @@
case APP_NRF:
nrf_driver_init();
+ rfhelp_restart();
break;
case APP_CUSTOM:
@@ -92,8 +94,9 @@
*/
void app_set_configuration(app_configuration *conf) {
appconf = *conf;
- app_ppm_configure(&appconf.app_ppm_conf);
+ app_ppm_configure(&appconf.app_ppm_conf, &appconf.app_throttle_conf);
app_adc_configure(&appconf.app_adc_conf);
app_uartcomm_configure(appconf.app_uart_baudrate);
- app_nunchuk_configure(&appconf.app_chuk_conf);
+ app_nunchuk_configure(&appconf.app_chuk_conf, &appconf.app_throttle_conf);
+ rfhelp_update_conf(&appconf.app_nrf_conf);
}
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/applications/app.h Raptor2 Sources/bldc-firmware_2_80/applications/app.h
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/applications/app.h 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/applications/app.h 2016-11-16 23:21:16.000000000 +0000
@@ -34,15 +34,17 @@
// Standard apps
void app_ppm_start(void);
-void app_ppm_configure(ppm_config *conf);
+void app_ppm_configure(ppm_config *conf, throttle_config *throt_config);
void app_adc_start(bool use_rx_tx);
void app_adc_configure(adc_config *conf);
float app_adc_get_decoded_level(void);
float app_adc_get_voltage(void);
+float app_adc_get_decoded_level2(void);
+float app_adc_get_voltage2(void);
void app_uartcomm_start(void);
void app_uartcomm_configure(uint32_t baudrate);
void app_nunchuk_start(void);
-void app_nunchuk_configure(chuk_config *conf);
+void app_nunchuk_configure(chuk_config *conf, throttle_config *throt_config);
float app_nunchuk_get_decoded_chuk(void);
void app_nunchuk_update_output(chuck_data *data);
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/applications/app_adc.c Raptor2 Sources/bldc-firmware_2_80/applications/app_adc.c
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/applications/app_adc.c 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/applications/app_adc.c 2016-09-19 10:07:22.000000000 +0100
@@ -49,6 +49,8 @@
static volatile float ms_without_power = 0;
static volatile float decoded_level = 0.0;
static volatile float read_voltage = 0.0;
+static volatile float decoded_level2 = 0.0;
+static volatile float read_voltage2 = 0.0;
static volatile bool use_rx_tx_as_buttons = false;
void app_adc_configure(adc_config *conf) {
@@ -69,6 +71,15 @@
return read_voltage;
}
+float app_adc_get_decoded_level2(void) {
+ return decoded_level2;
+}
+
+float app_adc_get_voltage2(void) {
+ return read_voltage2;
+}
+
+
static THD_FUNCTION(adc_thread, arg) {
(void)arg;
@@ -132,6 +143,46 @@
decoded_level = pwr;
+ // Read the external ADC pin and convert the value to a voltage.
+#ifdef ADC_IND_EXT2
+ float brake = (float)ADC_Value[ADC_IND_EXT2];
+ brake /= 4095;
+ brake *= V_REG;
+#else
+ float brake = 0.0;
+#endif
+
+ read_voltage2 = brake;
+
+ // Optionally apply a mean value filter
+ if (config.use_filter) {
+ static float filter_buffer2[FILTER_SAMPLES];
+ static int filter_ptr2 = 0;
+
+ filter_buffer2[filter_ptr2++] = brake;
+ if (filter_ptr2 >= FILTER_SAMPLES) {
+ filter_ptr2 = 0;
+ }
+
+ brake = 0.0;
+ for (int i = 0;i < FILTER_SAMPLES;i++) {
+ brake += filter_buffer2[i];
+ }
+ brake /= FILTER_SAMPLES;
+ }
+
+ // Map and truncate the read voltage
+ brake = utils_map(brake, config.voltage_start, config.voltage_end, 0.0, 1.0);
+ utils_truncate_number(&brake, 0.0, 1.0);
+
+ // Optionally invert the read voltage
+ if (config.voltage_inverted) {
+ brake = 1.0 - brake;
+ }
+
+ decoded_level2 = brake;
+
+
// Read the button pins
bool cc_button = false;
bool rev_button = false;
@@ -170,6 +221,13 @@
pwr -= 1.0;
break;
+ case ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_ADC:
+ if( brake > 0.01 ) {
+ pwr = -brake;
+ }
+
+ break;
+
case ADC_CTRL_TYPE_CURRENT_REV_BUTTON:
case ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_BUTTON:
case ADC_CTRL_TYPE_DUTY_REV_BUTTON:
@@ -211,6 +269,7 @@
case ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_CENTER:
case ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_BUTTON:
+ case ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_ADC:
current_mode = true;
if (pwr >= 0.0) {
current = pwr * mcconf->l_current_max;
@@ -232,7 +291,7 @@
}
if (!(ms_without_power < MIN_MS_WITHOUT_POWER && config.safe_start)) {
- mc_interface_set_duty(pwr);
+ mc_interface_set_duty(utils_map(pwr, -1.0, 1.0, -mcconf->l_max_duty, mcconf->l_max_duty));
send_duty = true;
}
break;
@@ -249,6 +308,17 @@
}
pulses_without_power_before = ms_without_power;
mc_interface_set_brake_current(timeout_get_brake_current());
+
+ if (config.multi_esc) {
+ for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+ can_status_msg *msg = comm_can_get_status_msg_index(i);
+
+ if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
+ comm_can_set_current_brake(msg->id, timeout_get_brake_current());
+ }
+ }
+ }
+
continue;
}
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/applications/app_nunchuk.c Raptor2 Sources/bldc-firmware_2_80/applications/app_nunchuk.c
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/applications/app_nunchuk.c 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/applications/app_nunchuk.c 2017-04-13 18:12:14.000000000 +0100
@@ -53,10 +53,71 @@
static volatile chuck_data chuck_d;
static volatile int chuck_error = 0;
static volatile chuk_config config;
+static volatile throttle_config throt_config;
+
static volatile bool output_running = false;
-void app_nunchuk_configure(chuk_config *conf) {
+static float px[5];
+static float py[5];
+static float nx[5];
+static float ny[5];
+
+float calculate_throttle_curve_chuk(float *x, float *y, float bezier_reduce_factor, float t) {
+
+ float directSteps;
+ if (t < x[1]){
+ directSteps = (y[1] / x[1] * t);
+ } else if (t > x[3]) {
+ directSteps = ((y[4] - y[3]) / (x[4] - x[3]) * (t-x[3]) + y[3]);
+ } else if (t > x[2]) {
+ directSteps = ((y[3] - y[2]) / (x[3] - x[2]) * (t-x[2]) + y[2]);
+ } else if (t > x[1]) {
+ directSteps = ((y[2] - y[1]) / (x[2] - x[1]) * (t-x[1]) + y[1]);
+ } else { // (throttle == x[1])
+ directSteps = y[1];
+ };
+
+ float f[5];
+ for (int i = 0; i < 5; i++) f[i] = y[i];
+
+ for (int j = 1; j < 5; j++ )
+ for (int i = 4; i >= j; i--)
+ f[i] = ( (t - x[i-j]) * f[i] - (t - x[i]) * f[i-1]) / (x[i] - x[i-j]);
+
+ float spline = f[4] - ((f[4] - directSteps) * bezier_reduce_factor);
+
+ // safety when stupid values are entered for x and y
+ if (spline > 1.0) return 1.0;
+ if (spline < 0.0) return 0.0;
+
+ return spline;
+}
+
+void app_nunchuk_configure(chuk_config *conf, throttle_config *throttle_conf) {
config = *conf;
+ throt_config = *throttle_conf;
+
+ px[0] = 0.0;
+ px[1] = throt_config.x1_throttle;
+ px[2] = throt_config.x2_throttle;
+ px[3] = throt_config.x3_throttle;
+ px[4] = 1.0;
+ py[0] = 0.0;
+ py[1] = throt_config.y1_throttle;
+ py[2] = throt_config.y2_throttle;
+ py[3] = throt_config.y3_throttle;
+ py[4] = 1.0;
+
+ nx[0] = 0.0;
+ nx[1] = throt_config.x1_neg_throttle;
+ nx[2] = throt_config.x2_neg_throttle;
+ nx[3] = throt_config.x3_neg_throttle;
+ nx[4] = 1.0;
+ ny[0] = 0.0;
+ ny[1] = throt_config.y1_neg_throttle;
+ ny[2] = throt_config.y2_neg_throttle;
+ ny[3] = throt_config.y3_neg_throttle;
+ ny[4] = 1.0;
}
void app_nunchuk_start(void) {
@@ -191,7 +252,7 @@
continue;
}
- if (chuck_d.bt_z && !was_z && config.ctrl_type == CHUK_CTRL_TYPE_CURRENT &&
+ if ((config.buttons_mirrored ? chuck_d.bt_c : chuck_d.bt_z) && !was_z && (config.ctrl_type == CHUK_CTRL_TYPE_CURRENT || config.ctrl_type == CHUK_CTRL_TYPE_WATT) &&
fabsf(current_now) < MAX_CURR_DIFFERENCE) {
if (is_reverse) {
is_reverse = false;
@@ -200,12 +261,20 @@
}
}
- was_z = chuck_d.bt_z;
+ was_z = config.buttons_mirrored ? chuck_d.bt_c : chuck_d.bt_z;
led_external_set_reversed(is_reverse);
float out_val = app_nunchuk_get_decoded_chuk();
utils_deadband(&out_val, config.hyst, 1.0);
+
+ if (throt_config.adjustable_throttle_enabled && out_val != 0.0){
+ if (out_val > 0.0) {
+ out_val = calculate_throttle_curve_chuk(px, py, throt_config.bezier_reduce_factor, out_val);
+ } else {
+ out_val = -calculate_throttle_curve_chuk(nx, ny, throt_config.bezier_neg_reduce_factor, -out_val);
+ }
+ }
// LEDs
float x_axis = ((float)chuck_d.js_x - 128.0) / 128.0;
@@ -245,7 +314,7 @@
}
rpm_filtered /= RPM_FILTER_SAMPLES;
- if (chuck_d.bt_c) {
+ if (config.buttons_mirrored ? chuck_d.bt_z : chuck_d.bt_c) {
static float pid_rpm = 0.0;
if (!was_pid) {
@@ -277,8 +346,21 @@
}
}
- mc_interface_set_pid_speed(pid_rpm);
-
+ // NEW
+ switch (config.ctrl_type) {
+ case CHUK_CTRL_TYPE_CURRENT:
+ case CHUK_CTRL_TYPE_CURRENT_NOREV:
+ mc_interface_set_pid_speed(pid_rpm);
+ break;
+ case CHUK_CTRL_TYPE_WATT:
+ case CHUK_CTRL_TYPE_WATT_NOREV:
+ mc_interface_set_pid_speed(pid_rpm);
+ break;
+ default:
+ break;
+ }
+ // END NEW
+
// Send the same duty cycle to the other controllers
if (config.multi_esc) {
float duty = mc_interface_get_duty_cycle_now();
@@ -302,12 +384,35 @@
was_pid = false;
float current = 0;
-
- if (out_val >= 0.0) {
- current = out_val * mcconf->l_current_max;
- } else {
- current = out_val * fabsf(mcconf->l_current_min);
+
+ // NEW
+ switch (config.ctrl_type) {
+ case CHUK_CTRL_TYPE_CURRENT:
+ case CHUK_CTRL_TYPE_CURRENT_NOREV:
+ if (out_val >= 0.0) {
+ current = out_val * mcconf->l_current_max;
+ } else {
+ current = out_val * fabsf(mcconf->l_current_min);
+ }
+ break;
+ case CHUK_CTRL_TYPE_WATT:
+ case CHUK_CTRL_TYPE_WATT_NOREV:
+ if (out_val >= 0.0) {
+ if (mcconf->use_max_watt_limit) {
+ current = utils_smallest_of_2(out_val * mcconf->l_current_max,
+ out_val * (mcconf->watts_max / GET_INPUT_VOLTAGE() / mc_interface_get_duty_cycle_for_watt_calculation()));
+ } else {
+ current = utils_smallest_of_2(out_val * mcconf->l_current_max,
+ out_val * mcconf->l_in_current_max / mc_interface_get_duty_cycle_for_watt_calculation());
+ }
+ } else {
+ current = out_val * fabsf(mcconf->l_current_min);
+ }
+ break;
+ default:
+ break;
}
+ // END NEW
// Find lowest RPM and highest current
float rpm_local = mc_interface_get_rpm();
@@ -334,7 +439,7 @@
// Make the current directional
float msg_current = msg->current;
- if (msg->duty < 0.0) {
+ if (msg->duty < 0) {
msg_current = -msg_current;
}
@@ -344,12 +449,17 @@
}
}
}
-
+
// Apply ramping
const float current_range = mcconf->l_current_max + fabsf(mcconf->l_current_min);
const float ramp_time = fabsf(current) > fabsf(prev_current) ? config.ramp_time_pos : config.ramp_time_neg;
-
- if (ramp_time > 0.01) {
+
+ if (ramp_time > 0.01 && current != 0.0) {
+
+ if (prev_current * current < 0.0) {
+ prev_current = 0.0;
+ }
+
const float ramp_step = ((float)OUTPUT_ITERATION_TIME_MS * current_range) / (ramp_time * 1000.0);
float current_goal = prev_current;
@@ -376,38 +486,55 @@
current_goal = goal_tmp2;
}
+ out_val = out_val / current * current_goal;
+
current = current_goal;
}
prev_current = current;
-
+
if (current < 0.0) {
+
mc_interface_set_brake_current(current);
- // Send brake command to all ESCs seen recently on the CAN bus
- for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
- can_status_msg *msg = comm_can_get_status_msg_index(i);
+ if (config.multi_esc) {
+ // Send brake command to all ESCs seen recently on the CAN bus
+ for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+ can_status_msg *msg = comm_can_get_status_msg_index(i);
- if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
- comm_can_set_current_brake(msg->id, current);
+ if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
+ if (config.ctrl_type == CHUK_CTRL_TYPE_WATT || config.ctrl_type == CHUK_CTRL_TYPE_WATT_NOREV) {
+ comm_can_set_brake_servo(msg->id, out_val);
+ } else {
+ comm_can_set_current_brake(msg->id, current);
+ }
+ }
}
}
} else {
+
+ bool use_min_current = false;
// Apply soft RPM limit
- if (rpm_lowest > config.rpm_lim_end && current > 0.0) {
- current = mcconf->cc_min_current;
- } else if (rpm_lowest > config.rpm_lim_start && current > 0.0) {
- current = utils_map(rpm_lowest, config.rpm_lim_start, config.rpm_lim_end, current, mcconf->cc_min_current);
- } else if (rpm_lowest < -config.rpm_lim_end && current < 0.0) {
- current = mcconf->cc_min_current;
- } else if (rpm_lowest < -config.rpm_lim_start && current < 0.0) {
- rpm_lowest = -rpm_lowest;
- current = -current;
- current = utils_map(rpm_lowest, config.rpm_lim_start, config.rpm_lim_end, current, mcconf->cc_min_current);
- current = -current;
+ if (rpm_lowest > config.rpm_lim_end) {
+ if (out_val > 0.0) {
+ use_min_current = true;
+ out_val = 0.000001; // make sure min current ius used
+ }
+ if (current > 0.0) {
+ current = mcconf->cc_min_current;
+ }
+ } else if (rpm_lowest > config.rpm_lim_start) {
+ if (out_val > 0.0) {
+ use_min_current = true;
+ out_val = utils_map(rpm_lowest, config.rpm_lim_start, config.rpm_lim_end, out_val, 0.000001);
+ }
+ if (current > 0.0) {
+ current = utils_highest_of_2(utils_map(rpm_lowest, config.rpm_lim_start, config.rpm_lim_end, current, 0.0), mcconf->cc_min_current);
+ }
}
float current_out = current;
+ float servo_val_out = out_val;
// Traction control
if (config.multi_esc) {
@@ -422,25 +549,39 @@
}
float diff = rpm_tmp - rpm_lowest;
- current_out = utils_map(diff, 0.0, config.tc_max_diff, current, 0.0);
- if (current_out < mcconf->cc_min_current) {
- current_out = 0.0;
+
+ if (diff > config.tc_offset) {
+ current_out = utils_map(diff - config.tc_offset, 0.0, config.tc_max_diff - config.tc_offset, current, 0.0);
+ servo_val_out = utils_map(diff - config.tc_offset, 0.0, config.tc_max_diff - config.tc_offset, out_val, 0.0);
+ } else {
+ current_out = current;
+ servo_val_out = out_val;
}
}
if (is_reverse) {
- comm_can_set_current(msg->id, -current_out);
+ if (config.ctrl_type == CHUK_CTRL_TYPE_WATT || config.ctrl_type == CHUK_CTRL_TYPE_WATT_NOREV) {
+ comm_can_set_servo(msg->id, -servo_val_out, use_min_current);
+ } else {
+ comm_can_set_current(msg->id, -current_out);
+ }
} else {
- comm_can_set_current(msg->id, current_out);
+ if (config.ctrl_type == CHUK_CTRL_TYPE_WATT || config.ctrl_type == CHUK_CTRL_TYPE_WATT_NOREV) {
+ comm_can_set_servo(msg->id, servo_val_out, use_min_current);
+ } else {
+ comm_can_set_current(msg->id, current_out);
+ }
}
}
}
if (config.tc) {
float diff = rpm_local - rpm_lowest;
- current_out = utils_map(diff, 0.0, config.tc_max_diff, current, 0.0);
- if (current_out < mcconf->cc_min_current) {
- current_out = 0.0;
+
+ if (diff > config.tc_offset) {
+ current_out = utils_map(diff - config.tc_offset, 0.0, config.tc_max_diff - config.tc_offset, current, 0.0);
+ } else {
+ current_out = current;
}
}
}
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/applications/app_ppm.c Raptor2 Sources/bldc-firmware_2_80/applications/app_ppm.c
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/applications/app_ppm.c 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/applications/app_ppm.c 2017-04-13 18:12:22.000000000 +0100
@@ -26,6 +26,7 @@
#include "ch.h"
#include "hal.h"
+#include "hw.h"
#include "stm32f4xx_conf.h"
#include "servo_dec.h"
#include "mc_interface.h"
@@ -33,13 +34,15 @@
#include "utils.h"
#include "comm_can.h"
#include <math.h>
+//#include "commands.h"
// Only available if servo output is not active
#if !SERVO_OUT_ENABLE
// Settings
-#define MAX_CAN_AGE 0.1
-#define MIN_PULSES_WITHOUT_POWER 50
+#define MAX_CAN_AGE 0.1
+#define MIN_PULSES_WITHOUT_POWER 50
+#define RPM_FILTER_SAMPLES 8
// Threads
static THD_FUNCTION(ppm_thread, arg);
@@ -52,21 +55,96 @@
// Private variables
static volatile bool is_running = false;
+static volatile float pid_rpm = 0;
+//static volatile int mode_switch_pulses = 0;
static volatile ppm_config config;
+static volatile throttle_config throt_config;
static volatile int pulses_without_power = 0;
+//static volatile int count = 0;
+
+static volatile float filter_buffer[RPM_FILTER_SAMPLES];
+static volatile int filter_ptr = 0;
+static volatile bool has_enough_pid_filter_data = false;
+static volatile float direction_hyst = 0;
+
+static float px[5];
+static float py[5];
+static float nx[5];
+static float ny[5];
+
// Private functions
static void update(void *p);
#endif
-void app_ppm_configure(ppm_config *conf) {
+float calculate_throttle_curve_ppm(float *x, float *y, float bezier_reduce_factor, float t) {
+
+ float directSteps;
+ if (t < x[1]){
+ directSteps = (y[1] / x[1] * t);
+ } else if (t > x[3]) {
+ directSteps = ((y[4] - y[3]) / (x[4] - x[3]) * (t-x[3]) + y[3]);
+ } else if (t > x[2]) {
+ directSteps = ((y[3] - y[2]) / (x[3] - x[2]) * (t-x[2]) + y[2]);
+ } else if (t > x[1]) {
+ directSteps = ((y[2] - y[1]) / (x[2] - x[1]) * (t-x[1]) + y[1]);
+ } else { // (throttle == x[1])
+ directSteps = y[1];
+ };
+
+ float f[5];
+ for (int i = 0; i < 5; i++) f[i] = y[i];
+
+ for (int j = 1; j < 5; j++ )
+ for (int i = 4; i >= j; i--)
+ f[i] = ( (t - x[i-j]) * f[i] - (t - x[i]) * f[i-1]) / (x[i] - x[i-j]);
+
+ float spline = f[4] - ((f[4] - directSteps) * bezier_reduce_factor);
+
+ // safety when stupid values are entered for x and y
+ if (spline > 1.0) return 1.0;
+ if (spline < 0.0) return 0.0;
+
+ return spline;
+}
+
+void app_ppm_configure(ppm_config *conf, throttle_config *throttle_conf) {
#if !SERVO_OUT_ENABLE
config = *conf;
+ throt_config = *throttle_conf;
pulses_without_power = 0;
+
+ has_enough_pid_filter_data = false;
+ filter_ptr = 0;
+ mc_interface_set_cruise_control_status(CRUISE_CONTROL_INACTIVE);
if (is_running) {
- servodec_set_pulse_options(config.pulse_start, config.pulse_end, config.median_filter);
+ servodec_set_pulse_options(config.pulse_start, config.pulse_center, config.pulse_end, config.median_filter);
}
+
+ px[0] = 0.0;
+ px[1] = throt_config.x1_throttle;
+ px[2] = throt_config.x2_throttle;
+ px[3] = throt_config.x3_throttle;
+ px[4] = 1.0;
+ py[0] = 0.0;
+ py[1] = throt_config.y1_throttle;
+ py[2] = throt_config.y2_throttle;
+ py[3] = throt_config.y3_throttle;
+ py[4] = 1.0;
+
+ nx[0] = 0.0;
+ nx[1] = throt_config.x1_neg_throttle;
+ nx[2] = throt_config.x2_neg_throttle;
+ nx[3] = throt_config.x3_neg_throttle;
+ nx[4] = 1.0;
+ ny[0] = 0.0;
+ ny[1] = throt_config.y1_neg_throttle;
+ ny[2] = throt_config.y2_neg_throttle;
+ ny[3] = throt_config.y3_neg_throttle;
+ ny[4] = 1.0;
+
+ direction_hyst = config.max_erpm_for_dir * 0.20;
#else
(void)conf;
#endif
@@ -103,8 +181,9 @@
chRegSetThreadName("APP_PPM");
ppm_tp = chThdGetSelfX();
- servodec_set_pulse_options(config.pulse_start, config.pulse_end, config.median_filter);
+ servodec_set_pulse_options(config.pulse_start, config.pulse_center, config.pulse_end, config.median_filter);
servodec_init(servodec_func);
+
is_running = true;
for(;;) {
@@ -124,26 +203,99 @@
case PPM_CTRL_TYPE_PID_NOREV:
servo_val += 1.0;
servo_val /= 2.0;
+
+ utils_deadband(&servo_val, config.hyst, 1.0);
+
+ servo_val = calculate_throttle_curve_ppm(px, py, throt_config.bezier_reduce_factor, servo_val);
+
+ break;
+ //case PPM_CTRL_TYPE_CURRENT:
+ case PPM_CTRL_TYPE_WATT:
+ utils_deadband(&servo_val, config.hyst, 1.0);
+
+ if (throt_config.adjustable_throttle_enabled && servo_val != 0.0){
+ if (servo_val > 0.0) {
+ servo_val = calculate_throttle_curve_ppm(px, py, throt_config.bezier_reduce_factor, servo_val);
+ } else if (config.max_erpm_for_dir_active == false) {
+ servo_val = -calculate_throttle_curve_ppm(nx, ny, throt_config.bezier_neg_reduce_factor, -servo_val);
+ }
+ }
break;
-
default:
+
+ utils_deadband(&servo_val, config.hyst, 1.0);
+
+ if (throt_config.adjustable_throttle_enabled && servo_val != 0.0){
+ if (servo_val > 0.0) {
+ servo_val = calculate_throttle_curve_ppm(px, py, throt_config.bezier_reduce_factor, servo_val);
+ } else {
+ servo_val = -calculate_throttle_curve_ppm(nx, ny, throt_config.bezier_neg_reduce_factor, -servo_val);
+ }
+ }
+
break;
}
- utils_deadband(&servo_val, config.hyst, 1.0);
-
float current = 0;
+ //float desired_watt = 0;
bool current_mode = false;
bool current_mode_brake = false;
const volatile mc_configuration *mcconf = mc_interface_get_configuration();
bool send_duty = false;
+ //bool send_watt = false;
+ bool send_pid = false;
+
+ // Find lowest RPM and cruise control
+ float rpm_local = mc_interface_get_rpm();
+ float rpm_lowest = rpm_local;
+ float mid_rpm = rpm_local;
+ int motor_count = 1;
+ ppm_cruise cruise_control_status = CRUISE_CONTROL_INACTIVE;
+ if (config.multi_esc) {
+ for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+ can_status_msg *msg = comm_can_get_status_msg_index(i);
+
+ if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
+ // add to middle rpm count
+ mid_rpm += msg->rpm;
+ motor_count += 1;
+ if (fabsf(msg->rpm) < fabsf(rpm_lowest)) {
+ rpm_lowest = msg->rpm;
+ }
+
+ // if any controller (VESC) sends the cruise contol status
+ if (msg->cruise_control_status != CRUISE_CONTROL_INACTIVE) {
+ cruise_control_status = msg->cruise_control_status;
+ }
+ }
+ }
+ }
+
+ // get middle rpm
+ mid_rpm /= motor_count;
+
switch (config.ctrl_type) {
case PPM_CTRL_TYPE_CURRENT:
- case PPM_CTRL_TYPE_CURRENT_NOREV:
current_mode = true;
if (servo_val >= 0.0) {
- current = servo_val * mcconf->l_current_max;
+ // check of can bus send cruise control command
+ if (cruise_control_status != CRUISE_CONTROL_INACTIVE && servo_val == 0.0) {
+ // is rpm in range for cruise control
+ if (rpm_lowest > mcconf->s_pid_min_erpm) {
+ if (pid_rpm == 0) {
+ pid_rpm = rpm_lowest;
+ }
+ current_mode = false;
+ send_pid = true;
+ mc_interface_set_pid_speed_with_cruise_status(rpm_local + pid_rpm - mid_rpm, cruise_control_status);
+ } else {
+ pid_rpm = 0;
+ current = 0.0;
+ }
+ }else{
+ current = servo_val * mcconf->l_current_max;
+ }
} else {
current = servo_val * fabsf(mcconf->l_current_min);
}
@@ -152,21 +304,294 @@
pulses_without_power++;
}
break;
-
+ case PPM_CTRL_TYPE_CURRENT_NOREV:
case PPM_CTRL_TYPE_CURRENT_NOREV_BRAKE:
current_mode = true;
+
if (servo_val >= 0.0) {
- current = servo_val * mcconf->l_current_max;
+ // check of can bus send cruise control command
+ if (cruise_control_status != CRUISE_CONTROL_INACTIVE && servo_val == 0.0) {
+ // is rpm in range for cruise control
+ if (rpm_lowest > mcconf->s_pid_min_erpm) {
+ if (pid_rpm == 0) {
+ pid_rpm = rpm_lowest;
+ }
+ current_mode = false;
+ send_pid = true;
+ mc_interface_set_pid_speed_with_cruise_status(rpm_local + pid_rpm - mid_rpm, cruise_control_status);
+ } else {
+ pid_rpm = 0;
+ current = 0.0;
+ }
+ }else{
+ current = servo_val * mcconf->l_current_max;
+ }
+
} else {
current = fabsf(servo_val * mcconf->l_current_min);
current_mode_brake = true;
}
+ if (servo_val < 0.001) {
+ pulses_without_power++;
+ }
+ break;
+ case PPM_CTRL_TYPE_WATT:
+ current_mode = true;
+
+ if (config.max_erpm_for_dir_active) { // advanced backwards
+ static bool force_brake = true;
+
+ static int8_t did_idle_once = 0;
+
+ // Hysteresis 20 % of actual RPM
+ if (force_brake) {
+ if (rpm_local < config.max_erpm_for_dir - direction_hyst) { // for 2500 it's 2000
+ force_brake = false;
+ did_idle_once = 0;
+ }
+ } else {
+ if (rpm_local > config.max_erpm_for_dir + direction_hyst) { // for 2500 it's 3000
+ force_brake = true;
+ did_idle_once = 0;
+ }
+ }
+
+ if (servo_val >= 0.0) {
+ if (servo_val == 0.0) {
+ // if there was a idle in between then allow going backwards
+ if (did_idle_once == 1 && !force_brake) {
+ did_idle_once = 2;
+ }
+ }else{
+ // accelerated forward or fast enough at least
+ if (rpm_local > -config.max_erpm_for_dir){ // for 2500 it's -2500
+ did_idle_once = 0;
+ }
+ }
+
+ // check of can bus send cruise control command
+ if (cruise_control_status != CRUISE_CONTROL_INACTIVE && servo_val == 0.0) {
+ // is rpm in range for cruise control
+ if (fabsf(rpm_lowest) > mcconf->s_pid_min_erpm) {
+ if (pid_rpm == 0) {
+ pid_rpm = mid_rpm;
+ }
+ current_mode = false;
+ send_pid = true;
+
+ mc_interface_set_pid_speed_with_cruise_status(rpm_local + pid_rpm - mid_rpm, cruise_control_status);
+ } else {
+ pid_rpm = 0;
+ current = 0.0;
+ }
+ }else{
+ if (mcconf->use_max_watt_limit) {
+ current = utils_smallest_of_2(servo_val * mcconf->l_current_max,
+ servo_val * (mcconf->watts_max / GET_INPUT_VOLTAGE() / mc_interface_get_duty_cycle_for_watt_calculation()));
+ } else {
+ current = utils_smallest_of_2(servo_val * mcconf->l_current_max,
+ servo_val * mcconf->l_in_current_max / mc_interface_get_duty_cycle_for_watt_calculation());
+ }
+ }
+ } else {
+
+ // too fast
+ if (force_brake){
+ current_mode_brake = true;
+ }else{
+ // not too fast backwards
+ if (rpm_local > -config.max_erpm_for_dir) { // for 2500 it's -2500
+ // first time that we brake and we are not too fast
+ if (did_idle_once != 2) {
+ did_idle_once = 1;
+ current_mode_brake = true;
+ }
+ // too fast backwards
+ } else {
+ // if brake was active already
+ if (did_idle_once == 1) {
+ current_mode_brake = true;
+ } else {
+ // it's ok to go backwards now braking would be strange now
+ did_idle_once = 2;
+ }
+ }
+ }
+
+ if (current_mode_brake) {
+ // braking
+ if (throt_config.adjustable_throttle_enabled) {
+ // negative calculation
+ servo_val = -calculate_throttle_curve_ppm(nx, ny, throt_config.bezier_neg_reduce_factor, -servo_val);
+ }
+ current = fabsf(servo_val * mcconf->l_current_min);
+ }else {
+ // reverse acceleration
+ if (throt_config.adjustable_throttle_enabled) {
+ // positive calculation
+ servo_val = -calculate_throttle_curve_ppm(px, py, throt_config.bezier_reduce_factor, -servo_val);
+ }
+
+ if (mcconf->use_max_watt_limit) {
+ current = -utils_smallest_of_2(fabsf(servo_val * mcconf->l_current_min),
+ fabsf(servo_val * (mcconf->watts_max / GET_INPUT_VOLTAGE() / mc_interface_get_duty_cycle_for_watt_calculation())));
+ } else {
+ current = -utils_smallest_of_2(fabsf(servo_val * mcconf->l_current_min),
+ fabsf(servo_val * mcconf->l_in_current_max / mc_interface_get_duty_cycle_for_watt_calculation()));
+ }
+ }
+ }
+ } else { // normal backwards
+ if (servo_val >= 0.0) {
+ // check of can bus send cruise control command
+ if (cruise_control_status != CRUISE_CONTROL_INACTIVE && servo_val == 0.0) {
+ // is rpm in range for cruise control
+ if (fabsf(rpm_lowest) > mcconf->s_pid_min_erpm) {
+ if (pid_rpm == 0) {
+ pid_rpm = mid_rpm;
+ }
+ current_mode = false;
+ send_pid = true;
+
+ mc_interface_set_pid_speed_with_cruise_status(rpm_local + pid_rpm - mid_rpm, cruise_control_status);
+ } else {
+ pid_rpm = 0;
+ current = 0.0;
+ }
+ }else{
+ if (mcconf->use_max_watt_limit) {
+ current = utils_smallest_of_2(servo_val * mcconf->l_current_max,
+ servo_val * (mcconf->watts_max / GET_INPUT_VOLTAGE() / mc_interface_get_duty_cycle_for_watt_calculation()));
+ } else {
+ current = utils_smallest_of_2(servo_val * mcconf->l_current_max,
+ servo_val * mcconf->l_in_current_max / mc_interface_get_duty_cycle_for_watt_calculation());
+ }
+ }
+ } else {
+ current = servo_val * fabsf(mcconf->l_current_min);
+ }
+ }
if (servo_val < 0.001) {
pulses_without_power++;
}
break;
+ case PPM_CTRL_TYPE_WATT_NOREV_BRAKE:
+ current_mode = true;
+
+ if (servo_val >= 0.0) {
+ // check of can bus send cruise control command
+ if (cruise_control_status != CRUISE_CONTROL_INACTIVE && servo_val == 0.0) {
+ // is rpm in range for cruise control
+ if (rpm_lowest > mcconf->s_pid_min_erpm) {
+ if (pid_rpm == 0) {
+ pid_rpm = mid_rpm;
+ }
+ current_mode = false;
+ send_pid = true;
+ mc_interface_set_pid_speed_with_cruise_status(rpm_local + pid_rpm - mid_rpm, cruise_control_status);
+
+ } else {
+ pid_rpm = 0;
+ current = 0.0;
+ }
+ }else{
+ if (mcconf->use_max_watt_limit) {
+ current = utils_smallest_of_2(servo_val * mcconf->l_current_max,
+ servo_val * (mcconf->watts_max / GET_INPUT_VOLTAGE() / mc_interface_get_duty_cycle_for_watt_calculation()));
+ } else {
+ current = utils_smallest_of_2(servo_val * mcconf->l_current_max,
+ servo_val * mcconf->l_in_current_max / mc_interface_get_duty_cycle_for_watt_calculation());
+ }
+ }
+ } else {
+
+ current_mode_brake = true;
+
+ // brake as in current mode if not enabled. Calculating via Battery Voltage would brake far too hard
+ current = fabsf(servo_val * mcconf->l_current_min);
+ }
+ if (servo_val < 0.001) {
+ pulses_without_power++;
+ }
+ break;
+ case PPM_CTRL_TYPE_PID_NOACCELERATION:
+ current_mode = true;
+
+ filter_buffer[filter_ptr++] = mid_rpm;
+ if (filter_ptr >= RPM_FILTER_SAMPLES) {
+ filter_ptr = 0;
+ has_enough_pid_filter_data = true;
+ }
+
+ float rpm_filtered = 0.0;
+ // only send when enough values are collected
+ if (has_enough_pid_filter_data) {
+ for (int i = 0; i < RPM_FILTER_SAMPLES; i++) {
+ rpm_filtered += filter_buffer[i];
+ }
+ rpm_filtered /= RPM_FILTER_SAMPLES;
+ }
+
+ if (servo_val >= 0.0) {
+ // check if pid needs to be lowered
+ if (servo_val > 0.0) {
+ // needs to be set first ?
+ if (pid_rpm == 0) {
+ pid_rpm = rpm_filtered;
+ }
+
+ if(rpm_filtered > 1000){
+ float diff = pid_rpm - rpm_filtered;
+ if (diff > 1500) {
+ pid_rpm = pid_rpm - 10;
+ }else if(diff > 500 && rpm_filtered < 1500) {
+ pid_rpm = pid_rpm - 10;
+ }
+ }else{
+ pid_rpm = 0;
+ }
+
+ // if not too slow than set the pid
+ if(pid_rpm > 0 && pid_rpm < config.pid_max_erpm) {
+ current_mode = false;
+
+ send_pid = true;
+ mc_interface_set_pid_speed(rpm_local + pid_rpm - rpm_filtered);
+
+ // overwrite mid_rpm
+ mid_rpm = rpm_filtered;
+ } else {
+ current = 0.0;
+ }
+ }else{
+ current = 0.0;
+ }
+ } else {
+ current = fabsf(servo_val * mcconf->l_current_min);
+ current_mode_brake = true;
+ }
+
+ if (servo_val < 0.001) {
+ pulses_without_power++;
+ }
+ break;
+ case PPM_CTRL_TYPE_CRUISE_CONTROL_SECONDARY_CHANNEL:
+ if (servo_val < -0.3 && config.cruise_left != CRUISE_CONTROL_INACTIVE) {
+ mc_interface_set_cruise_control_status(config.cruise_left);
+ } else if (servo_val > 0.3 && config.cruise_right != CRUISE_CONTROL_INACTIVE) {
+ mc_interface_set_cruise_control_status(config.cruise_right);
+ } else {
+ mc_interface_set_cruise_control_status(CRUISE_CONTROL_INACTIVE);
+ }
+ // Run this loop at 500Hz
+ chThdSleepMilliseconds(2);
+
+ // Reset the timeout
+ //timeout_reset();
+ continue;
+ break;
case PPM_CTRL_TYPE_DUTY:
case PPM_CTRL_TYPE_DUTY_NOREV:
if (fabsf(servo_val) < 0.001) {
@@ -174,7 +599,7 @@
}
if (!(pulses_without_power < MIN_PULSES_WITHOUT_POWER && config.safe_start)) {
- mc_interface_set_duty(servo_val);
+ mc_interface_set_duty(utils_map(servo_val, -1.0, 1.0, -mcconf->l_max_duty, mcconf->l_max_duty));
send_duty = true;
}
break;
@@ -194,6 +619,17 @@
default:
continue;
}
+
+ // switch the mode
+ /*if (servo_val == -1.0){
+ mode_switch_pulses++;
+ }else{
+ if (mode_switch_pulses > 1000){
+ //switch mode
+ //mc_interface_change_speed_mode(); // ???
+ }
+ mode_switch_pulses = 0;
+ }*/
if (pulses_without_power < MIN_PULSES_WITHOUT_POWER && config.safe_start) {
static int pulses_without_power_before = 0;
@@ -205,69 +641,99 @@
continue;
}
- // Find lowest RPM
- float rpm_local = mc_interface_get_rpm();
- float rpm_lowest = rpm_local;
- if (config.multi_esc) {
+ if (send_duty && config.multi_esc) {
+ float duty = mc_interface_get_duty_cycle_now();
+
for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
can_status_msg *msg = comm_can_get_status_msg_index(i);
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
- float rpm_tmp = msg->rpm;
-
- if (fabsf(rpm_tmp) < fabsf(rpm_lowest)) {
- rpm_lowest = rpm_tmp;
- }
+ comm_can_set_duty(msg->id, duty);
}
}
}
- if (send_duty && config.multi_esc) {
- float duty = mc_interface_get_duty_cycle_now();
-
+ if (send_pid && config.multi_esc) {
+
for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
can_status_msg *msg = comm_can_get_status_msg_index(i);
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
- comm_can_set_duty(msg->id, duty);
+ comm_can_set_rpm(msg->id, msg->rpm + pid_rpm - mid_rpm, cruise_control_status);
}
}
}
if (current_mode) {
+
+ pid_rpm = 0; // always reset in current, not that something stupid happens
+
if (current_mode_brake) {
mc_interface_set_brake_current(current);
- // Send brake command to all ESCs seen recently on the CAN bus
- for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
- can_status_msg *msg = comm_can_get_status_msg_index(i);
+ if (config.multi_esc) {
+ // Send brake command to all ESCs seen recently on the CAN bus
+ for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+ can_status_msg *msg = comm_can_get_status_msg_index(i);
- if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
- comm_can_set_current_brake(msg->id, current);
+ if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
+ if (config.ctrl_type == PPM_CTRL_TYPE_WATT_NOREV_BRAKE || config.ctrl_type == PPM_CTRL_TYPE_WATT) {
+ comm_can_set_brake_servo(msg->id, fabsf(servo_val));
+ } else {
+ comm_can_set_current_brake(msg->id, current);
+ }
+ }
}
}
} else {
// Apply soft RPM limit
- if (rpm_lowest > config.rpm_lim_end && current > 0.0) {
- current = mcconf->cc_min_current;
- } else if (rpm_lowest > config.rpm_lim_start && current > 0.0) {
- current = utils_map(rpm_lowest, config.rpm_lim_start, config.rpm_lim_end, current, mcconf->cc_min_current);
- } else if (rpm_lowest < -config.rpm_lim_end && current < 0.0) {
- current = mcconf->cc_min_current;
- } else if (rpm_lowest < -config.rpm_lim_start && current < 0.0) {
- rpm_lowest = -rpm_lowest;
- current = -current;
- current = utils_map(rpm_lowest, config.rpm_lim_start, config.rpm_lim_end, current, mcconf->cc_min_current);
- current = -current;
- rpm_lowest = -rpm_lowest;
- }
+ bool use_min_current = false;
+ if (rpm_lowest > config.rpm_lim_end) {
+ if (servo_val > 0.0) {
+ use_min_current = true;
+ servo_val = 0.000001; // make sure min current ius used
+ }
+ if (current > 0.0) {
+ current = mcconf->cc_min_current;
+ }
+
+ } else if (rpm_lowest > config.rpm_lim_start) {
+ if (servo_val > 0.0) {
+ use_min_current = true;
+ servo_val = utils_map(rpm_lowest, config.rpm_lim_start, config.rpm_lim_end, servo_val, 0.000001);
+ }
+ if (current > 0.0) {
+ current = utils_highest_of_2(utils_map(rpm_lowest, config.rpm_lim_start, config.rpm_lim_end, current, 0.0), mcconf->cc_min_current);
+ }
+ } else if (rpm_lowest < -config.rpm_lim_end) {
+ if (servo_val < 0.0) {
+ use_min_current = true;
+ servo_val = -0.000001; // make sure min current ius used
+ }
+ if (current < 0.0) {
+ current = -mcconf->cc_min_current; // wrong in original
+ }
+
+ } else if (rpm_lowest < -config.rpm_lim_start) {
+ if (servo_val < 0.0) {
+ use_min_current = true;
+ servo_val = -utils_map(-rpm_lowest, config.rpm_lim_start, config.rpm_lim_end, -servo_val, 0.000001);
+ }
+ if (current < 0.0) {
+ current = utils_smallest_of_2(-utils_map(-rpm_lowest, config.rpm_lim_start, config.rpm_lim_end, -current, 0.0), -mcconf->cc_min_current);
+ }
+ }
+
float current_out = current;
+ float servo_val_out = servo_val;
bool is_reverse = false;
- if (current_out < 0.0) {
+ if (servo_val < 0.0) {
is_reverse = true;
current_out = -current_out;
current = -current;
+ servo_val_out = -servo_val_out;
+ servo_val = -servo_val;
rpm_local = -rpm_local;
rpm_lowest = -rpm_lowest;
}
@@ -285,25 +751,38 @@
}
float diff = rpm_tmp - rpm_lowest;
- current_out = utils_map(diff, 0.0, config.tc_max_diff, current, 0.0);
- if (current_out < mcconf->cc_min_current) {
- current_out = 0.0;
+ if (diff > config.tc_offset) {
+ current_out = utils_map(diff - config.tc_offset, 0.0, config.tc_max_diff - config.tc_offset, current, 0.0);
+ servo_val_out = utils_map(diff - config.tc_offset, 0.0, config.tc_max_diff - config.tc_offset, servo_val, 0.0);
+ } else {
+ current_out = current;
+ servo_val_out = servo_val;
}
}
if (is_reverse) {
- comm_can_set_current(msg->id, -current_out);
+ if (config.ctrl_type == PPM_CTRL_TYPE_WATT_NOREV_BRAKE || (config.ctrl_type == PPM_CTRL_TYPE_WATT && config.max_erpm_for_dir_active)) { // if not active you want real brakes
+ comm_can_set_servo(msg->id, -servo_val_out, use_min_current);
+ } else {
+ comm_can_set_current(msg->id, -current_out);
+ }
} else {
- comm_can_set_current(msg->id, current_out);
+ if (config.ctrl_type == PPM_CTRL_TYPE_WATT_NOREV_BRAKE || config.ctrl_type == PPM_CTRL_TYPE_WATT) {
+ comm_can_set_servo(msg->id, servo_val_out, use_min_current);
+ } else {
+ comm_can_set_current(msg->id, current_out);
+ }
}
}
}
if (config.tc) {
+
float diff = rpm_local - rpm_lowest;
- current_out = utils_map(diff, 0.0, config.tc_max_diff, current, 0.0);
- if (current_out < mcconf->cc_min_current) {
- current_out = 0.0;
+ if (diff > config.tc_offset) {
+ current_out = utils_map(diff - config.tc_offset, 0.0, config.tc_max_diff - config.tc_offset, current, 0.0);
+ } else {
+ current_out = current;
}
}
}
@@ -315,7 +794,6 @@
}
}
}
-
}
}
#endif
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/applications/app_sten.c Raptor2 Sources/bldc-firmware_2_80/applications/app_sten.c
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/applications/app_sten.c 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/applications/app_sten.c 2016-09-19 10:07:18.000000000 +0100
@@ -32,6 +32,8 @@
#include "utils.h"
#include "hw.h"
#include "timeout.h"
+#include "comm_can.h"
+
#include <math.h>
// Settings
@@ -147,15 +149,7 @@
}
static void set_output(float output) {
- output /= (1.0 - HYST);
-
- if (output > HYST) {
- output -= HYST;
- } else if (output < -HYST) {
- output += HYST;
- } else {
- output = 0.0;
- }
+ utils_deadband(&output, HYST, 1.0);
const float rpm = mc_interface_get_rpm();
@@ -182,13 +176,11 @@
current_p2 = current_p1;
current_p1 = current;
- if (fabsf(current) < mc_interface_get_configuration()->cc_min_current) {
- current = -mc_interface_get_configuration()->cc_min_current;
- }
-
mc_interface_set_current(current);
+ comm_can_set_current(255, current);
} else {
mc_interface_set_brake_current(output * mc_interface_get_configuration()->l_current_min);
+ comm_can_set_current_brake(255, output * mc_interface_get_configuration()->l_current_min);
}
}
Only in Raptor2 Sources/bldc-firmware_2_80: build
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/comm_can.c Raptor2 Sources/bldc-firmware_2_80/comm_can.c
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/comm_can.c 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/comm_can.c 2017-04-13 17:59:58.000000000 +0100
@@ -23,6 +23,7 @@
*/
#include <string.h>
+#include <math.h>
#include "comm_can.h"
#include "ch.h"
#include "hal.h"
@@ -179,7 +180,19 @@
case CAN_PACKET_SET_RPM:
ind = 0;
- mc_interface_set_pid_speed((float)buffer_get_int32(rxmsg.data8, &ind));
+ mc_interface_set_pid_speed_with_cruise_status((float)buffer_get_int32(rxmsg.data8, &ind), rxmsg.data8[ind++]);
+ timeout_reset();
+ break;
+
+ case CAN_PACKET_SET_SERVO:
+ ind = 0;
+ mc_interface_set_servo((float)buffer_get_int32(rxmsg.data8, &ind) / 1000000.0, rxmsg.data8[ind++]);
+ timeout_reset();
+ break;
+
+ case CAN_PACKET_SET_BRAKE_SERVO:
+ ind = 0;
+ mc_interface_set_brake_servo((float)buffer_get_int32(rxmsg.data8, &ind) / 1000000.0);
timeout_reset();
break;
@@ -256,7 +269,8 @@
stat_tmp->rx_time = chVTGetSystemTime();
stat_tmp->rpm = (float)buffer_get_int32(rxmsg.data8, &ind);
stat_tmp->current = (float)buffer_get_int16(rxmsg.data8, &ind) / 10.0;
- stat_tmp->duty = (float)buffer_get_int16(rxmsg.data8, &ind) / 1000.0;
+ stat_tmp->duty = rxmsg.data8[ind++];
+ stat_tmp->cruise_control_status = rxmsg.data8[ind++];
break;
}
}
@@ -285,7 +299,9 @@
uint8_t buffer[8];
buffer_append_int32(buffer, (int32_t)mc_interface_get_rpm(), &send_index);
buffer_append_int16(buffer, (int16_t)(mc_interface_get_tot_current() * 10.0), &send_index);
- buffer_append_int16(buffer, (int16_t)(mc_interface_get_duty_cycle_now() * 1000.0), &send_index);
+ buffer[send_index++] = (int8_t) floor(mc_interface_get_duty_cycle_now() * 100);
+ buffer[send_index++] = mc_interface_get_cruise_control_status();
+
comm_can_transmit(app_get_configuration()->controller_id | ((uint32_t)CAN_PACKET_STATUS << 8), buffer, send_index);
}
@@ -410,6 +426,21 @@
comm_can_transmit(controller_id | ((uint32_t)CAN_PACKET_SET_CURRENT << 8), buffer, send_index);
}
+void comm_can_set_servo(uint8_t controller_id, float servo_val, bool use_min_current) {
+ int32_t send_index = 0;
+ uint8_t buffer[5];
+ buffer_append_int32(buffer, (int32_t)(servo_val * 1000000.0), &send_index);
+ buffer[send_index++] = use_min_current;
+ comm_can_transmit(controller_id | ((uint32_t)CAN_PACKET_SET_SERVO << 8), buffer, send_index);
+}
+
+void comm_can_set_brake_servo(uint8_t controller_id, float servo_val) {
+ int32_t send_index = 0;
+ uint8_t buffer[4];
+ buffer_append_int32(buffer, (int32_t)(servo_val * 1000000.0), &send_index);
+ comm_can_transmit(controller_id | ((uint32_t)CAN_PACKET_SET_BRAKE_SERVO << 8), buffer, send_index);
+}
+
void comm_can_set_current_brake(uint8_t controller_id, float current) {
int32_t send_index = 0;
uint8_t buffer[4];
@@ -417,10 +448,12 @@
comm_can_transmit(controller_id | ((uint32_t)CAN_PACKET_SET_CURRENT_BRAKE << 8), buffer, send_index);
}
-void comm_can_set_rpm(uint8_t controller_id, float rpm) {
+void comm_can_set_rpm(uint8_t controller_id, float rpm, ppm_cruise cruise_status) {
int32_t send_index = 0;
- uint8_t buffer[4];
+ uint8_t buffer[5];
buffer_append_int32(buffer, (int32_t)rpm, &send_index);
+ buffer[send_index++] = cruise_status;
+
comm_can_transmit(controller_id | ((uint32_t)CAN_PACKET_SET_RPM << 8), buffer, send_index);
}
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/comm_can.h Raptor2 Sources/bldc-firmware_2_80/comm_can.h
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/comm_can.h 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/comm_can.h 2017-04-13 18:00:08.000000000 +0100
@@ -29,7 +29,7 @@
// Settings
#define CAN_STATUS_MSG_INT_MS 1
-#define CAN_STATUS_MSGS_TO_STORE 10
+#define CAN_STATUS_MSGS_TO_STORE 3
// Functions
void comm_can_init(void);
@@ -38,7 +38,9 @@
void comm_can_set_duty(uint8_t controller_id, float duty);
void comm_can_set_current(uint8_t controller_id, float current);
void comm_can_set_current_brake(uint8_t controller_id, float current);
-void comm_can_set_rpm(uint8_t controller_id, float rpm);
+void comm_can_set_servo(uint8_t controller_id, float servo_val, bool use_min_current);
+void comm_can_set_brake_servo(uint8_t controller_id, float servo_val);
+void comm_can_set_rpm(uint8_t controller_id, float rpm, ppm_cruise cruise_status);
void comm_can_set_pos(uint8_t controller_id, float pos);
can_status_msg *comm_can_get_status_msg_index(int index);
can_status_msg *comm_can_get_status_msg_id(int id);
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/commands.c Raptor2 Sources/bldc-firmware_2_80/commands.c
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/commands.c 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/commands.c 2017-06-16 22:45:08.000000000 +0100
@@ -25,7 +25,7 @@
#include "commands.h"
#include "ch.h"
#include "hal.h"
-#include "main.h"
+#include "mc_interface.h"
#include "stm32f4xx_conf.h"
#include "servo.h"
#include "servo_simple.h"
@@ -67,8 +67,11 @@
static void(*appdata_func)(unsigned char *data, unsigned int len) = 0;
static disp_pos_mode display_position_mode;
+static uint8_t remote_Mode;
+
void commands_init(void) {
chThdCreateStatic(detect_thread_wa, sizeof(detect_thread_wa), NORMALPRIO, detect_thread, NULL);
+ remote_Mode = 0;
}
/**
@@ -120,6 +123,8 @@
uint16_t flash_res;
uint32_t new_app_offset;
chuck_data chuck_d_tmp;
+
+ bool sendActualSpeedMode = false;
packet_id = data[0];
data++;
@@ -173,7 +178,7 @@
buffer_append_float32(send_buffer, mc_interface_read_reset_avg_input_current(), 1e2, &ind);
buffer_append_float16(send_buffer, mc_interface_get_duty_cycle_now(), 1e3, &ind);
buffer_append_float32(send_buffer, mc_interface_get_rpm(), 1e0, &ind);
- buffer_append_float16(send_buffer, GET_INPUT_VOLTAGE(), 1e1, &ind);
+ buffer_append_float16(send_buffer, GET_INPUT_VOLTAGE(), 1e2, &ind);
buffer_append_float32(send_buffer, mc_interface_get_amp_hours(false), 1e4, &ind);
buffer_append_float32(send_buffer, mc_interface_get_amp_hours_charged(false), 1e4, &ind);
buffer_append_float32(send_buffer, mc_interface_get_watt_hours(false), 1e4, &ind);
@@ -245,6 +250,7 @@
case COMM_SET_MCCONF:
mcconf = *mc_interface_get_configuration();
+ appconf = *app_get_configuration();
ind = 0;
mcconf.pwm_mode = data[ind++];
@@ -253,22 +259,61 @@
mcconf.sensor_mode = data[ind++];
mcconf.l_current_max = buffer_get_float32(data, 1000.0, &ind);
+ if (mcconf.l_current_max > 80.0){
+ mcconf.l_current_max = 80.0;
+ }
+
mcconf.l_current_min = buffer_get_float32(data, 1000.0, &ind);
+ if (mcconf.l_current_min < -80.0){
+ mcconf.l_current_min = -80.0;
+ }
+
mcconf.l_in_current_max = buffer_get_float32(data, 1000.0, &ind);
+ if (mcconf.l_in_current_max > 40.0){
+ mcconf.l_in_current_max = 40.0;
+ }
+
mcconf.l_in_current_min = buffer_get_float32(data, 1000.0, &ind);
+ if (mcconf.l_in_current_min < -12.0){
+ mcconf.l_in_current_min = -12.0;
+ }
+
mcconf.l_abs_current_max = buffer_get_float32(data, 1000.0, &ind);
mcconf.l_min_erpm = buffer_get_float32(data, 1000.0, &ind);
mcconf.l_max_erpm = buffer_get_float32(data, 1000.0, &ind);
mcconf.l_max_erpm_fbrake = buffer_get_float32(data, 1000.0, &ind);
mcconf.l_max_erpm_fbrake_cc = buffer_get_float32(data, 1000.0, &ind);
mcconf.l_min_vin = buffer_get_float32(data, 1000.0, &ind);
+ if (mcconf.l_min_vin < 8.0){
+ mcconf.l_min_vin = 8.0;
+ }
+
mcconf.l_max_vin = buffer_get_float32(data, 1000.0, &ind);
+ if (mcconf.l_max_vin > 57.0){
+ mcconf.l_max_vin = 57.0;
+ }
+
mcconf.l_battery_cut_start = buffer_get_float32(data, 1000.0, &ind);
+ if (mcconf.l_battery_cut_start < 31.0){
+ mcconf.l_battery_cut_start = 31.0;
+ }
+
mcconf.l_battery_cut_end = buffer_get_float32(data, 1000.0, &ind);
+ if (mcconf.l_battery_cut_end < 29.0){
+ mcconf.l_battery_cut_end = 29.0;
+ }
mcconf.l_slow_abs_current = data[ind++];
mcconf.l_rpm_lim_neg_torque = data[ind++];
mcconf.l_temp_fet_start = buffer_get_float32(data, 1000.0, &ind);
+ if (mcconf.l_temp_fet_start > 80.0){
+ mcconf.l_temp_fet_start = 80.0;
+ }
+
mcconf.l_temp_fet_end = buffer_get_float32(data, 1000.0, &ind);
+ if (mcconf.l_temp_fet_end > 100.0){
+ mcconf.l_temp_fet_end = 100.0;
+ }
+
mcconf.l_temp_motor_start = buffer_get_float32(data, 1000.0, &ind);
mcconf.l_temp_motor_end = buffer_get_float32(data, 1000.0, &ind);
mcconf.l_min_duty = buffer_get_float32(data, 1000000.0, &ind);
@@ -282,17 +327,32 @@
mcconf.sl_min_erpm = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.sl_min_erpm_cycle_int_limit = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.sl_max_fullbreak_current_dir_change = (float)buffer_get_int32(data, &ind) / 1000.0;
- mcconf.sl_cycle_int_limit = (float)buffer_get_int32(data, &ind) / 1000.0;
+ if (!appconf.send_can_status || mcconf.l_temp_motor_end == 101.0) {
+ mcconf.sl_cycle_int_limit = (float)buffer_get_int32(data, &ind) / 1000.0;
+ }else{
+ ind += 4;
+ }
mcconf.sl_phase_advance_at_br = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.sl_cycle_int_rpm_br = (float)buffer_get_int32(data, &ind) / 1000.0;
- mcconf.sl_bemf_coupling_k = (float)buffer_get_int32(data, &ind) / 1000.0;
+ if (!appconf.send_can_status || mcconf.l_temp_motor_end == 101.0) {
+ mcconf.sl_bemf_coupling_k = (float)buffer_get_int32(data, &ind) / 1000.0;
+ }else{
+ ind += 4;
+ }
- memcpy(mcconf.hall_table, data + ind, 8);
+ if (!appconf.send_can_status || mcconf.l_temp_motor_end == 101.0) {
+ memcpy(mcconf.hall_table, data + ind, 8);
+ }
ind += 8;
+
mcconf.hall_sl_erpm = (float)buffer_get_int32(data, &ind) / 1000.0;
- mcconf.foc_current_kp = buffer_get_float32(data, 1e5, &ind);
- mcconf.foc_current_ki = buffer_get_float32(data, 1e5, &ind);
+ if (!appconf.send_can_status || mcconf.l_temp_motor_end == 101.0) {
+ mcconf.foc_current_kp = buffer_get_float32(data, 1e5, &ind);
+ mcconf.foc_current_ki = buffer_get_float32(data, 1e5, &ind);
+ } else {
+ ind += 8;
+ }
mcconf.foc_f_sw = buffer_get_float32(data, 1e3, &ind);
mcconf.foc_dt_us = buffer_get_float32(data, 1e6, &ind);
mcconf.foc_encoder_inverted = data[ind++];
@@ -301,10 +361,15 @@
mcconf.foc_sensor_mode = data[ind++];
mcconf.foc_pll_kp = buffer_get_float32(data, 1e3, &ind);
mcconf.foc_pll_ki = buffer_get_float32(data, 1e3, &ind);
- mcconf.foc_motor_l = buffer_get_float32(data, 1e8, &ind);
- mcconf.foc_motor_r = buffer_get_float32(data, 1e5, &ind);
- mcconf.foc_motor_flux_linkage = buffer_get_float32(data, 1e5, &ind);
- mcconf.foc_observer_gain = buffer_get_float32(data, 1e0, &ind);
+ if (!appconf.send_can_status || mcconf.l_temp_motor_end == 101.0) {
+ mcconf.foc_motor_l = buffer_get_float32(data, 1e8, &ind);
+ mcconf.foc_motor_r = buffer_get_float32(data, 1e5, &ind);
+ mcconf.foc_motor_flux_linkage = buffer_get_float32(data, 1e5, &ind);
+ mcconf.foc_observer_gain = buffer_get_float32(data, 1e0, &ind);
+ }else{
+ ind += 16;
+ }
+
mcconf.foc_duty_dowmramp_kp = buffer_get_float32(data, 1e3, &ind);
mcconf.foc_duty_dowmramp_ki = buffer_get_float32(data, 1e3, &ind);
mcconf.foc_openloop_rpm = buffer_get_float32(data, 1e3, &ind);
@@ -312,33 +377,64 @@
mcconf.foc_sl_openloop_time = buffer_get_float32(data, 1e3, &ind);
mcconf.foc_sl_d_current_duty = buffer_get_float32(data, 1e3, &ind);
mcconf.foc_sl_d_current_factor = buffer_get_float32(data, 1e3, &ind);
+
+ if (!appconf.send_can_status || mcconf.l_temp_motor_end == 101.0) {
+ memcpy(mcconf.foc_hall_table, data + ind, 8);
+ }
+ ind += 8;
+
+ mcconf.foc_sl_erpm = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.s_pid_kp = (float)buffer_get_int32(data, &ind) / 1000000.0;
mcconf.s_pid_ki = (float)buffer_get_int32(data, &ind) / 1000000.0;
mcconf.s_pid_kd = (float)buffer_get_int32(data, &ind) / 1000000.0;
mcconf.s_pid_min_erpm = (float)buffer_get_int32(data, &ind) / 1000.0;
-
+
mcconf.p_pid_kp = (float)buffer_get_int32(data, &ind) / 1000000.0;
mcconf.p_pid_ki = (float)buffer_get_int32(data, &ind) / 1000000.0;
mcconf.p_pid_kd = (float)buffer_get_int32(data, &ind) / 1000000.0;
+ mcconf.p_pid_ang_div = (float)buffer_get_int32(data, &ind) / 100000.0;
mcconf.cc_startup_boost_duty = (float)buffer_get_int32(data, &ind) / 1000000.0;
mcconf.cc_min_current = (float)buffer_get_int32(data, &ind) / 1000.0;
mcconf.cc_gain = (float)buffer_get_int32(data, &ind) / 1000000.0;
- mcconf.cc_ramp_step_max = (float)buffer_get_int32(data, &ind) / 1000000.0;
+ mcconf.cc_ramp_step_max = buffer_get_float32(data, 1e6, &ind);
mcconf.m_fault_stop_time_ms = buffer_get_int32(data, &ind);
mcconf.m_duty_ramp_step = (float)buffer_get_float32(data, 1000000.0, &ind);
mcconf.m_duty_ramp_step_rpm_lim = (float)buffer_get_float32(data, 1000000.0, &ind);
mcconf.m_current_backoff_gain = (float)buffer_get_float32(data, 1000000.0, &ind);
mcconf.m_encoder_counts = buffer_get_uint32(data, &ind);
+ mcconf.m_sensor_port_mode = data[ind++];
+
+ // new config
+ mcconf.s_pid_breaking_enabled = data[ind++];
+ mcconf.use_max_watt_limit = data[ind++];
+ mcconf.watts_max = (float)buffer_get_int16(data, &ind);
+ // new config end
conf_general_store_mc_configuration(&mcconf);
mc_interface_set_configuration(&mcconf);
-
-#if ENCODER_ENABLE
- encoder_set_counts(mcconf.m_encoder_counts);
-#endif
+
+ if (!appconf.send_can_status) {
+
+ ind = 0;
+ // send to all others
+ uint8_t send_buffer_can[PACKET_MAX_PL_LEN];
+
+ send_buffer_can[ind++] = packet_id;
+ for(unsigned int i = 0; i < len; i++){
+ send_buffer_can[ind++] = data[i];
+ }
+
+ for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+ can_status_msg *msg = comm_can_get_status_msg_index(i);
+
+ if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < 1) {
+ comm_can_send_buffer(msg->id, send_buffer_can, len + 1, false);
+ }
+ }
+ }
ind = 0;
send_buffer[ind++] = packet_id;
@@ -416,15 +512,19 @@
buffer_append_float32(send_buffer, mcconf.foc_sl_openloop_time, 1e3, &ind);
buffer_append_float32(send_buffer, mcconf.foc_sl_d_current_duty, 1e3, &ind);
buffer_append_float32(send_buffer, mcconf.foc_sl_d_current_factor, 1e3, &ind);
+ memcpy(send_buffer + ind, mcconf.foc_hall_table, 8);
+ ind += 8;
+ buffer_append_int32(send_buffer, (int32_t)(mcconf.foc_sl_erpm * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.s_pid_kp * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.s_pid_ki * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.s_pid_kd * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.s_pid_min_erpm * 1000.0), &ind);
-
+
buffer_append_int32(send_buffer, (int32_t)(mcconf.p_pid_kp * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.p_pid_ki * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.p_pid_kd * 1000000.0), &ind);
+ buffer_append_float32(send_buffer, mcconf.p_pid_ang_div, 1e5, &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.cc_startup_boost_duty * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(mcconf.cc_min_current * 1000.0), &ind);
@@ -436,6 +536,13 @@
buffer_append_float32(send_buffer, mcconf.m_duty_ramp_step_rpm_lim, 1000000.0, &ind);
buffer_append_float32(send_buffer, mcconf.m_current_backoff_gain, 1000000.0, &ind);
buffer_append_uint32(send_buffer, mcconf.m_encoder_counts, &ind);
+ send_buffer[ind++] = mcconf.m_sensor_port_mode;
+
+ // new config
+ send_buffer[ind++] = mcconf.s_pid_breaking_enabled;
+ send_buffer[ind++] = mcconf.use_max_watt_limit;
+ buffer_append_int16(send_buffer, (int16_t)(mcconf.watts_max), &ind);
+ // new config end
commands_send_packet(send_buffer, ind);
break;
@@ -464,7 +571,7 @@
appconf.app_ppm_conf.multi_esc = data[ind++];
appconf.app_ppm_conf.tc = data[ind++];
appconf.app_ppm_conf.tc_max_diff = (float)buffer_get_int32(data, &ind) / 1000.0;
-
+
appconf.app_adc_conf.ctrl_type = data[ind++];
appconf.app_adc_conf.hyst = (float)buffer_get_int32(data, &ind) / 1000.0;
appconf.app_adc_conf.voltage_start = (float)buffer_get_int32(data, &ind) / 1000.0;
@@ -493,7 +600,45 @@
appconf.app_chuk_conf.multi_esc = data[ind++];
appconf.app_chuk_conf.tc = data[ind++];
appconf.app_chuk_conf.tc_max_diff = buffer_get_float32(data, 1000.0, &ind);
-
+
+ appconf.app_nrf_conf.speed = data[ind++];
+ appconf.app_nrf_conf.power = data[ind++];
+ appconf.app_nrf_conf.crc_type = data[ind++];
+ appconf.app_nrf_conf.retry_delay = data[ind++];
+ appconf.app_nrf_conf.retries = data[ind++];
+ appconf.app_nrf_conf.channel = data[ind++];
+ memcpy(appconf.app_nrf_conf.address, data + ind, 3);
+ ind += 3;
+ appconf.app_nrf_conf.send_crc_ack = data[ind++];
+
+ // new config
+ appconf.app_ppm_conf.pulse_center = (float)buffer_get_int32(data, &ind) / 1000.0;
+ appconf.app_ppm_conf.tc_offset = (float)buffer_get_int32(data, &ind) / 1000.0;
+ appconf.app_ppm_conf.cruise_left = data[ind++];
+ appconf.app_ppm_conf.cruise_right = data[ind++];
+ appconf.app_ppm_conf.max_erpm_for_dir_active = data[ind++];
+ appconf.app_ppm_conf.max_erpm_for_dir = buffer_get_float32(data, 1000.0, &ind);
+
+ appconf.app_chuk_conf.tc_offset = (float)buffer_get_int32(data, &ind) / 1000.0;
+ appconf.app_chuk_conf.buttons_mirrored = data[ind++];
+
+ appconf.app_throttle_conf.adjustable_throttle_enabled = data[ind++];
+ appconf.app_throttle_conf.y1_throttle = (float)buffer_get_int16(data, &ind) / 1000.0;
+ appconf.app_throttle_conf.y2_throttle = (float)buffer_get_int16(data, &ind) / 1000.0;
+ appconf.app_throttle_conf.y3_throttle = (float)buffer_get_int16(data, &ind) / 1000.0;
+ appconf.app_throttle_conf.x1_throttle = (float)buffer_get_int16(data, &ind) / 1000.0;
+ appconf.app_throttle_conf.x2_throttle = (float)buffer_get_int16(data, &ind) / 1000.0;
+ appconf.app_throttle_conf.x3_throttle = (float)buffer_get_int16(data, &ind) / 1000.0;
+ appconf.app_throttle_conf.bezier_reduce_factor = (float)buffer_get_int16(data, &ind) / 100.0;
+ appconf.app_throttle_conf.y1_neg_throttle = (float)buffer_get_int16(data, &ind) / 1000.0;
+ appconf.app_throttle_conf.y2_neg_throttle = (float)buffer_get_int16(data, &ind) / 1000.0;
+ appconf.app_throttle_conf.y3_neg_throttle = (float)buffer_get_int16(data, &ind) / 1000.0;
+ appconf.app_throttle_conf.x1_neg_throttle = (float)buffer_get_int16(data, &ind) / 1000.0;
+ appconf.app_throttle_conf.x2_neg_throttle = (float)buffer_get_int16(data, &ind) / 1000.0;
+ appconf.app_throttle_conf.x3_neg_throttle = (float)buffer_get_int16(data, &ind) / 1000.0;
+ appconf.app_throttle_conf.bezier_neg_reduce_factor = (float)buffer_get_int16(data, &ind) / 100.0;
+ // new config end
+
conf_general_store_app_configuration(&appconf);
app_set_configuration(&appconf);
timeout_configure(appconf.timeout_msec, appconf.timeout_brake_current);
@@ -533,7 +678,7 @@
send_buffer[ind++] = appconf.app_ppm_conf.multi_esc;
send_buffer[ind++] = appconf.app_ppm_conf.tc;
buffer_append_int32(send_buffer, (int32_t)(appconf.app_ppm_conf.tc_max_diff * 1000.0), &ind);
-
+
send_buffer[ind++] = appconf.app_adc_conf.ctrl_type;
buffer_append_int32(send_buffer, (int32_t)(appconf.app_adc_conf.hyst * 1000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(appconf.app_adc_conf.voltage_start * 1000.0), &ind);
@@ -562,7 +707,45 @@
send_buffer[ind++] = appconf.app_chuk_conf.multi_esc;
send_buffer[ind++] = appconf.app_chuk_conf.tc;
buffer_append_int32(send_buffer, (int32_t)(appconf.app_chuk_conf.tc_max_diff * 1000.0), &ind);
-
+
+ send_buffer[ind++] = appconf.app_nrf_conf.speed;
+ send_buffer[ind++] = appconf.app_nrf_conf.power;
+ send_buffer[ind++] = appconf.app_nrf_conf.crc_type;
+ send_buffer[ind++] = appconf.app_nrf_conf.retry_delay;
+ send_buffer[ind++] = appconf.app_nrf_conf.retries;
+ send_buffer[ind++] = appconf.app_nrf_conf.channel;
+ memcpy(send_buffer + ind, appconf.app_nrf_conf.address, 3);
+ ind += 3;
+ send_buffer[ind++] = appconf.app_nrf_conf.send_crc_ack;
+
+ // new config
+ buffer_append_int32(send_buffer, (int32_t)(appconf.app_ppm_conf.pulse_center * 1000.0), &ind);
+ buffer_append_int32(send_buffer, (int32_t)(appconf.app_ppm_conf.tc_offset * 1000.0), &ind);
+ send_buffer[ind++] = appconf.app_ppm_conf.cruise_left;
+ send_buffer[ind++] = appconf.app_ppm_conf.cruise_right;
+ send_buffer[ind++] = appconf.app_ppm_conf.max_erpm_for_dir_active;
+ buffer_append_float32(send_buffer, appconf.app_ppm_conf.max_erpm_for_dir, 1000.0, &ind);
+
+ buffer_append_int32(send_buffer, (int32_t)(appconf.app_chuk_conf.tc_offset * 1000.0), &ind);
+ send_buffer[ind++] = appconf.app_chuk_conf.buttons_mirrored;
+
+ send_buffer[ind++] = appconf.app_throttle_conf.adjustable_throttle_enabled;
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.y1_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.y2_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.y3_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.x1_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.x2_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.x3_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.bezier_reduce_factor * 100.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.y1_neg_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.y2_neg_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.y3_neg_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.x1_neg_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.x2_neg_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.x3_neg_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.bezier_neg_reduce_factor * 100.0), &ind);
+ // end new config
+
commands_send_packet(send_buffer, ind);
break;
@@ -571,7 +754,7 @@
at_start = data[ind++];
sample_len = buffer_get_uint16(data, &ind);
decimation = data[ind++];
- main_sample_print_data(at_start, sample_len, decimation);
+ mc_interface_sample_print_data(at_start, sample_len, decimation);
break;
case COMM_TERMINAL_CMD:
@@ -635,39 +818,74 @@
break;
case COMM_DETECT_ENCODER: {
-#if ENCODER_ENABLE
- mcconf = *mc_interface_get_configuration();
- mcconf_old = mcconf;
+ if (encoder_is_configured()) {
+ mcconf = *mc_interface_get_configuration();
+ mcconf_old = mcconf;
- ind = 0;
- float current = buffer_get_float32(data, 1e3, &ind);
+ ind = 0;
+ float current = buffer_get_float32(data, 1e3, &ind);
- mcconf.motor_type = MOTOR_TYPE_FOC;
- mcconf.foc_f_sw = 10000.0;
- mcconf.foc_current_kp = 0.01;
- mcconf.foc_current_ki = 10.0;
- mc_interface_set_configuration(&mcconf);
+ mcconf.motor_type = MOTOR_TYPE_FOC;
+ mcconf.foc_f_sw = 10000.0;
+ mcconf.foc_current_kp = 0.01;
+ mcconf.foc_current_ki = 10.0;
+ mc_interface_set_configuration(&mcconf);
+
+ float offset = 0.0;
+ float ratio = 0.0;
+ bool inverted = false;
+ mcpwm_foc_encoder_detect(current, false, &offset, &ratio, &inverted);
+ mc_interface_set_configuration(&mcconf_old);
+
+ ind = 0;
+ send_buffer[ind++] = COMM_DETECT_ENCODER;
+ buffer_append_float32(send_buffer, offset, 1e6, &ind);
+ buffer_append_float32(send_buffer, ratio, 1e6, &ind);
+ send_buffer[ind++] = inverted;
+ commands_send_packet(send_buffer, ind);
+ } else {
+ ind = 0;
+ send_buffer[ind++] = COMM_DETECT_ENCODER;
+ buffer_append_float32(send_buffer, 1001.0, 1e6, &ind);
+ buffer_append_float32(send_buffer, 0.0, 1e6, &ind);
+ send_buffer[ind++] = false;
+ commands_send_packet(send_buffer, ind);
+ }
+ }
+ break;
- float offset = 0.0;
- float ratio = 0.0;
- bool inverted = false;
- mcpwm_foc_encoder_detect(current, false, &offset, &ratio, &inverted);
- mc_interface_set_configuration(&mcconf_old);
+ case COMM_DETECT_HALL_FOC: {
+ mcconf = *mc_interface_get_configuration();
- ind = 0;
- send_buffer[ind++] = COMM_DETECT_ENCODER;
- buffer_append_float32(send_buffer, offset, 1e6, &ind);
- buffer_append_float32(send_buffer, ratio, 1e6, &ind);
- send_buffer[ind++] = inverted;
- commands_send_packet(send_buffer, ind);
-#else
- ind = 0;
- send_buffer[ind++] = COMM_DETECT_ENCODER;
- buffer_append_float32(send_buffer, 1001.0, 1e6, &ind);
- buffer_append_float32(send_buffer, 0.0, 1e6, &ind);
- send_buffer[ind++] = false;
- commands_send_packet(send_buffer, ind);
-#endif
+ if (mcconf.m_sensor_port_mode == SENSOR_PORT_MODE_HALL) {
+ mcconf_old = mcconf;
+ ind = 0;
+ float current = buffer_get_float32(data, 1e3, &ind);
+
+ mcconf.motor_type = MOTOR_TYPE_FOC;
+ mcconf.foc_f_sw = 10000.0;
+ mcconf.foc_current_kp = 0.01;
+ mcconf.foc_current_ki = 10.0;
+ mc_interface_set_configuration(&mcconf);
+
+ uint8_t hall_tab[8];
+ bool res = mcpwm_foc_hall_detect(current, hall_tab);
+ mc_interface_set_configuration(&mcconf_old);
+
+ ind = 0;
+ send_buffer[ind++] = COMM_DETECT_HALL_FOC;
+ memcpy(send_buffer + ind, hall_tab, 8);
+ ind += 8;
+ send_buffer[ind++] = res ? 0 : 1;
+
+ commands_send_packet(send_buffer, ind);
+ } else {
+ ind = 0;
+ send_buffer[ind++] = COMM_DETECT_HALL_FOC;
+ memset(send_buffer, 255, 8);
+ ind += 8;
+ send_buffer[ind++] = 0;
+ }
}
break;
@@ -694,6 +912,8 @@
send_buffer[ind++] = COMM_GET_DECODED_ADC;
buffer_append_int32(send_buffer, (int32_t)(app_adc_get_decoded_level() * 1000000.0), &ind);
buffer_append_int32(send_buffer, (int32_t)(app_adc_get_voltage() * 1000000.0), &ind);
+ buffer_append_int32(send_buffer, (int32_t)(app_adc_get_decoded_level2() * 1000000.0), &ind);
+ buffer_append_int32(send_buffer, (int32_t)(app_adc_get_voltage2() * 1000000.0), &ind);
commands_send_packet(send_buffer, ind);
break;
@@ -725,10 +945,518 @@
appdata_func(data, len);
}
break;
+
+ case COMM_SET_SPEED_MODE:
+ ind = 0;
+ uint8_t new_remote_Mode = data[ind++];
+ uint8_t new_speed_mode_int = data[ind++];
+ bool use_max_watt = data[ind++];
+ float watts_max = buffer_get_float16(data, 1, &ind);
+ float cur_mot_max = buffer_get_float16(data, 10, &ind);
+ float cur_bat_max = buffer_get_float16(data, 10, &ind);
+ float cur_mot_min = buffer_get_float16(data, 10, &ind);
+ float cur_bat_min = buffer_get_float16(data, 10, &ind);
+ float max_erpm = buffer_get_float16(data, 0.1, &ind);
+
+ bool useSpecialThrottleCurve = data[ind++];
+ float newY1 = 0.0, newY2 = 0.0, newY3 = 0.0, newX1 = 0.0, newX2 = 0.0, newX3 = 0.0, newBezier_pos = 0.0, newY1_neg = 0.0, newY2_neg = 0.0, newY3_neg = 0.0, newX1_neg = 0.0, newX2_neg = 0.0, newX3_neg = 0.0, newBezier_neg = 0.0;
+ if (useSpecialThrottleCurve) {
+ newY1 = buffer_get_float16(data, 1000, &ind);
+ newY2 = buffer_get_float16(data, 1000, &ind);
+ newY3 = buffer_get_float16(data, 1000, &ind);
+ newX1 = buffer_get_float16(data, 1000, &ind);
+ newX2 = buffer_get_float16(data, 1000, &ind);
+ newX3 = buffer_get_float16(data, 1000, &ind);
+ newBezier_pos = buffer_get_float16(data, 100, &ind);
+ newY1_neg = buffer_get_float16(data, 1000, &ind);
+ newY2_neg = buffer_get_float16(data, 1000, &ind);
+ newY3_neg = buffer_get_float16(data, 1000, &ind);
+ newX1_neg = buffer_get_float16(data, 1000, &ind);
+ newX2_neg = buffer_get_float16(data, 1000, &ind);
+ newX3_neg = buffer_get_float16(data, 1000, &ind);
+ newBezier_neg = buffer_get_float16(data, 100, &ind);
+ }
+
+ bool useSpecialPIDBrakingEnabled = data[ind++];
+ bool pidBrakingEnabled = false;
+ if (useSpecialPIDBrakingEnabled) {
+ pidBrakingEnabled = data[ind++];
+ }
+
+ bool useSpecialSpeedPID = data[ind++];
+ float speed_pid_kp = 0.0, speed_pid_ki = 0.0, speed_pid_kd = 0.0;
+ if (useSpecialSpeedPID) {
+ speed_pid_kp = buffer_get_float32(data, 1000000.0, &ind);
+ speed_pid_ki = buffer_get_float32(data, 1000000.0, &ind);
+ speed_pid_kd = buffer_get_float32(data, 1000000.0, &ind);
+ }
+
+ uint8_t front_controller_first = data[ind++];
+ uint8_t front_controller_second = data[ind++];
+ bool front_use_max_watt = false;
+ float front_watts_max = 0.0, front_cur_mot_max = 0.0, front_cur_bat_max = 0.0, front_cur_mot_min = 0.0, front_cur_bat_min = 0.0;
+ if (front_controller_first != 9 || front_controller_second != 9) {
+ front_use_max_watt = data[ind++];
+ front_watts_max = buffer_get_float16(data, 1, &ind);
+ front_cur_mot_max = buffer_get_float16(data, 10, &ind);
+ front_cur_bat_max = buffer_get_float16(data, 10, &ind);
+ front_cur_mot_min = buffer_get_float16(data, 10, &ind);
+ front_cur_bat_min = buffer_get_float16(data, 10, &ind);
+ }
+
+ appconf = *app_get_configuration();
+
+ if (appconf.app_to_use == APP_PPM_UART
+ || appconf.app_to_use == APP_PPM
+ || appconf.app_to_use == APP_NONE
+ || appconf.app_to_use == APP_UART
+ || appconf.app_to_use == APP_NUNCHUK
+ || appconf.app_to_use == APP_NRF) {
+
+ app_configuration saved_appconf;
+
+ conf_general_read_app_configuration(&saved_appconf);
+
+ mcconf = *mc_interface_get_configuration();
+
+ mc_configuration saved_mcconf;
+ // gett base settings for motor
+ conf_general_read_mc_configuration(&saved_mcconf);
+
+ // reset app ppm
+ appconf.app_ppm_conf.ctrl_type = saved_appconf.app_ppm_conf.ctrl_type;
+ appconf.app_ppm_conf.pid_max_erpm = saved_appconf.app_ppm_conf.pid_max_erpm;
+ appconf.app_ppm_conf.rpm_lim_end = saved_appconf.app_ppm_conf.rpm_lim_end;
+ appconf.app_ppm_conf.rpm_lim_start = saved_appconf.app_ppm_conf.rpm_lim_start;
+
+ // reset app chuk
+ appconf.app_chuk_conf.ctrl_type = saved_appconf.app_chuk_conf.ctrl_type;
+ appconf.app_chuk_conf.rpm_lim_end = saved_appconf.app_chuk_conf.rpm_lim_end;
+ appconf.app_chuk_conf.rpm_lim_start = saved_appconf.app_chuk_conf.rpm_lim_start;
+
+ // reset throttle curve
+ appconf.app_throttle_conf.adjustable_throttle_enabled = saved_appconf.app_throttle_conf.adjustable_throttle_enabled;
+ appconf.app_throttle_conf.y1_throttle = saved_appconf.app_throttle_conf.y1_throttle;
+ appconf.app_throttle_conf.y2_throttle = saved_appconf.app_throttle_conf.y2_throttle;
+ appconf.app_throttle_conf.y3_throttle = saved_appconf.app_throttle_conf.y3_throttle;
+ appconf.app_throttle_conf.x1_throttle = saved_appconf.app_throttle_conf.x1_throttle;
+ appconf.app_throttle_conf.x2_throttle = saved_appconf.app_throttle_conf.x2_throttle;
+ appconf.app_throttle_conf.x3_throttle = saved_appconf.app_throttle_conf.x3_throttle;
+ appconf.app_throttle_conf.bezier_reduce_factor = saved_appconf.app_throttle_conf.bezier_reduce_factor;
+ appconf.app_throttle_conf.y1_neg_throttle = saved_appconf.app_throttle_conf.y1_neg_throttle;
+ appconf.app_throttle_conf.y2_neg_throttle = saved_appconf.app_throttle_conf.y2_neg_throttle;
+ appconf.app_throttle_conf.y3_neg_throttle = saved_appconf.app_throttle_conf.y3_neg_throttle;
+ appconf.app_throttle_conf.x1_neg_throttle = saved_appconf.app_throttle_conf.x1_neg_throttle;
+ appconf.app_throttle_conf.x2_neg_throttle = saved_appconf.app_throttle_conf.x2_neg_throttle;
+ appconf.app_throttle_conf.x3_neg_throttle = saved_appconf.app_throttle_conf.x3_neg_throttle;
+ appconf.app_throttle_conf.bezier_neg_reduce_factor = saved_appconf.app_throttle_conf.bezier_neg_reduce_factor;
+
+ // reset motor
+ mcconf.l_current_max = saved_mcconf.l_current_max;
+ mcconf.l_current_min = saved_mcconf.l_current_min;
+ mcconf.l_in_current_max = saved_mcconf.l_in_current_max;
+ mcconf.l_in_current_min = saved_mcconf.l_in_current_min;
+
+ mcconf.use_max_watt_limit = saved_mcconf.use_max_watt_limit;
+ mcconf.watts_max = saved_mcconf.watts_max;
+
+ mcconf.s_pid_breaking_enabled = saved_mcconf.s_pid_breaking_enabled;
+
+ mcconf.s_pid_kp = saved_mcconf.s_pid_kp;
+ mcconf.s_pid_ki = saved_mcconf.s_pid_ki;
+ mcconf.s_pid_kd = saved_mcconf.s_pid_kd;
+
+ if (new_remote_Mode == 0){
+ remote_Mode = new_remote_Mode;
+ } else {
+ if (appconf.app_to_use == APP_NONE || appconf.app_to_use == APP_UART) {
+ if (appconf.send_can_status) {
+ remote_Mode = new_remote_Mode;
+ }
+ }
+
+ if (appconf.app_to_use == APP_PPM_UART || appconf.app_to_use == APP_PPM) {
+ switch (new_speed_mode_int) {
+ case PPM_CTRL_TYPE_PID_NOACCELERATION:
+ case PPM_CTRL_TYPE_NONE:
+ case PPM_CTRL_TYPE_CURRENT:
+ case PPM_CTRL_TYPE_CURRENT_NOREV:
+ case PPM_CTRL_TYPE_CURRENT_NOREV_BRAKE:
+ case PPM_CTRL_TYPE_DUTY:
+ case PPM_CTRL_TYPE_DUTY_NOREV:
+ case PPM_CTRL_TYPE_PID:
+ case PPM_CTRL_TYPE_PID_NOREV:
+ case PPM_CTRL_TYPE_WATT_NOREV_BRAKE:
+ case PPM_CTRL_TYPE_WATT:
+ if (!appconf.send_can_status) {
+ appconf.app_ppm_conf.ctrl_type = new_speed_mode_int;
+
+ if (max_erpm > 0 && max_erpm <= 100000.0) {
+ appconf.app_ppm_conf.pid_max_erpm = max_erpm;
+ appconf.app_ppm_conf.rpm_lim_end = max_erpm;
+ appconf.app_ppm_conf.rpm_lim_start = max_erpm - ((int16_t)(max_erpm / 10)); // 10% off
+ }
+ }
+
+ remote_Mode = new_remote_Mode;
+ break;
+ default:
+ //only the basic settings which are defined by the bldc-tool
+ remote_Mode = 0;
+ break;
+ }
+ }
+
+ if (appconf.app_to_use == APP_NUNCHUK || appconf.app_to_use == APP_NRF) {
+ switch (new_speed_mode_int) {
+ case CHUK_CTRL_TYPE_NONE:
+ case CHUK_CTRL_TYPE_CURRENT:
+ case CHUK_CTRL_TYPE_CURRENT_NOREV:
+ case CHUK_CTRL_TYPE_WATT:
+ case CHUK_CTRL_TYPE_WATT_NOREV:
+ if (!appconf.send_can_status) {
+ appconf.app_chuk_conf.ctrl_type = new_speed_mode_int;
+
+ if (max_erpm > 0 && max_erpm <= 100000.0) {
+ appconf.app_chuk_conf.rpm_lim_end = max_erpm;
+ appconf.app_chuk_conf.rpm_lim_start = max_erpm - ((int16_t)(max_erpm / 10)); // 10% off
+ }
+
+ }
+
+ remote_Mode = new_remote_Mode;
+ break;
+ default:
+ //only the basic settings which are defined by the bldc-tool
+ remote_Mode = 0;
+ break;
+ }
+ }
+
+ if(remote_Mode != 0){
+ if (!appconf.send_can_status) {
+ if (useSpecialThrottleCurve) {
+ appconf.app_throttle_conf.adjustable_throttle_enabled = true;
+ appconf.app_throttle_conf.y1_throttle = newY1;
+ appconf.app_throttle_conf.y2_throttle = newY2;
+ appconf.app_throttle_conf.y3_throttle = newY3;
+ appconf.app_throttle_conf.x1_throttle = newX1;
+ appconf.app_throttle_conf.x2_throttle = newX2;
+ appconf.app_throttle_conf.x3_throttle = newX3;
+ appconf.app_throttle_conf.bezier_reduce_factor = newBezier_pos;
+ appconf.app_throttle_conf.y1_neg_throttle = newY1_neg;
+ appconf.app_throttle_conf.y2_neg_throttle = newY2_neg;
+ appconf.app_throttle_conf.y3_neg_throttle = newY3_neg;
+ appconf.app_throttle_conf.x1_neg_throttle = newX1_neg;
+ appconf.app_throttle_conf.x2_neg_throttle = newX2_neg;
+ appconf.app_throttle_conf.x3_neg_throttle = newX3_neg;
+ appconf.app_throttle_conf.bezier_neg_reduce_factor = newBezier_neg;
+ }
+ }
+
+ if (useSpecialPIDBrakingEnabled) {
+ mcconf.s_pid_breaking_enabled = pidBrakingEnabled;
+ }
+
+ if (useSpecialSpeedPID) {
+ mcconf.s_pid_kp = speed_pid_kp;
+ mcconf.s_pid_ki = speed_pid_ki;
+ mcconf.s_pid_kd = speed_pid_kd;
+ }
+
+ if ((front_controller_first != 9 && appconf.controller_id == front_controller_first)
+ || (front_controller_second != 9 && appconf.controller_id == front_controller_second)) { // or last position
+
+ mcconf.use_max_watt_limit = front_use_max_watt;
+ mcconf.watts_max = front_watts_max;
+
+ if (front_cur_mot_max >= mcconf.cc_min_current && front_cur_mot_max <= 80) mcconf.l_current_max = front_cur_mot_max;
+
+ if (front_cur_bat_max >= mcconf.cc_min_current && front_cur_bat_max <= 40) mcconf.l_in_current_max = front_cur_bat_max;
+
+ if (front_cur_mot_min <= -mcconf.cc_min_current && front_cur_mot_min >= -80) mcconf.l_current_min = front_cur_mot_min;
+
+ if (front_cur_bat_min <= -mcconf.cc_min_current && front_cur_bat_min >= -12) mcconf.l_in_current_min = front_cur_bat_min;
+
+ } else { // normal or rear setup
+ mcconf.use_max_watt_limit = use_max_watt;
+ mcconf.watts_max = watts_max;
+
+ if (cur_mot_max >= mcconf.cc_min_current && cur_mot_max <= 80) mcconf.l_current_max = cur_mot_max;
+
+ if (cur_bat_max >= mcconf.cc_min_current && cur_bat_max <= 40) mcconf.l_in_current_max = cur_bat_max;
+
+ if (cur_mot_min <= -mcconf.cc_min_current && cur_mot_min >= -80) mcconf.l_current_min = cur_mot_min;
+
+ if (cur_bat_min <= -mcconf.cc_min_current && cur_bat_min >= -12) mcconf.l_in_current_min = cur_bat_min;
+ }
+ }
+ }
+
+ if (!appconf.send_can_status) {
+ sendActualSpeedMode = true;
+
+ ind = 0;
+ // send to all others
+ uint8_t send_buffer_can[PACKET_MAX_PL_LEN];
+
+ send_buffer_can[ind++] = packet_id;
+ send_buffer_can[ind++] = remote_Mode;
+
+ for(unsigned int i = 1; i < len; i++){
+ send_buffer_can[ind++] = data[i];
+ }
+
+ for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+ can_status_msg *msg = comm_can_get_status_msg_index(i);
+
+ if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < 1) {
+ comm_can_send_buffer(msg->id, send_buffer_can, len + 1, false);
+ }
+ }
+ }
+
+ mc_interface_set_configuration(&mcconf);
+ app_set_configuration(&appconf);
+
+ timeout_configure(appconf.timeout_msec, appconf.timeout_brake_current);
+
+ timeout_reset();
+ }
+
+ break;
+ case COMM_CHANGE_SPEED_MODE:
+/*
+ ind = 0;
+
+ app_configuration saved_appconf2;
+
+ conf_general_read_app_configuration(&saved_appconf2);
+
+ appconf = *app_get_configuration();
+
+ appconf.app_ppm_conf.ctrl_type = saved_appconf2.app_ppm_conf.ctrl_type;
+
+ mcconf = *mc_interface_get_configuration();
+
+ mc_configuration saved_mcconf2;
+ // gett base settings for motor
+ conf_general_read_mc_configuration(&saved_mcconf2);
+
+ mcconf.l_current_max = saved_mcconf2.l_current_max;
+ mcconf.l_in_current_max = saved_mcconf2.l_in_current_max;
+
+ if (appconf.app_to_use == APP_PPM_UART || appconf.app_to_use == APP_PPM) {
+ if (appconf.app_ppm_conf.ctrl_type == PPM_CTRL_TYPE_WATT_NOREV_BRAKE) {
+ appconf.app_ppm_conf.ctrl_type = PPM_CTRL_TYPE_PID_NOACCELERATION;
+ float watt = 250 / 0.9;
+ } else if (appconf.app_ppm_conf.ctrl_type == PPM_CTRL_TYPE_PID_NOACCELERATION) {
+ appconf.app_ppm_conf.ctrl_type = PPM_CTRL_TYPE_WATT_NOREV_BRAKE;
+ *//*} else if (new_speed_mode_int == PPM_CTRL_TYPE_CURRENT_NOREV_BRAKE) {
+ appconf.app_ppm_conf.ctrl_type = new_speed_mode_int;
+
+ if (current_motor >= mcconf.cc_min_current && current_motor <= mcconf.l_current_max){
+ mcconf.l_current_max = current_motor;
+ }
+
+ if (current_battery >= mcconf.cc_min_current && current_battery <= mcconf.l_in_current_max){
+ mcconf.l_in_current_max = current_battery;
+ }*/
+/*
+ } else if (appconf.app_ppm_conf.ctrl_type == PPM_CTRL_TYPE_WATT_NOREV_BRAKE) {
+ appconf.app_ppm_conf.ctrl_type = PPM_CTRL_TYPE_CURRENT_NOREV_BRAKE;
+ //currents will be the default values
+ } else{
+ //only the basic settings which are defined by the bldc-tool
+ }
+ }
+
+ mc_interface_set_configuration(&mcconf);
+ app_set_configuration(&appconf);
+ timeout_configure(appconf.timeout_msec, appconf.timeout_brake_current);
+
+ timeout_reset();
+ sendActualSpeedMode = true;
+*/
+ break;
+ case COMM_GET_SPEED_MODE:
+ sendActualSpeedMode = true;
+ break;
+ case COMM_SET_CURRENT_CONF_AS_DEFAULT:
+
+ mcconf = *mc_interface_get_configuration();
+
+ appconf = *app_get_configuration();
+
+ if (!appconf.send_can_status) {
+ sendActualSpeedMode = true;
+
+ ind = 0;
+ // send to all others
+ uint8_t send_buffer_can[PACKET_MAX_PL_LEN];
+
+ send_buffer_can[ind++] = packet_id;
+
+ for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+ can_status_msg *msg = comm_can_get_status_msg_index(i);
+
+ if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < 1) {
+ comm_can_send_buffer(msg->id, send_buffer_can, len + 1, false);
+ }
+ }
+ }
+
+ conf_general_store_mc_configuration(&mcconf);
+ mc_interface_set_configuration(&mcconf);
+
+ conf_general_store_app_configuration(&appconf);
+ app_set_configuration(&appconf);
+
+ timeout_configure(appconf.timeout_msec, appconf.timeout_brake_current);
+
+ remote_Mode = 0;
+ break;
+ case COMM_SET_MOTOR_TYPE:
+ appconf = *app_get_configuration();
+
+ bool change_allowed = false;
+
+ ind = 0;
+ uint8_t new_motor_type = data[ind++];
+
+ if (new_motor_type == MOTOR_TYPE_FOC || new_motor_type == MOTOR_TYPE_BLDC) {
+
+ mcconf = *mc_interface_get_configuration();
+
+ if (mcconf.motor_type != new_motor_type) {
+
+ mc_configuration default_mcconf;
+ //get default settings
+ conf_general_get_default_mc_configuration(&default_mcconf);
+
+ change_allowed = true;
+
+ /*if (new_motor_type == MOTOR_TYPE_FOC &&
+ (mcconf.foc_motor_l != default_mcconf.foc_motor_l
+ || mcconf.foc_motor_r != default_mcconf.foc_motor_r
+ || mcconf.foc_motor_flux_linkage != default_mcconf.foc_motor_flux_linkage)) {
+ change_allowed = true;
+ }
+
+ if (new_motor_type == MOTOR_TYPE_BLDC &&
+ (mcconf.sl_cycle_int_limit != default_mcconf.sl_cycle_int_limit
+ || mcconf.sl_bemf_coupling_k != default_mcconf.sl_bemf_coupling_k)) {
+ change_allowed = true;
+ }*/
+
+
+ if (change_allowed) {
+ if (!appconf.send_can_status) {
+ ind = 0;
+ // send to all others
+ uint8_t send_buffer_can[PACKET_MAX_PL_LEN];
+
+ send_buffer_can[ind++] = packet_id;
+ send_buffer_can[ind++] = new_motor_type;
+
+ for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+ can_status_msg *msg = comm_can_get_status_msg_index(i);
+
+ if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < 1) {
+ comm_can_send_buffer(msg->id, send_buffer_can, len + 1, false);
+ }
+ }
+ }
+
+ //change motor config
+ mcconf.motor_type = new_motor_type;
+
+ //write motor config
+ //conf_general_store_mc_configuration(&saved_mcconf);
+ mc_interface_set_configuration(&mcconf);
+
+ if (!appconf.send_can_status) {
+ int32_t ind_motor_type = 0;
+ uint8_t send_buffer_motor_type[PACKET_MAX_PL_LEN];
+
+ send_buffer_motor_type[ind_motor_type++] = packet_id;
+ send_buffer_motor_type[ind_motor_type++] = new_motor_type;
+ commands_send_packet(send_buffer_motor_type, ind_motor_type);
+ }
+ }
+ }
+ }
+
+ //if (change_allowed == false && !appconf.send_can_status) {
+ if (!appconf.send_can_status) {
+ sendActualSpeedMode = true;
+ }
+
+ break;
default:
break;
}
+
+ if (sendActualSpeedMode) {
+ appconf = *app_get_configuration();
+ mcconf = *mc_interface_get_configuration();
+
+ ind = 0;
+ send_buffer[ind++] = COMM_GET_SPEED_MODE;
+ send_buffer[ind++] = remote_Mode;
+ if(appconf.app_to_use == APP_NUNCHUK || appconf.app_to_use == APP_NRF){
+ send_buffer[ind++] = appconf.app_chuk_conf.ctrl_type;
+ }else{
+ send_buffer[ind++] = appconf.app_ppm_conf.ctrl_type;
+ }
+
+ send_buffer[ind++] = mcconf.use_max_watt_limit;
+ buffer_append_int16(send_buffer, (int16_t) (mcconf.watts_max), &ind);
+
+ buffer_append_int16(send_buffer, (int16_t) (mcconf.l_current_max * 10), &ind);
+ buffer_append_int16(send_buffer, (int16_t) (mcconf.l_in_current_max * 10), &ind);
+ buffer_append_int16(send_buffer, (int16_t) (mcconf.l_current_min * 10), &ind);
+ buffer_append_int16(send_buffer, (int16_t) (mcconf.l_in_current_min * 10), &ind);
+
+ if(appconf.app_to_use == APP_NUNCHUK || appconf.app_to_use == APP_NRF){
+ buffer_append_int16(send_buffer, (int16_t) (appconf.app_chuk_conf.rpm_lim_end / 10), &ind);
+ }else{
+ buffer_append_int16(send_buffer, (int16_t) (appconf.app_ppm_conf.rpm_lim_end / 10), &ind);
+ }
+
+ buffer_append_int16(send_buffer, (int16_t) (mcconf.l_battery_cut_start * 100), &ind);
+ buffer_append_int16(send_buffer, (int16_t) (mcconf.l_battery_cut_end * 100), &ind);
+ buffer_append_int16(send_buffer, (int16_t) (mcconf.l_temp_fet_start * 100), &ind);
+ buffer_append_int16(send_buffer, (int16_t) (mcconf.l_temp_fet_end * 100), &ind);
+
+ send_buffer[ind++] = appconf.app_to_use;
+
+ send_buffer[ind++] = appconf.app_throttle_conf.adjustable_throttle_enabled;
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.y1_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.y2_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.y3_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.x1_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.x2_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.x3_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.bezier_reduce_factor * 100.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.y1_neg_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.y2_neg_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.y3_neg_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.x1_neg_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.x2_neg_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.x3_neg_throttle * 1000.0), &ind);
+ buffer_append_int16(send_buffer, (int16_t)(appconf.app_throttle_conf.bezier_neg_reduce_factor * 100.0), &ind);
+
+ send_buffer[ind++] = mcconf.s_pid_breaking_enabled;
+ buffer_append_int32(send_buffer, (int32_t)(mcconf.s_pid_kp * 1000000.0), &ind);
+ buffer_append_int32(send_buffer, (int32_t)(mcconf.s_pid_ki * 1000000.0), &ind);
+ buffer_append_int32(send_buffer, (int32_t)(mcconf.s_pid_kd * 1000000.0), &ind);
+
+ send_buffer[ind++] = mcconf.motor_type;
+
+ commands_send_packet(send_buffer, ind);
+ }
}
void commands_printf(char* format, ...) {
@@ -798,7 +1526,7 @@
int32_t index = 0;
send_buffer[index++] = COMM_CUSTOM_APP_DATA;
- memcpy(send_buffer, data, len);
+ memcpy(send_buffer + index, data, len);
index += len;
commands_send_packet(send_buffer, index);
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/commands.h Raptor2 Sources/bldc-firmware_2_80/commands.h
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/commands.h 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/commands.h 2016-09-19 10:06:42.000000000 +0100
@@ -38,5 +38,6 @@
void commands_send_experiment_samples(float *samples, int len);
disp_pos_mode commands_get_disp_pos_mode(void);
void commands_set_app_data_handler(void(*func)(unsigned char *data, unsigned int len));
+void commands_send_app_data(unsigned char *data, unsigned int len);
#endif /* COMMANDS_H_ */
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/conf_general.c Raptor2 Sources/bldc-firmware_2_80/conf_general.c
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/conf_general.c 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/conf_general.c 2017-04-15 03:04:20.000000000 +0100
@@ -47,7 +47,7 @@
memset(VirtAddVarTab, 0, sizeof(VirtAddVarTab));
int ind = 0;
- for (unsigned int i = 0;i < (sizeof(app_configuration) / 2);i++) {
+ for (unsigned int i = 0;i < (sizeof(mc_configuration) / 2);i++) {
VirtAddVarTab[ind++] = EEPROM_BASE_MCCONF + i;
}
@@ -89,7 +89,7 @@
conf->app_ppm_conf.multi_esc = APPCONF_PPM_MULTI_ESC;
conf->app_ppm_conf.tc = APPCONF_PPM_TC;
conf->app_ppm_conf.tc_max_diff = APPCONF_PPM_TC_MAX_DIFF;
-
+
conf->app_adc_conf.ctrl_type = APPCONF_ADC_CTRL_TYPE;
conf->app_adc_conf.hyst = APPCONF_ADC_HYST;
conf->app_adc_conf.voltage_start = APPCONF_ADC_VOLTAGE_START;
@@ -118,6 +118,45 @@
conf->app_chuk_conf.multi_esc = APPCONF_CHUK_MULTI_ESC;
conf->app_chuk_conf.tc = APPCONF_CHUK_TC;
conf->app_chuk_conf.tc_max_diff = APPCONF_CHUK_TC_MAX_DIFF;
+
+ conf->app_nrf_conf.speed = APPCONF_NRF_SPEED;
+ conf->app_nrf_conf.power = APPCONF_NRF_POWER;
+ conf->app_nrf_conf.crc_type = APPCONF_NRF_CRC;
+ conf->app_nrf_conf.retry_delay = APPCONF_NRF_RETR_DELAY;
+ conf->app_nrf_conf.retries = APPCONF_NRF_RETRIES;
+ conf->app_nrf_conf.channel = APPCONF_NRF_CHANNEL;
+ conf->app_nrf_conf.address[0] = APPCONF_NRF_ADDR_B0;
+ conf->app_nrf_conf.address[1] = APPCONF_NRF_ADDR_B1;
+ conf->app_nrf_conf.address[2] = APPCONF_NRF_ADDR_B2;
+ conf->app_nrf_conf.send_crc_ack = APPCONF_NRF_SEND_CRC_ACK;
+
+ //new config
+ conf->app_ppm_conf.pulse_center = APPCONF_PPM_PULSE_CENTER;
+ conf->app_ppm_conf.tc_offset = APPCONF_PPM_TC_OFFSET;
+ conf->app_ppm_conf.cruise_left = CRUISE_CONTROL_MOTOR_SETTINGS;
+ conf->app_ppm_conf.cruise_right = CRUISE_CONTROL_MOTOR_SETTINGS;
+ conf->app_ppm_conf.max_erpm_for_dir_active = APPCONF_PPM_MAX_ERPM_FOR_DIR_ACTIVE;
+ conf->app_ppm_conf.max_erpm_for_dir = APPCONF_PPM_MAX_ERPM_FOR_DIR;
+
+ conf->app_chuk_conf.tc_offset = APPCONF_CHUK_TC_OFFSET;
+ conf->app_chuk_conf.buttons_mirrored = APPCONF_CHUK_BUTTONS_MIRRORED;
+
+ conf->app_throttle_conf.adjustable_throttle_enabled = APPCONF_ADJUSTABLE_THROTTLE_ENABLED;
+ conf->app_throttle_conf.y1_throttle = APPCONF_Y1_THROTTLE;
+ conf->app_throttle_conf.y2_throttle = APPCONF_Y2_THROTTLE;
+ conf->app_throttle_conf.y3_throttle = APPCONF_Y3_THROTTLE;
+ conf->app_throttle_conf.x1_throttle = APPCONF_X1_THROTTLE;
+ conf->app_throttle_conf.x2_throttle = APPCONF_X2_THROTTLE;
+ conf->app_throttle_conf.x3_throttle = APPCONF_X3_THROTTLE;
+ conf->app_throttle_conf.bezier_reduce_factor = APPCONF_BEZIER_REDUCE_FACTOR;
+ conf->app_throttle_conf.y1_neg_throttle = APPCONF_Y1_NEG_THROTTLE;
+ conf->app_throttle_conf.y2_neg_throttle = APPCONF_Y2_NEG_THROTTLE;
+ conf->app_throttle_conf.y3_neg_throttle = APPCONF_Y3_NEG_THROTTLE;
+ conf->app_throttle_conf.x1_neg_throttle = APPCONF_X1_NEG_THROTTLE;
+ conf->app_throttle_conf.x2_neg_throttle = APPCONF_X2_NEG_THROTTLE;
+ conf->app_throttle_conf.x3_neg_throttle = APPCONF_X3_NEG_THROTTLE;
+ conf->app_throttle_conf.bezier_neg_reduce_factor = APPCONF_BEZIER_REDUCE_NEG_FACTOR;
+ //new config end
}
/**
@@ -168,6 +207,16 @@
conf->sl_cycle_int_rpm_br = MCCONF_SL_CYCLE_INT_BR;
conf->sl_bemf_coupling_k = MCCONF_SL_BEMF_COUPLING_K;
+ conf->hall_table[0] = MCCONF_HALL_TAB_0;
+ conf->hall_table[1] = MCCONF_HALL_TAB_1;
+ conf->hall_table[2] = MCCONF_HALL_TAB_2;
+ conf->hall_table[3] = MCCONF_HALL_TAB_3;
+ conf->hall_table[4] = MCCONF_HALL_TAB_4;
+ conf->hall_table[5] = MCCONF_HALL_TAB_5;
+ conf->hall_table[6] = MCCONF_HALL_TAB_6;
+ conf->hall_table[7] = MCCONF_HALL_TAB_7;
+ conf->hall_sl_erpm = MCCONF_HALL_ERPM;
+
conf->foc_current_kp = MCCONF_FOC_CURRENT_KP;
conf->foc_current_ki = MCCONF_FOC_CURRENT_KI;
conf->foc_f_sw = MCCONF_FOC_F_SW;
@@ -189,25 +238,26 @@
conf->foc_sl_openloop_time = MCCONF_FOC_SL_OPENLOOP_TIME;
conf->foc_sl_d_current_duty = MCCONF_FOC_SL_D_CURRENT_DUTY;
conf->foc_sl_d_current_factor = MCCONF_FOC_SL_D_CURRENT_FACTOR;
-
- conf->hall_table[0] = MCCONF_HALL_TAB_0;
- conf->hall_table[1] = MCCONF_HALL_TAB_1;
- conf->hall_table[2] = MCCONF_HALL_TAB_2;
- conf->hall_table[3] = MCCONF_HALL_TAB_3;
- conf->hall_table[4] = MCCONF_HALL_TAB_4;
- conf->hall_table[5] = MCCONF_HALL_TAB_5;
- conf->hall_table[6] = MCCONF_HALL_TAB_6;
- conf->hall_table[7] = MCCONF_HALL_TAB_7;
- conf->hall_sl_erpm = MCCONF_HALL_ERPM;
+ conf->foc_hall_table[0] = MCCONF_FOC_HALL_TAB_0;
+ conf->foc_hall_table[1] = MCCONF_FOC_HALL_TAB_1;
+ conf->foc_hall_table[2] = MCCONF_FOC_HALL_TAB_2;
+ conf->foc_hall_table[3] = MCCONF_FOC_HALL_TAB_3;
+ conf->foc_hall_table[4] = MCCONF_FOC_HALL_TAB_4;
+ conf->foc_hall_table[5] = MCCONF_FOC_HALL_TAB_5;
+ conf->foc_hall_table[6] = MCCONF_FOC_HALL_TAB_6;
+ conf->foc_hall_table[7] = MCCONF_FOC_HALL_TAB_7;
+ conf->foc_sl_erpm = MCCONF_FOC_SL_ERPM;
conf->s_pid_kp = MCCONF_S_PID_KP;
conf->s_pid_ki = MCCONF_S_PID_KI;
conf->s_pid_kd = MCCONF_S_PID_KD;
conf->s_pid_min_erpm = MCCONF_S_PID_MIN_RPM;
+ conf->s_pid_breaking_enabled = MCCONF_S_PID_BREAKING_ENABLED;
conf->p_pid_kp = MCCONF_P_PID_KP;
conf->p_pid_ki = MCCONF_P_PID_KI;
conf->p_pid_kd = MCCONF_P_PID_KD;
+ conf->p_pid_ang_div = MCCONF_P_PID_ANG_DIV;
conf->cc_startup_boost_duty = MCCONF_CC_STARTUP_BOOST_DUTY;
conf->cc_min_current = MCCONF_CC_MIN_CURRENT;
@@ -219,6 +269,10 @@
conf->m_duty_ramp_step_rpm_lim = MCCONF_M_RAMP_STEP_RPM_LIM;
conf->m_current_backoff_gain = MCCONF_M_CURRENT_BACKOFF_GAIN;
conf->m_encoder_counts = MCCONF_M_ENCODER_COUNTS;
+ conf->m_sensor_port_mode = MCCONF_M_SENSOR_PORT_MODE;
+
+ conf->use_max_watt_limit = MCCONF_USE_MAX_WATT_LIMIT;
+ conf->watts_max = MCCONF_WATT_MAX;
}
/**
@@ -242,6 +296,32 @@
}
}
+ if(is_ok == false){
+ chThdSleepMilliseconds(10);
+ for (unsigned int i = 0;i < (sizeof(app_configuration) / 2);i++) {
+ if (EE_ReadVariable(EEPROM_BASE_APPCONF + i, &var) == 0) {
+ conf_addr[2 * i] = (var >> 8) & 0xFF;
+ conf_addr[2 * i + 1] = var & 0xFF;
+ } else {
+ is_ok = false;
+ break;
+ }
+ }
+ }
+
+ if(is_ok == false){
+ chThdSleepMilliseconds(10);
+ for (unsigned int i = 0;i < (sizeof(app_configuration) / 2);i++) {
+ if (EE_ReadVariable(EEPROM_BASE_APPCONF + i, &var) == 0) {
+ conf_addr[2 * i] = (var >> 8) & 0xFF;
+ conf_addr[2 * i + 1] = var & 0xFF;
+ } else {
+ is_ok = false;
+ break;
+ }
+ }
+ }
+
// Set the default configuration
if (!is_ok) {
conf_general_get_default_app_configuration(conf);
@@ -277,6 +357,34 @@
break;
}
}
+
+ // try one more time
+ if(is_ok == false){
+ chThdSleepMilliseconds(10);
+ for (unsigned int i = 0;i < (sizeof(app_configuration) / 2);i++) {
+ var = (conf_addr[2 * i] << 8) & 0xFF00;
+ var |= conf_addr[2 * i + 1] & 0xFF;
+
+ if (EE_WriteVariable(EEPROM_BASE_APPCONF + i, var) != FLASH_COMPLETE) {
+ is_ok = false;
+ break;
+ }
+ }
+ }
+
+ // try again one more time
+ if(is_ok == false){
+ chThdSleepMilliseconds(10);
+ for (unsigned int i = 0;i < (sizeof(app_configuration) / 2);i++) {
+ var = (conf_addr[2 * i] << 8) & 0xFF00;
+ var |= conf_addr[2 * i + 1] & 0xFF;
+
+ if (EE_WriteVariable(EEPROM_BASE_APPCONF + i, var) != FLASH_COMPLETE) {
+ is_ok = false;
+ break;
+ }
+ }
+ }
RCC_APB1PeriphClockCmd(RCC_APB1Periph_WWDG, ENABLE);
utils_sys_unlock_cnt();
@@ -305,6 +413,34 @@
}
}
+ // try one more time
+ if(is_ok == false){
+ chThdSleepMilliseconds(10);
+ for (unsigned int i = 0;i < (sizeof(mc_configuration) / 2);i++) {
+ if (EE_ReadVariable(EEPROM_BASE_MCCONF + i, &var) == 0) {
+ conf_addr[2 * i] = (var >> 8) & 0xFF;
+ conf_addr[2 * i + 1] = var & 0xFF;
+ } else {
+ is_ok = false;
+ break;
+ }
+ }
+ }
+
+ // try another one more time
+ if(is_ok == false){
+ chThdSleepMilliseconds(10);
+ for (unsigned int i = 0;i < (sizeof(mc_configuration) / 2);i++) {
+ if (EE_ReadVariable(EEPROM_BASE_MCCONF + i, &var) == 0) {
+ conf_addr[2 * i] = (var >> 8) & 0xFF;
+ conf_addr[2 * i + 1] = var & 0xFF;
+ } else {
+ is_ok = false;
+ break;
+ }
+ }
+ }
+
if (!is_ok) {
conf_general_get_default_mc_configuration(conf);
}
@@ -339,6 +475,34 @@
break;
}
}
+
+ // try one more time
+ if(is_ok == false){
+ chThdSleepMilliseconds(10);
+ for (unsigned int i = 0;i < (sizeof(mc_configuration) / 2);i++) {
+ var = (conf_addr[2 * i] << 8) & 0xFF00;
+ var |= conf_addr[2 * i + 1] & 0xFF;
+
+ if (EE_WriteVariable(EEPROM_BASE_MCCONF + i, var) != FLASH_COMPLETE) {
+ is_ok = false;
+ break;
+ }
+ }
+ }
+
+ // try again one more time
+ if(is_ok == false){
+ chThdSleepMilliseconds(10);
+ for (unsigned int i = 0;i < (sizeof(mc_configuration) / 2);i++) {
+ var = (conf_addr[2 * i] << 8) & 0xFF00;
+ var |= conf_addr[2 * i + 1] & 0xFF;
+
+ if (EE_WriteVariable(EEPROM_BASE_MCCONF + i, var) != FLASH_COMPLETE) {
+ is_ok = false;
+ break;
+ }
+ }
+ }
RCC_APB1PeriphClockCmd(RCC_APB1Periph_WWDG, ENABLE);
utils_sys_unlock_cnt();
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/conf_general.h Raptor2 Sources/bldc-firmware_2_80/conf_general.h
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/conf_general.h 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/conf_general.h 2017-04-17 04:06:52.000000000 +0100
@@ -27,7 +27,7 @@
// Firmware version
#define FW_VERSION_MAJOR 2
-#define FW_VERSION_MINOR 9
+#define FW_VERSION_MINOR 80
#include "datatypes.h"
@@ -38,6 +38,7 @@
// Settings and parameters to override
//#define VIN_R1 33000.0
+#define VIN_R1 39000.0
//#define VIN_R2 2200.0
//#define CURRENT_AMP_GAIN 10.0
//#define CURRENT_SHUNT_RES 0.0005
@@ -56,7 +57,7 @@
//#define HW_VERSION_46 // Also for 4.7
//#define HW_VERSION_48
//#define HW_VERSION_49
-#define HW_VERSION_410 // Also for 4.11
+#define HW_VERSION_410 // Also for 4.11 and 4.12
//#define HW_VERSION_R2
//#define HW_VERSION_VICTOR_R1A
#endif
@@ -64,15 +65,15 @@
/*
* Select default user motor configuration
*/
-//#define MCCONF_DEFAULT_USER "mcconf_outrunner2.h"
//#define MCCONF_DEFAULT_USER "mcconf_sten.h"
-//#define MCCONF_DEFAULT_USER "mcconf_foc_erwin.h"
-//#define MCCONF_DEFAULT_USER "mcconf_foc_scorpion.h"
+//#define MCCONF_DEFAULT_USER "mcconf_sp_540kv.h"
+//#define MCCONF_DEFAULT_USER "mcconf_castle_2028.h"
/*
* Select default user app configuration
*/
//#define APPCONF_DEFAULT_USER "appconf_example_ppm.h"
+//#define APPCONF_DEFAULT_USER "appconf_custom.h"
/*
* Select which custom application to use. To configure the default applications and
@@ -82,13 +83,6 @@
//#define USE_APP_STEN
/*
- * Use encoder
- */
-#ifndef ENCODER_ENABLE
-#define ENCODER_ENABLE 0
-#endif
-
-/*
* Enable CAN-bus
*/
#define CAN_ENABLE 1
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/datatypes.h Raptor2 Sources/bldc-firmware_2_80/datatypes.h
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/datatypes.h 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/datatypes.h 2017-04-13 18:02:30.000000000 +0100
@@ -38,6 +38,13 @@
} mc_state;
typedef enum {
+ CRUISE_CONTROL_MOTOR_SETTINGS = 0,
+ CRUISE_CONTROL_BRAKING_DISABLED,
+ CRUISE_CONTROL_BRAKING_ENABLED,
+ CRUISE_CONTROL_INACTIVE
+} ppm_cruise;
+
+typedef enum {
PWM_MODE_NONSYNCHRONOUS_HISW = 0, // This mode is not recommended
PWM_MODE_SYNCHRONOUS, // The recommended and most tested mode
PWM_MODE_BIPOLAR // Some glitches occasionally, can kill MOSFETs
@@ -56,7 +63,8 @@
typedef enum {
FOC_SENSOR_MODE_SENSORLESS = 0,
- FOC_SENSOR_MODE_ENCODER
+ FOC_SENSOR_MODE_ENCODER,
+ FOC_SENSOR_MODE_HALL
} mc_foc_sensor_mode;
typedef enum {
@@ -89,10 +97,17 @@
DISP_POS_MODE_INDUCTANCE,
DISP_POS_MODE_OBSERVER,
DISP_POS_MODE_ENCODER,
- DISP_POS_MODE_ENCODER_POS_ERROR,
+ DISP_POS_MODE_PID_POS,
+ DISP_POS_MODE_PID_POS_ERROR,
DISP_POS_MODE_ENCODER_OBSERVER_ERROR
} disp_pos_mode;
+typedef enum {
+ SENSOR_PORT_MODE_HALL = 0,
+ SENSOR_PORT_MODE_ABI,
+ SENSOR_PORT_MODE_AS5047_SPI
+} sensor_port_mode;
+
typedef struct {
float cycle_int_limit;
float cycle_int_limit_running;
@@ -115,6 +130,8 @@
float l_in_current_max;
float l_in_current_min;
float l_abs_current_max;
+ bool use_max_watt_limit;
+ float watts_max;
float l_min_erpm;
float l_max_erpm;
float l_max_erpm_fbrake;
@@ -169,15 +186,19 @@
float foc_sl_d_current_duty;
float foc_sl_d_current_factor;
mc_foc_sensor_mode foc_sensor_mode;
+ uint8_t foc_hall_table[8];
+ float foc_sl_erpm;
// Speed PID
float s_pid_kp;
float s_pid_ki;
float s_pid_kd;
float s_pid_min_erpm;
+ bool s_pid_breaking_enabled;
// Pos PID
float p_pid_kp;
float p_pid_ki;
float p_pid_kd;
+ float p_pid_ang_div;
// Current controller
float cc_startup_boost_duty;
float cc_min_current;
@@ -189,6 +210,7 @@
float m_duty_ramp_step_rpm_lim;
float m_current_backoff_gain;
uint32_t m_encoder_counts;
+ sensor_port_mode m_sensor_port_mode;
} mc_configuration;
// Applications to use
@@ -213,7 +235,11 @@
PPM_CTRL_TYPE_DUTY,
PPM_CTRL_TYPE_DUTY_NOREV,
PPM_CTRL_TYPE_PID,
- PPM_CTRL_TYPE_PID_NOREV
+ PPM_CTRL_TYPE_PID_NOREV,
+ PPM_CTRL_TYPE_WATT_NOREV_BRAKE,
+ PPM_CTRL_TYPE_PID_NOACCELERATION,
+ PPM_CTRL_TYPE_CRUISE_CONTROL_SECONDARY_CHANNEL,
+ PPM_CTRL_TYPE_WATT
} ppm_control_type;
typedef struct {
@@ -229,6 +255,12 @@
bool multi_esc;
bool tc;
float tc_max_diff;
+ float pulse_center;
+ float tc_offset;
+ ppm_cruise cruise_left;
+ ppm_cruise cruise_right;
+ bool max_erpm_for_dir_active;
+ float max_erpm_for_dir;
} ppm_config;
// ADC control types
@@ -239,6 +271,7 @@
ADC_CTRL_TYPE_CURRENT_REV_BUTTON,
ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_CENTER,
ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_BUTTON,
+ ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_ADC,
ADC_CTRL_TYPE_DUTY,
ADC_CTRL_TYPE_DUTY_REV_CENTER,
ADC_CTRL_TYPE_DUTY_REV_BUTTON
@@ -266,7 +299,9 @@
typedef enum {
CHUK_CTRL_TYPE_NONE = 0,
CHUK_CTRL_TYPE_CURRENT,
- CHUK_CTRL_TYPE_CURRENT_NOREV
+ CHUK_CTRL_TYPE_CURRENT_NOREV,
+ CHUK_CTRL_TYPE_WATT,
+ CHUK_CTRL_TYPE_WATT_NOREV
} chuk_control_type;
typedef struct {
@@ -280,8 +315,84 @@
bool multi_esc;
bool tc;
float tc_max_diff;
+ float tc_offset;
+ bool buttons_mirrored;
} chuk_config;
+// NRF Datatypes
+typedef enum {
+ NRF_SPEED_250K = 0,
+ NRF_SPEED_1M,
+ NRF_SPEED_2M
+} NRF_SPEED;
+
+typedef enum {
+ NRF_POWER_M18DBM = 0,
+ NRF_POWER_M12DBM,
+ NRF_POWER_M6DBM,
+ NRF_POWER_0DBM
+} NRF_POWER;
+
+typedef enum {
+ NRF_AW_3 = 0,
+ NRF_AW_4,
+ NRF_AW_5
+} NRF_AW;
+
+typedef enum {
+ NRF_CRC_DISABLED = 0,
+ NRF_CRC_1B,
+ NRF_CRC_2B
+} NRF_CRC;
+
+typedef enum {
+ NRF_RETR_DELAY_250US = 0,
+ NRF_RETR_DELAY_500US,
+ NRF_RETR_DELAY_750US,
+ NRF_RETR_DELAY_1000US,
+ NRF_RETR_DELAY_1250US,
+ NRF_RETR_DELAY_1500US,
+ NRF_RETR_DELAY_1750US,
+ NRF_RETR_DELAY_2000US,
+ NRF_RETR_DELAY_2250US,
+ NRF_RETR_DELAY_2500US,
+ NRF_RETR_DELAY_2750US,
+ NRF_RETR_DELAY_3000US,
+ NRF_RETR_DELAY_3250US,
+ NRF_RETR_DELAY_3500US,
+ NRF_RETR_DELAY_3750US,
+ NRF_RETR_DELAY_4000US
+} NRF_RETR_DELAY;
+
+typedef struct {
+ NRF_SPEED speed;
+ NRF_POWER power;
+ NRF_CRC crc_type;
+ NRF_RETR_DELAY retry_delay;
+ unsigned char retries;
+ unsigned char channel;
+ unsigned char address[3];
+ bool send_crc_ack;
+} nrf_config;
+
+typedef struct {
+ bool adjustable_throttle_enabled;
+ float y1_throttle;
+ float y2_throttle;
+ float y3_throttle;
+ float x1_throttle;
+ float x2_throttle;
+ float x3_throttle;
+ float bezier_reduce_factor;
+ float y1_neg_throttle;
+ float y2_neg_throttle;
+ float y3_neg_throttle;
+ float x1_neg_throttle;
+ float x2_neg_throttle;
+ float x3_neg_throttle;
+ float bezier_neg_reduce_factor;
+} throttle_config;
+
typedef struct {
// Settings
uint8_t controller_id;
@@ -304,6 +415,11 @@
// Nunchuk application settings
chuk_config app_chuk_conf;
+
+ // NRF application settings
+ nrf_config app_nrf_conf;
+
+ throttle_config app_throttle_conf;
} app_configuration;
// Communication commands
@@ -335,6 +451,7 @@
COMM_DETECT_MOTOR_R_L,
COMM_DETECT_MOTOR_FLUX_LINKAGE,
COMM_DETECT_ENCODER,
+ COMM_DETECT_HALL_FOC,
COMM_REBOOT,
COMM_ALIVE,
COMM_GET_DECODED_PPM,
@@ -342,7 +459,12 @@
COMM_GET_DECODED_CHUK,
COMM_FORWARD_CAN,
COMM_SET_CHUCK_DATA,
- COMM_CUSTOM_APP_DATA
+ COMM_CUSTOM_APP_DATA,
+ COMM_SET_SPEED_MODE,
+ COMM_CHANGE_SPEED_MODE,
+ COMM_GET_SPEED_MODE,
+ COMM_SET_CURRENT_CONF_AS_DEFAULT,
+ COMM_SET_MOTOR_TYPE
} COMM_PACKET_ID;
// CAN commands
@@ -356,7 +478,9 @@
CAN_PACKET_FILL_RX_BUFFER_LONG,
CAN_PACKET_PROCESS_RX_BUFFER,
CAN_PACKET_PROCESS_SHORT_BUFFER,
- CAN_PACKET_STATUS
+ CAN_PACKET_STATUS,
+ CAN_PACKET_SET_SERVO,
+ CAN_PACKET_SET_BRAKE_SERVO
} CAN_PACKET_ID;
// Logged fault data
@@ -403,7 +527,8 @@
systime_t rx_time;
float rpm;
float current;
- float duty;
+ int8_t duty;
+ ppm_cruise cruise_control_status;
} can_status_msg;
typedef struct {
@@ -431,20 +556,20 @@
float temp_mos2;
float temp_mos3;
float temp_mos4;
- float temp_mos5;
- float temp_mos6;
- float temp_pcb;
- float current_motor;
- float current_in;
- float rpm;
- float duty_now;
- float amp_hours;
- float amp_hours_charged;
- float watt_hours;
- float watt_hours_charged;
- int tachometer;
- int tachometer_abs;
- mc_fault_code fault_code;
+ float temp_mos5;
+ float temp_mos6;
+ float temp_pcb;
+ float current_motor;
+ float current_in;
+ float rpm;
+ float duty_now;
+ float amp_hours;
+ float amp_hours_charged;
+ float watt_hours;
+ float watt_hours_charged;
+ int tachometer;
+ int tachometer_abs;
+ mc_fault_code fault_code;
} mc_values;
#endif /* DATATYPES_H_ */
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/eeprom.h Raptor2 Sources/bldc-firmware_2_80/eeprom.h
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/eeprom.h 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/eeprom.h 2017-04-13 18:03:20.000000000 +0100
@@ -69,7 +69,7 @@
#define PAGE_FULL ((uint8_t)0x80)
/* Variables' number */
-#define NB_OF_VAR ((uint8_t)160)
+#define NB_OF_VAR ((uint8_t)255)
/* Exported types ------------------------------------------------------------*/
/* Exported macro ------------------------------------------------------------*/
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/encoder.c Raptor2 Sources/bldc-firmware_2_80/encoder.c
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/encoder.c 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/encoder.c 2016-09-19 10:06:24.000000000 +0100
@@ -1,5 +1,5 @@
/*
- Copyright 2012-2015 Benjamin Vedder benjamin@vedder.se
+ Copyright 2012-2016 Benjamin Vedder benjamin@vedder.se
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
@@ -15,26 +15,64 @@
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
-/*
- * encoder.c
- *
- * Created on: 7 mar 2015
- * Author: benjamin
- */
-
#include "encoder.h"
#include "ch.h"
#include "hal.h"
#include "stm32f4xx_conf.h"
#include "hw.h"
+#include "utils.h"
+
+// Defines
+#define AS5047P_READ_ANGLECOM (0x3FFF | 0x4000 | 0x8000) // This is just ones
+#define AS5047_SAMPLE_RATE_HZ 20000
+
+#define SPI_SW_MISO_GPIO HW_HALL_ENC_GPIO2
+#define SPI_SW_MISO_PIN HW_HALL_ENC_PIN2
+#define SPI_SW_SCK_GPIO HW_HALL_ENC_GPIO1
+#define SPI_SW_SCK_PIN HW_HALL_ENC_PIN1
+#define SPI_SW_CS_GPIO HW_HALL_ENC_GPIO3
+#define SPI_SW_CS_PIN HW_HALL_ENC_PIN3
+
+// Private types
+typedef enum {
+ ENCODER_MODE_NONE = 0,
+ ENCODER_MODE_ABI,
+ ENCODER_MODE_AS5047P_SPI
+} encoder_mode;
// Private variables
-static bool index_found;
+static bool index_found = false;
static uint32_t enc_counts = 10000;
+static encoder_mode mode = ENCODER_MODE_NONE;
+static float last_enc_angle = 0.0;
+
+// Private functions
+uint16_t spi_exchange(uint16_t x);
+static void spi_transfer(uint16_t *in_buf, const uint16_t *out_buf, int length);
+static void spi_begin(void);
+static void spi_end(void);
+static void spi_delay(void);
+
+void encoder_deinit(void) {
+ nvicDisableVector(HW_ENC_EXTI_CH);
+ nvicDisableVector(HW_ENC_TIM_ISR_CH);
+
+ TIM_DeInit(HW_ENC_TIM);
+
+ palSetPadMode(SPI_SW_MISO_GPIO, SPI_SW_MISO_PIN, PAL_MODE_INPUT_PULLUP);
+ palSetPadMode(SPI_SW_SCK_GPIO, SPI_SW_SCK_PIN, PAL_MODE_INPUT_PULLUP);
+ palSetPadMode(SPI_SW_CS_GPIO, SPI_SW_CS_PIN, PAL_MODE_INPUT_PULLUP);
-void encoder_init(uint32_t counts) {
+ palSetPadMode(HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1, PAL_MODE_INPUT_PULLUP);
+ palSetPadMode(HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2, PAL_MODE_INPUT_PULLUP);
+
+ index_found = false;
+ mode = ENCODER_MODE_NONE;
+ last_enc_angle = 0.0;
+}
+
+void encoder_init_abi(uint32_t counts) {
EXTI_InitTypeDef EXTI_InitStructure;
- NVIC_InitTypeDef NVIC_InitStructure;
// Initialize variables
index_found = false;
@@ -61,7 +99,7 @@
TIM_ICPolarity_Rising);
TIM_SetAutoreload(HW_ENC_TIM, enc_counts - 1);
- TIM_Cmd (HW_ENC_TIM, ENABLE);
+ TIM_Cmd(HW_ENC_TIM, ENABLE);
// Interrupt on index pulse
@@ -76,15 +114,62 @@
EXTI_Init(&EXTI_InitStructure);
// Enable and set EXTI Line Interrupt to the highest priority
- NVIC_InitStructure.NVIC_IRQChannel = HW_ENC_EXTI_CH;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x00;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x00;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- NVIC_Init(&NVIC_InitStructure);
+ nvicEnableVector(HW_ENC_EXTI_CH, 0);
+
+ mode = ENCODER_MODE_ABI;
+}
+
+void encoder_init_as5047p_spi(void) {
+ TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
+
+ palSetPadMode(SPI_SW_MISO_GPIO, SPI_SW_MISO_PIN, PAL_MODE_INPUT);
+ palSetPadMode(SPI_SW_SCK_GPIO, SPI_SW_SCK_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
+ palSetPadMode(SPI_SW_CS_GPIO, SPI_SW_CS_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
+
+ // Enable timer clock
+ HW_ENC_TIM_CLK_EN();
+
+ // Time Base configuration
+ TIM_TimeBaseStructure.TIM_Prescaler = 0;
+ TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
+ TIM_TimeBaseStructure.TIM_Period = ((168000000 / 2 / AS5047_SAMPLE_RATE_HZ) - 1);
+ TIM_TimeBaseStructure.TIM_ClockDivision = 0;
+ TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
+ TIM_TimeBaseInit(HW_ENC_TIM, &TIM_TimeBaseStructure);
+
+ // Enable overflow interrupt
+ TIM_ITConfig(HW_ENC_TIM, TIM_IT_Update, ENABLE);
+
+ // Enable timer
+ TIM_Cmd(HW_ENC_TIM, ENABLE);
+
+ nvicEnableVector(HW_ENC_TIM_ISR_CH, 6);
+
+ mode = ENCODER_MODE_AS5047P_SPI;
+ index_found = true;
+}
+
+bool encoder_is_configured(void) {
+ return mode != ENCODER_MODE_NONE;
}
float encoder_read_deg(void) {
- return ((float)HW_ENC_TIM->CNT * 360.0) / (float)enc_counts;
+ static float angle = 0.0;
+
+ switch (mode) {
+ case ENCODER_MODE_ABI:
+ angle = ((float)HW_ENC_TIM->CNT * 360.0) / (float)enc_counts;
+ break;
+
+ case ENCODER_MODE_AS5047P_SPI:
+ angle = last_enc_angle;
+ break;
+
+ default:
+ break;
+ }
+
+ return angle;
}
/**
@@ -96,6 +181,20 @@
}
/**
+ * Timer interrupt
+ */
+void encoder_tim_isr(void) {
+ uint16_t pos;
+
+ spi_begin();
+ spi_transfer(&pos, 0, 1);
+ spi_end();
+
+ pos &= 0x3FFF;
+ last_enc_angle = ((float)pos * 360.0) / 16384.0;
+}
+
+/**
* Set the number of encoder counts.
*
* @param counts
@@ -118,3 +217,53 @@
bool encoder_index_found(void) {
return index_found;
}
+
+// Software SPI
+uint16_t spi_exchange(uint16_t x) {
+ uint16_t rx;
+ spi_transfer(&rx, &x, 1);
+ return rx;
+}
+
+static void spi_transfer(uint16_t *in_buf, const uint16_t *out_buf, int length) {
+ for (int i = 0;i < length;i++) {
+ uint16_t send = out_buf ? out_buf[i] : 0xFFFF;
+ uint16_t recieve = 0;
+
+ for (int bit = 0;bit < 16;bit++) {
+ //palWritePad(HW_SPI_PORT_MOSI, HW_SPI_PIN_MOSI, send >> 15);
+ send <<= 1;
+
+ spi_delay();
+ palSetPad(SPI_SW_SCK_GPIO, SPI_SW_SCK_PIN);
+ spi_delay();
+
+ recieve <<= 1;
+ if (palReadPad(SPI_SW_MISO_GPIO, SPI_SW_MISO_PIN)) {
+ recieve |= 1;
+ }
+
+ palClearPad(SPI_SW_SCK_GPIO, SPI_SW_SCK_PIN);
+ spi_delay();
+ }
+
+ if (in_buf) {
+ in_buf[i] = recieve;
+ }
+ }
+}
+
+static void spi_begin(void) {
+ palClearPad(SPI_SW_CS_GPIO, SPI_SW_CS_PIN);
+}
+
+static void spi_end(void) {
+ palSetPad(SPI_SW_CS_GPIO, SPI_SW_CS_PIN);
+}
+
+static void spi_delay(void) {
+ __NOP();
+ __NOP();
+ __NOP();
+ __NOP();
+}
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/encoder.h Raptor2 Sources/bldc-firmware_2_80/encoder.h
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/encoder.h 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/encoder.h 2016-09-19 10:06:22.000000000 +0100
@@ -28,9 +28,13 @@
#include "conf_general.h"
// Functions
-void encoder_init(uint32_t counts);
+void encoder_deinit(void);
+void encoder_init_abi(uint32_t counts);
+void encoder_init_as5047p_spi(void);
+bool encoder_is_configured(void);
float encoder_read_deg(void);
void encoder_reset(void);
+void encoder_tim_isr(void);
void encoder_set_counts(uint32_t counts);
bool encoder_index_found(void);
Only in Raptor2 Sources/bldc-firmware_2_80: gpl.txt
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/halconf.h Raptor2 Sources/bldc-firmware_2_80/halconf.h
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/halconf.h 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/halconf.h 2016-09-19 10:06:16.000000000 +0100
@@ -146,7 +146,7 @@
* @brief Enables the SPI subsystem.
*/
#if !defined(HAL_USE_SPI) || defined(__DOXYGEN__)
-#define HAL_USE_SPI FALSE
+#define HAL_USE_SPI TRUE
#endif
/**
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/hwconf/hw_40.h Raptor2 Sources/bldc-firmware_2_80/hwconf/hw_40.h
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/hwconf/hw_40.h 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/hwconf/hw_40.h 2016-08-04 23:47:30.000000000 +0100
@@ -146,6 +146,8 @@
#define HW_ENC_EXTI_CH EXTI9_5_IRQn
#define HW_ENC_EXTI_LINE EXTI_Line8
#define HW_ENC_EXTI_ISR_VEC EXTI9_5_IRQHandler
+#define HW_ENC_TIM_ISR_CH TIM4_IRQn
+#define HW_ENC_TIM_ISR_VEC TIM4_IRQHandler
// NRF pins
#define NRF_PORT_CSN HW_ICU_GPIO
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/hwconf/hw_410.h Raptor2 Sources/bldc-firmware_2_80/hwconf/hw_410.h
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/hwconf/hw_410.h 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/hwconf/hw_410.h 2016-08-04 23:47:30.000000000 +0100
@@ -148,6 +148,8 @@
#define HW_ENC_EXTI_CH EXTI15_10_IRQn
#define HW_ENC_EXTI_LINE EXTI_Line11
#define HW_ENC_EXTI_ISR_VEC EXTI15_10_IRQHandler
+#define HW_ENC_TIM_ISR_CH TIM4_IRQn
+#define HW_ENC_TIM_ISR_VEC TIM4_IRQHandler
// NRF pins
#define NRF_PORT_CSN GPIOA
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/hwconf/hw_45.h Raptor2 Sources/bldc-firmware_2_80/hwconf/hw_45.h
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/hwconf/hw_45.h 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/hwconf/hw_45.h 2016-08-04 23:47:30.000000000 +0100
@@ -146,6 +146,8 @@
#define HW_ENC_EXTI_CH EXTI15_10_IRQn
#define HW_ENC_EXTI_LINE EXTI_Line11
#define HW_ENC_EXTI_ISR_VEC EXTI15_10_IRQHandler
+#define HW_ENC_TIM_ISR_CH TIM4_IRQn
+#define HW_ENC_TIM_ISR_VEC TIM4_IRQHandler
// NRF pins
#define NRF_PORT_CSN HW_ICU_GPIO
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/hwconf/hw_46.h Raptor2 Sources/bldc-firmware_2_80/hwconf/hw_46.h
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/hwconf/hw_46.h 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/hwconf/hw_46.h 2016-08-04 23:47:30.000000000 +0100
@@ -154,6 +154,8 @@
#define HW_ENC_EXTI_CH EXTI15_10_IRQn
#define HW_ENC_EXTI_LINE EXTI_Line11
#define HW_ENC_EXTI_ISR_VEC EXTI15_10_IRQHandler
+#define HW_ENC_TIM_ISR_CH TIM4_IRQn
+#define HW_ENC_TIM_ISR_VEC TIM4_IRQHandler
// NRF pins
#define NRF_PORT_CSN HW_ICU_GPIO
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/hwconf/hw_48.h Raptor2 Sources/bldc-firmware_2_80/hwconf/hw_48.h
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/hwconf/hw_48.h 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/hwconf/hw_48.h 2016-08-04 23:47:30.000000000 +0100
@@ -148,6 +148,8 @@
#define HW_ENC_EXTI_CH EXTI15_10_IRQn
#define HW_ENC_EXTI_LINE EXTI_Line11
#define HW_ENC_EXTI_ISR_VEC EXTI15_10_IRQHandler
+#define HW_ENC_TIM_ISR_CH TIM4_IRQn
+#define HW_ENC_TIM_ISR_VEC TIM4_IRQHandler
// NRF pins
#define NRF_PORT_CSN GPIOB
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/hwconf/hw_49.h Raptor2 Sources/bldc-firmware_2_80/hwconf/hw_49.h
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/hwconf/hw_49.h 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/hwconf/hw_49.h 2016-08-04 23:47:30.000000000 +0100
@@ -148,6 +148,8 @@
#define HW_ENC_EXTI_CH EXTI15_10_IRQn
#define HW_ENC_EXTI_LINE EXTI_Line11
#define HW_ENC_EXTI_ISR_VEC EXTI15_10_IRQHandler
+#define HW_ENC_TIM_ISR_CH TIM4_IRQn
+#define HW_ENC_TIM_ISR_VEC TIM4_IRQHandler
// NRF pins
#define NRF_PORT_CSN GPIOB
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/hwconf/hw_r2.h Raptor2 Sources/bldc-firmware_2_80/hwconf/hw_r2.h
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/hwconf/hw_r2.h 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/hwconf/hw_r2.h 2016-08-04 23:47:30.000000000 +0100
@@ -156,6 +156,8 @@
#define HW_ENC_EXTI_CH EXTI9_5_IRQn
#define HW_ENC_EXTI_LINE EXTI_Line8
#define HW_ENC_EXTI_ISR_VEC EXTI9_5_IRQHandler
+#define HW_ENC_TIM_ISR_CH TIM3_IRQn
+#define HW_ENC_TIM_ISR_VEC TIM3_IRQHandler
// NRF pins
#define NRF_PORT_CSN HW_ICU_GPIO
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/hwconf/hw_victor_r1a.h Raptor2 Sources/bldc-firmware_2_80/hwconf/hw_victor_r1a.h
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/hwconf/hw_victor_r1a.h 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/hwconf/hw_victor_r1a.h 2016-08-04 23:47:30.000000000 +0100
@@ -146,6 +146,8 @@
#define HW_ENC_EXTI_CH EXTI15_10_IRQn
#define HW_ENC_EXTI_LINE EXTI_Line11
#define HW_ENC_EXTI_ISR_VEC EXTI15_10_IRQHandler
+#define HW_ENC_TIM_ISR_CH TIM4_IRQn
+#define HW_ENC_TIM_ISR_VEC TIM4_IRQHandler
// NRF pins
#define NRF_PORT_CSN HW_ICU_GPIO
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/irq_handlers.c Raptor2 Sources/bldc-firmware_2_80/irq_handlers.c
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/irq_handlers.c 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/irq_handlers.c 2016-09-19 10:06:12.000000000 +0100
@@ -19,7 +19,6 @@
#include "hal.h"
#include "stm32f4xx_conf.h"
#include "isr_vector_table.h"
-#include "main.h"
#include "mc_interface.h"
#include "servo.h"
#include "hw.h"
@@ -49,3 +48,12 @@
EXTI_ClearITPendingBit(HW_ENC_EXTI_LINE);
}
}
+
+CH_IRQ_HANDLER(HW_ENC_TIM_ISR_VEC) {
+ if (TIM_GetITStatus(HW_ENC_TIM, TIM_IT_Update) != RESET) {
+ encoder_tim_isr();
+
+ // Clear the IT pending bit
+ TIM_ClearITPendingBit(HW_ENC_TIM, TIM_IT_Update);
+ }
+}
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/main.c Raptor2 Sources/bldc-firmware_2_80/main.c
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/main.c 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/main.c 2016-09-19 10:05:50.000000000 +0100
@@ -24,7 +24,6 @@
#include <string.h>
#include <stdlib.h>
-#include "main.h"
#include "mc_interface.h"
#include "mcpwm.h"
#include "mcpwm_foc.h"
@@ -68,48 +67,12 @@
*
*/
-/*
- * Notes:
- *
- * Disable USB VBUS sensing:
- * ChibiOS-RT-master/os/hal/platforms/STM32/OTGv1/usb_lld.c
- *
- * change
- * otgp->GCCFG = GCCFG_VBUSASEN | GCCFG_VBUSBSEN | GCCFG_PWRDWN;
- * to
- * otgp->GCCFG = GCCFG_NOVBUSSENS | GCCFG_PWRDWN;
- *
- * This should be handled automatically with the latest version of
- * ChibiOS since I have added an option to the makefile.
- *
- */
-
// Private variables
-#define ADC_SAMPLE_MAX_LEN 2000
-static volatile int16_t curr0_samples[ADC_SAMPLE_MAX_LEN];
-static volatile int16_t curr1_samples[ADC_SAMPLE_MAX_LEN];
-static volatile int16_t ph1_samples[ADC_SAMPLE_MAX_LEN];
-static volatile int16_t ph2_samples[ADC_SAMPLE_MAX_LEN];
-static volatile int16_t ph3_samples[ADC_SAMPLE_MAX_LEN];
-static volatile int16_t vzero_samples[ADC_SAMPLE_MAX_LEN];
-static volatile uint8_t status_samples[ADC_SAMPLE_MAX_LEN];
-static volatile int16_t curr_fir_samples[ADC_SAMPLE_MAX_LEN];
-static volatile int16_t f_sw_samples[ADC_SAMPLE_MAX_LEN];
-
-static volatile int sample_len = 1000;
-static volatile int sample_int = 1;
-static volatile int sample_ready = 1;
-static volatile int sample_now = 0;
-static volatile int sample_at_start = 0;
-static volatile int start_comm = 0;
-static volatile float main_last_adc_duration = 0.0;
+
static THD_WORKING_AREA(periodic_thread_wa, 1024);
-static THD_WORKING_AREA(sample_send_thread_wa, 1024);
static THD_WORKING_AREA(timer_thread_wa, 128);
-static thread_t *sample_send_tp;
-
static THD_FUNCTION(periodic_thread, arg) {
(void)arg;
@@ -147,8 +110,12 @@
commands_send_rotor_pos(encoder_read_deg());
break;
- case DISP_POS_MODE_ENCODER_POS_ERROR:
- commands_send_rotor_pos(utils_angle_difference(mc_interface_get_pos_set(), encoder_read_deg()));
+ case DISP_POS_MODE_PID_POS:
+ commands_send_rotor_pos(mc_interface_get_pid_pos_now());
+ break;
+
+ case DISP_POS_MODE_PID_POS_ERROR:
+ commands_send_rotor_pos(utils_angle_difference(mc_interface_get_pid_pos_set(), mc_interface_get_pid_pos_now()));
break;
default:
@@ -174,43 +141,6 @@
}
}
-static THD_FUNCTION(sample_send_thread, arg) {
- (void)arg;
-
- chRegSetThreadName("Main sample");
-
- sample_send_tp = chThdGetSelfX();
-
- for(;;) {
- chEvtWaitAny((eventmask_t) 1);
-
- for (int i = 0;i < sample_len;i++) {
- uint8_t buffer[20];
- int index = 0;
-
- buffer[index++] = curr0_samples[i] >> 8;
- buffer[index++] = curr0_samples[i];
- buffer[index++] = curr1_samples[i] >> 8;
- buffer[index++] = curr1_samples[i];
- buffer[index++] = ph1_samples[i] >> 8;
- buffer[index++] = ph1_samples[i];
- buffer[index++] = ph2_samples[i] >> 8;
- buffer[index++] = ph2_samples[i];
- buffer[index++] = ph3_samples[i] >> 8;
- buffer[index++] = ph3_samples[i];
- buffer[index++] = vzero_samples[i] >> 8;
- buffer[index++] = vzero_samples[i];
- buffer[index++] = status_samples[i];
- buffer[index++] = curr_fir_samples[i] >> 8;
- buffer[index++] = curr_fir_samples[i];
- buffer[index++] = f_sw_samples[i] >> 8;
- buffer[index++] = f_sw_samples[i];
-
- commands_send_samples(buffer, index);
- }
- }
-}
-
static THD_FUNCTION(timer_thread, arg) {
(void)arg;
@@ -222,85 +152,6 @@
}
}
-/*
- * Called every time new ADC values are available. Note that
- * the ADC is initialized from mcpwm.c
- */
-void main_dma_adc_handler(void) {
- ledpwm_update_pwm();
-
- if (sample_at_start && (mc_interface_get_state() == MC_STATE_RUNNING ||
- start_comm != mcpwm_get_comm_step())) {
- sample_now = 0;
- sample_ready = 0;
- sample_at_start = 0;
- }
-
- static int a = 0;
- if (!sample_ready) {
- a++;
- if (a >= sample_int) {
- a = 0;
-
- if (mc_interface_get_state() == MC_STATE_DETECTING) {
- curr0_samples[sample_now] = (int16_t)mcpwm_detect_currents[mcpwm_get_comm_step() - 1];
- curr1_samples[sample_now] = (int16_t)mcpwm_detect_currents_diff[mcpwm_get_comm_step() - 1];
-
- ph1_samples[sample_now] = (int16_t)mcpwm_detect_voltages[0];
- ph2_samples[sample_now] = (int16_t)mcpwm_detect_voltages[1];
- ph3_samples[sample_now] = (int16_t)mcpwm_detect_voltages[2];
- } else {
- curr0_samples[sample_now] = ADC_curr_norm_value[0];
- curr1_samples[sample_now] = ADC_curr_norm_value[1];
-
- ph1_samples[sample_now] = ADC_V_L1 - mcpwm_vzero;
- ph2_samples[sample_now] = ADC_V_L2 - mcpwm_vzero;
- ph3_samples[sample_now] = ADC_V_L3 - mcpwm_vzero;
- }
-
- vzero_samples[sample_now] = mcpwm_vzero;
-
- curr_fir_samples[sample_now] = (int16_t)(mc_interface_get_tot_current() * 100.0);
- f_sw_samples[sample_now] = (int16_t)(mc_interface_get_switching_frequency_now() / 10.0);
-
- status_samples[sample_now] = mcpwm_get_comm_step() | (mcpwm_read_hall_phase() << 3);
-
- sample_now++;
-
- if (sample_now == sample_len) {
- sample_ready = 1;
- sample_now = 0;
- chSysLockFromISR();
- chEvtSignalI(sample_send_tp, (eventmask_t) 1);
- chSysUnlockFromISR();
- }
-
- main_last_adc_duration = mcpwm_get_last_adc_isr_duration();
- }
- }
-}
-
-float main_get_last_adc_isr_duration(void) {
- return main_last_adc_duration;
-}
-
-void main_sample_print_data(bool at_start, uint16_t len, uint8_t decimation) {
- if (len > ADC_SAMPLE_MAX_LEN) {
- len = ADC_SAMPLE_MAX_LEN;
- }
-
- sample_len = len;
- sample_int = decimation;
-
- if (at_start) {
- sample_at_start = 1;
- start_comm = mcpwm_get_comm_step();
- } else {
- sample_now = 0;
- sample_ready = 0;
- }
-}
-
int main(void) {
halInit();
chSysInit();
@@ -316,11 +167,6 @@
mc_configuration mcconf;
conf_general_read_mc_configuration(&mcconf);
-
-#if ENCODER_ENABLE
- encoder_init(mcconf.m_encoder_counts);
-#endif
-
mc_interface_init(&mcconf);
commands_init();
@@ -352,14 +198,13 @@
// Threads
chThdCreateStatic(periodic_thread_wa, sizeof(periodic_thread_wa), NORMALPRIO, periodic_thread, NULL);
- chThdCreateStatic(sample_send_thread_wa, sizeof(sample_send_thread_wa), NORMALPRIO - 1, sample_send_thread, NULL);
chThdCreateStatic(timer_thread_wa, sizeof(timer_thread_wa), NORMALPRIO, timer_thread, NULL);
for(;;) {
chThdSleepMilliseconds(10);
-#if ENCODER_ENABLE
-// comm_can_set_pos(0, encoder_read_deg());
-#endif
+ if (encoder_is_configured()) {
+ // comm_can_set_pos(0, encoder_read_deg());
+ }
}
}
Only in bldc-90986c31defe50830b4795a388ee5b2687dc2516: main.h
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/mc_interface.c Raptor2 Sources/bldc-firmware_2_80/mc_interface.c
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/mc_interface.c 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/mc_interface.c 2017-06-16 22:00:36.000000000 +0100
@@ -25,13 +25,15 @@
#include "mc_interface.h"
#include "mcpwm.h"
#include "mcpwm_foc.h"
-#include "main.h"
+#include "ledpwm.h"
#include "stm32f4xx_conf.h"
#include "hw.h"
#include "terminal.h"
#include "utils.h"
#include "ch.h"
#include "hal.h"
+#include "commands.h"
+#include "encoder.h"
#include <math.h>
// Global variables
@@ -39,21 +41,43 @@
volatile int ADC_curr_norm_value[3];
// Private variables
-static volatile mc_configuration conf;
-static mc_fault_code fault_now;
-static int ignore_iterations;
-static volatile unsigned int cycles_running;
-static volatile bool lock_enabled;
-static volatile bool lock_override_once;
-static volatile float motor_current_sum;
-static volatile float input_current_sum;
-static volatile float motor_current_iterations;
-static volatile float input_current_iterations;
-static volatile float amp_seconds;
-static volatile float amp_seconds_charged;
-static volatile float watt_seconds;
-static volatile float watt_seconds_charged;
-static volatile float position_set;
+static volatile mc_configuration m_conf;
+static volatile mc_configuration m_conf_original;
+static mc_fault_code m_fault_now;
+static int m_ignore_iterations;
+static volatile unsigned int m_cycles_running;
+static volatile bool m_lock_enabled;
+static volatile bool m_lock_override_once;
+static volatile float m_motor_current_sum;
+static volatile float m_input_current_sum;
+static volatile float m_motor_current_iterations;
+static volatile float m_input_current_iterations;
+static volatile float m_amp_seconds;
+static volatile float m_amp_seconds_charged;
+static volatile float m_watt_seconds;
+static volatile float m_watt_seconds_charged;
+static volatile float m_position_set;
+// new
+static volatile ppm_cruise cruise_control_status;
+
+// Sampling variables
+#define ADC_SAMPLE_MAX_LEN 2000
+static volatile int16_t m_curr0_samples[ADC_SAMPLE_MAX_LEN];
+static volatile int16_t m_curr1_samples[ADC_SAMPLE_MAX_LEN];
+static volatile int16_t m_ph1_samples[ADC_SAMPLE_MAX_LEN];
+static volatile int16_t m_ph2_samples[ADC_SAMPLE_MAX_LEN];
+static volatile int16_t m_ph3_samples[ADC_SAMPLE_MAX_LEN];
+static volatile int16_t m_vzero_samples[ADC_SAMPLE_MAX_LEN];
+static volatile uint8_t m_status_samples[ADC_SAMPLE_MAX_LEN];
+static volatile int16_t m_curr_fir_samples[ADC_SAMPLE_MAX_LEN];
+static volatile int16_t m_f_sw_samples[ADC_SAMPLE_MAX_LEN];
+static volatile int m_sample_len;
+static volatile int m_sample_int;
+static volatile int m_sample_ready;
+static volatile int m_sample_now;
+static volatile int m_sample_at_start;
+static volatile int m_start_comm;
+static volatile float m_last_adc_duration_sample;
// Private functions
static void update_override_limits(volatile mc_configuration *conf);
@@ -64,36 +88,66 @@
// Threads
static THD_WORKING_AREA(timer_thread_wa, 1024);
static THD_FUNCTION(timer_thread, arg);
+static THD_WORKING_AREA(sample_send_thread_wa, 1024);
+static THD_FUNCTION(sample_send_thread, arg);
+static thread_t *sample_send_tp;
void mc_interface_init(mc_configuration *configuration) {
- conf = *configuration;
- fault_now = FAULT_CODE_NONE;
- ignore_iterations = 0;
- cycles_running = 0;
- lock_enabled = false;
- lock_override_once = false;
- motor_current_sum = 0.0;
- input_current_sum = 0.0;
- motor_current_iterations = 0.0;
- input_current_iterations = 0.0;
- amp_seconds = 0.0;
- amp_seconds_charged = 0.0;
- watt_seconds = 0.0;
- watt_seconds_charged = 0.0;
- position_set = 0.0;
+ m_conf = *configuration;
+ m_conf_original = *configuration;
+ m_fault_now = FAULT_CODE_NONE;
+ m_ignore_iterations = 0;
+ m_cycles_running = 0;
+ m_lock_enabled = false;
+ m_lock_override_once = false;
+ m_motor_current_sum = 0.0;
+ m_input_current_sum = 0.0;
+ m_motor_current_iterations = 0.0;
+ m_input_current_iterations = 0.0;
+ m_amp_seconds = 0.0;
+ m_amp_seconds_charged = 0.0;
+ m_watt_seconds = 0.0;
+ m_watt_seconds_charged = 0.0;
+ m_position_set = 0.0;
+ m_last_adc_duration_sample = 0.0;
+ cruise_control_status = CRUISE_CONTROL_INACTIVE;
+
+ m_sample_len = 1000;
+ m_sample_int = 1;
+ m_sample_ready = 1;
+ m_sample_now = 0;
+ m_sample_at_start = 0;
+ m_start_comm = 0;
// Start threads
chThdCreateStatic(timer_thread_wa, sizeof(timer_thread_wa), NORMALPRIO, timer_thread, NULL);
+ chThdCreateStatic(sample_send_thread_wa, sizeof(sample_send_thread_wa), NORMALPRIO - 1, sample_send_thread, NULL);
+
+ // Initialize encoder
+#if !WS2811_ENABLE
+ switch (m_conf.m_sensor_port_mode) {
+ case SENSOR_PORT_MODE_ABI:
+ encoder_init_abi(m_conf.m_encoder_counts);
+ break;
+
+ case SENSOR_PORT_MODE_AS5047_SPI:
+ encoder_init_as5047p_spi();
+ break;
+
+ default:
+ break;
+ }
+#endif
// Initialize selected implementation
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
- mcpwm_init(&conf);
+ mcpwm_init(&m_conf);
break;
case MOTOR_TYPE_FOC:
- mcpwm_foc_init(&conf);
+ mcpwm_foc_init(&m_conf);
break;
default:
@@ -102,34 +156,56 @@
}
const volatile mc_configuration* mc_interface_get_configuration(void) {
- return &conf;
+ return &m_conf;
}
void mc_interface_set_configuration(mc_configuration *configuration) {
- if (conf.motor_type == MOTOR_TYPE_FOC
+#if !WS2811_ENABLE
+ if (m_conf.m_sensor_port_mode != configuration->m_sensor_port_mode) {
+ encoder_deinit();
+ switch (configuration->m_sensor_port_mode) {
+ case SENSOR_PORT_MODE_ABI:
+ encoder_init_abi(configuration->m_encoder_counts);
+ break;
+
+ case SENSOR_PORT_MODE_AS5047_SPI:
+ encoder_init_as5047p_spi();
+ break;
+
+ default:
+ break;
+ }
+ }
+
+ if (configuration->m_sensor_port_mode == SENSOR_PORT_MODE_ABI) {
+ encoder_set_counts(configuration->m_encoder_counts);
+ }
+#endif
+
+ if (m_conf.motor_type == MOTOR_TYPE_FOC
&& configuration->motor_type != MOTOR_TYPE_FOC) {
mcpwm_foc_deinit();
- conf = *configuration;
- mcpwm_init(&conf);
- } else if (conf.motor_type != MOTOR_TYPE_FOC
+ m_conf = *configuration;
+ mcpwm_init(&m_conf);
+ } else if (m_conf.motor_type != MOTOR_TYPE_FOC
&& configuration->motor_type == MOTOR_TYPE_FOC) {
mcpwm_deinit();
- conf = *configuration;
- mcpwm_foc_init(&conf);
+ m_conf = *configuration;
+ mcpwm_foc_init(&m_conf);
} else {
- conf = *configuration;
+ m_conf = *configuration;
}
- update_override_limits(&conf);
+ update_override_limits(&m_conf);
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
- mcpwm_set_configuration(&conf);
+ mcpwm_set_configuration(&m_conf);
break;
case MOTOR_TYPE_FOC:
- mcpwm_foc_set_configuration(&conf);
+ mcpwm_foc_set_configuration(&m_conf);
break;
default:
@@ -139,7 +215,7 @@
bool mc_interface_dccal_done(void) {
bool ret = false;
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
ret = mcpwm_is_dccal_done();
@@ -170,25 +246,25 @@
* Lock the control by disabling all control commands.
*/
void mc_interface_lock(void) {
- lock_enabled = true;
+ m_lock_enabled = true;
}
/**
* Unlock all control commands.
*/
void mc_interface_unlock(void) {
- lock_enabled = false;
+ m_lock_enabled = false;
}
/**
* Allow just one motor control command in the locked state.
*/
void mc_interface_lock_override_once(void) {
- lock_override_once = true;
+ m_lock_override_once = true;
}
mc_fault_code mc_interface_get_fault(void) {
- return fault_now;
+ return m_fault_now;
}
const char* mc_interface_fault_to_string(mc_fault_code fault) {
@@ -206,7 +282,7 @@
mc_state mc_interface_get_state(void) {
mc_state ret = MC_STATE_OFF;
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
ret = mcpwm_get_state();
@@ -228,7 +304,7 @@
return;
}
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
mcpwm_set_duty(dutyCycle);
@@ -248,7 +324,7 @@
return;
}
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
mcpwm_set_duty_noramp(dutyCycle);
@@ -268,7 +344,7 @@
return;
}
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
mcpwm_set_pid_speed(rpm);
@@ -283,14 +359,43 @@
}
}
+void mc_interface_set_pid_speed_with_cruise_status(float rpm, ppm_cruise cruise_status) {
+ if (mc_interface_try_input()) {
+ return;
+ }
+
+ switch (m_conf.motor_type) {
+ case MOTOR_TYPE_BLDC:
+ case MOTOR_TYPE_DC:
+ mcpwm_set_pid_speed_with_cruise_status(rpm, cruise_status);
+ break;
+
+ case MOTOR_TYPE_FOC:
+ mcpwm_foc_set_pid_speed_with_cruise_status(rpm, cruise_status);
+ break;
+
+ default:
+ break;
+ }
+}
+
+ppm_cruise mc_interface_get_cruise_control_status(void){
+ return cruise_control_status;
+}
+
+// true = active false = inactive
+void mc_interface_set_cruise_control_status(ppm_cruise status){
+ cruise_control_status = status;
+}
+
void mc_interface_set_pid_pos(float pos) {
if (mc_interface_try_input()) {
return;
}
- position_set = pos;
+ m_position_set = pos;
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
mcpwm_set_pid_pos(pos);
@@ -310,7 +415,7 @@
return;
}
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
mcpwm_set_current(current);
@@ -330,7 +435,75 @@
return;
}
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
+ case MOTOR_TYPE_BLDC:
+ case MOTOR_TYPE_DC:
+ mcpwm_set_brake_current(current);
+ break;
+
+ case MOTOR_TYPE_FOC:
+ mcpwm_foc_set_brake_current(current);
+ break;
+
+ default:
+ break;
+ }
+}
+
+void mc_interface_set_servo(float servo_val, bool use_min_current) {
+ if (mc_interface_try_input()) {
+ return;
+ }
+
+ float current = 0.0;
+
+ if (servo_val > 0.0){
+ if (m_conf.use_max_watt_limit) {
+ current = utils_smallest_of_2(servo_val * m_conf.l_current_max,
+ servo_val * (m_conf.watts_max / GET_INPUT_VOLTAGE() / mc_interface_get_duty_cycle_for_watt_calculation()));
+ } else {
+ current = utils_smallest_of_2(servo_val * m_conf.l_current_max,
+ servo_val * m_conf.l_in_current_max / mc_interface_get_duty_cycle_for_watt_calculation());
+ }
+ if (use_min_current) {
+ current = utils_highest_of_2(current, m_conf.cc_min_current);
+ }
+ } else if (servo_val < 0.0){
+ if (m_conf.use_max_watt_limit) {
+ current = -utils_smallest_of_2(fabsf(servo_val * m_conf.l_current_min),
+ fabsf(servo_val * (m_conf.watts_max / GET_INPUT_VOLTAGE() / mc_interface_get_duty_cycle_for_watt_calculation())));
+ } else {
+ current = -utils_smallest_of_2(fabsf(servo_val * m_conf.l_current_min),
+ fabsf(servo_val * m_conf.l_in_current_max / mc_interface_get_duty_cycle_for_watt_calculation()));
+ }
+ if (use_min_current) {
+ current = utils_smallest_of_2(current, -m_conf.cc_min_current);
+ }
+ }
+
+ switch (m_conf.motor_type) {
+ case MOTOR_TYPE_BLDC:
+ case MOTOR_TYPE_DC:
+ mcpwm_set_current(current);
+ break;
+
+ case MOTOR_TYPE_FOC:
+ mcpwm_foc_set_current(current);
+ break;
+
+ default:
+ break;
+ }
+}
+
+void mc_interface_set_brake_servo(float servo_val) {
+ if (mc_interface_try_input()) {
+ return;
+ }
+
+ float current = servo_val * fabsf(m_conf.l_current_min);
+
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
mcpwm_set_brake_current(current);
@@ -356,13 +529,34 @@
mc_interface_set_current(0.0);
}
+float mc_interface_get_duty_cycle_for_watt_calculation(void) {
+ float actual_duty;
+
+ if (m_conf.motor_type == MOTOR_TYPE_FOC) {
+ //float battery_current = mcpwm_foc_get_tot_current_in_filtered();
+ //float motor_current = mcpwm_foc_get_tot_current_filtered();
+
+ //if (fabsf(battery_current) > 1.0 && fabsf(motor_current) > 1.0) {
+ // actual_duty = fabsf(battery_current / motor_current);
+ //} else {
+ actual_duty = fabsf(mcpwm_foc_get_duty_cycle_now()) * 0.86602540378;
+ //}
+ } else {
+ actual_duty = fabsf(mc_interface_get_duty_cycle_now());
+ }
+ if (actual_duty < m_conf.l_min_duty) {
+ return m_conf.l_min_duty;
+ }
+ return actual_duty;
+}
+
/**
* Stop the motor and use braking.
*/
float mc_interface_get_duty_cycle_set(void) {
float ret = 0.0;
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
ret = mcpwm_get_duty_cycle_set();
@@ -382,7 +576,7 @@
float mc_interface_get_duty_cycle_now(void) {
float ret = 0.0;
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
ret = mcpwm_get_duty_cycle_now();
@@ -399,17 +593,17 @@
return ret;
}
-float mc_interface_get_switching_frequency_now(void) {
+float mc_interface_get_sampling_frequency_now(void) {
float ret = 0.0;
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
ret = mcpwm_get_switching_frequency_now();
break;
case MOTOR_TYPE_FOC:
- ret = mcpwm_foc_get_switching_frequency_now();
+ ret = mcpwm_foc_get_switching_frequency_now() / 2.0;
break;
default:
@@ -422,7 +616,7 @@
float mc_interface_get_rpm(void) {
float ret = 0.0;
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
ret = mcpwm_get_rpm();
@@ -449,10 +643,10 @@
* The amount of amp hours drawn.
*/
float mc_interface_get_amp_hours(bool reset) {
- float val = amp_seconds / 3600;
+ float val = m_amp_seconds / 3600;
if (reset) {
- amp_seconds = 0.0;
+ m_amp_seconds = 0.0;
}
return val;
@@ -468,10 +662,10 @@
* The amount of amp hours fed back.
*/
float mc_interface_get_amp_hours_charged(bool reset) {
- float val = amp_seconds_charged / 3600;
+ float val = m_amp_seconds_charged / 3600;
if (reset) {
- amp_seconds_charged = 0.0;
+ m_amp_seconds_charged = 0.0;
}
return val;
@@ -487,10 +681,10 @@
* The amount of watt hours drawn.
*/
float mc_interface_get_watt_hours(bool reset) {
- float val = watt_seconds / 3600;
+ float val = m_watt_seconds / 3600;
if (reset) {
- amp_seconds = 0.0;
+ m_amp_seconds = 0.0;
}
return val;
@@ -506,10 +700,10 @@
* The amount of watt hours fed back.
*/
float mc_interface_get_watt_hours_charged(bool reset) {
- float val = watt_seconds_charged / 3600;
+ float val = m_watt_seconds_charged / 3600;
if (reset) {
- watt_seconds_charged = 0.0;
+ m_watt_seconds_charged = 0.0;
}
return val;
@@ -518,7 +712,7 @@
float mc_interface_get_tot_current(void) {
float ret = 0.0;
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
ret = mcpwm_get_tot_current();
@@ -538,7 +732,7 @@
float mc_interface_get_tot_current_filtered(void) {
float ret = 0.0;
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
ret = mcpwm_get_tot_current_filtered();
@@ -558,7 +752,7 @@
float mc_interface_get_tot_current_directional(void) {
float ret = 0.0;
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
ret = mcpwm_get_tot_current_directional();
@@ -578,7 +772,7 @@
float mc_interface_get_tot_current_directional_filtered(void) {
float ret = 0.0;
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
ret = mcpwm_get_tot_current_directional_filtered();
@@ -598,7 +792,7 @@
float mc_interface_get_tot_current_in(void) {
float ret = 0.0;
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
ret = mcpwm_get_tot_current_in();
@@ -618,7 +812,7 @@
float mc_interface_get_tot_current_in_filtered(void) {
float ret = 0.0;
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
ret = mcpwm_get_tot_current_in_filtered();
@@ -638,7 +832,7 @@
int mc_interface_get_tachometer_value(bool reset) {
int ret = 0;
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
ret = mcpwm_get_tachometer_value(reset);
@@ -658,7 +852,7 @@
int mc_interface_get_tachometer_abs_value(bool reset) {
int ret = 0;
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
ret = mcpwm_get_tachometer_abs_value(reset);
@@ -678,7 +872,7 @@
float mc_interface_get_last_inj_adc_isr_duration(void) {
float ret = 0.0;
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
ret = mcpwm_get_last_inj_adc_isr_duration();
@@ -696,21 +890,62 @@
}
float mc_interface_read_reset_avg_motor_current(void) {
- float res = motor_current_sum / motor_current_iterations;
- motor_current_sum = 0;
- motor_current_iterations = 0;
+ float res = m_motor_current_sum / m_motor_current_iterations;
+ m_motor_current_sum = 0;
+ m_motor_current_iterations = 0;
return res;
}
float mc_interface_read_reset_avg_input_current(void) {
- float res = input_current_sum / input_current_iterations;
- input_current_sum = 0;
- input_current_iterations = 0;
+ float res = m_input_current_sum / m_input_current_iterations;
+ m_input_current_sum = 0;
+ m_input_current_iterations = 0;
return res;
}
-float mc_interface_get_pos_set(void) {
- return position_set;
+float mc_interface_get_pid_pos_set(void) {
+ return m_position_set;
+}
+
+float mc_interface_get_pid_pos_now(void) {
+ float ret = 0.0;
+
+ switch (m_conf.motor_type) {
+ case MOTOR_TYPE_BLDC:
+ case MOTOR_TYPE_DC:
+ ret = encoder_read_deg();
+ break;
+
+ case MOTOR_TYPE_FOC:
+ ret = mcpwm_foc_get_pid_pos_now();
+ break;
+
+ default:
+ break;
+ }
+
+ return ret;
+}
+
+float mc_interface_get_last_sample_adc_isr_duration(void) {
+ return m_last_adc_duration_sample;
+}
+
+void mc_interface_sample_print_data(bool at_start, uint16_t len, uint8_t decimation) {
+ if (len > ADC_SAMPLE_MAX_LEN) {
+ len = ADC_SAMPLE_MAX_LEN;
+ }
+
+ m_sample_len = len;
+ m_sample_int = decimation;
+
+ if (at_start) {
+ m_sample_at_start = 1;
+ m_start_comm = mcpwm_get_comm_step();
+ } else {
+ m_sample_now = 0;
+ m_sample_ready = 0;
+ }
}
// MC implementation functions
@@ -726,16 +961,16 @@
// TODO: Remove this later
if (mc_interface_get_state() == MC_STATE_DETECTING) {
mcpwm_stop_pwm();
- ignore_iterations = MCPWM_DETECT_STOP_TIME;
+ m_ignore_iterations = MCPWM_DETECT_STOP_TIME;
}
- int retval = ignore_iterations;
+ int retval = m_ignore_iterations;
- if (!ignore_iterations && lock_enabled) {
- if (!lock_override_once) {
+ if (!m_ignore_iterations && m_lock_enabled) {
+ if (!m_lock_override_once) {
retval = 1;
} else {
- lock_override_once = false;
+ m_lock_override_once = false;
}
}
@@ -743,7 +978,7 @@
}
void mc_interface_fault_stop(mc_fault_code fault) {
- if (mc_interface_dccal_done() && fault_now == FAULT_CODE_NONE) {
+ if (mc_interface_dccal_done() && m_fault_now == FAULT_CODE_NONE) {
// Sent to terminal fault logger so that all faults and their conditions
// can be printed for debugging.
chSysLock();
@@ -754,13 +989,18 @@
fault_data fdata;
fdata.fault = fault;
- fdata.current = mc_interface_get_tot_current();
- fdata.current_filtered = mc_interface_get_tot_current_filtered();
+ if (m_conf.motor_type == MOTOR_TYPE_FOC) {
+ fdata.current = mcpwm_foc_get_abs_motor_current();
+ fdata.current_filtered = mcpwm_foc_get_abs_motor_current_filtered();
+ }else{
+ fdata.current = mc_interface_get_tot_current();
+ fdata.current_filtered = mc_interface_get_tot_current_filtered();
+ }
fdata.voltage = GET_INPUT_VOLTAGE();
fdata.duty = mc_interface_get_duty_cycle_now();
fdata.rpm = mc_interface_get_rpm();
fdata.tacho = mc_interface_get_tachometer_value(false);
- fdata.cycles_running = cycles_running;
+ fdata.cycles_running = m_cycles_running;
fdata.tim_val_samp = val_samp;
fdata.tim_current_samp = current_samp;
fdata.tim_top = tim_top;
@@ -769,9 +1009,9 @@
terminal_add_fault_data(&fdata);
}
- ignore_iterations = conf.m_fault_stop_time_ms;
+ m_ignore_iterations = m_conf.m_fault_stop_time_ms;
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
mcpwm_stop_pwm();
@@ -785,20 +1025,22 @@
break;
}
- fault_now = fault;
+ m_fault_now = fault;
}
void mc_interface_mc_timer_isr(void) {
+ ledpwm_update_pwm(); // LED PWM Driver update
+
const float input_voltage = GET_INPUT_VOLTAGE();
// Check for faults that should stop the motor
static int wrong_voltage_iterations = 0;
- if (input_voltage < conf.l_min_vin ||
- input_voltage > conf.l_max_vin) {
+ if (input_voltage < m_conf.l_min_vin ||
+ input_voltage > m_conf.l_max_vin) {
wrong_voltage_iterations++;
if ((wrong_voltage_iterations >= 8)) {
- mc_interface_fault_stop(input_voltage < conf.l_min_vin ?
+ mc_interface_fault_stop(input_voltage < m_conf.l_min_vin ?
FAULT_CODE_UNDER_VOLTAGE : FAULT_CODE_OVER_VOLTAGE);
}
} else {
@@ -806,70 +1048,119 @@
}
if (mc_interface_get_state() == MC_STATE_RUNNING) {
- cycles_running++;
+ m_cycles_running++;
} else {
- cycles_running = 0;
+ m_cycles_running = 0;
}
- main_dma_adc_handler();
-
if (pwn_done_func) {
pwn_done_func();
}
const float current = mc_interface_get_tot_current_filtered();
const float current_in = mc_interface_get_tot_current_in_filtered();
- motor_current_sum += current;
- input_current_sum += current_in;
- motor_current_iterations++;
- input_current_iterations++;
+ m_motor_current_sum += current;
+ m_input_current_sum += current_in;
+ m_motor_current_iterations++;
+ m_input_current_iterations++;
float abs_current = mc_interface_get_tot_current();
float abs_current_filtered = current;
- if (conf.motor_type == MOTOR_TYPE_FOC) {
+ if (m_conf.motor_type == MOTOR_TYPE_FOC) {
// TODO: Make this more general
abs_current = mcpwm_foc_get_abs_motor_current();
abs_current_filtered = mcpwm_foc_get_abs_motor_current_filtered();
}
// Current fault code
- if (conf.l_slow_abs_current) {
- if (fabsf(abs_current_filtered) > conf.l_abs_current_max) {
+ if (m_conf.l_slow_abs_current) {
+ if (fabsf(abs_current_filtered) > m_conf.l_abs_current_max) {
mc_interface_fault_stop(FAULT_CODE_ABS_OVER_CURRENT);
}
} else {
- if (fabsf(abs_current) > conf.l_abs_current_max) {
+ if (fabsf(abs_current) > m_conf.l_abs_current_max) {
mc_interface_fault_stop(FAULT_CODE_ABS_OVER_CURRENT);
}
}
// Watt and ah counters
- const float f_sw = mc_interface_get_switching_frequency_now();
+ const float f_samp = mc_interface_get_sampling_frequency_now();
if (fabsf(current) > 1.0) {
// Some extra filtering
static float curr_diff_sum = 0.0;
static float curr_diff_samples = 0;
- curr_diff_sum += current_in / f_sw;
- curr_diff_samples += 1.0 / f_sw;
+ curr_diff_sum += current_in / f_samp;
+ curr_diff_samples += 1.0 / f_samp;
if (curr_diff_samples >= 0.01) {
if (curr_diff_sum > 0.0) {
- amp_seconds += curr_diff_sum;
- watt_seconds += curr_diff_sum * input_voltage;
+ m_amp_seconds += curr_diff_sum;
+ m_watt_seconds += curr_diff_sum * input_voltage;
} else {
- amp_seconds_charged -= curr_diff_sum;
- watt_seconds_charged -= curr_diff_sum * input_voltage;
+ m_amp_seconds_charged -= curr_diff_sum;
+ m_watt_seconds_charged -= curr_diff_sum * input_voltage;
}
curr_diff_samples = 0.0;
curr_diff_sum = 0.0;
}
}
+
+ // Sample collection
+ if (m_sample_at_start && (mc_interface_get_state() == MC_STATE_RUNNING ||
+ m_start_comm != mcpwm_get_comm_step())) {
+ m_sample_now = 0;
+ m_sample_ready = 0;
+ m_sample_at_start = 0;
+ }
+
+ static int a = 0;
+ if (!m_sample_ready) {
+ a++;
+ if (a >= m_sample_int) {
+ a = 0;
+
+ if (mc_interface_get_state() == MC_STATE_DETECTING) {
+ m_curr0_samples[m_sample_now] = (int16_t)mcpwm_detect_currents[mcpwm_get_comm_step() - 1];
+ m_curr1_samples[m_sample_now] = (int16_t)mcpwm_detect_currents_diff[mcpwm_get_comm_step() - 1];
+
+ m_ph1_samples[m_sample_now] = (int16_t)mcpwm_detect_voltages[0];
+ m_ph2_samples[m_sample_now] = (int16_t)mcpwm_detect_voltages[1];
+ m_ph3_samples[m_sample_now] = (int16_t)mcpwm_detect_voltages[2];
+ } else {
+ m_curr0_samples[m_sample_now] = ADC_curr_norm_value[0];
+ m_curr1_samples[m_sample_now] = ADC_curr_norm_value[1];
+
+ m_ph1_samples[m_sample_now] = ADC_V_L1 - mcpwm_vzero;
+ m_ph2_samples[m_sample_now] = ADC_V_L2 - mcpwm_vzero;
+ m_ph3_samples[m_sample_now] = ADC_V_L3 - mcpwm_vzero;
+ }
+
+ m_vzero_samples[m_sample_now] = mcpwm_vzero;
+
+ m_curr_fir_samples[m_sample_now] = (int16_t)(mc_interface_get_tot_current() * 100.0);
+ m_f_sw_samples[m_sample_now] = (int16_t)(f_samp / 10.0);
+
+ m_status_samples[m_sample_now] = mcpwm_get_comm_step() | (mcpwm_read_hall_phase() << 3);
+
+ m_sample_now++;
+
+ if (m_sample_now == m_sample_len) {
+ m_sample_ready = 1;
+ m_sample_now = 0;
+ chSysLockFromISR();
+ chEvtSignalI(sample_send_tp, (eventmask_t) 1);
+ chSysUnlockFromISR();
+ }
+
+ m_last_adc_duration_sample = mcpwm_get_last_adc_isr_duration();
+ }
+ }
}
void mc_interface_adc_inj_int_handler(void) {
- switch (conf.motor_type) {
+ switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
mcpwm_adc_inj_int_handler();
@@ -891,7 +1182,7 @@
* The configaration to update.
*/
static void update_override_limits(volatile mc_configuration *conf) {
- const float temp = NTC_TEMP(ADC_IND_TEMP_MOS1);
+ const float temp = NTC_TEMP(ADC_IND_TEMP_MOS2);
const float v_in = GET_INPUT_VOLTAGE();
// Temperature
@@ -944,16 +1235,53 @@
}
// Decrease fault iterations
- if (ignore_iterations > 0) {
- ignore_iterations--;
+ if (m_ignore_iterations > 0) {
+ m_ignore_iterations--;
} else {
if (!IS_DRV_FAULT()) {
- fault_now = FAULT_CODE_NONE;
+ m_fault_now = FAULT_CODE_NONE;
}
}
- update_override_limits(&conf);
+ update_override_limits(&m_conf);
chThdSleepMilliseconds(1);
}
}
+
+static THD_FUNCTION(sample_send_thread, arg) {
+ (void)arg;
+
+ chRegSetThreadName("SampleSender");
+
+ sample_send_tp = chThdGetSelfX();
+
+ for(;;) {
+ chEvtWaitAny((eventmask_t) 1);
+
+ for (int i = 0;i < m_sample_len;i++) {
+ uint8_t buffer[20];
+ int index = 0;
+
+ buffer[index++] = m_curr0_samples[i] >> 8;
+ buffer[index++] = m_curr0_samples[i];
+ buffer[index++] = m_curr1_samples[i] >> 8;
+ buffer[index++] = m_curr1_samples[i];
+ buffer[index++] = m_ph1_samples[i] >> 8;
+ buffer[index++] = m_ph1_samples[i];
+ buffer[index++] = m_ph2_samples[i] >> 8;
+ buffer[index++] = m_ph2_samples[i];
+ buffer[index++] = m_ph3_samples[i] >> 8;
+ buffer[index++] = m_ph3_samples[i];
+ buffer[index++] = m_vzero_samples[i] >> 8;
+ buffer[index++] = m_vzero_samples[i];
+ buffer[index++] = m_status_samples[i];
+ buffer[index++] = m_curr_fir_samples[i] >> 8;
+ buffer[index++] = m_curr_fir_samples[i];
+ buffer[index++] = m_f_sw_samples[i] >> 8;
+ buffer[index++] = m_f_sw_samples[i];
+
+ commands_send_samples(buffer, index);
+ }
+ }
+}
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/mc_interface.h Raptor2 Sources/bldc-firmware_2_80/mc_interface.h
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/mc_interface.h 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/mc_interface.h 2017-04-13 18:05:06.000000000 +0100
@@ -41,14 +41,20 @@
void mc_interface_set_duty(float dutyCycle);
void mc_interface_set_duty_noramp(float dutyCycle);
void mc_interface_set_pid_speed(float rpm);
+void mc_interface_set_pid_speed_with_cruise_status(float rpm, ppm_cruise cruise_status);
+ppm_cruise mc_interface_get_cruise_control_status(void);
+void mc_interface_set_cruise_control_status(ppm_cruise status); // 1 = active 0 = inactive
void mc_interface_set_pid_pos(float pos);
void mc_interface_set_current(float current);
void mc_interface_set_brake_current(float current);
+void mc_interface_set_servo(float servo_val, bool use_min_current);
+void mc_interface_set_brake_servo(float servo_val);
void mc_interface_brake_now(void);
void mc_interface_release_motor(void);
+float mc_interface_get_duty_cycle_for_watt_calculation(void);
float mc_interface_get_duty_cycle_set(void);
float mc_interface_get_duty_cycle_now(void);
-float mc_interface_get_switching_frequency_now(void);
+float mc_interface_get_sampling_frequency_now(void);
float mc_interface_get_rpm(void);
float mc_interface_get_amp_hours(bool reset);
float mc_interface_get_amp_hours_charged(bool reset);
@@ -65,7 +71,10 @@
float mc_interface_get_last_inj_adc_isr_duration(void);
float mc_interface_read_reset_avg_motor_current(void);
float mc_interface_read_reset_avg_input_current(void);
-float mc_interface_get_pos_set(void);
+float mc_interface_get_pid_pos_set(void);
+float mc_interface_get_pid_pos_now(void);
+float mc_interface_get_last_sample_adc_isr_duration(void);
+void mc_interface_sample_print_data(bool at_start, uint16_t len, uint8_t decimation);
// MC implementation functions
void mc_interface_fault_stop(mc_fault_code fault);
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/mcconf/mcconf_default.h Raptor2 Sources/bldc-firmware_2_80/mcconf/mcconf_default.h
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/mcconf/mcconf_default.h 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/mcconf/mcconf_default.h 2018-08-28 19:44:30.000000000 +0100
@@ -20,16 +20,16 @@
// Default settings
#ifndef MCCONF_DEFAULT_MOTOR_TYPE
-#define MCCONF_DEFAULT_MOTOR_TYPE MOTOR_TYPE_BLDC
+#define MCCONF_DEFAULT_MOTOR_TYPE MOTOR_TYPE_FOC
#endif
#ifndef MCCONF_PWM_MODE
-#define MCCONF_PWM_MODE PWM_MODE_SYNCHRONOUS // Default PWM mode
+#define MCCONF_PWM_MODE PWM_MODE_SYNCHRONOUS // Default PWM mode
#endif
#ifndef MCCONF_SENSOR_MODE
-#define MCCONF_SENSOR_MODE SENSOR_MODE_SENSORLESS // Sensor mode
+#define MCCONF_SENSOR_MODE SENSOR_MODE_HYBRID // Sensor mode
#endif
#ifndef MCCONF_COMM_MODE
-#define MCCONF_COMM_MODE COMM_MODE_INTEGRATE // The commutation mode to use
+#define MCCONF_COMM_MODE COMM_MODE_INTEGRATE // The commutation mode to use
#endif
// Limits
@@ -37,13 +37,13 @@
#define MCCONF_L_CURRENT_MAX 60.0 // Current limit in Amperes (Upper)
#endif
#ifndef MCCONF_L_CURRENT_MIN
-#define MCCONF_L_CURRENT_MIN -60.0 // Current limit in Amperes (Lower)
+#define MCCONF_L_CURRENT_MIN -50.0 // Current limit in Amperes (Lower)
#endif
#ifndef MCCONF_L_IN_CURRENT_MAX
-#define MCCONF_L_IN_CURRENT_MAX 60.0 // Input current limit in Amperes (Upper)
+#define MCCONF_L_IN_CURRENT_MAX 25.0 // Input current limit in Amperes (Upper)
#endif
#ifndef MCCONF_L_IN_CURRENT_MIN
-#define MCCONF_L_IN_CURRENT_MIN -20.0 // Input current limit in Amperes (Lower)
+#define MCCONF_L_IN_CURRENT_MIN -10.0 // Input current limit in Amperes (Lower)
#endif
#ifndef MCCONF_L_MAX_ABS_CURRENT
#define MCCONF_L_MAX_ABS_CURRENT 130.0 // The maximum absolute current above which a fault is generated
@@ -52,37 +52,37 @@
#define MCCONF_L_MIN_VOLTAGE 8.0 // Minimum input voltage
#endif
#ifndef MCCONF_L_MAX_VOLTAGE
-#define MCCONF_L_MAX_VOLTAGE 50.0 // Maximum input voltage
+#define MCCONF_L_MAX_VOLTAGE 57.0 // Maximum input voltage
#endif
#ifndef MCCONF_L_BATTERY_CUT_START
-#define MCCONF_L_BATTERY_CUT_START 10.0 // Start limiting the positive current at this voltage
+#define MCCONF_L_BATTERY_CUT_START 33.5 // Start limiting the positive current at this voltage
#endif
#ifndef MCCONF_L_BATTERY_CUT_END
-#define MCCONF_L_BATTERY_CUT_END 8.0 // Limit the positive current completely at this voltage
+#define MCCONF_L_BATTERY_CUT_END 29.0 // Limit the positive current completely at this voltage
#endif
#ifndef MCCONF_L_RPM_MAX
-#define MCCONF_L_RPM_MAX 100000.0 // The motor speed limit (Upper)
+#define MCCONF_L_RPM_MAX 60000.0 // The motor speed limit (Upper)
#endif
#ifndef MCCONF_L_RPM_MIN
-#define MCCONF_L_RPM_MIN -100000.0 // The motor speed limit (Lower)
+#define MCCONF_L_RPM_MIN -60000.0 // The motor speed limit (Lower)
#endif
#ifndef MCCONF_L_SLOW_ABS_OVERCURRENT
-#define MCCONF_L_SLOW_ABS_OVERCURRENT true // Use the filtered (and hence slower) current for the overcurrent fault detection
+#define MCCONF_L_SLOW_ABS_OVERCURRENT true // Use the filtered (and hence slower) current for the overcurrent fault detection
#endif
#ifndef MCCONF_L_MIN_DUTY
-#define MCCONF_L_MIN_DUTY 0.005 // Minimum duty cycle
+#define MCCONF_L_MIN_DUTY 0.005 // Minimum duty cycle
#endif
#ifndef MCCONF_L_MAX_DUTY
-#define MCCONF_L_MAX_DUTY 0.95 // Maximum duty cycle
+#define MCCONF_L_MAX_DUTY 0.95 // Maximum duty cycle
#endif
#ifndef MCCONF_L_RPM_LIMIT_NEG_TORQUE
-#define MCCONF_L_RPM_LIMIT_NEG_TORQUE true // Use negative torque to limit the RPM
+#define MCCONF_L_RPM_LIMIT_NEG_TORQUE false // Use negative torque to limit the RPM
#endif
#ifndef MCCONF_L_CURR_MAX_RPM_FBRAKE
-#define MCCONF_L_CURR_MAX_RPM_FBRAKE 300 // Maximum electrical RPM to use full brake at
+#define MCCONF_L_CURR_MAX_RPM_FBRAKE 300 // Maximum electrical RPM to use full brake at
#endif
#ifndef MCCONF_L_CURR_MAX_RPM_FBRAKE_CC
-#define MCCONF_L_CURR_MAX_RPM_FBRAKE_CC 1500 // Maximum electrical RPM to use full brake at with current control
+#define MCCONF_L_CURR_MAX_RPM_FBRAKE_CC 1500 // Maximum electrical RPM to use full brake at with current control
#endif
#ifndef MCCONF_L_LIM_TEMP_FET_START
#define MCCONF_L_LIM_TEMP_FET_START 80.0 // MOSFET temperature where current limiting should begin
@@ -91,7 +91,7 @@
#define MCCONF_L_LIM_TEMP_FET_END 100.0 // MOSFET temperature where everything should be shut off
#endif
#ifndef MCCONF_L_LIM_TEMP_MOTOR_START
-#define MCCONF_L_LIM_TEMP_MOTOR_START 80.0 // MOTOR temperature where current limiting should begin
+#define MCCONF_L_LIM_TEMP_MOTOR_START 80.0 // MOTOR temperature where current limiting should begin
#endif
#ifndef MCCONF_L_LIM_TEMP_MOTOR_END
#define MCCONF_L_LIM_TEMP_MOTOR_END 100.0 // MOTOR temperature where everything should be shut off
@@ -99,58 +99,64 @@
// Speed PID parameters
#ifndef MCCONF_S_PID_KP
-#define MCCONF_S_PID_KP 0.0001 // Proportional gain
+#define MCCONF_S_PID_KP 0.003 // Proportional gain
#endif
#ifndef MCCONF_S_PID_KI
-#define MCCONF_S_PID_KI 0.015 // Integral gain
+#define MCCONF_S_PID_KI 0.003 // Integral gain
#endif
#ifndef MCCONF_S_PID_KD
-#define MCCONF_S_PID_KD 0.0 // Derivative gain
+#define MCCONF_S_PID_KD 0.0 // Derivative gain
#endif
#ifndef MCCONF_S_PID_MIN_RPM
#define MCCONF_S_PID_MIN_RPM 900.0 // Minimum allowed RPM
#endif
+#ifndef MCCONF_S_PID_BREAKING_ENABLED
+#define MCCONF_S_PID_BREAKING_ENABLED true // Break at PID speed mode (cruiso control)
+#endif
// Position PID parameters
#ifndef MCCONF_P_PID_KP
-#define MCCONF_P_PID_KP 0.03 // Proportional gain
+#define MCCONF_P_PID_KP 0.03 // Proportional gain
#endif
#ifndef MCCONF_P_PID_KI
-#define MCCONF_P_PID_KI 0.0 // Integral gain
+#define MCCONF_P_PID_KI 0.0 // Integral gain
#endif
#ifndef MCCONF_P_PID_KD
-#define MCCONF_P_PID_KD 0.0004 // Derivative gain
+#define MCCONF_P_PID_KD 0.0004 // Derivative gain
+#endif
+#ifndef MCCONF_P_PID_ANG_DIV
+#define MCCONF_P_PID_ANG_DIV 1.0 // Divide angle by this value
#endif
// Current control parameters
#ifndef MCCONF_CC_GAIN
-#define MCCONF_CC_GAIN 0.0046 // Current controller error gain
+#define MCCONF_CC_GAIN 0.0046 // Current controller error gain
#endif
#ifndef MCCONF_CC_MIN_CURRENT
-#define MCCONF_CC_MIN_CURRENT 1.0 // Minimum allowed current
+#define MCCONF_CC_MIN_CURRENT 0.5 // Minimum allowed current
#endif
#ifndef MCCONF_CC_STARTUP_BOOST_DUTY
-#define MCCONF_CC_STARTUP_BOOST_DUTY 0.01 // The lowest duty cycle to use in current control mode (has to be > MCPWM_MIN_DUTY_CYCLE)
+#define MCCONF_CC_STARTUP_BOOST_DUTY 0.01 // The lowest duty cycle to use in current control mode (has to be > MCPWM_MIN_DUTY_CYCLE)
#endif
#ifndef MCCONF_CC_RAMP_STEP
-#define MCCONF_CC_RAMP_STEP 0.04 // Maximum duty cycle ramping step in CC mode
+#define MCCONF_CC_RAMP_STEP 0.04 // Maximum duty cycle ramping step in CC mode
#endif
// BLDC
#ifndef MCCONF_SL_MIN_RPM
-#define MCCONF_SL_MIN_RPM 150 // Auto-commutate below this RPM
+#define MCCONF_SL_MIN_RPM 150 // Auto-commutate below this RPM
#endif
#ifndef MCCONF_SL_MIN_ERPM_CYCLE_INT_LIMIT
#define MCCONF_SL_MIN_ERPM_CYCLE_INT_LIMIT 1100.0 // Minimum RPM to calculate the BEMF coupling from
#endif
#ifndef MCCONF_SL_CYCLE_INT_LIMIT
-#define MCCONF_SL_CYCLE_INT_LIMIT 62.0 // Flux integrator limit 0 ERPM
+#define MCCONF_SL_CYCLE_INT_LIMIT 100.0 // Flux integrator limit 0 ERPM
#endif
#ifndef MCCONF_SL_BEMF_COUPLING_K
#define MCCONF_SL_BEMF_COUPLING_K 600.0 // Input voltage to bemf coupling constant
#endif
#ifndef MCCONF_SL_PHASE_ADVANCE_AT_BR
-#define MCCONF_SL_PHASE_ADVANCE_AT_BR 0.8 // Flux integrator limit percentage at MCPWM_CYCLE_INT_START_RPM_BR ERPM
+#define MCCONF_SL_PHASE_ADVANCE_AT_BR 0.8 // Flux integrator limit percentage at MCPWM_CYCLE_INT_START_RPM_BR ERPM
#endif
#ifndef MCCONF_SL_CYCLE_INT_BR
#define MCCONF_SL_CYCLE_INT_BR 80000.0 // RPM border between the START and LOW interval
@@ -159,18 +165,47 @@
#define MCCONF_SL_MAX_FB_CURR_DIR_CHANGE 10.0 // Maximum current during full brake during which a direction change is allowed
#endif
+// BLDC hall sensor table
+#ifndef MCCONF_HALL_TAB_0
+#define MCCONF_HALL_TAB_0 -1
+#endif
+#ifndef MCCONF_HALL_TAB_1
+#define MCCONF_HALL_TAB_1 1
+#endif
+#ifndef MCCONF_HALL_TAB_2
+#define MCCONF_HALL_TAB_2 3
+#endif
+#ifndef MCCONF_HALL_TAB_3
+#define MCCONF_HALL_TAB_3 2
+#endif
+#ifndef MCCONF_HALL_TAB_4
+#define MCCONF_HALL_TAB_4 5
+#endif
+#ifndef MCCONF_HALL_TAB_5
+#define MCCONF_HALL_TAB_5 6
+#endif
+#ifndef MCCONF_HALL_TAB_6
+#define MCCONF_HALL_TAB_6 4
+#endif
+#ifndef MCCONF_HALL_TAB_7
+#define MCCONF_HALL_TAB_7 -1
+#endif
+#ifndef MCCONF_HALL_ERPM
+#define MCCONF_HALL_ERPM 5000.0 // ERPM above which sensorless commutation is used in hybrid mode
+#endif
+
// FOC
#ifndef MCCONF_FOC_CURRENT_KP
-#define MCCONF_FOC_CURRENT_KP 0.03
+#define MCCONF_FOC_CURRENT_KP 0.0215 //0.0255
#endif
#ifndef MCCONF_FOC_CURRENT_KI
-#define MCCONF_FOC_CURRENT_KI 50.0
+#define MCCONF_FOC_CURRENT_KI 47.80 //41.00
#endif
#ifndef MCCONF_FOC_F_SW
-#define MCCONF_FOC_F_SW 20000.0
+#define MCCONF_FOC_F_SW 20000.0
#endif
#ifndef MCCONF_FOC_DT_US
-#define MCCONF_FOC_DT_US 0.15 // Microseconds for dead time compensation
+#define MCCONF_FOC_DT_US 0.08 // Microseconds for dead time compensation
#endif
#ifndef MCCONF_FOC_ENCODER_INVERTED
#define MCCONF_FOC_ENCODER_INVERTED false
@@ -182,25 +217,25 @@
#define MCCONF_FOC_ENCODER_RATIO 7.0
#endif
#ifndef MCCONF_FOC_SENSOR_MODE
-#define MCCONF_FOC_SENSOR_MODE FOC_SENSOR_MODE_SENSORLESS
+#define MCCONF_FOC_SENSOR_MODE FOC_SENSOR_MODE_HALL
#endif
#ifndef MCCONF_FOC_PLL_KP
-#define MCCONF_FOC_PLL_KP 40.0
+#define MCCONF_FOC_PLL_KP 2000.0
#endif
#ifndef MCCONF_FOC_PLL_KI
-#define MCCONF_FOC_PLL_KI 40000.0
+#define MCCONF_FOC_PLL_KI 20000.0
#endif
#ifndef MCCONF_FOC_MOTOR_L
-#define MCCONF_FOC_MOTOR_L 0.000007
+#define MCCONF_FOC_MOTOR_L 0.0000215 //0.0000255
#endif
#ifndef MCCONF_FOC_MOTOR_R
-#define MCCONF_FOC_MOTOR_R 0.015
+#define MCCONF_FOC_MOTOR_R 0.04780 //0.04100
#endif
#ifndef MCCONF_FOC_MOTOR_FLUX_LINKAGE
-#define MCCONF_FOC_MOTOR_FLUX_LINKAGE 0.00245
+#define MCCONF_FOC_MOTOR_FLUX_LINKAGE 0.006278//0.004363
#endif
#ifndef MCCONF_FOC_OBSERVER_GAIN
-#define MCCONF_FOC_OBSERVER_GAIN 9e7 // Can be something like 600 / L
+#define MCCONF_FOC_OBSERVER_GAIN 4.651e7 //3.922e7 // Can be something like 600 / L
#endif
#ifndef MCCONF_FOC_DUTY_DOWNRAMP_KP
#define MCCONF_FOC_DUTY_DOWNRAMP_KP 10.0 // PI controller for duty control when decreasing the duty
@@ -209,48 +244,46 @@
#define MCCONF_FOC_DUTY_DOWNRAMP_KI 200.0 // PI controller for duty control when decreasing the duty
#endif
#ifndef MCCONF_FOC_OPENLOOP_RPM
-#define MCCONF_FOC_OPENLOOP_RPM 350.0 // Openloop RPM (sensorless low speed or when finding index pulse)
+#define MCCONF_FOC_OPENLOOP_RPM 1200.0 // Openloop RPM (sensorless low speed or when finding index pulse)
#endif
#ifndef MCCONF_FOC_SL_OPENLOOP_HYST
-#define MCCONF_FOC_SL_OPENLOOP_HYST 0.5 // Time below min RPM to activate openloop (s)
+#define MCCONF_FOC_SL_OPENLOOP_HYST 0.5 // Time below min RPM to activate openloop (s)
#endif
#ifndef MCCONF_FOC_SL_OPENLOOP_TIME
-#define MCCONF_FOC_SL_OPENLOOP_TIME 0.5 // Time to remain in openloop (s)
+#define MCCONF_FOC_SL_OPENLOOP_TIME 0.5 // Time to remain in openloop (s)
#endif
#ifndef MCCONF_FOC_SL_D_CURRENT_DUTY
-#define MCCONF_FOC_SL_D_CURRENT_DUTY 0.0 // Inject d-axis current below this duty cycle in sensorless more
+#define MCCONF_FOC_SL_D_CURRENT_DUTY 0.0 // Inject d-axis current below this duty cycle in sensorless more
#endif
#ifndef MCCONF_FOC_SL_D_CURRENT_FACTOR
-#define MCCONF_FOC_SL_D_CURRENT_FACTOR 0.0 // Maximum q-axis current factor
+#define MCCONF_FOC_SL_D_CURRENT_FACTOR 0.0 // Maximum q-axis current factor
#endif
-
-// Default hall sensor table
-#ifndef MCCONF_HALL_TAB_0
-#define MCCONF_HALL_TAB_0 -1
+#ifndef MCCONF_FOC_HALL_TAB_0
+#define MCCONF_FOC_HALL_TAB_0 255
#endif
-#ifndef MCCONF_HALL_TAB_1
-#define MCCONF_HALL_TAB_1 1
+#ifndef MCCONF_FOC_HALL_TAB_1
+#define MCCONF_FOC_HALL_TAB_1 255
#endif
-#ifndef MCCONF_HALL_TAB_2
-#define MCCONF_HALL_TAB_2 3
+#ifndef MCCONF_FOC_HALL_TAB_2
+#define MCCONF_FOC_HALL_TAB_2 255
#endif
-#ifndef MCCONF_HALL_TAB_3
-#define MCCONF_HALL_TAB_3 2
+#ifndef MCCONF_FOC_HALL_TAB_3
+#define MCCONF_FOC_HALL_TAB_3 255
#endif
-#ifndef MCCONF_HALL_TAB_4
-#define MCCONF_HALL_TAB_4 5
+#ifndef MCCONF_FOC_HALL_TAB_4
+#define MCCONF_FOC_HALL_TAB_4 255
#endif
-#ifndef MCCONF_HALL_TAB_5
-#define MCCONF_HALL_TAB_5 6
+#ifndef MCCONF_FOC_HALL_TAB_5
+#define MCCONF_FOC_HALL_TAB_5 255
#endif
-#ifndef MCCONF_HALL_TAB_6
-#define MCCONF_HALL_TAB_6 4
+#ifndef MCCONF_FOC_HALL_TAB_6
+#define MCCONF_FOC_HALL_TAB_6 255
#endif
-#ifndef MCCONF_HALL_TAB_7
-#define MCCONF_HALL_TAB_7 -1
+#ifndef MCCONF_FOC_HALL_TAB_7
+#define MCCONF_FOC_HALL_TAB_7 255
#endif
-#ifndef MCCONF_HALL_ERPM
-#define MCCONF_HALL_ERPM 2000.0 // ERPM above which sensorless commutation is used in hybrid mode
+#ifndef MCCONF_FOC_SL_ERPM
+#define MCCONF_FOC_SL_ERPM 5000.0 // ERPM above which only the observer is used
#endif
// Misc
@@ -258,16 +291,25 @@
#define MCCONF_M_FAULT_STOP_TIME 3000 // Ignore commands for this duration in msec when faults occur
#endif
#ifndef MCCONF_M_RAMP_STEP
-#define MCCONF_M_RAMP_STEP 0.02 // Duty cycle ramping step (1000 times/sec) at maximum duty cycle
+#define MCCONF_M_RAMP_STEP 0.02 // Duty cycle ramping step (1000 times/sec) at maximum duty cycle
#endif
#ifndef MCCONF_M_RAMP_STEP_RPM_LIM
#define MCCONF_M_RAMP_STEP_RPM_LIM 0.0005 // Ramping step when limiting the RPM
#endif
#ifndef MCCONF_M_CURRENT_BACKOFF_GAIN
-#define MCCONF_M_CURRENT_BACKOFF_GAIN 0.5 // The error gain of the current limiting algorithm
+#define MCCONF_M_CURRENT_BACKOFF_GAIN 0.5 // The error gain of the current limiting algorithm
#endif
#ifndef MCCONF_M_ENCODER_COUNTS
#define MCCONF_M_ENCODER_COUNTS 8192 // The number of encoder counts
#endif
+#ifndef MCCONF_M_SENSOR_PORT_MODE
+#define MCCONF_M_SENSOR_PORT_MODE SENSOR_PORT_MODE_HALL // The mode of the hall_encoder port
+#endif
+#ifndef MCCONF_USE_MAX_WATT_LIMIT
+#define MCCONF_USE_MAX_WATT_LIMIT false
+#endif
+#ifndef MCCONF_WATT_MAX
+#define MCCONF_WATT_MAX 1000.0 // Input current limit in Amperes (Upper)
+#endif
#endif /* MCCONF_DEFAULT_H_ */
Only in Raptor2 Sources/bldc-firmware_2_80/mcconf: mcconf_default.h~
Only in bldc-90986c31defe50830b4795a388ee5b2687dc2516/mcconf: mcconf_foc_erwin.h
Only in bldc-90986c31defe50830b4795a388ee5b2687dc2516/mcconf: mcconf_foc_scorpion.h
Only in bldc-90986c31defe50830b4795a388ee5b2687dc2516/mcconf: mcconf_outrunner2.h
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/mcconf/mcconf_sten.h Raptor2 Sources/bldc-firmware_2_80/mcconf/mcconf_sten.h
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/mcconf/mcconf_sten.h 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/mcconf/mcconf_sten.h 2016-09-23 23:02:14.000000000 +0100
@@ -26,43 +26,54 @@
#define MCCONF_STEN_H_
/*
+ * HW: 4.10
+ */
+
+/*
* Parameters
*/
+#define MCCONF_DEFAULT_MOTOR_TYPE MOTOR_TYPE_BLDC
#define MCCONF_L_CURRENT_MAX 35.0 // Current limit in Amperes (Upper)
#define MCCONF_L_CURRENT_MIN -30.0 // Current limit in Amperes (Lower)
-#define MCCONF_L_MAX_ABS_CURRENT 100.0 // The maximum absolute current above which a fault is generated
+#define MCCONF_L_MAX_ABS_CURRENT 130.0 // The maximum absolute current above which a fault is generated
#define MCCONF_L_SLOW_ABS_OVERCURRENT 1 // Use the filtered (and hence slower) current for the overcurrent fault detection
-#define MCCONF_L_IN_CURRENT_MAX 25.0 // Input current limit in Amperes (Upper)
-#define MCCONF_L_IN_CURRENT_MIN -20.0 // Input current limit in Amperes (Lower)
+#define MCCONF_L_IN_CURRENT_MAX 25.0 // Input current limit in Amperes (Upper)
+#define MCCONF_L_IN_CURRENT_MIN -20.0 // Input current limit in Amperes (Lower)
#define MCCONF_L_RPM_MAX 80000.0 // The motor speed limit (Upper)
#define MCCONF_L_RPM_MIN -80000.0 // The motor speed limit (Lower)
-#define MCCONF_L_MIN_VOLTAGE 20.0 // Minimum input voltage
-#define MCCONF_L_MAX_VOLTAGE 50.0 // Maximum input voltage
-#define MCCONF_CC_STARTUP_BOOST_DUTY 0.02 // The lowest duty cycle to use in current control mode (has to be > MCPWM_MIN_DUTY_CYCLE)
+#define MCCONF_L_MIN_VOLTAGE 10.0 // Minimum input voltage
+#define MCCONF_L_MAX_VOLTAGE 57.0 // Maximum input voltage
+#define MCCONF_CC_STARTUP_BOOST_DUTY 0.03 // The lowest duty cycle to use in current control mode (has to be > MCPWM_MIN_DUTY_CYCLE)
#define MCCONF_L_RPM_LIMIT_NEG_TORQUE 0 // Use negative torque to limit the RPM
#define MCCONF_L_CURR_MAX_RPM_FBRAKE 1500 // Maximum electrical RPM to use full brake at
+#define MCCONF_L_BATTERY_CUT_START 41.0 // Start limiting the positive current at this voltage
+#define MCCONF_L_BATTERY_CUT_END 39.0 // Limit the positive current completely at this voltage
+
// Sensorless settings
#define MCCONF_SENSOR_MODE SENSOR_MODE_SENSORLESS // Sensor mode
#define MCCONF_SL_MIN_RPM 250 // Auto-commutate below this RPM
-#define MCCONF_SL_MIN_ERPM_CYCLE_INT_LIMIT 500.0 // Minimum RPM to calculate the BEMF coupling from
+#define MCCONF_SL_MIN_ERPM_CYCLE_INT_LIMIT 1100.0 // Minimum RPM to calculate the BEMF coupling from
#define MCCONF_SL_CYCLE_INT_LIMIT 80.0 // Flux integrator limit 0 ERPM
#define MCCONF_SL_PHASE_ADVANCE_AT_BR 0.8 // Flux integrator limit percentage at MCPWM_CYCLE_INT_START_RPM_BR ERPM
#define MCCONF_SL_BEMF_COUPLING_K 750.0 // Input voltage to bemf coupling constant
+// FOC settings
+#define MCCONF_FOC_CURRENT_KP 0.03
+#define MCCONF_FOC_CURRENT_KI 50.0
+#define MCCONF_FOC_F_SW 20000.0
+#define MCCONF_FOC_MOTOR_L 0.000007
+#define MCCONF_FOC_MOTOR_R 0.015
+#define MCCONF_FOC_MOTOR_FLUX_LINKAGE 0.00245
+#define MCCONF_FOC_OBSERVER_GAIN 9e7
+#define MCCONF_FOC_OPENLOOP_RPM 600.0
+#define MCCONF_FOC_SL_OPENLOOP_HYST 0.5
+#define MCCONF_FOC_SL_OPENLOOP_TIME 0.5
+
// Speed PID parameters
#define MCCONF_S_PID_KP 0.0001 // Proportional gain
#define MCCONF_S_PID_KI 0.002 // Integral gain
#define MCCONF_S_PID_KD 0.0 // Derivative gain
-#define MCCONF_S_PID_MIN_RPM 1200.0 // Minimum allowed RPM
-
-// Position PID parameters
-#define MCCONF_P_PID_KP 0.0001 // Proportional gain
-#define MCCONF_P_PID_KI 0.002 // Integral gain
-#define MCCONF_P_PID_KD 0.0 // Derivative gain
-
-// Current control parameters
-#define MCCONF_CC_GAIN 0.0046 // Current controller error gain
-#define MCCONF_CC_MIN_CURRENT 0.05 // Minimum allowed current
-
+#define MCCONF_S_PID_MIN_RPM 1200.0 // Minimum allowed RPM
+#define MCCONF_S_PID_BREAKING_ENABLED 1 // enable braking at pid speed mode (cruise control)
#endif /* MCCONF_STEN_H_ */
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/mcpwm.c Raptor2 Sources/bldc-firmware_2_80/mcpwm.c
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/mcpwm.c 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/mcpwm.c 2017-04-13 18:06:02.000000000 +0100
@@ -56,6 +56,7 @@
static volatile float dutycycle_now;
static volatile float rpm_now;
static volatile float speed_pid_set_rpm;
+static volatile ppm_cruise speed_pid_cruise_control_type;
static volatile float pos_pid_set_pos;
static volatile float current_set;
static volatile int tachometer;
@@ -90,6 +91,8 @@
static volatile float last_pwm_cycles_sums[6];
static volatile bool dccal_done;
static volatile bool sensorless_now;
+static volatile float sensor_hyst_min;
+static volatile float sensor_hyst_max;
static volatile int hall_detect_table[8][7];
// KV FIR filter
@@ -174,6 +177,7 @@
dutycycle_set = 0.0;
dutycycle_now = 0.0;
speed_pid_set_rpm = 0.0;
+ speed_pid_cruise_control_type = CRUISE_CONTROL_MOTOR_SETTINGS;
pos_pid_set_pos = 0.0;
current_set = 0.0;
tachometer = 0;
@@ -494,7 +498,7 @@
* The commutations corresponding to the hall sensor states in the forward direction-
*/
void mcpwm_init_hall_table(int8_t *table) {
- const int fwd_to_rev[7] = {-1,4,3,2,1,6,5};
+ const int fwd_to_rev[7] = {-1,1,6,5,4,3,2};
for (int i = 0;i < 8;i++) {
hall_to_phase_table[8 + i] = table[i];
@@ -505,14 +509,16 @@
continue;
}
- ind_now += 2;
- if (ind_now > 6) {
- ind_now -= 6;
- }
- ind_now = fwd_to_rev[ind_now];
-
- hall_to_phase_table[i] = ind_now;
+ hall_to_phase_table[i] = fwd_to_rev[ind_now];
+ }
+
+ // init the sensor hyst
+ float sensor_hyst = conf->hall_sl_erpm * 0.10;
+ if(sensor_hyst > 500.0){
+ sensor_hyst = 500.0;
}
+ sensor_hyst_min = conf->hall_sl_erpm - sensor_hyst;
+ sensor_hyst_max = conf->hall_sl_erpm + sensor_hyst;
}
static void do_dc_cal(void) {
@@ -571,8 +577,31 @@
* The electrical RPM goal value to use.
*/
void mcpwm_set_pid_speed(float rpm) {
+ if (fabsf(rpm) <conf->s_pid_min_erpm) {
+ mcpwm_set_duty(0.0);
+ return;
+ }
control_mode = CONTROL_MODE_SPEED;
speed_pid_set_rpm = rpm;
+ speed_pid_cruise_control_type = CRUISE_CONTROL_MOTOR_SETTINGS;
+
+ if (state != MC_STATE_RUNNING) {
+ set_duty_cycle_hl(conf->l_min_duty);
+ }
+}
+
+void mcpwm_set_pid_speed_with_cruise_status(float rpm, ppm_cruise cruise_status) {
+ if (fabsf(rpm) <conf->s_pid_min_erpm) {
+ mcpwm_set_duty(0.0);
+ return;
+ }
+ control_mode = CONTROL_MODE_SPEED;
+ speed_pid_set_rpm = rpm;
+ speed_pid_cruise_control_type = cruise_status;
+
+ if (state != MC_STATE_RUNNING) {
+ set_duty_cycle_hl(conf->l_min_duty);
+ }
}
/**
@@ -1064,23 +1093,23 @@
}
static void run_pid_control_speed(void) {
- static float i_term = 0;
- static float prev_error = 0;
+ static float i_term = 0.0;
+ static float prev_error = 0.0;
float p_term;
float d_term;
// PID is off. Return.
if (control_mode != CONTROL_MODE_SPEED) {
- i_term = dutycycle_now;
- prev_error = 0;
+ i_term = 0.0;
+ prev_error = 0.0;
return;
}
- // Too low RPM set. Stop and return.
- if (fabsf(speed_pid_set_rpm) < conf->s_pid_min_erpm) {
- i_term = dutycycle_now;
+ // Too low RPM set. Reset state and return.
+ if (fabsf(speed_pid_set_rpm) <conf->s_pid_min_erpm) {
+ i_term = 0.0;
prev_error = 0;
- mcpwm_set_duty(0.0);
+ current_set = 0.0;
return;
}
@@ -1104,21 +1133,18 @@
// Calculate output
float output = p_term + i_term + d_term;
- // Make sure that at least minimum output is used
- if (fabsf(output) < conf->l_min_duty) {
- output = SIGN(output) * conf->l_min_duty;
- }
-
- // Do not output in reverse direction to oppose too high rpm
- if (speed_pid_set_rpm > 0.0 && output < 0.0) {
- output = conf->l_min_duty;
- i_term = 0.0;
- } else if (speed_pid_set_rpm < 0.0 && output > 0.0) {
- output = -conf->l_min_duty;
- i_term = 0.0;
+ if ((speed_pid_cruise_control_type == CRUISE_CONTROL_MOTOR_SETTINGS && conf->s_pid_breaking_enabled) || speed_pid_cruise_control_type == CRUISE_CONTROL_BRAKING_ENABLED) {
+ utils_truncate_number(&output, -1.0, 1.0);
+ } else {
+ if(speed_pid_set_rpm < 0.0){
+ utils_truncate_number(&output, -1.0, 0.0);
+ }else{
+ utils_truncate_number(&output, 0.0, 1.0);
+ }
}
-
- set_duty_cycle_hl(output);
+
+ current_set = output * conf->lo_current_max;
+
}
static void run_pid_control_pos(float dt) {
@@ -1784,7 +1810,7 @@
float dutycycle_now_tmp = dutycycle_now;
- if (control_mode == CONTROL_MODE_CURRENT || control_mode == CONTROL_MODE_POS) {
+ if (control_mode == CONTROL_MODE_CURRENT || control_mode == CONTROL_MODE_POS || control_mode == CONTROL_MODE_SPEED) {
// Compute error
const float error = current_set - (direction ? current_nofilter : -current_nofilter);
float step = error * conf->cc_gain * voltage_scale;
@@ -1922,7 +1948,7 @@
mc_interface_mc_timer_isr();
- if (ENCODER_ENABLE) {
+ if (encoder_is_configured()) {
run_pid_control_pos(1.0 / switching_frequency_now);
}
@@ -2051,7 +2077,7 @@
int mcpwm_get_hall_detect_result(int8_t *table) {
if (WS2811_ENABLE) {
return -2;
- } else if (ENCODER_ENABLE) {
+ } else if (conf->m_sensor_port_mode != SENSOR_PORT_MODE_HALL) {
return -3;
}
@@ -2336,6 +2362,7 @@
}
}
+/*
static void update_sensor_mode(void) {
if (conf->sensor_mode == SENSOR_MODE_SENSORLESS ||
(conf->sensor_mode == SENSOR_MODE_HYBRID &&
@@ -2345,6 +2372,26 @@
sensorless_now = false;
}
}
+*/
+
+static void update_sensor_mode(void) {
+ if (conf->sensor_mode == SENSOR_MODE_SENSORLESS ) {
+ sensorless_now = true;
+ } else if (conf->sensor_mode == SENSOR_MODE_HYBRID) {
+ // Hysteresis 5 % of total speed
+ if (sensorless_now) {
+ if (fabsf(rpm_now) < sensor_hyst_min) {
+ sensorless_now = false;
+ }
+ } else {
+ if (fabsf(rpm_now) > sensor_hyst_max) {
+ sensorless_now = true;
+ }
+ }
+ }else{
+ sensorless_now = false;
+ }
+}
static void commutate(int steps) {
last_pwm_cycles_sum = pwm_cycles_sum;
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/mcpwm.h Raptor2 Sources/bldc-firmware_2_80/mcpwm.h
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/mcpwm.h 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/mcpwm.h 2017-04-13 18:06:14.000000000 +0100
@@ -35,6 +35,7 @@
void mcpwm_set_duty(float dutyCycle);
void mcpwm_set_duty_noramp(float dutyCycle);
void mcpwm_set_pid_speed(float rpm);
+void mcpwm_set_pid_speed_with_cruise_status(float rpm, ppm_cruise cruise_status);
void mcpwm_set_pid_pos(float pos);
void mcpwm_set_current(float current);
void mcpwm_set_brake_current(float current);
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/mcpwm_foc.c Raptor2 Sources/bldc-firmware_2_80/mcpwm_foc.c
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/mcpwm_foc.c 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/mcpwm_foc.c 2017-06-05 00:12:34.000000000 +0100
@@ -51,6 +51,7 @@
float i_abs;
float i_abs_filter;
float i_bus;
+ float i_bus_filter;
float v_bus;
float v_alpha;
float v_beta;
@@ -89,8 +90,9 @@
static volatile float m_iq_set;
static volatile bool m_dccal_done;
static volatile bool m_output_on;
-static volatile float m_pos_pid_set_pos;
+static volatile float m_pos_pid_set;
static volatile float m_speed_pid_set_rpm;
+static volatile ppm_cruise m_speed_pid_cruise_control_type;
static volatile float m_phase_now_observer;
static volatile float m_phase_now_observer_override;
static volatile bool m_phase_observer_override;
@@ -101,9 +103,10 @@
static volatile float m_pll_phase;
static volatile float m_pll_speed;
static volatile mc_sample_t m_samples;
-static volatile int m_tachometer;
-static volatile int m_tachometer_abs;
+static volatile float m_tachometer;
+static volatile float m_tachometer_abs;
static volatile float last_inj_adc_isr_duration;
+static volatile float m_pos_pid_now;
// Private functions
static void do_dc_cal(void);
@@ -114,10 +117,13 @@
static void control_current(volatile motor_state_t *state_m, float dt);
static void svm(float alpha, float beta, uint32_t PWMHalfPeriod,
uint32_t* tAout, uint32_t* tBout, uint32_t* tCout);
-static void run_pid_control_pos(float dt);
+static void run_pid_control_pos(float angle_now, float angle_set, float dt);
static void run_pid_control_speed(float dt);
static void stop_pwm_hw(void);
static void start_pwm_hw(void);
+static int read_hall(void);
+static float correct_encoder(float obs_angle, float enc_angle, float speed);
+static float correct_hall(float angle, float speed, float dt);
// Threads
static THD_WORKING_AREA(timer_thread_wa, 2048);
@@ -199,8 +205,9 @@
m_id_set = 0.0;
m_iq_set = 0.0;
m_output_on = false;
- m_pos_pid_set_pos = 0.0;
+ m_pos_pid_set = 0.0;
m_speed_pid_set_rpm = 0.0;
+ m_speed_pid_cruise_control_type = CRUISE_CONTROL_MOTOR_SETTINGS;
m_phase_now_observer = 0.0;
m_phase_now_observer_override = 0.0;
m_phase_observer_override = false;
@@ -210,9 +217,10 @@
m_observer_x2 = 0.0;
m_pll_phase = 0.0;
m_pll_speed = 0.0;
- m_tachometer = 0;
- m_tachometer_abs = 0;
+ m_tachometer = 0.0;
+ m_tachometer_abs = 0.0;
last_inj_adc_isr_duration = 0;
+ m_pos_pid_now = 0.0;
memset((void*)&m_motor_state, 0, sizeof(motor_state_t));
memset((void*)&m_samples, 0, sizeof(mc_sample_t));
@@ -512,10 +520,30 @@
* The electrical RPM goal value to use.
*/
void mcpwm_foc_set_pid_speed(float rpm) {
+ // Too low RPM set. Reset state and return.
+ if (fabsf(rpm) < m_conf->s_pid_min_erpm) {
+ mcpwm_foc_set_duty(0.0);
+ return;
+ }
+ m_control_mode = CONTROL_MODE_SPEED;
+ m_speed_pid_set_rpm = rpm;
+ m_speed_pid_cruise_control_type = CRUISE_CONTROL_MOTOR_SETTINGS;
+
+ if (m_state != MC_STATE_RUNNING) {
+ m_state = MC_STATE_RUNNING;
+ }
+}
+
+void mcpwm_foc_set_pid_speed_with_cruise_status(float rpm, ppm_cruise cruise_status) {
+ if (fabsf(rpm) < m_conf->s_pid_min_erpm) {
+ mcpwm_foc_set_duty(0.0);
+ return;
+ }
m_control_mode = CONTROL_MODE_SPEED;
m_speed_pid_set_rpm = rpm;
+ m_speed_pid_cruise_control_type = cruise_status;
- if (m_state != MC_STATE_RUNNING && fabsf(rpm) > m_conf->s_pid_min_erpm) {
+ if (m_state != MC_STATE_RUNNING) {
m_state = MC_STATE_RUNNING;
}
}
@@ -529,7 +557,7 @@
*/
void mcpwm_foc_set_pid_pos(float pos) {
m_control_mode = CONTROL_MODE_POS;
- m_pos_pid_set_pos = pos;
+ m_pos_pid_set = pos;
if (m_state != MC_STATE_RUNNING) {
m_state = MC_STATE_RUNNING;
@@ -577,7 +605,7 @@
return;
}
- utils_truncate_number(&current, m_conf->lo_current_min, m_conf->lo_current_max);
+ utils_truncate_number(&current, m_conf->lo_current_min, fabsf(m_conf->lo_current_min));
m_control_mode = CONTROL_MODE_CURRENT_BRAKE;
m_iq_set = current;
@@ -595,6 +623,14 @@
return m_motor_state.duty_now;
}
+float mcpwm_foc_get_pid_pos_set(void) {
+ return m_pos_pid_set;
+}
+
+float mcpwm_foc_get_pid_pos_now(void) {
+ return m_pos_pid_now;
+}
+
/**
* Get the current switching frequency.
*
@@ -702,7 +738,7 @@
* The filtered input current.
*/
float mcpwm_foc_get_tot_current_in_filtered(void) {
- return m_motor_state.i_bus; // TODO: Calculate filtered current?
+ return m_motor_state.i_bus_filter; // TODO: Calculate filtered current?
}
/**
@@ -717,10 +753,10 @@
* be this number divided by (3 * MOTOR_POLE_NUMBER).
*/
int mcpwm_foc_get_tachometer_value(bool reset) {
- int val = m_tachometer;
+ int val = (int) m_tachometer;
if (reset) {
- m_tachometer = 0;
+ m_tachometer = 0.0;
}
return val;
@@ -737,10 +773,10 @@
* be this number divided by (3 * MOTOR_POLE_NUMBER).
*/
int mcpwm_foc_get_tachometer_abs_value(bool reset) {
- int val = m_tachometer_abs;
+ int val = (int) m_tachometer_abs;
if (reset) {
- m_tachometer_abs = 0;
+ m_tachometer_abs = 0.0;
}
return val;
@@ -806,6 +842,8 @@
* The detected direction.
*/
void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *ratio, bool *inverted) {
+ mc_interface_lock();
+
m_phase_override = true;
m_id_set = current;
m_iq_set = 0.0;
@@ -815,7 +853,7 @@
// Disable timeout
systime_t tout = timeout_get_timeout_msec();
float tout_c = timeout_get_brake_current();
- timeout_configure(60000, 0.0);
+ timeout_configure(600000, 0.0);
// Save configuration
float offset_old = m_conf->foc_encoder_offset;
@@ -858,34 +896,68 @@
// Inverted and ratio
chThdSleepMilliseconds(1000);
+ const int it_rat = 20;
float s_sum = 0.0;
float c_sum = 0.0;
- for (int i = 0; i < 10; i++) {
+ float first = m_phase_now_encoder;
+
+ for (int i = 0; i < it_rat; i++) {
float phase_old = m_phase_now_encoder;
float phase_ovr_tmp = m_phase_now_override;
- for (float i = phase_ovr_tmp; i < phase_ovr_tmp + 0.8 * M_PI;
+ for (float i = phase_ovr_tmp; i < phase_ovr_tmp + (2.0 / 3.0) * M_PI;
i += (2.0 * M_PI) / 500.0) {
m_phase_now_override = i;
- chThdSleepMilliseconds(2);
+ chThdSleepMilliseconds(1);
+ }
+ utils_norm_angle_rad((float*)&m_phase_now_override);
+ chThdSleepMilliseconds(300);
+ float diff = utils_angle_difference_rad(m_phase_now_encoder, phase_old);
+
+ float s, c;
+ sincosf(diff, &s, &c);
+ s_sum += s;
+ c_sum += c;
+
+ if (print) {
+ commands_printf("%.2f", (double)(diff * 180.0 / M_PI));
+ }
+
+ if (i > 3 && fabsf(utils_angle_difference_rad(m_phase_now_encoder, first)) < fabsf(diff / 2.0)) {
+ break;
+ }
+ }
+
+ first = m_phase_now_encoder;
+
+ for (int i = 0; i < it_rat; i++) {
+ float phase_old = m_phase_now_encoder;
+ float phase_ovr_tmp = m_phase_now_override;
+ for (float i = phase_ovr_tmp; i > phase_ovr_tmp - (2.0 / 3.0) * M_PI;
+ i -= (2.0 * M_PI) / 500.0) {
+ m_phase_now_override = i;
+ chThdSleepMilliseconds(1);
}
- chThdSleepMilliseconds(1000);
- float diff = utils_angle_difference(
- m_phase_now_encoder * (180.0 / M_PI),
- phase_old * (180.0 / M_PI));
+ utils_norm_angle_rad((float*)&m_phase_now_override);
+ chThdSleepMilliseconds(300);
+ float diff = utils_angle_difference_rad(phase_old, m_phase_now_encoder);
float s, c;
- sincosf(diff * M_PI / 180.0, &s, &c);
+ sincosf(diff, &s, &c);
s_sum += s;
c_sum += c;
if (print) {
- commands_printf("%.2f", (double)diff);
+ commands_printf("%.2f", (double)(diff * 180.0 / M_PI));
+ }
+
+ if (i > 3 && fabsf(utils_angle_difference_rad(m_phase_now_encoder, first)) < fabsf(diff / 2.0)) {
+ break;
}
}
float diff = atan2f(s_sum, c_sum) * 180.0 / M_PI;
*inverted = diff < 0.0;
- *ratio = roundf((0.8 * 180.0) /
+ *ratio = roundf(((2.0 / 3.0) * 180.0) /
fabsf(diff));
m_conf->foc_encoder_inverted = *inverted;
@@ -906,33 +978,37 @@
commands_printf("Enc: %.2f", (double)encoder_read_deg());
}
+ const int it_ofs = m_conf->foc_encoder_ratio * 3.0;
s_sum = 0.0;
c_sum = 0.0;
- for (int i = 0;i < 5;i++) {
- m_phase_now_override = (float)i * M_PI / 5.0;
- chThdSleepMilliseconds(1000);
+ for (int i = 0;i < it_ofs;i++) {
+ m_phase_now_override = ((float)i * 2.0 * M_PI * m_conf->foc_encoder_ratio) / ((float)it_ofs);
+ chThdSleepMilliseconds(500);
+
+ float diff = utils_angle_difference_rad(m_phase_now_encoder, m_phase_now_override);
float s, c;
- sincosf(m_phase_now_encoder - m_phase_now_override, &s, &c);
+ sincosf(diff, &s, &c);
s_sum += s;
c_sum += c;
if (print) {
- commands_printf("%.2f", (double)((m_phase_now_encoder - m_phase_now_override) * 180.0 / M_PI));
+ commands_printf("%.2f", (double)(diff * 180.0 / M_PI));
}
}
- for (int i = 3;i >= -1;i--) {
- m_phase_now_override = (float)i * M_PI / 5.0;
- chThdSleepMilliseconds(1000);
+ for (int i = it_ofs;i > 0;i--) {
+ m_phase_now_override = ((float)i * 2.0 * M_PI * m_conf->foc_encoder_ratio) / ((float)it_ofs);
+ chThdSleepMilliseconds(500);
+ float diff = utils_angle_difference_rad(m_phase_now_encoder, m_phase_now_override);
float s, c;
- sincosf(m_phase_now_encoder - m_phase_now_override, &s, &c);
+ sincosf(diff, &s, &c);
s_sum += s;
c_sum += c;
if (print) {
- commands_printf("%.2f", (double)((m_phase_now_encoder - m_phase_now_override) * 180.0 / M_PI));
+ commands_printf("%.2f", (double)(diff * 180.0 / M_PI));
}
}
@@ -962,6 +1038,8 @@
// Enable timeout
timeout_configure(tout, tout_c);
+
+ mc_interface_unlock();
}
/**
@@ -1117,7 +1195,7 @@
float res_tmp = 0.0;
float i_last = 0.0;
- for (float i = 2.0;i < 35.0;i *= 1.5) {
+ for (float i = 2.0;i < (m_conf->l_current_max / 2.0);i *= 1.5) {
res_tmp = mcpwm_foc_measure_resistance(i, 20);
if (i > (0.5 / res_tmp)) {
@@ -1127,7 +1205,7 @@
}
if (i_last < 0.01) {
- i_last = 35.0;
+ i_last = (m_conf->l_current_max / 2.0);
}
*res = mcpwm_foc_measure_resistance(i_last, 500);
@@ -1137,7 +1215,7 @@
TIMER_UPDATE_SAMP_TOP(top - 2, 5, top);
float duty_last = 0.0;
- for (float i = 0.02;i < 0.9;i *= 1.5) {
+ for (float i = 0.02;i < 0.5;i *= 1.5) {
float i_tmp;
mcpwm_foc_measure_inductance(i, 20, &i_tmp);
@@ -1159,6 +1237,101 @@
return true;
}
+/**
+ * Run the motor in open loop and figure out at which angles the hall sensors are.
+ *
+ * @param current
+ * Current to use.
+ *
+ * @param hall_table
+ * Table to store the result to.
+ *
+ * @return
+ * true: Success
+ * false: Something went wrong
+ */
+bool mcpwm_foc_hall_detect(float current, uint8_t *hall_table) {
+ mc_interface_lock();
+
+ m_phase_override = true;
+ m_id_set = current;
+ m_iq_set = 0.0;
+ m_control_mode = CONTROL_MODE_CURRENT;
+ m_state = MC_STATE_RUNNING;
+
+ // Disable timeout
+ systime_t tout = timeout_get_timeout_msec();
+ float tout_c = timeout_get_brake_current();
+ timeout_configure(60000, 0.0);
+
+ // Lock the motor
+ m_phase_now_override = 0;
+ chThdSleepMilliseconds(1000);
+
+ float sin_hall[8];
+ float cos_hall[8];
+ int hall_iterations[8];
+ memset(sin_hall, 0, sizeof(sin_hall));
+ memset(cos_hall, 0, sizeof(cos_hall));
+ memset(hall_iterations, 0, sizeof(hall_iterations));
+
+ // Forwards
+ for (int i = 0;i < 3;i++) {
+ for (int i = 0;i < 360;i++) {
+ m_phase_now_override = (float)i * M_PI / 180.0;
+ chThdSleepMilliseconds(5);
+
+ int hall = read_hall();
+ float s, c;
+ sincosf(m_phase_now_override, &s, &c);
+ sin_hall[hall] += s;
+ cos_hall[hall] += c;
+ hall_iterations[hall]++;
+ }
+ }
+
+ // Reverse
+ for (int i = 0;i < 3;i++) {
+ for (int i = 360;i >= 0;i--) {
+ m_phase_now_override = (float)i * M_PI / 180.0;
+ chThdSleepMilliseconds(5);
+
+ int hall = read_hall();
+ float s, c;
+ sincosf(m_phase_now_override, &s, &c);
+ sin_hall[hall] += s;
+ cos_hall[hall] += c;
+ hall_iterations[hall]++;
+ }
+ }
+
+ m_id_set = 0.0;
+ m_iq_set = 0.0;
+ m_phase_override = false;
+ m_control_mode = CONTROL_MODE_NONE;
+ m_state = MC_STATE_OFF;
+ stop_pwm_hw();
+
+ // Enable timeout
+ timeout_configure(tout, tout_c);
+
+ int fails = 0;
+ for(int i = 0;i < 8;i++) {
+ if (hall_iterations[i] > 30) {
+ float ang = atan2f(sin_hall[i], cos_hall[i]) * 180.0 / M_PI;
+ utils_norm_angle(&ang);
+ hall_table[i] = (uint8_t)(ang * 200.0 / 360.0);
+ } else {
+ hall_table[i] = 255;
+ fails++;
+ }
+ }
+
+ mc_interface_unlock();
+
+ return fails == 2;
+}
+
void mcpwm_foc_print_state(void) {
commands_printf("Mod d: %.2f", (double)m_motor_state.mod_d);
commands_printf("Mod q: %.2f", (double)m_motor_state.mod_q);
@@ -1259,16 +1432,18 @@
m_motor_state.v_bus = GET_INPUT_VOLTAGE();
-#if ENCODER_ENABLE
- float phase_tmp = encoder_read_deg();
- if (m_conf->foc_encoder_inverted) {
- phase_tmp = 360.0 - phase_tmp;
- }
- phase_tmp *= m_conf->foc_encoder_ratio;
- phase_tmp -= m_conf->foc_encoder_offset;
- utils_norm_angle((float*)&phase_tmp);
- m_phase_now_encoder = phase_tmp * (M_PI / 180.0);
-#endif
+ float enc_ang = 0;
+ if (encoder_is_configured()) {
+ enc_ang = encoder_read_deg();
+ float phase_tmp = enc_ang;
+ if (m_conf->foc_encoder_inverted) {
+ phase_tmp = 360.0 - phase_tmp;
+ }
+ phase_tmp *= m_conf->foc_encoder_ratio;
+ phase_tmp -= m_conf->foc_encoder_offset;
+ utils_norm_angle((float*)&phase_tmp);
+ m_phase_now_encoder = phase_tmp * (M_PI / 180.0);
+ }
static float phase_before = 0.0;
const float phase_diff = utils_angle_difference_rad(m_motor_state.phase, phase_before);
@@ -1299,12 +1474,19 @@
// observer has lost tracking. Use duty cycle control with the lowest duty cycle
// to get as smooth braking as possible.
if (m_control_mode == CONTROL_MODE_CURRENT_BRAKE
- && m_conf->foc_sensor_mode == FOC_SENSOR_MODE_SENSORLESS
+// && (m_conf->foc_sensor_mode != FOC_SENSOR_MODE_ENCODER) // Don't use this with encoderss
&& fabsf(duty_filtered) < 0.03) {
control_duty = true;
duty_set = 0.0;
}
+ // Brake when set ERPM is below min ERPM
+ if (m_control_mode == CONTROL_MODE_SPEED &&
+ fabsf(m_speed_pid_set_rpm) < m_conf->s_pid_min_erpm) {
+ control_duty = true;
+ duty_set = 0.0;
+ }
+
if (control_duty) {
// Duty cycle control
static float duty_i_term = 0.0;
@@ -1361,7 +1543,7 @@
switch (m_conf->foc_sensor_mode) {
case FOC_SENSOR_MODE_ENCODER:
if (encoder_index_found()) {
- m_motor_state.phase = m_phase_now_encoder;
+ m_motor_state.phase = correct_encoder(m_phase_now_observer, m_phase_now_encoder, m_pll_speed);
} else {
// Rotate the motor in open loop if the index isn't found.
m_motor_state.phase = m_phase_now_encoder_no_index;
@@ -1371,6 +1553,14 @@
id_set_tmp = 0.0;
}
break;
+ case FOC_SENSOR_MODE_HALL:
+ m_phase_now_observer = correct_hall(m_phase_now_observer, m_pll_speed, dt);
+ m_motor_state.phase = m_phase_now_observer;
+
+ if (!m_phase_override) {
+ id_set_tmp = 0.0;
+ }
+ break;
case FOC_SENSOR_MODE_SENSORLESS:
if (m_phase_observer_override) {
m_motor_state.phase = m_phase_now_observer_override;
@@ -1400,7 +1590,12 @@
// Apply current limits
const float mod_q = m_motor_state.mod_q;
utils_truncate_number(&iq_set_tmp, m_conf->lo_current_min, m_conf->lo_current_max);
- utils_saturate_vector_2d(&id_set_tmp, &iq_set_tmp, m_conf->lo_current_max);
+ if (iq_set_tmp < 0.0) {
+ utils_saturate_vector_2d(&id_set_tmp, &iq_set_tmp, m_conf->lo_current_min);
+ } else {
+
+ utils_saturate_vector_2d(&id_set_tmp, &iq_set_tmp, m_conf->lo_current_max);
+ }
if (mod_q > 0.001) {
utils_truncate_number(&iq_set_tmp, m_conf->lo_in_current_min / mod_q, m_conf->lo_in_current_max / mod_q);
} else if (mod_q < -0.001) {
@@ -1411,7 +1606,6 @@
m_motor_state.iq_target = iq_set_tmp;
control_current(&m_motor_state, dt);
- run_pid_control_pos(dt);
} else {
// Track back emf
float Va = ADC_VOLTS(ADC_IND_SENS1) * ((VIN_R1 + VIN_R2) / VIN_R2);
@@ -1440,6 +1634,7 @@
m_motor_state.id_filter = 0.0;
m_motor_state.iq_filter = 0.0;
m_motor_state.i_bus = 0.0;
+ m_motor_state.i_bus_filter = 0.0;
m_motor_state.i_abs = 0.0;
m_motor_state.i_abs_filter = 0.0;
@@ -1449,8 +1644,16 @@
&m_observer_x2, &m_phase_now_observer);
switch (m_conf->foc_sensor_mode) {
- case FOC_SENSOR_MODE_ENCODER: m_motor_state.phase = m_phase_now_encoder; break;
- case FOC_SENSOR_MODE_SENSORLESS: m_motor_state.phase = m_phase_now_observer; break;
+ case FOC_SENSOR_MODE_ENCODER:
+ m_motor_state.phase = correct_encoder(m_phase_now_observer, m_phase_now_encoder, m_pll_speed);
+ break;
+ case FOC_SENSOR_MODE_HALL:
+ m_phase_now_observer = correct_hall(m_phase_now_observer, m_pll_speed, dt);
+ m_motor_state.phase = m_phase_now_observer;
+ break;
+ case FOC_SENSOR_MODE_SENSORLESS:
+ m_motor_state.phase = m_phase_now_observer;
+ break;
}
}
@@ -1464,13 +1667,38 @@
// Update tachometer
static float phase_last = 0.0;
- int diff = (int)((utils_angle_difference_rad(m_motor_state.phase, phase_last) * (180.0 / M_PI)) / 60.0);
- if (diff != 0) {
+ // int diff = (int)((utils_angle_difference_rad(m_motor_state.phase, phase_last) * (180.0 / M_PI)) / 60.0);
+ float diff = (utils_angle_difference_rad(m_motor_state.phase, phase_last) * (180.0 / M_PI)) / 60.0;
+ if ((int) diff != 0) {
m_tachometer += diff;
- m_tachometer_abs += abs(diff);
+ m_tachometer_abs += fabsf(diff);
phase_last = m_motor_state.phase;
}
+ // Track position control angle
+ // TODO: Have another look at this.
+ float angle_now = 0.0;
+ if (encoder_is_configured()) {
+ angle_now = enc_ang;
+ } else {
+ angle_now = m_motor_state.phase * (180.0 / M_PI);
+ }
+
+ if (m_conf->p_pid_ang_div > 0.98 && m_conf->p_pid_ang_div < 1.02) {
+ m_pos_pid_now = angle_now;
+ } else {
+ static float angle_last = 0.0;
+ float diff_f = utils_angle_difference(angle_now, angle_last);
+ angle_last = angle_now;
+ m_pos_pid_now += diff_f / m_conf->p_pid_ang_div;
+ utils_norm_angle((float*)&m_pos_pid_now);
+ }
+
+ // Run position control
+ if (m_state == MC_STATE_RUNNING) {
+ run_pid_control_pos(m_pos_pid_now, m_pos_pid_set, dt);
+ }
+
// MCIF handler
mc_interface_mc_timer_isr();
@@ -1490,8 +1718,12 @@
return;
}
+ float openloop_rpm = utils_map(fabsf(m_motor_state.iq_target),
+ 0.0, m_conf->lo_current_max,
+ 0.0, m_conf->foc_openloop_rpm);
+
const float dt = 0.001;
- const float min_rads = (m_conf->foc_openloop_rpm * 2.0 * M_PI) / 60.0;
+ const float min_rads = (openloop_rpm * 2.0 * M_PI) / 60.0;
static float min_rpm_hyst_timer = 0.0;
static float min_rpm_timer = 0.0;
@@ -1514,7 +1746,7 @@
}
// Don't use this in brake mode.
- if (m_control_mode == CONTROL_MODE_CURRENT_BRAKE) {
+ if (m_control_mode == CONTROL_MODE_CURRENT_BRAKE || fabsf(m_motor_state.duty_now) < 0.001) {
min_rpm_hyst_timer = 0.0;
m_phase_observer_override = false;
}
@@ -1671,19 +1903,13 @@
// TODO: Have a look at this?
state_m->i_bus = state_m->mod_d * state_m->id + state_m->mod_q * state_m->iq;
+ UTILS_LP_FAST(state_m->i_bus_filter, state_m->i_bus, MCPWM_FOC_I_FILTER_CONST);
state_m->i_abs = sqrtf(state_m->id * state_m->id + state_m->iq * state_m->iq);
state_m->i_abs_filter = sqrtf(state_m->id_filter * state_m->id_filter + state_m->iq_filter * state_m->iq_filter);
float mod_alpha = c * state_m->mod_d - s * state_m->mod_q;
float mod_beta = c * state_m->mod_q + s * state_m->mod_d;
- state_m->v_alpha = mod_alpha * (2.0 / 3.0) * state_m->v_bus;
- state_m->v_beta = mod_beta * (2.0 / 3.0) * state_m->v_bus;
-
- // Set output (HW Dependent)
- uint32_t duty1, duty2, duty3, top;
- top = TIM1->ARR;
-
// Deadtime compensation
const float i_alpha_set = c * state_m->id_target - s * state_m->iq_target;
const float i_beta_set = c * state_m->iq_target + s * state_m->id_target;
@@ -1692,10 +1918,20 @@
const float ic_set = -0.5 * i_alpha_set - SQRT3_BY_2 * i_beta_set;
const float mod_alpha_set_sgn = (2.0 / 3.0) * SIGN(ia_set) - (1.0 / 3.0) * SIGN(ib_set) - (1.0 / 3.0) * SIGN(ic_set);
const float mod_beta_set_sgn = ONE_BY_SQRT3 * SIGN(ib_set) - ONE_BY_SQRT3 * SIGN(ic_set);
+ const float mod_comp_fact = m_conf->foc_dt_us * 1e-6 * m_conf->foc_f_sw;
+ const float mod_alpha_comp = mod_alpha_set_sgn * mod_comp_fact;
+ const float mod_beta_comp = mod_beta_set_sgn * mod_comp_fact;
+
+// mod_alpha += mod_alpha_comp;
+// mod_beta += mod_beta_comp;
+
+ // Apply compensation here so that 0 duty cyle has no glitches.
+ state_m->v_alpha = (mod_alpha - mod_alpha_comp) * (2.0 / 3.0) * state_m->v_bus;
+ state_m->v_beta = (mod_beta - mod_beta_comp) * (2.0 / 3.0) * state_m->v_bus;
- mod_alpha += mod_alpha_set_sgn * m_conf->foc_dt_us * 1e-6 * m_conf->foc_f_sw;
- mod_beta += mod_beta_set_sgn * m_conf->foc_dt_us * 1e-6 * m_conf->foc_f_sw;
-
+ // Set output (HW Dependent)
+ uint32_t duty1, duty2, duty3, top;
+ top = TIM1->ARR;
svm(-mod_alpha, -mod_beta, top, &duty1, &duty2, &duty3);
TIMER_UPDATE_DUTY(duty1, duty2, duty3);
@@ -1834,7 +2070,7 @@
*tCout = tC;
}
-static void run_pid_control_pos(float dt) {
+static void run_pid_control_pos(float angle_now, float angle_set, float dt) {
static float i_term = 0;
static float prev_error = 0;
float p_term;
@@ -1847,19 +2083,30 @@
return;
}
- // Compute error
- float angle = encoder_read_deg();
- float error = utils_angle_difference(m_pos_pid_set_pos, angle);
+ // Compute parameters
+ float error = utils_angle_difference(angle_set, angle_now);
- if (m_conf->foc_encoder_inverted) {
- error = -error;
+ if (encoder_is_configured()) {
+ if (m_conf->foc_encoder_inverted) {
+ error = -error;
+ }
}
-
- // Compute parameters
p_term = error * m_conf->p_pid_kp;
i_term += error * (m_conf->p_pid_ki * dt);
- d_term = (error - prev_error) * (m_conf->p_pid_kd / dt);
+
+ // Average DT for the D term when the error does not change. This likely
+ // happens at low speed when the position resolution is low and several
+ // control iterations run without position updates.
+ // TODO: Are there problems with this approach?
+ static float dt_int = 0.0;
+ dt_int += dt;
+ if (error == prev_error) {
+ d_term = 0.0;
+ } else {
+ d_term = (error - prev_error) * (m_conf->p_pid_kd / dt_int);
+ dt_int = 0.0;
+ }
// I-term wind-up protection
utils_truncate_number(&i_term, -1.0, 1.0);
@@ -1871,11 +2118,15 @@
float output = p_term + i_term + d_term;
utils_truncate_number(&output, -1.0, 1.0);
- if (encoder_index_found()) {
- m_iq_set = output * m_conf->lo_current_max;
+ if (encoder_is_configured()) {
+ if (encoder_index_found()) {
+ m_iq_set = output * m_conf->lo_current_max;
+ } else {
+ // Rotate the motor with 40 % power until the encoder index is found.
+ m_iq_set = 0.4 * m_conf->lo_current_max;
+ }
} else {
- // Rotate the motor with 40 % power until the encoder index is found.
- m_iq_set = 0.4 * m_conf->lo_current_max;
+ m_iq_set = output * m_conf->lo_current_max;
}
}
@@ -1892,15 +2143,11 @@
return;
}
- // Too low RPM set. Stop and return.
+ // Too low RPM set. Reset state and return.
if (fabsf(m_speed_pid_set_rpm) < m_conf->s_pid_min_erpm) {
i_term = 0.0;
prev_error = 0;
m_iq_set = 0.0;
- m_state = MC_STATE_OFF;
- if (m_output_on) {
- stop_pwm_hw();
- }
return;
}
@@ -1923,8 +2170,17 @@
// Calculate output
float output = p_term + i_term + d_term;
- utils_truncate_number(&output, -1.0, 1.0);
+ if ((m_speed_pid_cruise_control_type == CRUISE_CONTROL_MOTOR_SETTINGS && m_conf->s_pid_breaking_enabled) || m_speed_pid_cruise_control_type == CRUISE_CONTROL_BRAKING_ENABLED) {
+ utils_truncate_number(&output, -1.0, 1.0);
+ } else {
+ if(m_speed_pid_set_rpm < 0.0){
+ utils_truncate_number(&output, -1.0, 0.0);
+ }else{
+ utils_truncate_number(&output, 0.0, 1.0);
+ }
+ }
+
m_iq_set = output * m_conf->lo_current_max;
}
@@ -1963,3 +2219,113 @@
m_output_on = true;
}
+
+static int read_hall(void) {
+ return READ_HALL1() | (READ_HALL2() << 1) | (READ_HALL3() << 2);
+}
+
+static float correct_encoder(float obs_angle, float enc_angle, float speed) {
+ float rpm_abs = fabsf(speed / ((2.0 * M_PI) / 60.0));
+ static bool using_encoder = true;
+
+ // Hysteresis 5 % of total speed
+ float hyst = m_conf->foc_sl_erpm * 0.05;
+ if (using_encoder) {
+ if (rpm_abs > (m_conf->foc_sl_erpm + hyst)) {
+ using_encoder = false;
+ }
+ } else {
+ if (rpm_abs < (m_conf->foc_sl_erpm - hyst)) {
+ using_encoder = true;
+ }
+ }
+
+ return using_encoder ? enc_angle : obs_angle;
+}
+
+static float correct_hall(float angle, float speed, float dt) {
+ static int ang_hall_int_prev = -1;
+ float rpm_abs = fabsf(speed / ((2.0 * M_PI) / 60.0));
+ static bool using_hall = true;
+
+ // Hysteresis 10 % of total speed
+ float hyst = m_conf->foc_sl_erpm * 0.10;
+ if (using_hall) {
+ if (rpm_abs > (m_conf->foc_sl_erpm + hyst)) {
+ using_hall = false;
+ }
+ } else {
+ if (rpm_abs < (m_conf->foc_sl_erpm - hyst)) {
+ using_hall = true;
+ }
+ }
+
+ if (using_hall) {
+ int ang_hall_int = m_conf->foc_hall_table[read_hall()];
+
+ // Only override the observer if the hall sensor value is valid.
+ if (ang_hall_int < 201) {
+ static float ang_hall = 0.0;
+ float ang_hall_now = (((float)ang_hall_int / 200.0) * 360.0) * M_PI / 180.0;
+
+ if (ang_hall_int_prev < 0) {
+ // Previous angle not valid
+ ang_hall_int_prev = ang_hall_int;
+
+ if (ang_hall_int_prev == -2) {
+ // Before was sensorless, initialize with the provided angle
+ ang_hall = angle;
+ } else {
+ // A boot or error has occurred. Use center of hall sensor angle.
+ ang_hall = ((ang_hall_int / 200.0) * 360.0) * M_PI / 180.0;
+ }
+ } else if (ang_hall_int != ang_hall_int_prev) {
+ // A transition was just made. The angle is in the middle of the new and old angle.
+ int ang_avg = abs(ang_hall_int - ang_hall_int_prev);
+ if (ang_avg < 100) {
+ ang_avg = (ang_hall_int + ang_hall_int_prev) / 2;
+ } else if (ang_avg != 100) {
+ ang_avg = (ang_hall_int + ang_hall_int_prev) / 2 + 100;
+ }
+ ang_avg %= 200;
+ ang_hall = (((float)ang_avg / 200.0) * 360.0) * M_PI / 180.0;
+ }
+
+ ang_hall_int_prev = ang_hall_int;
+
+ if (rpm_abs < 100) {
+ // Don't interpolate on very low speed, just use the closest hall sensor
+ ang_hall = ang_hall_now;
+ } else {
+ // Interpolate
+ float diff = utils_angle_difference_rad(ang_hall, ang_hall_now);
+ if (fabsf(diff) < ((2.0 * M_PI) / 12.0)) {
+ // Do interpolation
+ ang_hall += speed * dt;
+ } else {
+ // We are too far away with the interpolation
+ ang_hall -= diff / 100.0;
+ }
+ }
+
+ utils_norm_angle_rad(&ang_hall);
+
+ angle = ang_hall;
+ } else {
+ // Invalid hall reading. Don't update angle.
+ ang_hall_int_prev = -1;
+
+ // Also allow open loop in order to behave like normal sensorless
+ // operation. Then the motor works even if the hall sensor cable
+ // gets disconnected (when the sensor spacing is 120 degrees).
+ if (m_phase_observer_override && m_state == MC_STATE_RUNNING) {
+ angle = m_phase_now_observer_override;
+ }
+ }
+ } else {
+ // We are running sensorless.
+ ang_hall_int_prev = -2;
+ }
+
+ return angle;
+}
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/mcpwm_foc.h Raptor2 Sources/bldc-firmware_2_80/mcpwm_foc.h
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/mcpwm_foc.h 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/mcpwm_foc.h 2017-04-13 18:06:44.000000000 +0100
@@ -39,11 +39,14 @@
void mcpwm_foc_set_duty(float dutyCycle);
void mcpwm_foc_set_duty_noramp(float dutyCycle);
void mcpwm_foc_set_pid_speed(float rpm);
+void mcpwm_foc_set_pid_speed_with_cruise_status(float rpm, ppm_cruise cruise_status);
void mcpwm_foc_set_pid_pos(float pos);
void mcpwm_foc_set_current(float current);
void mcpwm_foc_set_brake_current(float current);
float mcpwm_foc_get_duty_cycle_set(void);
float mcpwm_foc_get_duty_cycle_now(void);
+float mcpwm_foc_get_pid_pos_set(void);
+float mcpwm_foc_get_pid_pos_now(void);
float mcpwm_foc_get_switching_frequency_now(void);
float mcpwm_foc_get_rpm(void);
float mcpwm_foc_get_tot_current(void);
@@ -65,6 +68,7 @@
float mcpwm_foc_measure_resistance(float current, int samples);
float mcpwm_foc_measure_inductance(float duty, int samples, float *curr);
bool mcpwm_foc_measure_res_ind(float *res, float *ind);
+bool mcpwm_foc_hall_detect(float current, uint8_t *hall_table);
void mcpwm_foc_print_state(void);
float mcpwm_foc_get_last_inj_adc_isr_duration(void);
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/mcuconf.h Raptor2 Sources/bldc-firmware_2_80/mcuconf.h
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/mcuconf.h 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/mcuconf.h 2016-09-19 10:05:44.000000000 +0100
@@ -248,7 +248,7 @@
/*
* SPI driver system settings.
*/
-#define STM32_SPI_USE_SPI1 FALSE
+#define STM32_SPI_USE_SPI1 TRUE
#define STM32_SPI_USE_SPI2 FALSE
#define STM32_SPI_USE_SPI3 FALSE
#define STM32_SPI_SPI1_RX_DMA_STREAM STM32_DMA_STREAM_ID(2, 0)
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/nrf/nrf_driver.c Raptor2 Sources/bldc-firmware_2_80/nrf/nrf_driver.c
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/nrf/nrf_driver.c 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/nrf/nrf_driver.c 2016-08-04 23:47:30.000000000 +0100
@@ -37,9 +37,6 @@
#define MAX_PL_LEN 25
#define RX_BUFFER_SIZE PACKET_MAX_PL_LEN
-#define NOACK 0
-#define TX_RESENDS 1
-
#define ALIVE_INTERVAL 50 // Send alive packets at this rate
#define NRF_RESTART_TIMEOUT 500 // Restart the NRF if nothing has been received or acked for this time
@@ -58,28 +55,18 @@
static int rf_tx_wrapper(char *data, int len);
void nrf_driver_init(void) {
- rf_init();
rfhelp_init();
nosend_cnt = 0;
nrf_restart_rx_time = 0;
nrf_restart_tx_time = 0;
- // Set RF address
- const char addr[3] = {0xC6, 0xC7, app_get_configuration()->controller_id};
- rfhelp_set_rx_addr(0, addr, 3);
- rfhelp_set_tx_addr(addr, 3);
-
chThdCreateStatic(rx_thread_wa, sizeof(rx_thread_wa), NORMALPRIO - 1, rx_thread, NULL);
chThdCreateStatic(tx_thread_wa, sizeof(tx_thread_wa), NORMALPRIO - 1, tx_thread, NULL);
}
static int rf_tx_wrapper(char *data, int len) {
-#if NOACK
- int res = rfhelp_send_data_crc_noack(data, len, TX_RESENDS);
-#else
int res = rfhelp_send_data_crc(data, len);
-#endif
if (res == 0) {
nrf_restart_tx_time = NRF_RESTART_TIMEOUT;
@@ -120,11 +107,7 @@
int pipe;
for(;;) {
-#if NOACK
- int res = rfhelp_read_rx_data_crc_noack((char*)buf, &len, &pipe);
-#else
int res = rfhelp_read_rx_data_crc((char*)buf, &len, &pipe);
-#endif
chuck_data cdata;
int32_t ind = 0;
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/nrf/rf.c Raptor2 Sources/bldc-firmware_2_80/nrf/rf.c
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/nrf/rf.c 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/nrf/rf.c 2016-08-04 23:47:30.000000000 +0100
@@ -22,32 +22,6 @@
void rf_init(void) {
spi_sw_init();
-
- rf_power_down();
-
- // Set default register values (TODO for the rest)
- rf_write_reg_byte(NRF_REG_EN_RXADDR, 0);
- rf_write_reg_byte(NRF_REG_DYNPD, 0);
-
- rf_set_crc_type(NRF_CRC_1B);
- rf_set_retr_retries(3);
- rf_set_retr_delay(NRF_RETR_DELAY_250US);
- rf_set_power(NRF_POWER_0DBM);
- rf_set_speed(NRF_SPEED_2M);
- rf_set_address_width(NRF_AW_3);
- rf_set_frequency(2400 + 76);
- rf_enable_features(NRF_FEATURE_DPL | NRF_FEATURE_DYN_ACK);
-
- rf_enable_pipe_autoack(NRF_MASK_PIPE0);
- rf_enable_pipe_address(NRF_MASK_PIPE0);
- rf_enable_pipe_dlp(NRF_MASK_PIPE0);
-
- // Note: The address should be set by the application.
-
- rf_power_up();
- rf_mode_rx();
- rf_flush_all();
- rf_clear_irq();
}
void rf_set_speed(NRF_SPEED speed) {
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/nrf/rf.h Raptor2 Sources/bldc-firmware_2_80/nrf/rf.h
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/nrf/rf.h 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/nrf/rf.h 2016-08-04 23:47:30.000000000 +0100
@@ -20,51 +20,7 @@
#define RF_H_
#include <stdint.h>
-
-// Data types
-typedef enum {
- NRF_SPEED_250K = 0,
- NRF_SPEED_1M,
- NRF_SPEED_2M
-} NRF_SPEED;
-
-typedef enum {
- NRF_POWER_M18DBM = 0,
- NRF_POWER_M12DBM,
- NRF_POWER_M6DBM,
- NRF_POWER_0DBM
-} NRF_POWER;
-
-typedef enum {
- NRF_AW_3 = 0,
- NRF_AW_4,
- NRF_AW_5
-} NRF_AW;
-
-typedef enum {
- NRF_CRC_DISABLED = 0,
- NRF_CRC_1B,
- NRF_CRC_2B
-} NRF_CRC;
-
-typedef enum {
- NRF_RETR_DELAY_250US = 0,
- NRF_RETR_DELAY_500US,
- NRF_RETR_DELAY_750US,
- NRF_RETR_DELAY_1000US,
- NRF_RETR_DELAY_1250US,
- NRF_RETR_DELAY_1500US,
- NRF_RETR_DELAY_1750US,
- NRF_RETR_DELAY_2000US,
- NRF_RETR_DELAY_2250US,
- NRF_RETR_DELAY_2500US,
- NRF_RETR_DELAY_2750US,
- NRF_RETR_DELAY_3000US,
- NRF_RETR_DELAY_3250US,
- NRF_RETR_DELAY_3500US,
- NRF_RETR_DELAY_3750US,
- NRF_RETR_DELAY_4000US
-} NRF_RETR_DELAY;
+#include <datatypes.h>
// Status register masks
#define NRF_STATUS_TX_FULL (1<<0)
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/nrf/rfhelp.c Raptor2 Sources/bldc-firmware_2_80/nrf/rfhelp.c
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/nrf/rfhelp.c 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/nrf/rfhelp.c 2016-08-04 23:47:30.000000000 +0100
@@ -29,25 +29,41 @@
static bool rx_addr_set[6];
static int address_length;
static bool tx_pipe0_addr_eq;
+static nrf_config nrf_conf;
+static bool init_done = false;
void rfhelp_init(void) {
chMtxObjectInit(&rf_mutex);
+ rf_init();
// address_length = rf_get_address_width();
address_length = 3; // We assume length 3
// This should not happen
if (address_length > 5 || address_length < 3) {
- address_length = 5;
+ address_length = 3;
}
for (int i = 0;i < 6;i++) {
rf_read_reg(NRF_REG_RX_ADDR_P0, rx_addr[i], address_length);
rx_addr_set[i] = false;
}
- rf_read_reg(NRF_REG_TX_ADDR, tx_addr, address_length);
+ rf_read_reg(NRF_REG_TX_ADDR, tx_addr, address_length);
tx_pipe0_addr_eq = memcmp(rx_addr[0], tx_addr, address_length) == 0;
+
+ // TODO: fill nrf_conf with values from the nrf chip. For now we assume
+ // that nrf_conf is already set when rfhelp_restart is called.
+
+ init_done = true;
+}
+
+void rfhelp_update_conf(nrf_config *conf) {
+ nrf_conf = *conf;
+
+ if (init_done) {
+ rfhelp_restart();
+ }
}
/**
@@ -56,13 +72,38 @@
void rfhelp_restart(void) {
chMtxLock(&rf_mutex);
- rf_init();
+ rf_power_down();
+
+ // Set default register values.
+ // TODO: make this file consistent with multiple
+ // rx_addr and tx_addr, and the rest in general.
+ rf_write_reg_byte(NRF_REG_EN_RXADDR, 0);
+ rf_write_reg_byte(NRF_REG_DYNPD, 0);
+
+ rf_set_crc_type(nrf_conf.crc_type);
+ rf_set_retr_retries(nrf_conf.retries);
+ rf_set_retr_delay(nrf_conf.retry_delay);
+ rf_set_power(nrf_conf.power);
+ rf_set_speed(nrf_conf.speed);
+ rf_set_address_width(NRF_AW_3); // Always use 3 byte address
+ rf_set_frequency(2400 + (unsigned int)nrf_conf.channel);
+ rf_enable_features(NRF_FEATURE_DPL | NRF_FEATURE_DYN_ACK);
+
+ rf_enable_pipe_autoack(NRF_MASK_PIPE0);
+ rf_enable_pipe_address(NRF_MASK_PIPE0);
+ rf_enable_pipe_dlp(NRF_MASK_PIPE0);
+
+ memcpy(tx_addr, nrf_conf.address, 3);
+ memcpy(rx_addr[0], nrf_conf.address, 3);
+ tx_pipe0_addr_eq = true;
+
rf_set_tx_addr(tx_addr, address_length);
- for (int i = 0;i < 6;i++) {
- if (rx_addr_set[i]) {
- rf_set_rx_addr(i, rx_addr[i], address_length);
- }
- }
+ rf_set_rx_addr(0, rx_addr[0], address_length);
+
+ rf_power_up();
+ rf_mode_rx();
+ rf_flush_all();
+ rf_clear_irq();
chMtxUnlock(&rf_mutex);
}
@@ -157,48 +198,7 @@
buffer[len] = (char)(crc >> 8);
buffer[len + 1] = (char)(crc & 0xFF);
- return rfhelp_send_data(buffer, len + 2, true);
-}
-
-/**
- * Same as rfhelp_send_data_crc, but without ack. A counter is included for
- * duplicate packets and a number of resends can be specified.
- *
- * @param data
- * The data to be sent.
- *
- * @param len
- * Length of the data. Should be no more than 29 bytes.
- *
- * @param resends
- * The amount of resends for this packet. Should be at least 1.
- *
- * @return
- * 0: Send OK.
- * -1: Max RT. (Should not happen)
- * -2: Timeout
- */
-int rfhelp_send_data_crc_noack(char *data, int len, int resends) {
- char buffer[len + 3];
- static unsigned char counter = 0;
-
- memcpy(buffer, data, len);
- buffer[len] = counter++;
-
- unsigned short crc = crc16((unsigned char*)buffer, len + 1);
-
- buffer[len + 1] = (char)(crc >> 8);
- buffer[len + 2] = (char)(crc & 0xFF);
-
- int res = 0;
- for (int i = 0;i < resends;i++) {
- res = rfhelp_send_data(buffer, len + 3, false);
- if (res != 0) {
- break;
- }
- }
-
- return res;
+ return rfhelp_send_data(buffer, len + 2, nrf_conf.send_crc_ack);
}
/**
@@ -290,51 +290,6 @@
return res;
}
-/**
- * Same as rfhelp_read_rx_data_crc, but for use with the corresponding
- * nocak send function.
- *
- * @param data
- * Pointer to the array in which to store the data.
- *
- * @param len
- * Pointer to variable storing the data length.
- *
- * @param pipe
- * Pointer to the pipe on which the data was received. Can be 0.
- *
- * @return
- * 1: Read OK, more data to read.
- * 0: Read OK
- * -1: No RX data
- * -2: Wrong length read. Something is likely wrong.
- * -3: Data read, but CRC does not match.
- * -4: Duplicate packet received.
- */
-int rfhelp_read_rx_data_crc_noack(char *data, int *len, int *pipe) {
- int res = rfhelp_read_rx_data(data, len, pipe);
- static unsigned char counter = 0;
-
- if (res >= 0 && *len > 3) {
- unsigned short crc = crc16((unsigned char*)data, *len - 2);
-
- if (crc != ((unsigned short) data[*len - 2] << 8 | (unsigned short) data[*len - 1])) {
- res = -3;
- } else {
- unsigned char cnt = data[*len - 3];
- if (cnt == counter) {
- res = -4;
- }
-
- counter = cnt;
- }
- }
-
- *len -= 3;
-
- return res;
-}
-
int rfhelp_rf_status(void) {
chMtxLock(&rf_mutex);
int s = rf_status();
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/nrf/rfhelp.h Raptor2 Sources/bldc-firmware_2_80/nrf/rfhelp.h
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/nrf/rfhelp.h 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/nrf/rfhelp.h 2016-08-04 23:47:30.000000000 +0100
@@ -19,16 +19,16 @@
#define RFHELP_H_
#include <stdbool.h>
+#include "datatypes.h"
// Functions
void rfhelp_init(void);
+void rfhelp_update_conf(nrf_config *conf);
void rfhelp_restart(void);
int rfhelp_send_data(char *data, int len, bool ack);
int rfhelp_send_data_crc(char *data, int len);
-int rfhelp_send_data_crc_noack(char *data, int len, int resends);
int rfhelp_read_rx_data(char *data, int *len, int *pipe);
int rfhelp_read_rx_data_crc(char *data, int *len, int *pipe);
-int rfhelp_read_rx_data_crc_noack(char *data, int *len, int *pipe);
int rfhelp_rf_status(void);
void rfhelp_set_tx_addr(const char *addr, int addr_len);
void rfhelp_set_rx_addr(int pipe, const char *addr, int addr_len);
Only in Raptor2 Sources/bldc-firmware_2_80: oh5la4w3.ag1
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/packet.h Raptor2 Sources/bldc-firmware_2_80/packet.h
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/packet.h 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/packet.h 2017-04-13 18:06:50.000000000 +0100
@@ -28,7 +28,7 @@
#include <stdint.h>
// Settings
-#define PACKET_RX_TIMEOUT 2
+#define PACKET_RX_TIMEOUT 10
#define PACKET_HANDLERS 2
#define PACKET_MAX_PL_LEN 1024
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/servo_dec.c Raptor2 Sources/bldc-firmware_2_80/servo_dec.c
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/servo_dec.c 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/servo_dec.c 2016-10-24 21:35:06.000000000 +0100
@@ -39,6 +39,7 @@
static volatile systime_t last_update_time;
static volatile float servo_pos[SERVO_NUM];
static volatile float pulse_start = 1.0;
+static volatile float pulse_center = 1.5;
static volatile float pulse_end = 2.0;
static volatile float last_len_received[SERVO_NUM];
static volatile bool use_median_filter = false;
@@ -68,8 +69,16 @@
}
if (len >= 0.0) {
+ float len_to_center = len + pulse_start - pulse_center;
+
if (use_median_filter) {
- float c = (len * 2.0 - len_set) / len_set;
+ float c;
+ if (len_to_center >= 0.0) {
+ c = (1.0 / (pulse_end - pulse_center)) * len_to_center;
+ } else {
+ c = (1.0 / (pulse_center - pulse_start)) * len_to_center;
+ }
+ //float c = (len * 2.0 - len_set) / len_set;
static float c1 = 0.5;
static float c2 = 0.5;
float med = utils_middle_of_3(c, c1, c2);
@@ -79,7 +88,12 @@
servo_pos[0] = med;
} else {
- servo_pos[0] = (len * 2.0 - len_set) / len_set;
+ if (len_to_center >= 0.0) {
+ servo_pos[0] = (1 / (pulse_end - pulse_center)) * len_to_center;
+ } else {
+ servo_pos[0] = (1 / (pulse_center - pulse_start)) * len_to_center;
+ }
+ //servo_pos[0] = (len * 2.0 - len_set) / len_set;
}
last_update_time = chVTGetSystemTime();
@@ -135,8 +149,9 @@
* @param end
* he amount of milliseconds the pulse ends at (default is 2.0)
*/
-void servodec_set_pulse_options(float start, float end, bool median_filter) {
+void servodec_set_pulse_options(float start, float center, float end, bool median_filter) {
pulse_start = start;
+ pulse_center = center;
pulse_end = end;
use_median_filter = median_filter;
}
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/servo_dec.h Raptor2 Sources/bldc-firmware_2_80/servo_dec.h
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/servo_dec.h 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/servo_dec.h 2016-10-24 16:46:32.000000000 +0100
@@ -30,7 +30,7 @@
// Functions
void servodec_init(void (*d_func)(void));
-void servodec_set_pulse_options(float start, float end, bool median_filter);
+void servodec_set_pulse_options(float start, float center, float end, bool median_filter);
float servodec_get_servo(int servo_num);
uint32_t servodec_get_time_since_update(void);
float servodec_get_last_pulse_len(int servo_num);
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/terminal.c Raptor2 Sources/bldc-firmware_2_80/terminal.c
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/terminal.c 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/terminal.c 2017-04-13 18:07:08.000000000 +0100
@@ -29,11 +29,12 @@
#include "mcpwm_foc.h"
#include "mc_interface.h"
#include "commands.h"
-#include "main.h"
+#include "app.h"
#include "hw.h"
#include "comm_can.h"
#include "utils.h"
#include "timeout.h"
+#include "encoder.h"
#include <string.h>
#include <stdio.h>
@@ -73,7 +74,7 @@
} else if (strcmp(argv[0], "last_adc_duration") == 0) {
commands_printf("Latest ADC duration: %.4f ms", (double)(mcpwm_get_last_adc_isr_duration() * 1000.0));
commands_printf("Latest injected ADC duration: %.4f ms", (double)(mc_interface_get_last_inj_adc_isr_duration() * 1000.0));
- commands_printf("Latest main ADC duration: %.4f ms\n", (double)(main_get_last_adc_isr_duration() * 1000.0));
+ commands_printf("Latest sample ADC duration: %.4f ms\n", (double)(mc_interface_get_last_sample_adc_isr_duration() * 1000.0));
} else if (strcmp(argv[0], "kv") == 0) {
commands_printf("Calculated KV: %.2f rpm/volt\n", (double)mcpwm_get_kv_filtered());
} else if (strcmp(argv[0], "mem") == 0) {
@@ -98,6 +99,27 @@
commands_printf("");
} else if (strcmp(argv[0], "fault") == 0) {
commands_printf("%s\n", mc_interface_fault_to_string(mc_interface_get_fault()));
+ } else if (strcmp(argv[0], "faulta") == 0) {
+ if (fault_vec_write == 0) {
+ commands_printf("No faults");
+ } else {
+ commands_printf("Fault: %s\nID: %i\nCurrent: %.1f\nCurrent filtered: %.1f\nVoltage: %.2f\nDuty: %.2f\nRPM: %.1f\nTacho: %d\nCycles running: %d\nTIM duty: %d\nTIM val samp: %d\nTIM current samp: %d\nTIM top: %d\nComm step: %d\nTemperature: %.2f",
+ mc_interface_fault_to_string(fault_vec[fault_vec_write - 1].fault),
+ app_get_configuration()->controller_id,
+ (double)fault_vec[fault_vec_write - 1].current,
+ (double)fault_vec[fault_vec_write - 1].current_filtered,
+ (double)fault_vec[fault_vec_write - 1].voltage,
+ (double)fault_vec[fault_vec_write - 1].duty,
+ (double)fault_vec[fault_vec_write - 1].rpm,
+ fault_vec[fault_vec_write - 1].tacho,
+ fault_vec[fault_vec_write - 1].cycles_running,
+ (int)((float)fault_vec[fault_vec_write - 1].tim_top * fault_vec[fault_vec_write - 1].duty),
+ fault_vec[fault_vec_write - 1].tim_val_samp,
+ fault_vec[fault_vec_write - 1].tim_current_samp,
+ fault_vec[fault_vec_write - 1].tim_top,
+ fault_vec[fault_vec_write - 1].comm_step,
+ (double)fault_vec[fault_vec_write - 1].temperature);
+ }
} else if (strcmp(argv[0], "faults") == 0) {
if (fault_vec_write == 0) {
commands_printf("No faults registered since startup\n");
@@ -211,36 +233,66 @@
commands_printf("ID : %i", msg->id);
commands_printf("RX Time : %i", msg->rx_time);
commands_printf("Age (milliseconds) : %.2f", (double)(UTILS_AGE_S(msg->rx_time) * 1000.0));
- commands_printf("RPM : %.2f", (double)msg->rpm);
+ commands_printf("RPM : %.2f", (double)(msg->rpm));
commands_printf("Current : %.2f", (double)msg->current);
- commands_printf("Duty : %.2f\n", (double)msg->duty);
+ commands_printf("Duty : %i", msg->duty);
+ commands_printf("Cruise Control : %i\n", msg->cruise_control_status);
+ }
+ }
+ } else if (strcmp(argv[0], "can_member") == 0) {
+ char controllerIds[100] = "CAN:\n";
+ for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+ can_status_msg *msg = comm_can_get_status_msg_index(i);
+
+ if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < 1.0) {
+ char idArray[5];
+ sprintf(idArray, "%i\n", msg->id);
+ strcat(controllerIds, idArray);
}
}
- } else if (strcmp(argv[0], "foc_encoder_detect") == 0) {
+ commands_printf(controllerIds);
+ } else if (strcmp(argv[0], "can_erpms") == 0) {
+ char controllerIds[100] = "CAN ERPMs:\n";
+
+ char rpmArrayMaster[15];
+ sprintf(rpmArrayMaster, "M %6.2f\n", (double)mc_interface_get_rpm());
+ strcat(controllerIds, rpmArrayMaster);
+
+ for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+ can_status_msg *msg = comm_can_get_status_msg_index(i);
+
+ if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < 1.0) {
+ char rpmArray[15];
+ sprintf(rpmArray, "%i %6.2f\n", msg->id, (double)(msg->rpm));
+ strcat(controllerIds, rpmArray);
+ }
+ }
+ commands_printf(controllerIds);
+ }else if (strcmp(argv[0], "foc_encoder_detect") == 0) {
if (argc == 2) {
float current = -1.0;
sscanf(argv[1], "%f", &current);
if (current > 0.0 && current <= mcconf.l_current_max) {
-#if ENCODER_ENABLE
- mc_motor_type type_old = mcconf.motor_type;
- mcconf.motor_type = MOTOR_TYPE_FOC;
- mc_interface_set_configuration(&mcconf);
-
- float offset = 0.0;
- float ratio = 0.0;
- bool inverted = false;
- mcpwm_foc_encoder_detect(current, true, &offset, &ratio, &inverted);
-
- mcconf.motor_type = type_old;
- mc_interface_set_configuration(&mcconf);
-
- commands_printf("Offset : %.2f", (double)offset);
- commands_printf("Ratio : %.2f", (double)ratio);
- commands_printf("Inverted : %s\n", inverted ? "true" : "false");
-#else
- commands_printf("Encoder not enabled.\n");
-#endif
+ if (encoder_is_configured()) {
+ mc_motor_type type_old = mcconf.motor_type;
+ mcconf.motor_type = MOTOR_TYPE_FOC;
+ mc_interface_set_configuration(&mcconf);
+
+ float offset = 0.0;
+ float ratio = 0.0;
+ bool inverted = false;
+ mcpwm_foc_encoder_detect(current, true, &offset, &ratio, &inverted);
+
+ mcconf.motor_type = type_old;
+ mc_interface_set_configuration(&mcconf);
+
+ commands_printf("Offset : %.2f", (double)offset);
+ commands_printf("Ratio : %.2f", (double)ratio);
+ commands_printf("Inverted : %s\n", inverted ? "true" : "false");
+ } else {
+ commands_printf("Encoder not enabled.\n");
+ }
} else {
commands_printf("Invalid argument(s).\n");
}
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/utils.c Raptor2 Sources/bldc-firmware_2_80/utils.c
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/utils.c 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/utils.c 2017-04-13 18:07:16.000000000 +0100
@@ -133,22 +133,28 @@
* The difference between the angles
*/
float utils_angle_difference(float angle1, float angle2) {
- utils_norm_angle(&angle1);
- utils_norm_angle(&angle2);
+// utils_norm_angle(&angle1);
+// utils_norm_angle(&angle2);
+//
+// if (fabsf(angle1 - angle2) > 180.0) {
+// if (angle1 < angle2) {
+// angle1 += 360.0;
+// } else {
+// angle2 += 360.0;
+// }
+// }
+//
+// return angle1 - angle2;
- if (fabsf(angle1 - angle2) > 180.0) {
- if (angle1 < angle2) {
- angle1 += 360.0;
- } else {
- angle2 += 360.0;
- }
- }
-
- return angle1 - angle2;
+ // Faster in most cases
+ float difference = angle1 - angle2;
+ while (difference < -180.0) difference += 2.0 * 180.0;
+ while (difference > 180.0) difference -= 2.0 * 180.0;
+ return difference;
}
/**
- * Get the difference between two angles. Will always be between -180 and +180 degrees.
+ * Get the difference between two angles. Will always be between -pi and +pi radians.
* @param angle1
* The first angle in radians
* @param angle2
@@ -221,6 +227,73 @@
}
/**
+ * Get the smallest value of two values
+ *
+ * @param a
+ * First value
+ *
+ * @param b
+ * Second value
+ *
+ * @return
+ * The smallest value
+ */
+float utils_smallest_of_2(float a, float b) {
+ if (b < a) {
+ return b;
+ }
+ return a;
+}
+
+/**
+ * Get the smallest value of three values
+ *
+ * @param a
+ * First value
+ *
+ * @param b
+ * Second value
+ *
+ * @param c
+ * Third value
+ *
+ * @return
+ * The smallest value
+ */
+float utils_smallest_of_3(float a, float b, float c) {
+ if (b < a) {
+ if (c < b) {
+ return c;
+ }
+ return b;
+ } else {
+ if (c < a) {
+ return c;
+ }
+ return a;
+ }
+}
+
+/**
+ * Get the smallest value of two values
+ *
+ * @param a
+ * First value
+ *
+ * @param b
+ * Second value
+ *
+ * @return
+ * The smallest value
+ */
+float utils_highest_of_2(float a, float b) {
+ if (b > a) {
+ return b;
+ }
+ return a;
+}
+
+/**
* Get the middle value of three values
*
* @param a
diff -ru bldc-90986c31defe50830b4795a388ee5b2687dc2516/utils.h Raptor2 Sources/bldc-firmware_2_80/utils.h
--- bldc-90986c31defe50830b4795a388ee5b2687dc2516/utils.h 2016-01-24 12:44:51.000000000 +0000
+++ Raptor2 Sources/bldc-firmware_2_80/utils.h 2017-04-13 18:07:22.000000000 +0100
@@ -39,6 +39,9 @@
float utils_angle_difference_rad(float angle1, float angle2);
float utils_avg_angles_rad_fast(float *angles, float *weights, int angles_num);
float utils_middle_of_3(float a, float b, float c);
+float utils_smallest_of_2(float a, float b);
+float utils_smallest_of_3(float a, float b, float c);
+float utils_highest_of_2(float a, float b);
int utils_middle_of_3_int(int a, int b, int c);
float utils_fast_inv_sqrt(float x);
float utils_fast_atan2(float y, float x);
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment