diff --git a/appconf/appconf_default.h b/appconf/appconf_default.h
index 4d9a998d..37346754 100644
--- a/appconf/appconf_default.h
+++ b/appconf/appconf_default.h
@@ -270,6 +270,68 @@
#define APPCONF_NRF_SEND_CRC_ACK true
#endif
+// Balance app
+#ifndef APPCONF_BALANCE_KP
+#define APPCONF_BALANCE_KP 0.0
+#endif
+#ifndef APPCONF_BALANCE_KI
+#define APPCONF_BALANCE_KI 0.0
+#endif
+#ifndef APPCONF_BALANCE_KD
+#define APPCONF_BALANCE_KD 0.0
+#endif
+#ifndef APPCONF_BALANCE_HERTZ
+#define APPCONF_BALANCE_HERTZ 1000
+#endif
+#ifndef APPCONF_BALANCE_M_AXIS
+#define APPCONF_BALANCE_M_AXIS 0
+#endif
+#ifndef APPCONF_BALANCE_C_AXIS
+#define APPCONF_BALANCE_C_AXIS 1
+#endif
+#ifndef APPCONF_BALANCE_M_FAULT
+#define APPCONF_BALANCE_M_FAULT 20
+#endif
+#ifndef APPCONF_BALANCE_C_FAULT
+#define APPCONF_BALANCE_C_FAULT 45
+#endif
+#ifndef APPCONF_BALANCE_USE_SWITCHES
+#define APPCONF_BALANCE_USE_SWITCHES false
+#endif
+#ifndef APPCONF_BALANCE_OVERSPEED_DUTY
+#define APPCONF_BALANCE_OVERSPEED_DUTY 0.9
+#endif
+#ifndef APPCONF_BALANCE_TILTBACK_DUTY
+#define APPCONF_BALANCE_TILTBACK_DUTY 0.75
+#endif
+#ifndef APPCONF_BALANCE_TILTBACK_ANGLE
+#define APPCONF_BALANCE_TILTBACK_ANGLE 15.0
+#endif
+#ifndef APPCONF_BALANCE_TILTBACK_SPEED
+#define APPCONF_BALANCE_TILTBACK_SPEED 5.0
+#endif
+#ifndef APPCONF_BALANCE_TILTBACK_HIGH_V
+#define APPCONF_BALANCE_TILTBACK_HIGH_V 100.0
+#endif
+#ifndef APPCONF_BALANCE_TILTBACK_LOW_V
+#define APPCONF_BALANCE_TILTBACK_LOW_V 0.0
+#endif
+#ifndef APPCONF_BALANCE_STARTUP_M_TOLERANCE
+#define APPCONF_BALANCE_STARTUP_M_TOLERANCE 20.0
+#endif
+#ifndef APPCONF_BALANCE_STARTUP_C_TOLERANCE
+#define APPCONF_BALANCE_STARTUP_C_TOLERANCE 8.0
+#endif
+#ifndef APPCONF_BALANCE_STARTUP_SPEED
+#define APPCONF_BALANCE_STARTUP_SPEED 30.0
+#endif
+#ifndef APPCONF_BALANCE_DEADZONE
+#define APPCONF_BALANCE_DEADZONE 0.0
+#endif
+#ifndef APPCONF_BALANCE_CURRENT_BOOST
+#define APPCONF_BALANCE_CURRENT_BOOST 0.0
+#endif
+
// IMU
#ifndef APPCONF_IMU_TYPE
#define APPCONF_IMU_TYPE IMU_TYPE_INTERNAL
diff --git a/applications/app.c b/applications/app.c
index 6abeee7b..7aa765d8 100644
--- a/applications/app.c
+++ b/applications/app.c
@@ -52,6 +52,7 @@ void app_set_configuration(app_configuration *conf) {
app_adc_stop();
app_uartcomm_stop();
app_nunchuk_stop();
+ app_balance_stop();
if (!conf_general_permanent_nrf_found) {
nrf_driver_stop();
@@ -67,6 +68,9 @@ void app_set_configuration(app_configuration *conf) {
imu_init(&conf->imu_conf);
+ // Configure balance app before starting it.
+ app_balance_configure(&appconf.app_balance_conf, &appconf.imu_conf);
+
switch (appconf.app_to_use) {
case APP_PPM:
app_ppm_start();
@@ -97,6 +101,10 @@ void app_set_configuration(app_configuration *conf) {
app_nunchuk_start();
break;
+ case APP_BALANCE:
+ app_balance_start();
+ break;
+
case APP_NRF:
if (!conf_general_permanent_nrf_found) {
nrf_driver_init();
diff --git a/applications/app.h b/applications/app.h
index 1cf23082..d1932c93 100644
--- a/applications/app.h
+++ b/applications/app.h
@@ -54,6 +54,18 @@ void app_nunchuk_configure(chuk_config *conf);
float app_nunchuk_get_decoded_chuk(void);
void app_nunchuk_update_output(chuck_data *data);
+void app_balance_start(void);
+void app_balance_stop(void);
+void app_balance_configure(balance_config *conf, imu_config *conf2);
+float app_balance_get_pid_output(void);
+float app_balance_get_m_angle(void);
+float app_balance_get_c_angle(void);
+uint32_t app_balance_get_diff_time(void);
+float app_balance_get_motor_current(void);
+float app_balance_get_motor_position(void);
+uint16_t app_balance_get_state(void);
+uint16_t app_balance_get_switch_value(void);
+
// Custom apps
void app_custom_start(void);
void app_custom_stop(void);
diff --git a/applications/app_balance.c b/applications/app_balance.c
new file mode 100644
index 00000000..e6d960eb
--- /dev/null
+++ b/applications/app_balance.c
@@ -0,0 +1,339 @@
+/*
+ Copyright 2019 Mitch Lustig
+
+ This file is part of the VESC firmware.
+
+ The VESC firmware 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 VESC firmware 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 .
+ */
+
+#include "conf_general.h"
+
+#include "ch.h" // ChibiOS
+#include "hal.h" // ChibiOS HAL
+#include "mc_interface.h" // Motor control functions
+#include "hw.h" // Pin mapping on this hardware
+#include "timeout.h" // To reset the timeout
+#include "commands.h"
+#include "imu/imu.h"
+#include "imu/ahrs.h"
+#include "utils.h"
+#include "datatypes.h"
+
+
+#include
+
+// Data type
+typedef enum {
+ STARTUP = 0,
+ RUNNING,
+ FAULT,
+ DEAD
+} BalanceState;
+
+typedef enum {
+ CENTERING = 0,
+ TILTBACK
+} SetpointAdjustmentType;
+
+// Balance thread
+static THD_FUNCTION(balance_thread, arg);
+static THD_WORKING_AREA(balance_thread_wa, 2048); // 2kb stack for this thread
+
+static volatile balance_config balance_conf;
+static volatile imu_config imu_conf;
+static thread_t *app_thread;
+
+// Values used in loop
+static BalanceState state;
+static float m_angle, c_angle;
+static float proportional, integral, derivative;
+static float last_proportional;
+static float pid_value;
+static float setpoint, setpoint_target;
+static SetpointAdjustmentType setpointAdjustmentType;
+static float startup_step_size, tiltback_step_size;
+static systime_t current_time, last_time, diff_time;
+static systime_t startup_start_time, startup_diff_time;
+static uint16_t switches_value;
+
+// Values read to pass in app data to GUI
+static float motor_current;
+static float motor_position;
+
+void app_balance_configure(balance_config *conf, imu_config *conf2) {
+ balance_conf = *conf;
+ imu_conf = *conf2;
+}
+
+void app_balance_start(void) {
+
+ // Reset all Values
+ state = STARTUP;
+ m_angle = 0;
+ c_angle = 0;
+ switches_value = 0;
+ proportional = 0;
+ integral = 0;
+ derivative = 0;
+ last_proportional = 0;
+ pid_value = 0;
+ setpoint = 0;
+ setpoint_target = 0;
+ setpointAdjustmentType = CENTERING;
+ startup_step_size = 0;
+ tiltback_step_size = 0;
+ current_time = 0;
+ last_time = 0;
+ diff_time = 0;
+ startup_start_time = 0;
+ startup_diff_time = 0;
+
+#ifdef HW_SPI_PORT_SCK
+ // Configure pins
+ if(balance_conf.use_switches){
+ palSetPadMode(HW_SPI_PORT_SCK, HW_SPI_PIN_SCK, PAL_MODE_INPUT_PULLDOWN);
+ palSetPadMode(HW_SPI_PORT_MISO, HW_SPI_PIN_MISO, PAL_MODE_INPUT_PULLDOWN);
+ }
+#endif
+
+ // Start the balance thread
+ app_thread = chThdCreateStatic(balance_thread_wa, sizeof(balance_thread_wa), NORMALPRIO, balance_thread, NULL);
+}
+
+void app_balance_stop(void) {
+ if(app_thread != NULL){
+ chThdTerminate(app_thread);
+ chThdWait(app_thread);
+ }
+ mc_interface_set_current(0);
+}
+
+float app_balance_get_pid_output(void) {
+ return pid_value;
+}
+float app_balance_get_m_angle(void) {
+ return m_angle;
+}
+float app_balance_get_c_angle(void) {
+ return c_angle;
+}
+uint32_t app_balance_get_diff_time(void) {
+ return ST2US(diff_time);
+}
+float app_balance_get_motor_current(void) {
+ return motor_current;
+}
+float app_balance_get_motor_position(void) {
+ return motor_position;
+}
+uint16_t app_balance_get_state(void) {
+ return state;
+}
+uint16_t app_balance_get_switch_value(void) {
+ return switches_value;
+}
+
+float get_setpoint_adjustment_step_size(void){
+ switch(setpointAdjustmentType){
+ case (CENTERING):
+ return startup_step_size;
+ case (TILTBACK):
+ return tiltback_step_size;
+ }
+ return 0;
+}
+
+float apply_deadzone(float error){
+ if(balance_conf.deadzone == 0){
+ return error;
+ }
+
+ if(error < balance_conf.deadzone && error > -balance_conf.deadzone){
+ return 0;
+ } else if(error > balance_conf.deadzone){
+ return error - balance_conf.deadzone;
+ } else {
+ return error + balance_conf.deadzone;
+ }
+}
+
+static THD_FUNCTION(balance_thread, arg) {
+ (void)arg;
+ chRegSetThreadName("APP_BALANCE");
+
+ // Do one off config
+ startup_step_size = balance_conf.startup_speed / balance_conf.hertz;
+ tiltback_step_size = balance_conf.tiltback_speed / balance_conf.hertz;
+
+ state = STARTUP;
+ setpointAdjustmentType = CENTERING;
+
+ while (!chThdShouldTerminateX()) {
+ // Update times
+ current_time = chVTGetSystemTimeX();
+ if(last_time == 0){
+ last_time = current_time;
+ }
+ diff_time = current_time - last_time;
+ last_time = current_time;
+
+ // Read values for GUI
+ motor_current = mc_interface_get_tot_current_directional_filtered();
+ motor_position = mc_interface_get_pid_pos_now();
+
+ // Get the values we want
+ switch(balance_conf.m_axis){
+ case (PITCH):
+ m_angle = imu_get_pitch() * 180.0f / M_PI;;
+ break;
+ case (ROLL):
+ m_angle = imu_get_roll() * 180.0f / M_PI;
+ break;
+ case (YAW):
+ m_angle = imu_get_yaw() * 180.0f / M_PI;
+ break;
+ }
+ switch(balance_conf.c_axis){
+ case (PITCH):
+ c_angle = imu_get_pitch() * 180.0f / M_PI;;
+ break;
+ case (ROLL):
+ c_angle = imu_get_roll() * 180.0f / M_PI;
+ break;
+ case (YAW):
+ c_angle = imu_get_yaw() * 180.0f / M_PI;
+ break;
+ }
+
+ if(!balance_conf.use_switches){
+ switches_value = 2;
+ }else{
+ switches_value = 0;
+#ifdef HW_SPI_PORT_SCK
+ if(palReadPad(HW_SPI_PORT_SCK, HW_SPI_PIN_SCK)){
+ switches_value += 1;
+ }
+ if(palReadPad(HW_SPI_PORT_MISO, HW_SPI_PIN_MISO)){
+ switches_value += 1;
+ }
+#endif
+ }
+
+ // State based logic
+ switch(state){
+ case (STARTUP):
+ while(!imu_startup_done()){
+ chThdSleepMilliseconds(50);
+ }
+ state = FAULT;
+ startup_start_time = 0;
+ startup_diff_time = 0;
+ break;
+ case (RUNNING):
+ // Check for overspeed
+ if(fabsf(mc_interface_get_duty_cycle_now()) > balance_conf.overspeed_duty){
+ state = DEAD;
+ }
+
+ // Check for fault
+ if(
+ fabsf(m_angle) > balance_conf.m_fault || // Balnce axis tip over
+ fabsf(c_angle) > balance_conf.c_fault || // Cross axis tip over
+ app_balance_get_switch_value() == 0 || // Switch fully open
+ (app_balance_get_switch_value() == 1 && fabsf(mc_interface_get_duty_cycle_now()) < 0.003) // Switch partially open and stopped
+ ){
+ state = FAULT;
+ }
+
+ // Over speed tilt back safety
+ if(fabsf(mc_interface_get_duty_cycle_now()) > balance_conf.tiltback_duty ||
+ (fabsf(mc_interface_get_duty_cycle_now()) > 0.05 && GET_INPUT_VOLTAGE() > balance_conf.tiltback_high_voltage) ||
+ (fabsf(mc_interface_get_duty_cycle_now()) > 0.05 && GET_INPUT_VOLTAGE() < balance_conf.tiltback_low_voltage)){
+ if(mc_interface_get_duty_cycle_now() > 0){
+ setpoint_target = balance_conf.tiltback_angle;
+ } else {
+ setpoint_target = -balance_conf.tiltback_angle;
+ }
+ setpointAdjustmentType = TILTBACK;
+ }else{
+ setpoint_target = 0;
+ }
+
+ // Adjust setpoint
+ if(setpoint != setpoint_target){
+ // If we are less than one step size away, go all the way
+ if(fabsf(setpoint_target - setpoint) < get_setpoint_adjustment_step_size()){
+ setpoint = setpoint_target;
+ }else if (setpoint_target - setpoint > 0){
+ setpoint += get_setpoint_adjustment_step_size();
+ }else{
+ setpoint -= get_setpoint_adjustment_step_size();
+ }
+ }
+
+ // Do PID maths
+ proportional = setpoint - m_angle;
+ // Apply deadzone
+ proportional = apply_deadzone(proportional);
+ // Resume real PID maths
+ integral = integral + proportional;
+ derivative = proportional - last_proportional;
+
+ pid_value = (balance_conf.kp * proportional) + (balance_conf.ki * integral) + (balance_conf.kd * derivative);
+
+ last_proportional = proportional;
+
+ // Apply current boost
+ if(pid_value > 0){
+ pid_value += balance_conf.current_boost;
+ }else if(pid_value < 0){
+ pid_value -= balance_conf.current_boost;
+ }
+
+ // Reset the timeout
+ timeout_reset();
+
+ // Output to motor
+ if(pid_value == 0){
+ mc_interface_release_motor();
+ }else {
+ mc_interface_set_current(pid_value);
+ }
+ break;
+ case (FAULT):
+ // Check for valid startup position and switch state
+ if(fabsf(m_angle) < balance_conf.startup_m_tolerance && fabsf(c_angle) < balance_conf.startup_c_tolerance && app_balance_get_switch_value() == 2){
+ setpoint = m_angle;
+ setpoint_target = 0;
+ setpointAdjustmentType = CENTERING;
+ state = RUNNING;
+ break;
+ }
+
+ // Disable output
+ mc_interface_set_current(0);
+ break;
+ case (DEAD):
+ // Disable output
+ mc_interface_set_current(0);
+ break;
+ }
+
+ // Delay between loops
+ chThdSleepMicroseconds((int)((1000.0 / balance_conf.hertz) * 1000.0));
+ }
+
+ // Disable output
+ mc_interface_set_current(0);
+}
diff --git a/applications/applications.mk b/applications/applications.mk
index e3a504d3..d211db10 100644
--- a/applications/applications.mk
+++ b/applications/applications.mk
@@ -4,6 +4,7 @@ APPSRC = applications/app.c \
applications/app_sten.c \
applications/app_uartcomm.c \
applications/app_nunchuk.c \
+ applications/app_balance.c \
applications/app_custom.c
APPINC = applications
diff --git a/commands.c b/commands.c
index c7a4c286..94fd0be9 100644
--- a/commands.c
+++ b/commands.c
@@ -510,6 +510,21 @@ void commands_process_packet(unsigned char *data, unsigned int len,
reply_func(send_buffer, ind);
} break;
+ case COMM_GET_DECODED_BALANCE: {
+ int32_t ind = 0;
+ uint8_t send_buffer[50];
+ send_buffer[ind++] = COMM_GET_DECODED_BALANCE;
+ buffer_append_int32(send_buffer, (int32_t)(app_balance_get_pid_output() * 1000000.0), &ind);
+ buffer_append_int32(send_buffer, (int32_t)(app_balance_get_m_angle() * 1000000.0), &ind);
+ buffer_append_int32(send_buffer, (int32_t)(app_balance_get_c_angle() * 1000000.0), &ind);
+ buffer_append_uint32(send_buffer, app_balance_get_diff_time(), &ind);
+ buffer_append_int32(send_buffer, (int32_t)(app_balance_get_motor_current() * 1000000.0), &ind);
+ buffer_append_int32(send_buffer, (int32_t)(app_balance_get_motor_position() * 1000000.0), &ind);
+ buffer_append_uint16(send_buffer, app_balance_get_state(), &ind);
+ buffer_append_uint16(send_buffer, app_balance_get_switch_value(), &ind);
+ reply_func(send_buffer, ind);
+ } break;
+
case COMM_FORWARD_CAN:
comm_can_send_buffer(data[0], data + 1, len - 1, 0);
break;
diff --git a/confgenerator.c b/confgenerator.c
index c7502d63..6a587765 100644
--- a/confgenerator.c
+++ b/confgenerator.c
@@ -223,6 +223,26 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration
buffer[ind++] = (uint8_t)conf->app_nrf_conf.address[1];
buffer[ind++] = (uint8_t)conf->app_nrf_conf.address[2];
buffer[ind++] = conf->app_nrf_conf.send_crc_ack;
+ buffer_append_float32_auto(buffer, conf->app_balance_conf.kp, &ind);
+ buffer_append_float32_auto(buffer, conf->app_balance_conf.ki, &ind);
+ buffer_append_float32_auto(buffer, conf->app_balance_conf.kd, &ind);
+ buffer_append_uint16(buffer, conf->app_balance_conf.hertz, &ind);
+ buffer[ind++] = conf->app_balance_conf.m_axis;
+ buffer[ind++] = conf->app_balance_conf.c_axis;
+ buffer_append_float32_auto(buffer, conf->app_balance_conf.m_fault, &ind);
+ buffer_append_float32_auto(buffer, conf->app_balance_conf.c_fault, &ind);
+ buffer[ind++] = conf->app_balance_conf.use_switches;
+ buffer_append_float32_auto(buffer, conf->app_balance_conf.overspeed_duty, &ind);
+ buffer_append_float32_auto(buffer, conf->app_balance_conf.tiltback_duty, &ind);
+ buffer_append_float32_auto(buffer, conf->app_balance_conf.tiltback_angle, &ind);
+ buffer_append_float32_auto(buffer, conf->app_balance_conf.tiltback_speed, &ind);
+ buffer_append_float32_auto(buffer, conf->app_balance_conf.tiltback_high_voltage, &ind);
+ buffer_append_float32_auto(buffer, conf->app_balance_conf.tiltback_low_voltage, &ind);
+ buffer_append_float32_auto(buffer, conf->app_balance_conf.startup_m_tolerance, &ind);
+ buffer_append_float32_auto(buffer, conf->app_balance_conf.startup_c_tolerance, &ind);
+ buffer_append_float32_auto(buffer, conf->app_balance_conf.startup_speed, &ind);
+ buffer_append_float32_auto(buffer, conf->app_balance_conf.deadzone, &ind);
+ buffer_append_float32_auto(buffer, conf->app_balance_conf.current_boost, &ind);
buffer[ind++] = conf->imu_conf.type;
buffer[ind++] = conf->imu_conf.mode;
buffer_append_uint16(buffer, conf->imu_conf.sample_rate_hz, &ind);
@@ -472,6 +492,26 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration
conf->app_nrf_conf.address[1] = buffer[ind++];
conf->app_nrf_conf.address[2] = buffer[ind++];
conf->app_nrf_conf.send_crc_ack = buffer[ind++];
+ conf->app_balance_conf.kp = buffer_get_float32_auto(buffer, &ind);
+ conf->app_balance_conf.ki = buffer_get_float32_auto(buffer, &ind);
+ conf->app_balance_conf.kd = buffer_get_float32_auto(buffer, &ind);
+ conf->app_balance_conf.hertz = buffer_get_uint16(buffer, &ind);
+ conf->app_balance_conf.m_axis = buffer[ind++];
+ conf->app_balance_conf.c_axis = buffer[ind++];
+ conf->app_balance_conf.m_fault = buffer_get_float32_auto(buffer, &ind);
+ conf->app_balance_conf.c_fault = buffer_get_float32_auto(buffer, &ind);
+ conf->app_balance_conf.use_switches = buffer[ind++];
+ conf->app_balance_conf.overspeed_duty = buffer_get_float32_auto(buffer, &ind);
+ conf->app_balance_conf.tiltback_duty = buffer_get_float32_auto(buffer, &ind);
+ conf->app_balance_conf.tiltback_angle = buffer_get_float32_auto(buffer, &ind);
+ conf->app_balance_conf.tiltback_speed = buffer_get_float32_auto(buffer, &ind);
+ conf->app_balance_conf.tiltback_high_voltage = buffer_get_float32_auto(buffer, &ind);
+ conf->app_balance_conf.tiltback_low_voltage = buffer_get_float32_auto(buffer, &ind);
+ conf->app_balance_conf.startup_m_tolerance = buffer_get_float32_auto(buffer, &ind);
+ conf->app_balance_conf.startup_c_tolerance = buffer_get_float32_auto(buffer, &ind);
+ conf->app_balance_conf.startup_speed = buffer_get_float32_auto(buffer, &ind);
+ conf->app_balance_conf.deadzone = buffer_get_float32_auto(buffer, &ind);
+ conf->app_balance_conf.current_boost = buffer_get_float32_auto(buffer, &ind);
conf->imu_conf.type = buffer[ind++];
conf->imu_conf.mode = buffer[ind++];
conf->imu_conf.sample_rate_hz = buffer_get_uint16(buffer, &ind);
@@ -705,6 +745,26 @@ void confgenerator_set_defaults_appconf(app_configuration *conf) {
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;
+ conf->app_balance_conf.kp = APPCONF_BALANCE_KP;
+ conf->app_balance_conf.ki = APPCONF_BALANCE_KI;
+ conf->app_balance_conf.kd = APPCONF_BALANCE_KD;
+ conf->app_balance_conf.hertz = APPCONF_BALANCE_HERTZ;
+ conf->app_balance_conf.m_axis = APPCONF_BALANCE_M_AXIS;
+ conf->app_balance_conf.c_axis = APPCONF_BALANCE_C_AXIS;
+ conf->app_balance_conf.m_fault = APPCONF_BALANCE_M_FAULT;
+ conf->app_balance_conf.c_fault = APPCONF_BALANCE_C_FAULT;
+ conf->app_balance_conf.use_switches = APPCONF_BALANCE_USE_SWITCHES;
+ conf->app_balance_conf.overspeed_duty = APPCONF_BALANCE_OVERSPEED_DUTY;
+ conf->app_balance_conf.tiltback_duty = APPCONF_BALANCE_TILTBACK_DUTY;
+ conf->app_balance_conf.tiltback_angle = APPCONF_BALANCE_TILTBACK_ANGLE;
+ conf->app_balance_conf.tiltback_speed = APPCONF_BALANCE_TILTBACK_SPEED;
+ conf->app_balance_conf.tiltback_high_voltage = APPCONF_BALANCE_TILTBACK_HIGH_V;
+ conf->app_balance_conf.tiltback_low_voltage = APPCONF_BALANCE_TILTBACK_LOW_V;
+ conf->app_balance_conf.startup_m_tolerance = APPCONF_BALANCE_STARTUP_M_TOLERANCE;
+ conf->app_balance_conf.startup_c_tolerance = APPCONF_BALANCE_STARTUP_C_TOLERANCE;
+ conf->app_balance_conf.startup_speed = APPCONF_BALANCE_STARTUP_SPEED;
+ conf->app_balance_conf.deadzone = APPCONF_BALANCE_DEADZONE;
+ conf->app_balance_conf.current_boost = APPCONF_BALANCE_CURRENT_BOOST;
conf->imu_conf.type = APPCONF_IMU_TYPE;
conf->imu_conf.mode = APPCONF_IMU_AHRS_MODE;
conf->imu_conf.sample_rate_hz = APPCONF_IMU_SAMPLE_RATE_HZ;
diff --git a/confgenerator.h b/confgenerator.h
index 4d7a0ffb..6c02c9af 100644
--- a/confgenerator.h
+++ b/confgenerator.h
@@ -9,7 +9,7 @@
// Constants
#define MCCONF_SIGNATURE 503309878
-#define APPCONF_SIGNATURE 583966563
+#define APPCONF_SIGNATURE 3900592330
// Functions
int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf);
diff --git a/datatypes.h b/datatypes.h
index 3601cadf..fe8c70ec 100644
--- a/datatypes.h
+++ b/datatypes.h
@@ -317,7 +317,8 @@ typedef enum {
APP_ADC_UART,
APP_NUNCHUK,
APP_NRF,
- APP_CUSTOM
+ APP_CUSTOM,
+ APP_BALANCE
} app_use;
// Throttle curve mode
@@ -488,6 +489,36 @@ typedef struct {
bool send_crc_ack;
} nrf_config;
+// External LED state
+typedef enum {
+ PITCH = 0,
+ ROLL,
+ YAW
+} AXIS;
+
+typedef struct {
+ float kp;
+ float ki;
+ float kd;
+ uint16_t hertz;
+ AXIS m_axis;
+ AXIS c_axis;
+ float m_fault;
+ float c_fault;
+ bool use_switches;
+ float overspeed_duty;
+ float tiltback_duty;
+ float tiltback_angle;
+ float tiltback_speed;
+ float tiltback_high_voltage;
+ float tiltback_low_voltage;
+ float startup_m_tolerance;
+ float startup_c_tolerance;
+ float startup_speed;
+ float deadzone;
+ float current_boost;
+} balance_config;
+
// CAN status modes
typedef enum {
CAN_STATUS_DISABLED = 0,
@@ -575,6 +606,9 @@ typedef struct {
// NRF application settings
nrf_config app_nrf_conf;
+ // Balance application settings
+ balance_config app_balance_conf;
+
// IMU Settings
imu_config imu_conf;
} app_configuration;
@@ -660,6 +694,7 @@ typedef enum {
COMM_PLOT_DATA,
COMM_PLOT_ADD_GRAPH,
COMM_PLOT_SET_GRAPH,
+ COMM_GET_DECODED_BALANCE,
} COMM_PACKET_ID;
// CAN commands
diff --git a/imu/imu.c b/imu/imu.c
index 83d5a403..20156263 100644
--- a/imu/imu.c
+++ b/imu/imu.c
@@ -40,6 +40,8 @@ static ICM20948_STATE m_icm20948_state;
static BMI_STATE m_bmi_state;
static imu_config m_settings;
static float m_gyro_offset[3] = {0.0};
+static systime_t init_time;
+static bool imu_ready;
// Private functions
static void imu_read_callback(float *accel, float *gyro, float *mag);
@@ -53,8 +55,10 @@ void imu_init(imu_config *set) {
imu_stop();
- ahrs_update_all_parameters(set->accel_confidence_decay, set->mahony_kp,
- set->mahony_ki, set->madgwick_beta);
+ imu_ready = false;
+ init_time = chVTGetSystemTimeX();
+ ahrs_update_all_parameters(1.0, 0.3, 0.0, 2.0);
+
ahrs_init_attitude_info(&m_att);
mpu9150_set_rate_hz(set->sample_rate_hz);
@@ -149,6 +153,10 @@ void imu_stop(void) {
bmi160_wrapper_stop(&m_bmi_state);
}
+bool imu_startup_done(void) {
+ return imu_ready;
+}
+
float imu_get_roll(void) {
return ahrs_get_roll(&m_att);
}
@@ -215,6 +223,15 @@ static void imu_read_callback(float *accel, float *gyro, float *mag) {
float dt = timer_seconds_elapsed_since(last_time);
last_time = timer_time_now();
+ if(!imu_ready && ST2MS(chVTGetSystemTimeX() - init_time) > 1000){
+ ahrs_update_all_parameters(
+ m_settings.accel_confidence_decay,
+ m_settings.mahony_kp,
+ m_settings.mahony_ki,
+ m_settings.madgwick_beta);
+ imu_ready = true;
+ }
+
#ifdef IMU_FLIP
accel[0] *= -1.0;
accel[2] *= -1.0;
diff --git a/imu/imu.h b/imu/imu.h
index f1095967..e8e1b4b2 100644
--- a/imu/imu.h
+++ b/imu/imu.h
@@ -33,6 +33,7 @@ void imu_init_icm20948(stm32_gpio_t *sda_gpio, int sda_pin,
void imu_init_bmi160(stm32_gpio_t *sda_gpio, int sda_pin,
stm32_gpio_t *scl_gpio, int scl_pin);
void imu_stop(void);
+bool imu_startup_done(void);
float imu_get_roll(void);
float imu_get_pitch(void);
float imu_get_yaw(void);
diff --git a/imu/mpu9150.c b/imu/mpu9150.c
index 7027f744..3b61f979 100644
--- a/imu/mpu9150.c
+++ b/imu/mpu9150.c
@@ -115,7 +115,7 @@ void mpu9150_init(stm32_gpio_t *sda_gpio, int sda_pin,
terminal_read_reg);
uint8_t res = read_single_reg(MPU9150_WHO_AM_I);
- if (res == 0x68 || res == 0x69 || res == 0x71) {
+ if (res == 0x68 || res == 0x69 || res == 0x71 || res == 0x73) {
mpu_found = true;
if (!mpu_tp) {
should_stop = false;