2014-10-18 16:06:20 -07:00
|
|
|
/*
|
2016-11-04 07:18:34 -07:00
|
|
|
Copyright 2016 Benjamin Vedder benjamin@vedder.se
|
2014-10-18 16:06:20 -07:00
|
|
|
|
2016-11-04 07:18:34 -07:00
|
|
|
This file is part of the VESC firmware.
|
|
|
|
|
|
|
|
The VESC firmware is free software: you can redistribute it and/or modify
|
2014-10-18 16:06:20 -07:00
|
|
|
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.
|
|
|
|
|
2016-11-04 07:18:34 -07:00
|
|
|
The VESC firmware is distributed in the hope that it will be useful,
|
2014-10-18 16:06:20 -07:00
|
|
|
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/>.
|
|
|
|
*/
|
|
|
|
|
2014-10-18 16:00:53 -07:00
|
|
|
#include "app.h"
|
|
|
|
#include "ch.h"
|
|
|
|
#include "hal.h"
|
|
|
|
#include "hw.h"
|
2015-12-08 12:01:23 -08:00
|
|
|
#include "mc_interface.h"
|
2014-10-18 16:00:53 -07:00
|
|
|
#include "commands.h"
|
|
|
|
#include "utils.h"
|
|
|
|
#include "timeout.h"
|
|
|
|
#include <string.h>
|
|
|
|
#include <math.h>
|
2014-12-08 15:11:29 -08:00
|
|
|
#include "led_external.h"
|
2014-12-20 03:52:38 -08:00
|
|
|
#include "datatypes.h"
|
2015-02-19 12:20:07 -08:00
|
|
|
#include "comm_can.h"
|
2017-09-04 12:12:43 -07:00
|
|
|
#include "terminal.h"
|
2014-10-18 16:00:53 -07:00
|
|
|
|
2014-12-20 03:52:38 -08:00
|
|
|
// Settings
|
2019-04-18 14:43:07 -07:00
|
|
|
#define OUTPUT_ITERATION_TIME_MS 5
|
2015-02-19 12:20:07 -08:00
|
|
|
#define MAX_CAN_AGE 0.1
|
2015-05-04 14:25:43 -07:00
|
|
|
#define RPM_FILTER_SAMPLES 8
|
2016-11-04 07:18:34 -07:00
|
|
|
#define LOCAL_TIMEOUT 2000
|
2014-10-18 16:00:53 -07:00
|
|
|
|
|
|
|
// Threads
|
2015-10-08 14:09:39 -07:00
|
|
|
static THD_FUNCTION(chuk_thread, arg);
|
|
|
|
static THD_WORKING_AREA(chuk_thread_wa, 1024);
|
|
|
|
static THD_FUNCTION(output_thread, arg);
|
|
|
|
static THD_WORKING_AREA(output_thread_wa, 1024);
|
2014-10-18 16:00:53 -07:00
|
|
|
|
|
|
|
// Private variables
|
2016-11-04 07:18:34 -07:00
|
|
|
static volatile bool stop_now = true;
|
2014-10-18 16:00:53 -07:00
|
|
|
static volatile bool is_running = false;
|
2015-02-19 12:20:07 -08:00
|
|
|
static volatile chuck_data chuck_d;
|
2014-10-19 10:31:06 -07:00
|
|
|
static volatile int chuck_error = 0;
|
2015-02-19 12:20:07 -08:00
|
|
|
static volatile chuk_config config;
|
2015-04-11 01:07:36 -07:00
|
|
|
static volatile bool output_running = false;
|
2016-11-04 07:18:34 -07:00
|
|
|
static volatile systime_t last_update_time;
|
2015-02-19 12:20:07 -08:00
|
|
|
|
2017-09-04 12:12:43 -07:00
|
|
|
// Private functions
|
|
|
|
static void terminal_cmd_nunchuk_status(int argc, const char **argv);
|
|
|
|
|
2015-02-19 12:20:07 -08:00
|
|
|
void app_nunchuk_configure(chuk_config *conf) {
|
|
|
|
config = *conf;
|
2017-09-04 12:12:43 -07:00
|
|
|
|
|
|
|
terminal_register_command_callback(
|
|
|
|
"nunchuk_status",
|
|
|
|
"Print the status of the nunchuk app",
|
|
|
|
0,
|
|
|
|
terminal_cmd_nunchuk_status);
|
2014-10-18 16:00:53 -07:00
|
|
|
}
|
|
|
|
|
|
|
|
void app_nunchuk_start(void) {
|
2015-02-19 12:20:07 -08:00
|
|
|
chuck_d.js_y = 128;
|
2016-11-04 07:18:34 -07:00
|
|
|
stop_now = false;
|
|
|
|
hw_start_i2c();
|
2014-10-18 16:00:53 -07:00
|
|
|
chThdCreateStatic(chuk_thread_wa, sizeof(chuk_thread_wa), NORMALPRIO, chuk_thread, NULL);
|
|
|
|
}
|
|
|
|
|
2016-11-04 07:18:34 -07:00
|
|
|
void app_nunchuk_stop(void) {
|
|
|
|
stop_now = true;
|
|
|
|
|
|
|
|
if (is_running) {
|
|
|
|
hw_stop_i2c();
|
|
|
|
}
|
|
|
|
|
|
|
|
while (is_running) {
|
|
|
|
chThdSleepMilliseconds(1);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2014-10-18 16:00:53 -07:00
|
|
|
float app_nunchuk_get_decoded_chuk(void) {
|
2015-02-19 12:20:07 -08:00
|
|
|
return ((float)chuck_d.js_y - 128.0) / 128.0;
|
2014-10-18 16:00:53 -07:00
|
|
|
}
|
|
|
|
|
2015-04-11 01:07:36 -07:00
|
|
|
void app_nunchuk_update_output(chuck_data *data) {
|
|
|
|
if (!output_running) {
|
2016-11-04 07:18:34 -07:00
|
|
|
last_update_time = 0;
|
2015-04-11 01:07:36 -07:00
|
|
|
output_running = true;
|
|
|
|
chuck_d.js_y = 128;
|
|
|
|
chThdCreateStatic(output_thread_wa, sizeof(output_thread_wa), NORMALPRIO, output_thread, NULL);
|
|
|
|
}
|
|
|
|
|
|
|
|
chuck_d = *data;
|
2016-11-04 07:18:34 -07:00
|
|
|
last_update_time = chVTGetSystemTime();
|
2015-04-11 01:07:36 -07:00
|
|
|
timeout_reset();
|
|
|
|
}
|
|
|
|
|
2015-10-08 14:09:39 -07:00
|
|
|
static THD_FUNCTION(chuk_thread, arg) {
|
2014-10-18 16:00:53 -07:00
|
|
|
(void)arg;
|
|
|
|
|
2015-04-11 01:07:36 -07:00
|
|
|
chRegSetThreadName("Nunchuk i2c");
|
2014-10-18 16:00:53 -07:00
|
|
|
is_running = true;
|
|
|
|
|
|
|
|
uint8_t rxbuf[10];
|
|
|
|
uint8_t txbuf[10];
|
2015-10-08 14:09:39 -07:00
|
|
|
msg_t status = MSG_OK;
|
2014-10-18 16:00:53 -07:00
|
|
|
systime_t tmo = MS2ST(5);
|
|
|
|
i2caddr_t chuck_addr = 0x52;
|
2015-04-11 01:07:36 -07:00
|
|
|
chuck_data chuck_d_tmp;
|
2014-10-18 16:00:53 -07:00
|
|
|
|
2014-10-19 10:31:06 -07:00
|
|
|
hw_start_i2c();
|
2014-10-18 16:00:53 -07:00
|
|
|
chThdSleepMilliseconds(10);
|
|
|
|
|
|
|
|
for(;;) {
|
2014-11-23 11:29:36 -08:00
|
|
|
bool is_ok = true;
|
|
|
|
|
2016-11-04 07:18:34 -07:00
|
|
|
if (stop_now) {
|
|
|
|
is_running = false;
|
|
|
|
chuck_error = 0;
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2014-10-18 16:00:53 -07:00
|
|
|
txbuf[0] = 0xF0;
|
|
|
|
txbuf[1] = 0x55;
|
|
|
|
i2cAcquireBus(&HW_I2C_DEV);
|
|
|
|
status = i2cMasterTransmitTimeout(&HW_I2C_DEV, chuck_addr, txbuf, 2, rxbuf, 0, tmo);
|
|
|
|
i2cReleaseBus(&HW_I2C_DEV);
|
2015-10-08 14:09:39 -07:00
|
|
|
is_ok = status == MSG_OK;
|
2014-11-23 11:29:36 -08:00
|
|
|
|
|
|
|
if (is_ok) {
|
|
|
|
txbuf[0] = 0xFB;
|
|
|
|
txbuf[1] = 0x00;
|
|
|
|
i2cAcquireBus(&HW_I2C_DEV);
|
|
|
|
status = i2cMasterTransmitTimeout(&HW_I2C_DEV, chuck_addr, txbuf, 2, rxbuf, 0, tmo);
|
|
|
|
i2cReleaseBus(&HW_I2C_DEV);
|
2015-10-08 14:09:39 -07:00
|
|
|
is_ok = status == MSG_OK;
|
2014-11-23 11:29:36 -08:00
|
|
|
}
|
2014-10-18 16:00:53 -07:00
|
|
|
|
2014-11-23 11:29:36 -08:00
|
|
|
if (is_ok) {
|
|
|
|
txbuf[0] = 0x00;
|
|
|
|
i2cAcquireBus(&HW_I2C_DEV);
|
|
|
|
status = i2cMasterTransmitTimeout(&HW_I2C_DEV, chuck_addr, txbuf, 1, rxbuf, 0, tmo);
|
|
|
|
i2cReleaseBus(&HW_I2C_DEV);
|
2015-10-08 14:09:39 -07:00
|
|
|
is_ok = status == MSG_OK;
|
2014-11-23 11:29:36 -08:00
|
|
|
}
|
2014-10-18 16:00:53 -07:00
|
|
|
|
2014-11-23 11:29:36 -08:00
|
|
|
if (is_ok) {
|
|
|
|
chThdSleepMilliseconds(3);
|
2014-10-18 16:00:53 -07:00
|
|
|
|
2014-11-23 11:29:36 -08:00
|
|
|
i2cAcquireBus(&HW_I2C_DEV);
|
|
|
|
status = i2cMasterReceiveTimeout(&HW_I2C_DEV, chuck_addr, rxbuf, 6, tmo);
|
|
|
|
i2cReleaseBus(&HW_I2C_DEV);
|
2015-10-08 14:09:39 -07:00
|
|
|
is_ok = status == MSG_OK;
|
2014-11-23 11:29:36 -08:00
|
|
|
}
|
2014-10-18 16:00:53 -07:00
|
|
|
|
2014-11-23 11:29:36 -08:00
|
|
|
if (is_ok) {
|
2016-01-24 04:44:51 -08:00
|
|
|
static uint8_t last_buffer[6];
|
2014-10-18 16:00:53 -07:00
|
|
|
int same = 1;
|
2014-10-19 10:31:06 -07:00
|
|
|
|
2014-10-18 16:00:53 -07:00
|
|
|
for (int i = 0;i < 6;i++) {
|
|
|
|
if (last_buffer[i] != rxbuf[i]) {
|
|
|
|
same = 0;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
memcpy(last_buffer, rxbuf, 6);
|
|
|
|
|
2014-10-19 10:31:06 -07:00
|
|
|
if (!same) {
|
|
|
|
chuck_error = 0;
|
2015-04-11 01:07:36 -07:00
|
|
|
chuck_d_tmp.js_x = rxbuf[0];
|
|
|
|
chuck_d_tmp.js_y = rxbuf[1];
|
|
|
|
chuck_d_tmp.acc_x = (rxbuf[2] << 2) | ((rxbuf[5] >> 2) & 3);
|
|
|
|
chuck_d_tmp.acc_y = (rxbuf[3] << 2) | ((rxbuf[5] >> 4) & 3);
|
|
|
|
chuck_d_tmp.acc_z = (rxbuf[4] << 2) | ((rxbuf[5] >> 6) & 3);
|
|
|
|
chuck_d_tmp.bt_z = !((rxbuf[5] >> 0) & 1);
|
|
|
|
chuck_d_tmp.bt_c = !((rxbuf[5] >> 1) & 1);
|
2019-02-18 10:30:19 -08:00
|
|
|
chuck_d_tmp.rev_has_state = false;
|
|
|
|
chuck_d_tmp.is_rev = false;
|
2015-04-11 01:07:36 -07:00
|
|
|
|
|
|
|
app_nunchuk_update_output(&chuck_d_tmp);
|
2014-10-19 10:31:06 -07:00
|
|
|
}
|
|
|
|
|
|
|
|
if (timeout_has_timeout()) {
|
2014-10-18 16:00:53 -07:00
|
|
|
chuck_error = 1;
|
|
|
|
}
|
|
|
|
} else {
|
|
|
|
chuck_error = 2;
|
2014-11-23 11:29:36 -08:00
|
|
|
hw_try_restore_i2c();
|
2015-01-04 20:48:29 -08:00
|
|
|
chThdSleepMilliseconds(100);
|
2014-10-18 16:00:53 -07:00
|
|
|
}
|
|
|
|
|
|
|
|
chThdSleepMilliseconds(10);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2015-10-08 14:09:39 -07:00
|
|
|
static THD_FUNCTION(output_thread, arg) {
|
2014-10-18 16:00:53 -07:00
|
|
|
(void)arg;
|
|
|
|
|
|
|
|
chRegSetThreadName("Nunchuk output");
|
|
|
|
|
2019-04-18 11:00:26 -07:00
|
|
|
bool was_pid = false;
|
|
|
|
|
2014-10-18 16:00:53 -07:00
|
|
|
for(;;) {
|
2014-12-20 03:52:38 -08:00
|
|
|
chThdSleepMilliseconds(OUTPUT_ITERATION_TIME_MS);
|
2014-10-18 16:00:53 -07:00
|
|
|
|
2019-04-18 14:43:07 -07:00
|
|
|
static float rpm_filtered = 0.0;
|
|
|
|
UTILS_LP_FAST(rpm_filtered, mc_interface_get_rpm(), 0.5);
|
|
|
|
|
2015-02-19 12:20:07 -08:00
|
|
|
if (timeout_has_timeout() || chuck_error != 0 || config.ctrl_type == CHUK_CTRL_TYPE_NONE) {
|
2019-04-18 11:00:26 -07:00
|
|
|
was_pid = false;
|
2014-10-18 16:00:53 -07:00
|
|
|
continue;
|
|
|
|
}
|
|
|
|
|
2016-11-04 07:18:34 -07:00
|
|
|
// Local timeout to prevent this thread from causing problems after not
|
|
|
|
// being used for a while.
|
|
|
|
if (chVTTimeElapsedSinceX(last_update_time) > MS2ST(LOCAL_TIMEOUT)) {
|
2019-04-18 11:00:26 -07:00
|
|
|
was_pid = false;
|
2016-11-04 07:18:34 -07:00
|
|
|
continue;
|
|
|
|
}
|
|
|
|
|
2019-02-18 10:30:19 -08:00
|
|
|
if (app_is_output_disabled()) {
|
2019-04-18 11:00:26 -07:00
|
|
|
was_pid = false;
|
2019-02-18 10:30:19 -08:00
|
|
|
continue;
|
|
|
|
}
|
|
|
|
|
2017-09-04 12:12:43 -07:00
|
|
|
const volatile mc_configuration *mcconf = mc_interface_get_configuration();
|
2014-12-20 03:52:38 -08:00
|
|
|
static bool is_reverse = false;
|
|
|
|
static bool was_z = false;
|
2015-12-08 12:01:23 -08:00
|
|
|
const float current_now = mc_interface_get_tot_current_directional_filtered();
|
2015-08-03 14:43:34 -07:00
|
|
|
static float prev_current = 0.0;
|
2019-02-18 10:30:19 -08:00
|
|
|
const float max_current_diff = mcconf->l_current_max * mcconf->l_current_max_scale * 0.2;
|
2014-12-20 03:52:38 -08:00
|
|
|
|
2015-03-07 05:56:25 -08:00
|
|
|
if (chuck_d.bt_c && chuck_d.bt_z) {
|
|
|
|
led_external_set_state(LED_EXT_BATT);
|
2019-04-18 11:00:26 -07:00
|
|
|
was_pid = false;
|
2015-03-07 05:56:25 -08:00
|
|
|
continue;
|
|
|
|
}
|
|
|
|
|
2019-02-18 10:30:19 -08:00
|
|
|
if (fabsf(current_now) < max_current_diff) {
|
|
|
|
if (chuck_d.rev_has_state) {
|
|
|
|
is_reverse = chuck_d.is_rev;
|
|
|
|
} else if (chuck_d.bt_z && !was_z) {
|
|
|
|
if (is_reverse) {
|
|
|
|
is_reverse = false;
|
|
|
|
} else {
|
|
|
|
is_reverse = true;
|
|
|
|
}
|
2014-12-20 03:52:38 -08:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2019-02-18 10:30:19 -08:00
|
|
|
if (config.ctrl_type == CHUK_CTRL_TYPE_CURRENT_NOREV) {
|
|
|
|
is_reverse = false;
|
|
|
|
}
|
|
|
|
|
2015-02-19 12:20:07 -08:00
|
|
|
was_z = chuck_d.bt_z;
|
2014-12-20 03:52:38 -08:00
|
|
|
|
|
|
|
led_external_set_reversed(is_reverse);
|
|
|
|
|
2014-12-12 16:31:54 -08:00
|
|
|
float out_val = app_nunchuk_get_decoded_chuk();
|
2015-02-19 12:20:07 -08:00
|
|
|
utils_deadband(&out_val, config.hyst, 1.0);
|
2017-09-06 12:13:28 -07:00
|
|
|
out_val = utils_throttle_curve(out_val, config.throttle_exp, config.throttle_exp_brake, config.throttle_exp_mode);
|
2014-12-12 16:31:54 -08:00
|
|
|
|
2014-12-08 15:11:29 -08:00
|
|
|
// LEDs
|
2015-02-19 12:20:07 -08:00
|
|
|
float x_axis = ((float)chuck_d.js_x - 128.0) / 128.0;
|
2014-12-12 16:31:54 -08:00
|
|
|
if (out_val < -0.001) {
|
2014-12-08 15:11:29 -08:00
|
|
|
if (x_axis < -0.4) {
|
|
|
|
led_external_set_state(LED_EXT_BRAKE_TURN_LEFT);
|
|
|
|
} else if (x_axis > 0.4) {
|
|
|
|
led_external_set_state(LED_EXT_BRAKE_TURN_RIGHT);
|
|
|
|
} else {
|
|
|
|
led_external_set_state(LED_EXT_BRAKE);
|
|
|
|
}
|
|
|
|
} else {
|
|
|
|
if (x_axis < -0.4) {
|
|
|
|
led_external_set_state(LED_EXT_TURN_LEFT);
|
|
|
|
} else if (x_axis > 0.4) {
|
|
|
|
led_external_set_state(LED_EXT_TURN_RIGHT);
|
|
|
|
} else {
|
|
|
|
led_external_set_state(LED_EXT_NORMAL);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2015-09-17 14:24:55 -07:00
|
|
|
if (chuck_d.bt_c) {
|
2014-10-18 16:00:53 -07:00
|
|
|
static float pid_rpm = 0.0;
|
|
|
|
|
|
|
|
if (!was_pid) {
|
2015-05-04 14:25:43 -07:00
|
|
|
pid_rpm = rpm_filtered;
|
2014-10-18 16:00:53 -07:00
|
|
|
|
2015-09-17 14:24:55 -07:00
|
|
|
if ((is_reverse && pid_rpm > 0.0) || (!is_reverse && pid_rpm < 0.0)) {
|
|
|
|
if (fabsf(pid_rpm) > mcconf->s_pid_min_erpm) {
|
|
|
|
// Abort if the speed is too high in the opposite direction
|
|
|
|
continue;
|
|
|
|
} else {
|
|
|
|
pid_rpm = 0.0;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
was_pid = true;
|
|
|
|
} else {
|
|
|
|
if (is_reverse) {
|
|
|
|
if (pid_rpm > 0.0) {
|
|
|
|
pid_rpm = 0.0;
|
|
|
|
}
|
|
|
|
|
2019-04-18 14:43:07 -07:00
|
|
|
pid_rpm -= (out_val * config.stick_erpm_per_s_in_cc) * ((float)OUTPUT_ITERATION_TIME_MS / 1000.0);
|
2016-11-04 07:18:34 -07:00
|
|
|
|
|
|
|
if (pid_rpm < (rpm_filtered - config.stick_erpm_per_s_in_cc)) {
|
|
|
|
pid_rpm = rpm_filtered - config.stick_erpm_per_s_in_cc;
|
|
|
|
}
|
2015-09-17 14:24:55 -07:00
|
|
|
} else {
|
|
|
|
if (pid_rpm < 0.0) {
|
|
|
|
pid_rpm = 0.0;
|
|
|
|
}
|
|
|
|
|
2019-04-18 14:43:07 -07:00
|
|
|
pid_rpm += (out_val * config.stick_erpm_per_s_in_cc) * ((float)OUTPUT_ITERATION_TIME_MS / 1000.0);
|
2016-11-04 07:18:34 -07:00
|
|
|
|
|
|
|
if (pid_rpm > (rpm_filtered + config.stick_erpm_per_s_in_cc)) {
|
|
|
|
pid_rpm = rpm_filtered + config.stick_erpm_per_s_in_cc;
|
|
|
|
}
|
2015-09-17 14:24:55 -07:00
|
|
|
}
|
2014-10-18 16:00:53 -07:00
|
|
|
}
|
|
|
|
|
2015-12-08 12:01:23 -08:00
|
|
|
mc_interface_set_pid_speed(pid_rpm);
|
2015-09-17 14:24:55 -07:00
|
|
|
|
2017-09-04 12:12:43 -07:00
|
|
|
// Send the same current to the other controllers
|
2015-02-19 12:20:07 -08:00
|
|
|
if (config.multi_esc) {
|
2017-09-04 12:12:43 -07:00
|
|
|
float current = mc_interface_get_tot_current_directional_filtered();
|
2015-02-19 12:20:07 -08:00
|
|
|
|
|
|
|
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) {
|
2017-09-04 12:12:43 -07:00
|
|
|
comm_can_set_current(msg->id, current);
|
2015-02-19 12:20:07 -08:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2015-08-03 14:43:34 -07:00
|
|
|
// Set the previous ramping current to not get a spike when releasing
|
|
|
|
// PID control and to get a smooth transition.
|
|
|
|
prev_current = current_now;
|
|
|
|
|
2014-10-18 16:00:53 -07:00
|
|
|
continue;
|
|
|
|
}
|
|
|
|
|
|
|
|
was_pid = false;
|
|
|
|
|
|
|
|
float current = 0;
|
|
|
|
|
|
|
|
if (out_val >= 0.0) {
|
2017-09-04 12:12:43 -07:00
|
|
|
current = out_val * mcconf->lo_current_motor_max_now;
|
2014-10-18 16:00:53 -07:00
|
|
|
} else {
|
2017-09-04 12:12:43 -07:00
|
|
|
current = out_val * fabsf(mcconf->lo_current_motor_min_now);
|
2014-10-18 16:00:53 -07:00
|
|
|
}
|
|
|
|
|
2015-02-19 12:20:07 -08:00
|
|
|
// Find lowest RPM and highest current
|
2015-12-08 12:01:23 -08:00
|
|
|
float rpm_local = mc_interface_get_rpm();
|
2015-02-19 12:20:07 -08:00
|
|
|
if (is_reverse) {
|
|
|
|
rpm_local = -rpm_local;
|
|
|
|
}
|
|
|
|
|
|
|
|
float rpm_lowest = rpm_local;
|
|
|
|
float current_highest_abs = current_now;
|
|
|
|
|
|
|
|
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) {
|
|
|
|
float rpm_tmp = msg->rpm;
|
|
|
|
if (is_reverse) {
|
|
|
|
rpm_tmp = -rpm_tmp;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (rpm_tmp < rpm_lowest) {
|
|
|
|
rpm_lowest = rpm_tmp;
|
|
|
|
}
|
|
|
|
|
|
|
|
// Make the current directional
|
|
|
|
float msg_current = msg->current;
|
|
|
|
if (msg->duty < 0.0) {
|
|
|
|
msg_current = -msg_current;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (fabsf(msg_current) > fabsf(current_highest_abs)) {
|
|
|
|
current_highest_abs = msg_current;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2014-12-20 03:52:38 -08:00
|
|
|
// Apply ramping
|
2019-02-18 10:30:19 -08:00
|
|
|
const float current_range = mcconf->l_current_max * mcconf->l_current_max_scale +
|
|
|
|
fabsf(mcconf->l_current_min) * mcconf->l_current_min_scale;
|
2019-03-01 08:18:07 -08:00
|
|
|
float ramp_time = fabsf(current) > fabsf(prev_current) ? config.ramp_time_pos : config.ramp_time_neg;
|
|
|
|
|
|
|
|
if (fabsf(out_val) > 0.001) {
|
|
|
|
ramp_time = fminf(config.ramp_time_pos, config.ramp_time_neg);
|
|
|
|
}
|
2014-12-20 03:52:38 -08:00
|
|
|
|
|
|
|
if (ramp_time > 0.01) {
|
|
|
|
const float ramp_step = ((float)OUTPUT_ITERATION_TIME_MS * current_range) / (ramp_time * 1000.0);
|
|
|
|
|
|
|
|
float current_goal = prev_current;
|
|
|
|
const float goal_tmp = current_goal;
|
|
|
|
utils_step_towards(¤t_goal, current, ramp_step);
|
|
|
|
bool is_decreasing = current_goal < goal_tmp;
|
|
|
|
|
|
|
|
// Make sure the desired current is close to the actual current to avoid surprises
|
|
|
|
// when changing direction
|
|
|
|
float goal_tmp2 = current_goal;
|
|
|
|
if (is_reverse) {
|
2017-09-04 12:12:43 -07:00
|
|
|
if (fabsf(current_goal + current_highest_abs) > max_current_diff) {
|
2015-02-19 12:20:07 -08:00
|
|
|
utils_step_towards(&goal_tmp2, -current_highest_abs, 2.0 * ramp_step);
|
2014-12-20 03:52:38 -08:00
|
|
|
}
|
|
|
|
} else {
|
2017-09-04 12:12:43 -07:00
|
|
|
if (fabsf(current_goal - current_highest_abs) > max_current_diff) {
|
2015-02-19 12:20:07 -08:00
|
|
|
utils_step_towards(&goal_tmp2, current_highest_abs, 2.0 * ramp_step);
|
2014-12-20 03:52:38 -08:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// Always allow negative ramping
|
|
|
|
bool is_decreasing2 = goal_tmp2 < current_goal;
|
2015-02-21 14:13:33 -08:00
|
|
|
if ((!is_decreasing || is_decreasing2) && fabsf(out_val) > 0.001) {
|
2014-12-20 03:52:38 -08:00
|
|
|
current_goal = goal_tmp2;
|
|
|
|
}
|
|
|
|
|
|
|
|
current = current_goal;
|
|
|
|
}
|
|
|
|
|
|
|
|
prev_current = current;
|
|
|
|
|
|
|
|
if (current < 0.0) {
|
2015-12-08 12:01:23 -08:00
|
|
|
mc_interface_set_brake_current(current);
|
2015-02-19 12:20:07 -08:00
|
|
|
|
|
|
|
// 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);
|
|
|
|
}
|
|
|
|
}
|
2014-10-18 16:00:53 -07:00
|
|
|
} else {
|
2015-02-19 12:20:07 -08:00
|
|
|
float current_out = current;
|
|
|
|
|
|
|
|
// Traction control
|
|
|
|
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) {
|
|
|
|
if (config.tc) {
|
|
|
|
float rpm_tmp = msg->rpm;
|
|
|
|
if (is_reverse) {
|
|
|
|
rpm_tmp = -rpm_tmp;
|
|
|
|
}
|
|
|
|
|
|
|
|
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 (is_reverse) {
|
|
|
|
comm_can_set_current(msg->id, -current_out);
|
|
|
|
} 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;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2014-12-20 03:52:38 -08:00
|
|
|
if (is_reverse) {
|
2015-12-08 12:01:23 -08:00
|
|
|
mc_interface_set_current(-current_out);
|
2014-12-20 03:52:38 -08:00
|
|
|
} else {
|
2015-12-08 12:01:23 -08:00
|
|
|
mc_interface_set_current(current_out);
|
2014-12-20 03:52:38 -08:00
|
|
|
}
|
2014-10-18 16:00:53 -07:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2017-09-04 12:12:43 -07:00
|
|
|
|
|
|
|
static void terminal_cmd_nunchuk_status(int argc, const char **argv) {
|
|
|
|
(void)argc;
|
|
|
|
(void)argv;
|
|
|
|
|
|
|
|
commands_printf("Nunchuk Status");
|
|
|
|
commands_printf("Output: %s", output_running ? "On" : "Off");
|
|
|
|
commands_printf(" ");
|
|
|
|
}
|