From 271dd21dab83cd1a97e21259d25fbea2911f8be8 Mon Sep 17 00:00:00 2001 From: Mitch Lustig Date: Sat, 27 Jul 2019 21:25:56 -0700 Subject: [PATCH 01/31] start in balanace app --- appconf/appconf_default.h | 26 ++++++++ applications/app.c | 6 ++ applications/app.h | 6 ++ applications/app_balance.c | 124 +++++++++++++++++++++++++++++++++++ applications/applications.mk | 1 + confgenerator.c | 8 +++ datatypes.h | 16 +++++ hwconf/hw_410.h | 7 ++ hwconf/hw_60.h | 6 +- 9 files changed, 197 insertions(+), 3 deletions(-) create mode 100644 applications/app_balance.c diff --git a/appconf/appconf_default.h b/appconf/appconf_default.h index 1a4676d2..c46ea537 100644 --- a/appconf/appconf_default.h +++ b/appconf/appconf_default.h @@ -248,4 +248,30 @@ #define APPCONF_NRF_SEND_CRC_ACK true #endif +// Balance app +#ifndef APPCONF_BALANCE_KP +#define APPCONF_BALANCE_KP 1.45 +#endif +#ifndef APPCONF_BALANCE_KI +#define APPCONF_BALANCE_KI 0.78 +#endif +#ifndef APPCONF_BALANCE_KD +#define APPCONF_BALANCE_KD 2.85 +#endif +#ifndef APPCONF_BALANCE_PITCH_OFFSET +#define APPCONF_BALANCE_PITCH_OFFSET 0 +#endif +#ifndef APPCONF_BALANCE_ROLL_OFFSET +#define APPCONF_BALANCE_ROLL_OFFSET 0 +#endif +#ifndef APPCONF_BALANCE_PITCH_FAULT +#define APPCONF_BALANCE_PITCH_FAULT 45 +#endif +#ifndef APPCONF_BALANCE_ROLL_FAULT +#define APPCONF_BALANCE_ROLL_FAULT 45 +#endif +#ifndef APPCONF_BALANCE_START_DELAY +#define APPCONF_BALANCE_START_DELAY 10 +#endif + #endif /* APPCONF_DEFAULT_H_ */ diff --git a/applications/app.c b/applications/app.c index 18c1776e..5ee013a4 100644 --- a/applications/app.c +++ b/applications/app.c @@ -51,6 +51,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(); @@ -94,6 +95,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(); @@ -116,6 +121,7 @@ void app_set_configuration(app_configuration *conf) { app_adc_configure(&appconf.app_adc_conf); app_uartcomm_configure(appconf.app_uart_baudrate, appconf.permanent_uart_enabled); app_nunchuk_configure(&appconf.app_chuk_conf); + app_balance_configure(&appconf.app_balance_conf); #ifdef APP_CUSTOM_TO_USE app_custom_configure(&appconf); diff --git a/applications/app.h b/applications/app.h index 1cf23082..3a3800f2 100644 --- a/applications/app.h +++ b/applications/app.h @@ -54,9 +54,15 @@ 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(app_configuration *conf); +void app_balance_terminal_rpy(int argc, const char **argv); + // Custom apps void app_custom_start(void); void app_custom_stop(void); void app_custom_configure(app_configuration *conf); +void app_custom_terminal_rpy(int argc, const char **argv); #endif /* APP_H_ */ diff --git a/applications/app_balance.c b/applications/app_balance.c new file mode 100644 index 00000000..dd2185e3 --- /dev/null +++ b/applications/app_balance.c @@ -0,0 +1,124 @@ +/* + 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 + +// Example thread +static THD_FUNCTION(example_thread, arg); +static THD_WORKING_AREA(example_thread_wa, 2048); // 2kb stack for this thread + +// 1.45 0.78 2.85 +static thread_t *app_thread; +static double p = 1.45, i = 0.78, d = 2.0; +static double pitch, roll; +static double target_pitch = 0.0; + +static bool registered_terminal = false; + +void app_balance_terminal_rpy(int argc, const char **argv) { + if (argc != 4){ + commands_printf("PID values NOT set!"); + return; + } + float _p = 0, _i = 0, _d = 0; + sscanf(argv[1], "%f", &_p); + sscanf(argv[2], "%f", &_i); + sscanf(argv[2], "%f", &_d); + + p = _p; + i = _i; + d = _d; + + commands_printf("PID values set :) %.2f %.2f %.2f - %.2f %.2f %.2f", _p, _i, _d, p, i, d); +} + +void app_balance_configure(ppm_config *conf) { + if(!registered_terminal){ + terminal_register_command_callback("euc_pid", "Sets PID values!", 0, app_balance_terminal_rpy); + registered_terminal = true; + } +} + +void app_balance_start(void) { + // Set the UART TX pin as an input with pulldown + // palSetPadMode(HW_UART_TX_PORT, HW_UART_TX_PIN, PAL_MODE_INPUT_PULLDOWN); + commands_printf("Custom app start"); + hw_stop_i2c(); + hw_start_i2c(); + imu_init(); + // Start the example thread + app_thread = chThdCreateStatic(example_thread_wa, sizeof(example_thread_wa), NORMALPRIO, example_thread, NULL); +} + +void app_balance_stop(void) { + commands_printf("Custom app stop"); + hw_stop_i2c(); + hw_start_i2c(); + imu_init(); + chThdTerminate(app_thread); +} + +static THD_FUNCTION(example_thread, arg) { + (void)arg; + + chRegSetThreadName("APP_EXAMPLE"); + + commands_printf("Custom app thread"); + + chThdSleepSeconds(10); + + double error, error_average, error_change; + double last_error; + double pid_value; + + + while (!chThdShouldTerminateX()) { + + pitch = (double)(imu_get_pitch() * 180.0 / M_PI); + + error = target_pitch - pitch; + error_average = (error_average + error) / 2; + error_change = error - last_error; + + pid_value = (p*error) + (i*error_average) + (d*error_change); + + last_error = error; + + // commands_printf("euc_pid values %.4f %.4f - %.2f %.2f %.2f - %.2f %.2f %.2f", pitch, pid_value, p, i, d, error, error_average, error_change); + mc_interface_set_current(pid_value); + // Run this loop at 1000Hz + chThdSleepMilliseconds(5); + + // commands_printf("Custom app releasing motor"); + // mc_interface_release_motor(); + + // Reset the timeout + timeout_reset(); + } +} 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/confgenerator.c b/confgenerator.c index 42262381..5ba672b3 100644 --- a/confgenerator.c +++ b/confgenerator.c @@ -644,4 +644,12 @@ 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.pitch_offset = APPCONF_BALANCE_PITCH_OFFSET; + conf->app_balance_conf.roll_offset = APPCONF_BALANCE_ROLL_OFFSET; + conf->app_balance_conf.pitch_fault = APPCONF_BALANCE_PITCH_FAULT; + conf->app_balance_conf.roll_fault = APPCONF_BALANCE_ROLL_FAULT; + conf->app_balance_conf.start_delay = APPCONF_BALANCE_START_DELAY; } diff --git a/datatypes.h b/datatypes.h index 512c3c0a..a0092732 100644 --- a/datatypes.h +++ b/datatypes.h @@ -317,6 +317,7 @@ typedef enum { APP_ADC_UART, APP_NUNCHUK, APP_NRF, + APP_BALANCE, APP_CUSTOM } app_use; @@ -480,6 +481,18 @@ typedef struct { bool send_crc_ack; } nrf_config; +// Balance Datatypes +typedef struct { + float kp; + float ki; + float kd; + float pitch_offset; + float roll_offset; + float pitch_fault; + float roll_fault; + uint8_t start_delay; +} balance_config; + // CAN status modes typedef enum { CAN_STATUS_DISABLED = 0, @@ -521,6 +534,9 @@ typedef struct { // NRF application settings nrf_config app_nrf_conf; + + // Balance application settings + balance_config app_balance_conf; } app_configuration; // Communication commands diff --git a/hwconf/hw_410.h b/hwconf/hw_410.h index b9c4bfa3..9d3ee6d2 100644 --- a/hwconf/hw_410.h +++ b/hwconf/hw_410.h @@ -132,6 +132,13 @@ #define HW_I2C_SDA_PORT GPIOB #define HW_I2C_SDA_PIN 11 +// MPU9250 +#define MPU9X50_SDA_GPIO GPIOB +#define MPU9X50_SDA_PIN 11 +#define MPU9X50_SCL_GPIO GPIOB +#define MPU9X50_SCL_PIN 10 +#define IMU_FLIP + // Hall/encoder pins #define HW_HALL_ENC_GPIO1 GPIOB #define HW_HALL_ENC_PIN1 6 diff --git a/hwconf/hw_60.h b/hwconf/hw_60.h index b43ad0eb..10f4b5cb 100644 --- a/hwconf/hw_60.h +++ b/hwconf/hw_60.h @@ -218,9 +218,9 @@ // MPU9250 #define MPU9X50_SDA_GPIO GPIOB -#define MPU9X50_SDA_PIN 2 -#define MPU9X50_SCL_GPIO GPIOA -#define MPU9X50_SCL_PIN 15 +#define MPU9X50_SDA_PIN 11 +#define MPU9X50_SCL_GPIO GPIOB +#define MPU9X50_SCL_PIN 10 #define IMU_FLIP // Measurement macros From bbba7181406934c067d7ad9f71ec6111c27f9053 Mon Sep 17 00:00:00 2001 From: Mitch Lustig Date: Mon, 29 Jul 2019 00:31:48 -0700 Subject: [PATCH 02/31] Work to get GUI going --- appconf/appconf_default.h | 2 +- applications/app.h | 6 ++- applications/app_balance.c | 80 +++++++++++++++++++++++++------------- commands.c | 11 ++++++ confgenerator.c | 16 ++++++++ confgenerator.h | 2 +- datatypes.h | 1 + 7 files changed, 88 insertions(+), 30 deletions(-) diff --git a/appconf/appconf_default.h b/appconf/appconf_default.h index f867a5eb..68f032f3 100644 --- a/appconf/appconf_default.h +++ b/appconf/appconf_default.h @@ -274,7 +274,7 @@ #define APPCONF_BALANCE_ROLL_FAULT 45 #endif #ifndef APPCONF_BALANCE_START_DELAY -#define APPCONF_BALANCE_START_DELAY 10 +#define APPCONF_BALANCE_START_DELAY 15 #endif #endif /* APPCONF_DEFAULT_H_ */ diff --git a/applications/app.h b/applications/app.h index 3a3800f2..65bfe5ab 100644 --- a/applications/app.h +++ b/applications/app.h @@ -56,7 +56,11 @@ void app_nunchuk_update_output(chuck_data *data); void app_balance_start(void); void app_balance_stop(void); -void app_balance_configure(app_configuration *conf); +void app_balance_configure(balance_config *conf); +float app_balance_get_pid_output(void); +float app_balance_get_pitch(void); +float app_balance_get_roll(void); +float app_balance_get_motor_current(void); void app_balance_terminal_rpy(int argc, const char **argv); // Custom apps diff --git a/applications/app_balance.c b/applications/app_balance.c index dd2185e3..5492ecc0 100644 --- a/applications/app_balance.c +++ b/applications/app_balance.c @@ -33,32 +33,40 @@ static THD_FUNCTION(example_thread, arg); static THD_WORKING_AREA(example_thread_wa, 2048); // 2kb stack for this thread -// 1.45 0.78 2.85 +static volatile balance_config config; static thread_t *app_thread; -static double p = 1.45, i = 0.78, d = 2.0; -static double pitch, roll; -static double target_pitch = 0.0; static bool registered_terminal = false; +// Values used in loop +static double pitch; +static double error, error_average, error_change; +static double last_error; +static double pid_value; + +// Values read to pass in app data to GUI +static double motor_current; + void app_balance_terminal_rpy(int argc, const char **argv) { if (argc != 4){ commands_printf("PID values NOT set!"); return; } - float _p = 0, _i = 0, _d = 0; - sscanf(argv[1], "%f", &_p); - sscanf(argv[2], "%f", &_i); - sscanf(argv[2], "%f", &_d); - - p = _p; - i = _i; - d = _d; - - commands_printf("PID values set :) %.2f %.2f %.2f - %.2f %.2f %.2f", _p, _i, _d, p, i, d); + // float _p = 0, _i = 0, _d = 0; + // sscanf(argv[1], "%f", &_p); + // sscanf(argv[2], "%f", &_i); + // sscanf(argv[2], "%f", &_d); + // + // p = _p; + // i = _i; + // d = _d; + // + // commands_printf("PID values set :) %.2f %.2f %.2f - %.2f %.2f %.2f", _p, _i, _d, p, i, d); } -void app_balance_configure(ppm_config *conf) { +void app_balance_configure(balance_config *conf) { + config = *conf; + if(!registered_terminal){ terminal_register_command_callback("euc_pid", "Sets PID values!", 0, app_balance_terminal_rpy); registered_terminal = true; @@ -72,16 +80,38 @@ void app_balance_start(void) { hw_stop_i2c(); hw_start_i2c(); imu_init(); + + // Reset all Values + pitch = 0; + error = 0; + error_average = 0; + error_change = 0; + last_error = 0; + pid_value = 0; // Start the example thread app_thread = chThdCreateStatic(example_thread_wa, sizeof(example_thread_wa), NORMALPRIO, example_thread, NULL); } void app_balance_stop(void) { commands_printf("Custom app stop"); + chThdTerminate(app_thread); + mc_interface_set_current(0); hw_stop_i2c(); hw_start_i2c(); imu_init(); - chThdTerminate(app_thread); +} + +float app_balance_get_pid_output(void) { + return pid_value; +} +float app_balance_get_pitch(void) { + return pitch; +} +float app_balance_get_roll(void) { + return config.ki; +} +float app_balance_get_motor_current(void) { + return motor_current; } static THD_FUNCTION(example_thread, arg) { @@ -91,22 +121,20 @@ static THD_FUNCTION(example_thread, arg) { commands_printf("Custom app thread"); - chThdSleepSeconds(10); - - double error, error_average, error_change; - double last_error; - double pid_value; + chThdSleepSeconds(config.start_delay); while (!chThdShouldTerminateX()) { + // Read values for GUI + motor_current = mc_interface_get_tot_current_directional_filtered(); pitch = (double)(imu_get_pitch() * 180.0 / M_PI); - error = target_pitch - pitch; - error_average = (error_average + error) / 2; + error = config.pitch_offset - pitch; + error_average = error_average + error; error_change = error - last_error; - pid_value = (p*error) + (i*error_average) + (d*error_change); + pid_value = (config.kp*error) + (config.ki*error_average) + (config.kd*error_change); last_error = error; @@ -115,10 +143,8 @@ static THD_FUNCTION(example_thread, arg) { // Run this loop at 1000Hz chThdSleepMilliseconds(5); - // commands_printf("Custom app releasing motor"); - // mc_interface_release_motor(); - // Reset the timeout timeout_reset(); } + mc_interface_set_current(0); } diff --git a/commands.c b/commands.c index 764c9e11..bd7f5e57 100644 --- a/commands.c +++ b/commands.c @@ -502,6 +502,17 @@ 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_pitch() * 1000000.0), &ind); + buffer_append_int32(send_buffer, (int32_t)(app_balance_get_roll() * 1000000.0), &ind); + buffer_append_int32(send_buffer, (int32_t)(app_balance_get_motor_current() * 1000000.0), &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 b6a3bcc2..7fcfede8 100644 --- a/confgenerator.c +++ b/confgenerator.c @@ -217,6 +217,14 @@ 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_float32_auto(buffer, conf->app_balance_conf.pitch_offset, &ind); + buffer_append_float32_auto(buffer, conf->app_balance_conf.roll_offset, &ind); + buffer_append_float32_auto(buffer, conf->app_balance_conf.pitch_fault, &ind); + buffer_append_float32_auto(buffer, conf->app_balance_conf.roll_fault, &ind); + buffer[ind++] = (uint8_t)conf->app_balance_conf.start_delay; return ind; } @@ -440,6 +448,14 @@ 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.pitch_offset = buffer_get_float32_auto(buffer, &ind); + conf->app_balance_conf.roll_offset = buffer_get_float32_auto(buffer, &ind); + conf->app_balance_conf.pitch_fault = buffer_get_float32_auto(buffer, &ind); + conf->app_balance_conf.roll_fault = buffer_get_float32_auto(buffer, &ind); + conf->app_balance_conf.start_delay = buffer[ind++]; return true; } diff --git a/confgenerator.h b/confgenerator.h index 5f909e36..c7eb7f26 100644 --- a/confgenerator.h +++ b/confgenerator.h @@ -9,7 +9,7 @@ // Constants #define MCCONF_SIGNATURE 503309878 -#define APPCONF_SIGNATURE 1099508905 +#define APPCONF_SIGNATURE 2534075884 // Functions int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf); diff --git a/datatypes.h b/datatypes.h index 8b1d1edd..b08a448b 100644 --- a/datatypes.h +++ b/datatypes.h @@ -578,6 +578,7 @@ typedef enum { COMM_GET_DECODED_PPM, COMM_GET_DECODED_ADC, COMM_GET_DECODED_CHUK, + COMM_GET_DECODED_BALANCE, COMM_FORWARD_CAN, COMM_SET_CHUCK_DATA, COMM_CUSTOM_APP_DATA, From 800115f68393d1f01e10c9f02b61da9b8fbdb78d Mon Sep 17 00:00:00 2001 From: Mitch Lustig Date: Tue, 30 Jul 2019 00:27:23 -0700 Subject: [PATCH 03/31] Implement more data to help debug --- appconf/appconf_default.h | 4 ++ applications/app.h | 2 + applications/app_balance.c | 84 ++++++++++++++++++++++++++------------ commands.c | 2 + confgenerator.c | 3 ++ confgenerator.h | 2 +- datatypes.h | 1 + 7 files changed, 71 insertions(+), 27 deletions(-) diff --git a/appconf/appconf_default.h b/appconf/appconf_default.h index 68f032f3..a07ef73a 100644 --- a/appconf/appconf_default.h +++ b/appconf/appconf_default.h @@ -276,5 +276,9 @@ #ifndef APPCONF_BALANCE_START_DELAY #define APPCONF_BALANCE_START_DELAY 15 #endif +#ifndef APPCONF_BALANCE_LOOP_DELAY +#define APPCONF_BALANCE_LOOP_DELAY 5 +#endif + #endif /* APPCONF_DEFAULT_H_ */ diff --git a/applications/app.h b/applications/app.h index 65bfe5ab..a458a13a 100644 --- a/applications/app.h +++ b/applications/app.h @@ -60,7 +60,9 @@ void app_balance_configure(balance_config *conf); float app_balance_get_pid_output(void); float app_balance_get_pitch(void); float app_balance_get_roll(void); +float app_balance_get_diff_time(void); float app_balance_get_motor_current(void); +float app_balance_get_motor_position(void); void app_balance_terminal_rpy(int argc, const char **argv); // Custom apps diff --git a/applications/app_balance.c b/applications/app_balance.c index 5492ecc0..0b8f366c 100644 --- a/applications/app_balance.c +++ b/applications/app_balance.c @@ -39,29 +39,21 @@ static thread_t *app_thread; static bool registered_terminal = false; // Values used in loop -static double pitch; -static double error, error_average, error_change; -static double last_error; +static double pitch, roll; +static double proportional, integral, derivative; +static double last_proportional; static double pid_value; +static systime_t current_time, last_time, diff_time; // Values read to pass in app data to GUI static double motor_current; +static double motor_position; void app_balance_terminal_rpy(int argc, const char **argv) { if (argc != 4){ commands_printf("PID values NOT set!"); return; } - // float _p = 0, _i = 0, _d = 0; - // sscanf(argv[1], "%f", &_p); - // sscanf(argv[2], "%f", &_i); - // sscanf(argv[2], "%f", &_d); - // - // p = _p; - // i = _i; - // d = _d; - // - // commands_printf("PID values set :) %.2f %.2f %.2f - %.2f %.2f %.2f", _p, _i, _d, p, i, d); } void app_balance_configure(balance_config *conf) { @@ -83,11 +75,16 @@ void app_balance_start(void) { // Reset all Values pitch = 0; - error = 0; - error_average = 0; - error_change = 0; - last_error = 0; + roll = 0; + proportional = 0; + integral = 0; + derivative = 0; + last_proportional = 0; pid_value = 0; + current_time = NULL; + last_time = NULL; + diff_time = NULL; + // Start the example thread app_thread = chThdCreateStatic(example_thread_wa, sizeof(example_thread_wa), NORMALPRIO, example_thread, NULL); } @@ -108,11 +105,21 @@ float app_balance_get_pitch(void) { return pitch; } float app_balance_get_roll(void) { - return config.ki; + return roll; +} +uint32_t app_balance_get_diff_time(void) { + if(diff_time != NULL){ + return ST2US(diff_time); + }else{ + return 0; + } } float app_balance_get_motor_current(void) { return motor_current; } +float app_balance_get_motor_position(void) { + return motor_position; +} static THD_FUNCTION(example_thread, arg) { (void)arg; @@ -125,23 +132,48 @@ static THD_FUNCTION(example_thread, arg) { while (!chThdShouldTerminateX()) { + // Update times + current_time = chVTGetSystemTimeX(); + if(last_time == NULL){ + 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(); + // Read gyro values pitch = (double)(imu_get_pitch() * 180.0 / M_PI); + roll = (double)(imu_get_roll() * 180.0 / M_PI); - error = config.pitch_offset - pitch; - error_average = error_average + error; - error_change = error - last_error; + // Apply offsets + pitch = fmod(((pitch + 180.0) + config.pitch_offset), 360.0) - 180.0; + roll = fmod(((roll + 180.0) + config.roll_offset), 360.0) - 180.0; - pid_value = (config.kp*error) + (config.ki*error_average) + (config.kd*error_change); + // Do PID maths + proportional = 0 - pitch; + integral = integral + proportional; + derivative = proportional - last_proportional; - last_error = error; + pid_value = (config.kp * proportional) + (config.ki * integral) + (config.kd * derivative); - // commands_printf("euc_pid values %.4f %.4f - %.2f %.2f %.2f - %.2f %.2f %.2f", pitch, pid_value, p, i, d, error, error_average, error_change); + last_proportional = proportional; + + // Try to fix wierdness + if(pid_value >= 0 && pid_value < 0.01){ + pid_value = 0.01; + } + if(pid_value < 0 && pid_value > -0.01){ + pid_value = -0.01; + } + + // Output to motor mc_interface_set_current(pid_value); - // Run this loop at 1000Hz - chThdSleepMilliseconds(5); + + // Delay between loops + chThdSleepMilliseconds(config.loop_delay); // Reset the timeout timeout_reset(); diff --git a/commands.c b/commands.c index bd7f5e57..58d9da3b 100644 --- a/commands.c +++ b/commands.c @@ -509,7 +509,9 @@ void commands_process_packet(unsigned char *data, unsigned int len, 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_pitch() * 1000000.0), &ind); buffer_append_int32(send_buffer, (int32_t)(app_balance_get_roll() * 1000000.0), &ind); + buffer_append_int32(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); reply_func(send_buffer, ind); } break; diff --git a/confgenerator.c b/confgenerator.c index 7fcfede8..52cad0d1 100644 --- a/confgenerator.c +++ b/confgenerator.c @@ -225,6 +225,7 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration buffer_append_float32_auto(buffer, conf->app_balance_conf.pitch_fault, &ind); buffer_append_float32_auto(buffer, conf->app_balance_conf.roll_fault, &ind); buffer[ind++] = (uint8_t)conf->app_balance_conf.start_delay; + buffer[ind++] = (uint8_t)conf->app_balance_conf.loop_delay; return ind; } @@ -456,6 +457,7 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration conf->app_balance_conf.pitch_fault = buffer_get_float32_auto(buffer, &ind); conf->app_balance_conf.roll_fault = buffer_get_float32_auto(buffer, &ind); conf->app_balance_conf.start_delay = buffer[ind++]; + conf->app_balance_conf.loop_delay = buffer[ind++]; return true; } @@ -671,4 +673,5 @@ void confgenerator_set_defaults_appconf(app_configuration *conf) { conf->app_balance_conf.pitch_fault = APPCONF_BALANCE_PITCH_FAULT; conf->app_balance_conf.roll_fault = APPCONF_BALANCE_ROLL_FAULT; conf->app_balance_conf.start_delay = APPCONF_BALANCE_START_DELAY; + conf->app_balance_conf.loop_delay = APPCONF_BALANCE_LOOP_DELAY; } diff --git a/confgenerator.h b/confgenerator.h index c7eb7f26..85fd0eea 100644 --- a/confgenerator.h +++ b/confgenerator.h @@ -9,7 +9,7 @@ // Constants #define MCCONF_SIGNATURE 503309878 -#define APPCONF_SIGNATURE 2534075884 +#define APPCONF_SIGNATURE 3135150894 // Functions int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf); diff --git a/datatypes.h b/datatypes.h index b08a448b..bc31dae6 100644 --- a/datatypes.h +++ b/datatypes.h @@ -493,6 +493,7 @@ typedef struct { float pitch_fault; float roll_fault; uint8_t start_delay; + uint8_t loop_delay; } balance_config; // CAN status modes From 15703429f5e97a0c8a9b2ce210d074cba541ddc4 Mon Sep 17 00:00:00 2001 From: Mitch Lustig Date: Sun, 4 Aug 2019 11:47:23 -0700 Subject: [PATCH 04/31] Add gyro tuning functionality & state machine logic --- appconf/appconf_default.h | 50 +++++++++------- applications/app_balance.c | 113 ++++++++++++++++++++++++------------- confgenerator.c | 24 ++++++-- confgenerator.h | 2 +- datatypes.h | 12 ++-- imu/ahrs.c | 62 ++++++++++++++------ imu/ahrs.h | 16 ++++++ 7 files changed, 191 insertions(+), 88 deletions(-) diff --git a/appconf/appconf_default.h b/appconf/appconf_default.h index a07ef73a..da605d65 100644 --- a/appconf/appconf_default.h +++ b/appconf/appconf_default.h @@ -253,31 +253,43 @@ // Balance app #ifndef APPCONF_BALANCE_KP -#define APPCONF_BALANCE_KP 1.45 +#define APPCONF_BALANCE_KP 0.0 #endif #ifndef APPCONF_BALANCE_KI -#define APPCONF_BALANCE_KI 0.78 +#define APPCONF_BALANCE_KI 0.0 #endif #ifndef APPCONF_BALANCE_KD -#define APPCONF_BALANCE_KD 2.85 -#endif -#ifndef APPCONF_BALANCE_PITCH_OFFSET -#define APPCONF_BALANCE_PITCH_OFFSET 0 -#endif -#ifndef APPCONF_BALANCE_ROLL_OFFSET -#define APPCONF_BALANCE_ROLL_OFFSET 0 -#endif -#ifndef APPCONF_BALANCE_PITCH_FAULT -#define APPCONF_BALANCE_PITCH_FAULT 45 -#endif -#ifndef APPCONF_BALANCE_ROLL_FAULT -#define APPCONF_BALANCE_ROLL_FAULT 45 -#endif -#ifndef APPCONF_BALANCE_START_DELAY -#define APPCONF_BALANCE_START_DELAY 15 +#define APPCONF_BALANCE_KD 0.0 #endif #ifndef APPCONF_BALANCE_LOOP_DELAY -#define APPCONF_BALANCE_LOOP_DELAY 5 +#define APPCONF_BALANCE_LOOP_DELAY 5 +#endif +#ifndef APPCONF_BALANCE_M_ACD +#define APPCONF_BALANCE_M_ACD 1.0 +#endif +#ifndef APPCONF_BALANCE_M_B +#define APPCONF_BALANCE_M_B 0.1 +#endif +#ifndef APPCONF_BALANCE_CAL_DELAY +#define APPCONF_BALANCE_CAL_DELAY 1500 +#endif +#ifndef APPCONF_BALANCE_CAL_M_ACD +#define APPCONF_BALANCE_CAL_M_ACD 1.0 +#endif +#ifndef APPCONF_BALANCE_CAL_M_B +#define APPCONF_BALANCE_CAL_M_B 0.9 +#endif +#ifndef APPCONF_BALANCE_PITCH_OFFSET +#define APPCONF_BALANCE_PITCH_OFFSET 0 +#endif +#ifndef APPCONF_BALANCE_ROLL_OFFSET +#define APPCONF_BALANCE_ROLL_OFFSET 0 +#endif +#ifndef APPCONF_BALANCE_PITCH_FAULT +#define APPCONF_BALANCE_PITCH_FAULT 45 +#endif +#ifndef APPCONF_BALANCE_ROLL_FAULT +#define APPCONF_BALANCE_ROLL_FAULT 45 #endif diff --git a/applications/app_balance.c b/applications/app_balance.c index 0b8f366c..2e35e9dc 100644 --- a/applications/app_balance.c +++ b/applications/app_balance.c @@ -26,9 +26,17 @@ #include "timeout.h" // To reset the timeout #include "commands.h" #include "imu/imu.h" +#include "imu/ahrs.h" #include +// Data type +typedef enum { + CALIBRATING = 0, + RUNNING, + FAULT +} BalanceState; + // Example thread static THD_FUNCTION(example_thread, arg); static THD_WORKING_AREA(example_thread_wa, 2048); // 2kb stack for this thread @@ -39,11 +47,12 @@ static thread_t *app_thread; static bool registered_terminal = false; // Values used in loop +static BalanceState state; static double pitch, roll; static double proportional, integral, derivative; static double last_proportional; static double pid_value; -static systime_t current_time, last_time, diff_time; +static systime_t current_time, last_time, diff_time, cal_start_time, cal_diff_time; // Values read to pass in app data to GUI static double motor_current; @@ -84,6 +93,9 @@ void app_balance_start(void) { current_time = NULL; last_time = NULL; diff_time = NULL; + cal_start_time = NULL; + cal_diff_time = NULL; + state = CALIBRATING; // Start the example thread app_thread = chThdCreateStatic(example_thread_wa, sizeof(example_thread_wa), NORMALPRIO, example_thread, NULL); @@ -123,57 +135,78 @@ float app_balance_get_motor_position(void) { static THD_FUNCTION(example_thread, arg) { (void)arg; - chRegSetThreadName("APP_EXAMPLE"); - - commands_printf("Custom app thread"); - - chThdSleepSeconds(config.start_delay); - + commands_printf("Custom app thread"); +// chThdSleepSeconds(config.start_delay); while (!chThdShouldTerminateX()) { - // Update times - current_time = chVTGetSystemTimeX(); - if(last_time == NULL){ - last_time = current_time; - } - diff_time = current_time - last_time; - last_time = current_time; + // Update times + current_time = chVTGetSystemTimeX(); + if(last_time == NULL){ + 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(); + // Read values for GUI + motor_current = mc_interface_get_tot_current_directional_filtered(); + motor_position = mc_interface_get_pid_pos_now(); - // Read gyro values - pitch = (double)(imu_get_pitch() * 180.0 / M_PI); - roll = (double)(imu_get_roll() * 180.0 / M_PI); + // Read gyro values + pitch = (double)(imu_get_pitch() * 180.0 / M_PI); +// roll = (double)(imu_get_roll() * 180.0 / M_PI); +// roll = madgwick_beta; + roll = ahrs_get_madgwick_beta(); - // Apply offsets - pitch = fmod(((pitch + 180.0) + config.pitch_offset), 360.0) - 180.0; - roll = fmod(((roll + 180.0) + config.roll_offset), 360.0) - 180.0; + // Apply offsets + pitch = fmod(((pitch + 180.0) + config.pitch_offset), 360.0) - 180.0; +// roll = fmod(((roll + 180.0) + config.roll_offset), 360.0) - 180.0; - // Do PID maths - proportional = 0 - pitch; - integral = integral + proportional; - derivative = proportional - last_proportional; + // State based logic + switch(state){ + case (CALIBRATING): + if(cal_start_time == NULL){ + cal_start_time = current_time; + ahrs_set_madgwick_acc_confidence_decay(config.cal_m_acd); + ahrs_set_madgwick_beta(config.cal_m_b); + } + cal_diff_time = current_time - cal_start_time; + if(ST2MS(cal_diff_time) > config.cal_delay){ + state = RUNNING; + cal_start_time = NULL; + cal_diff_time = NULL; - pid_value = (config.kp * proportional) + (config.ki * integral) + (config.kd * derivative); + ahrs_set_madgwick_acc_confidence_decay(config.m_acd); + ahrs_set_madgwick_beta(config.m_b); + } + break; + case (RUNNING): + // Do PID maths + proportional = 0 - pitch; + integral = integral + proportional; + derivative = proportional - last_proportional; - last_proportional = proportional; + pid_value = (config.kp * proportional) + (config.ki * integral) + (config.kd * derivative); - // Try to fix wierdness - if(pid_value >= 0 && pid_value < 0.01){ - pid_value = 0.01; - } - if(pid_value < 0 && pid_value > -0.01){ - pid_value = -0.01; - } + last_proportional = proportional; - // Output to motor - mc_interface_set_current(pid_value); + // Try to fix wierdness + if(pid_value >= 0 && pid_value < 0.01){ + pid_value = 0.01; + } + if(pid_value < 0 && pid_value > -0.01){ + pid_value = -0.01; + } - // Delay between loops - chThdSleepMilliseconds(config.loop_delay); + // Output to motor + mc_interface_set_current(pid_value); + break; + case (FAULT): + break; + } + + // Delay between loops + chThdSleepMilliseconds(config.loop_delay); // Reset the timeout timeout_reset(); diff --git a/confgenerator.c b/confgenerator.c index 52cad0d1..9dbd8caa 100644 --- a/confgenerator.c +++ b/confgenerator.c @@ -220,12 +220,16 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration 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[ind++] = (uint8_t)conf->app_balance_conf.loop_delay; + buffer_append_float32_auto(buffer, conf->app_balance_conf.m_acd, &ind); + buffer_append_float32_auto(buffer, conf->app_balance_conf.m_b, &ind); + buffer_append_uint32(buffer, conf->app_balance_conf.cal_delay, &ind); + buffer_append_float32_auto(buffer, conf->app_balance_conf.cal_m_acd, &ind); + buffer_append_float32_auto(buffer, conf->app_balance_conf.cal_m_b, &ind); buffer_append_float32_auto(buffer, conf->app_balance_conf.pitch_offset, &ind); buffer_append_float32_auto(buffer, conf->app_balance_conf.roll_offset, &ind); buffer_append_float32_auto(buffer, conf->app_balance_conf.pitch_fault, &ind); buffer_append_float32_auto(buffer, conf->app_balance_conf.roll_fault, &ind); - buffer[ind++] = (uint8_t)conf->app_balance_conf.start_delay; - buffer[ind++] = (uint8_t)conf->app_balance_conf.loop_delay; return ind; } @@ -452,12 +456,16 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration 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.loop_delay = buffer[ind++]; + conf->app_balance_conf.m_acd = buffer_get_float32_auto(buffer, &ind); + conf->app_balance_conf.m_b = buffer_get_float32_auto(buffer, &ind); + conf->app_balance_conf.cal_delay = buffer_get_uint32(buffer, &ind); + conf->app_balance_conf.cal_m_acd = buffer_get_float32_auto(buffer, &ind); + conf->app_balance_conf.cal_m_b = buffer_get_float32_auto(buffer, &ind); conf->app_balance_conf.pitch_offset = buffer_get_float32_auto(buffer, &ind); conf->app_balance_conf.roll_offset = buffer_get_float32_auto(buffer, &ind); conf->app_balance_conf.pitch_fault = buffer_get_float32_auto(buffer, &ind); conf->app_balance_conf.roll_fault = buffer_get_float32_auto(buffer, &ind); - conf->app_balance_conf.start_delay = buffer[ind++]; - conf->app_balance_conf.loop_delay = buffer[ind++]; return true; } @@ -668,10 +676,14 @@ void confgenerator_set_defaults_appconf(app_configuration *conf) { 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.loop_delay = APPCONF_BALANCE_LOOP_DELAY; + conf->app_balance_conf.m_acd = APPCONF_BALANCE_M_ACD; + conf->app_balance_conf.m_b = APPCONF_BALANCE_M_B; + conf->app_balance_conf.cal_delay = APPCONF_BALANCE_CAL_DELAY; + conf->app_balance_conf.cal_m_acd = APPCONF_BALANCE_CAL_M_ACD; + conf->app_balance_conf.cal_m_b = APPCONF_BALANCE_CAL_M_B; conf->app_balance_conf.pitch_offset = APPCONF_BALANCE_PITCH_OFFSET; conf->app_balance_conf.roll_offset = APPCONF_BALANCE_ROLL_OFFSET; conf->app_balance_conf.pitch_fault = APPCONF_BALANCE_PITCH_FAULT; conf->app_balance_conf.roll_fault = APPCONF_BALANCE_ROLL_FAULT; - conf->app_balance_conf.start_delay = APPCONF_BALANCE_START_DELAY; - conf->app_balance_conf.loop_delay = APPCONF_BALANCE_LOOP_DELAY; } diff --git a/confgenerator.h b/confgenerator.h index 85fd0eea..03c0ea8e 100644 --- a/confgenerator.h +++ b/confgenerator.h @@ -9,7 +9,7 @@ // Constants #define MCCONF_SIGNATURE 503309878 -#define APPCONF_SIGNATURE 3135150894 +#define APPCONF_SIGNATURE 384122670 // Functions int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf); diff --git a/datatypes.h b/datatypes.h index bc31dae6..f1addc2f 100644 --- a/datatypes.h +++ b/datatypes.h @@ -488,12 +488,16 @@ typedef struct { float kp; float ki; float kd; - float pitch_offset; - float roll_offset; + uint8_t loop_delay; + float m_acd; + float m_b; + uint32_t cal_delay; + float cal_m_acd; + float cal_m_b; + float pitch_offset; + float roll_offset; float pitch_fault; float roll_fault; - uint8_t start_delay; - uint8_t loop_delay; } balance_config; // CAN status modes diff --git a/imu/ahrs.c b/imu/ahrs.c index 919104b7..070c1b2c 100644 --- a/imu/ahrs.c +++ b/imu/ahrs.c @@ -18,11 +18,11 @@ #include "utils.h" #include -// Settings -#define TWO_KP (2.0f * 0.3f) // 2 * proportional gain -#define TWO_KI (2.0f * 0.0f) // 2 * integral gain -#define ACC_CONFIDENCE_DECAY (1.0) -#define BETA (0.1f) // 2 * proportional gain?? +//// Settings +//#define TWO_KP (2.0f * 0.3f) // 2 * proportional gain +//#define TWO_KI (2.0f * 0.0f) // 2 * integral gain +//#define ACC_CONFIDENCE_DECAY (1.0) +//#define BETA (0.1f) // 2 * proportional gain?? // Private functions static float invSqrt(float x); @@ -37,7 +37,7 @@ static float calculateAccConfidence(float accMag, float *accMagP) { accMag = *accMagP * 0.9f + accMag * 0.1f; *accMagP = accMag; - confidence = 1.0 - (ACC_CONFIDENCE_DECAY * sqrtf(fabsf(accMag - 1.0f))); + confidence = 1.0 - (madgwickAccConfidenceDecay * sqrtf(fabsf(accMag - 1.0f))); utils_truncate_number(&confidence, 0.0, 1.0); return confidence; @@ -131,8 +131,8 @@ void ahrs_update_mahony(float *gyroXYZ, float *accelXYZ, float *magXYZ, float dt float halfex, halfey, halfez; float accelConfidence; - volatile float twoKp = TWO_KP; - volatile float twoKi = TWO_KI; + volatile float twoKp = mahonyKp; + volatile float twoKi = mahonyKi; accelConfidence = calculateAccConfidence(accelNorm, &att->accMagP); twoKp *= accelConfidence; @@ -242,8 +242,8 @@ void ahrs_update_mahony_imu(float *gyroXYZ, float *accelXYZ, float dt, ATTITUDE_ float halfex, halfey, halfez; float accelConfidence; - volatile float twoKp = TWO_KP; - volatile float twoKi = TWO_KI; + volatile float twoKp = mahonyKp; + volatile float twoKi = mahonyKi; accelConfidence = calculateAccConfidence(accelNorm, &att->accMagP); twoKp *= accelConfidence; @@ -405,10 +405,10 @@ void ahrs_update_madgwick(float *gyroXYZ, float *accelXYZ, float *magXYZ, float // Apply feedback step accelConfidence = calculateAccConfidence(accelNorm, &att->accMagP); - qDot1 -= BETA * s0 * accelConfidence; - qDot2 -= BETA * s1 * accelConfidence; - qDot3 -= BETA * s2 * accelConfidence; - qDot4 -= BETA * s3 * accelConfidence; + qDot1 -= madgwickBeta * s0 * accelConfidence; + qDot2 -= madgwickBeta * s1 * accelConfidence; + qDot3 -= madgwickBeta * s2 * accelConfidence; + qDot4 -= madgwickBeta * s3 * accelConfidence; } // Integrate rate of change of quaternion to yield quaternion @@ -496,10 +496,10 @@ void ahrs_update_madgwick_imu(float *gyroXYZ, float *accelXYZ, float dt, ATTITUD // Apply feedback step accelConfidence = calculateAccConfidence(accelNorm, &att->accMagP); - qDot1 -= BETA * s0 * accelConfidence; - qDot2 -= BETA * s1 * accelConfidence; - qDot3 -= BETA * s2 * accelConfidence; - qDot4 -= BETA * s3 * accelConfidence; + qDot1 -= madgwickBeta * s0 * accelConfidence; + qDot2 -= madgwickBeta * s1 * accelConfidence; + qDot3 -= madgwickBeta * s2 * accelConfidence; + qDot4 -= madgwickBeta * s3 * accelConfidence; } // Integrate rate of change of quaternion to yield quaternion @@ -560,6 +560,32 @@ void ahrs_get_roll_pitch_yaw(float *rpy, ATTITUDE_INFO *att) { rpy[2] = -atan2f(q0 * q3 + q1 * q2, 0.5 - (q2 * q2 + q3 * q3)); } +float ahrs_get_mahony_kp(){ + return mahonyKp; +} +float ahrs_get_mahony_ki(){ + return mahonyKi; +} +float ahrs_get_madgwick_acc_confidence_decay(){ + return madgwickAccConfidenceDecay; +} +float ahrs_get_madgwick_beta(){ + return madgwickBeta; +} + +float ahrs_set_mahony_kp(float kp){ + mahonyKp = kp; +} +float ahrs_set_mahony_ki(float ki){ + mahonyKi = ki; +} +float ahrs_set_madgwick_acc_confidence_decay(float accConfidenceDecay){ + madgwickAccConfidenceDecay = accConfidenceDecay; +} +float ahrs_set_madgwick_beta(float beta){ + madgwickBeta = beta; +} + static float invSqrt(float x) { // Fast inverse square-root // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root diff --git a/imu/ahrs.h b/imu/ahrs.h index 02ee484e..cfc99bb3 100644 --- a/imu/ahrs.h +++ b/imu/ahrs.h @@ -15,6 +15,12 @@ #include "conf_general.h" +// Settings +static float mahonyKp = (2.0f * 0.3f); // 2 * proportional gain +static float mahonyKi = (2.0f * 0.0f); // 2 * integral gain +static float madgwickAccConfidenceDecay = (1.0); +static float madgwickBeta = (0.5f); // 2 * proportional gain?? + // Function declarations void ahrs_init_attitude_info(ATTITUDE_INFO *att); void ahrs_update_initial_orientation(float *accelXYZ, float *magXYZ, ATTITUDE_INFO *att); @@ -30,4 +36,14 @@ float ahrs_get_pitch(ATTITUDE_INFO *att); float ahrs_get_yaw(ATTITUDE_INFO *att); void ahrs_get_roll_pitch_yaw(float *rpy, ATTITUDE_INFO *att); +float ahrs_get_mahony_kp(); +float ahrs_get_mahony_ki(); +float ahrs_get_madgwick_acc_confidence_decay(); +float ahrs_get_madgwick_beta(); + +float ahrs_set_mahony_kp(float mahony_kp); +float ahrs_set_mahony_ki(float mahony_ki); +float ahrs_set_madgwick_acc_confidence_decay(float madgwick_acc_confidence_decay); +float ahrs_set_madgwick_beta(float madgwick_beta); + #endif From bf566fe21af2713ffa3422d818becb868f48384d Mon Sep 17 00:00:00 2001 From: Mitch Lustig Date: Sun, 4 Aug 2019 23:13:35 -0700 Subject: [PATCH 05/31] Add fault, smooth startup, and over speed tilt back safety --- applications/app_balance.c | 89 ++++++++++++++++++++++++++++++-------- 1 file changed, 71 insertions(+), 18 deletions(-) diff --git a/applications/app_balance.c b/applications/app_balance.c index 2e35e9dc..9252d8c1 100644 --- a/applications/app_balance.c +++ b/applications/app_balance.c @@ -37,6 +37,11 @@ typedef enum { FAULT } BalanceState; +typedef enum { + SLOW = 40, + FAST = 5 +} SetpointAdjustmentSpeed; + // Example thread static THD_FUNCTION(example_thread, arg); static THD_WORKING_AREA(example_thread_wa, 2048); // 2kb stack for this thread @@ -52,7 +57,11 @@ static double pitch, roll; static double proportional, integral, derivative; static double last_proportional; static double pid_value; -static systime_t current_time, last_time, diff_time, cal_start_time, cal_diff_time; +static double setpoint, setpoint_target; +static SetpointAdjustmentSpeed setpointAdjustmentSpeed; +static systime_t current_time, last_time, diff_time; +static systime_t cal_start_time, cal_diff_time; +static systime_t last_setpoint_step_time; // Values read to pass in app data to GUI static double motor_current; @@ -83,6 +92,7 @@ void app_balance_start(void) { imu_init(); // Reset all Values + state = CALIBRATING; pitch = 0; roll = 0; proportional = 0; @@ -90,12 +100,15 @@ void app_balance_start(void) { derivative = 0; last_proportional = 0; pid_value = 0; + setpoint = 0; + setpoint_target = 0; + setpointAdjustmentSpeed = SLOW; current_time = NULL; last_time = NULL; diff_time = NULL; cal_start_time = NULL; cal_diff_time = NULL; - state = CALIBRATING; + last_setpoint_step_time = NULL; // Start the example thread app_thread = chThdCreateStatic(example_thread_wa, sizeof(example_thread_wa), NORMALPRIO, example_thread, NULL); @@ -150,17 +163,16 @@ static THD_FUNCTION(example_thread, arg) { // Read values for GUI motor_current = mc_interface_get_tot_current_directional_filtered(); +// motor_current = mc_interface_get_duty_cycle_now(); motor_position = mc_interface_get_pid_pos_now(); // Read gyro values pitch = (double)(imu_get_pitch() * 180.0 / M_PI); -// roll = (double)(imu_get_roll() * 180.0 / M_PI); -// roll = madgwick_beta; - roll = ahrs_get_madgwick_beta(); + roll = (double)(imu_get_roll() * 180.0 / M_PI); // Apply offsets pitch = fmod(((pitch + 180.0) + config.pitch_offset), 360.0) - 180.0; -// roll = fmod(((roll + 180.0) + config.roll_offset), 360.0) - 180.0; + roll = fmod(((roll + 180.0) + config.roll_offset), 360.0) - 180.0; // State based logic switch(state){ @@ -172,7 +184,7 @@ static THD_FUNCTION(example_thread, arg) { } cal_diff_time = current_time - cal_start_time; if(ST2MS(cal_diff_time) > config.cal_delay){ - state = RUNNING; + state = FAULT; cal_start_time = NULL; cal_diff_time = NULL; @@ -181,8 +193,44 @@ static THD_FUNCTION(example_thread, arg) { } break; case (RUNNING): + // Check for fault + if(pitch > config.pitch_fault || pitch < -config.pitch_fault || roll > config.roll_fault || roll < -config.roll_fault){ + state = FAULT; + } + + // Over speed tilt back safety + if(mc_interface_get_duty_cycle_now() > .75){ + setpoint_target = 15; + setpointAdjustmentSpeed = SLOW; + } else if(mc_interface_get_duty_cycle_now() < -.75){ + setpoint_target = -15; + setpointAdjustmentSpeed = SLOW; + }else{ + setpoint_target = 0; + } + + // Adjust setpoint + if(setpoint != setpoint_target){ + // If it is time to take a step + if(last_setpoint_step_time == NULL || ST2MS(current_time - last_setpoint_step_time) > setpointAdjustmentSpeed){ + // If we are less than one step size away, go all the way + if(fabs(setpoint_target - setpoint) < 0.2){ + setpoint = setpoint_target; + }else if (setpoint_target - setpoint > 0){ + setpoint += 0.2; + }else{ + setpoint -= 0.2; + } + + // Update step time + last_setpoint_step_time = current_time; + + + } + } + // Do PID maths - proportional = 0 - pitch; + proportional = setpoint - pitch; integral = integral + proportional; derivative = proportional - last_proportional; @@ -190,26 +238,31 @@ static THD_FUNCTION(example_thread, arg) { last_proportional = proportional; - // Try to fix wierdness - if(pid_value >= 0 && pid_value < 0.01){ - pid_value = 0.01; - } - if(pid_value < 0 && pid_value > -0.01){ - pid_value = -0.01; - } + // Reset the timeout + timeout_reset(); // Output to motor mc_interface_set_current(pid_value); break; case (FAULT): + // Check for running + if(pitch < 30.0 && pitch > -30.0 && roll < 5.0 && roll > -5.0){ + setpoint = pitch; + setpoint_target = 0; + setpointAdjustmentSpeed = FAST; + state = RUNNING; + break; + } + + // Disable output + mc_interface_set_current(0); break; } + + // Delay between loops chThdSleepMilliseconds(config.loop_delay); - - // Reset the timeout - timeout_reset(); } mc_interface_set_current(0); } From 14faa2e593c6de6a924ad7a2e7eed31c54bebb38 Mon Sep 17 00:00:00 2001 From: Mitch Lustig Date: Wed, 7 Aug 2019 00:22:51 -0700 Subject: [PATCH 06/31] Make all features configurable --- appconf/appconf_default.h | 30 +++++- applications/app.h | 3 +- applications/app_balance.c | 213 +++++++++++++++++++++---------------- commands.c | 3 +- datatypes.h | 9 ++ 5 files changed, 159 insertions(+), 99 deletions(-) diff --git a/appconf/appconf_default.h b/appconf/appconf_default.h index da605d65..5b00d396 100644 --- a/appconf/appconf_default.h +++ b/appconf/appconf_default.h @@ -277,7 +277,7 @@ #define APPCONF_BALANCE_CAL_M_ACD 1.0 #endif #ifndef APPCONF_BALANCE_CAL_M_B -#define APPCONF_BALANCE_CAL_M_B 0.9 +#define APPCONF_BALANCE_CAL_M_B 2.0 #endif #ifndef APPCONF_BALANCE_PITCH_OFFSET #define APPCONF_BALANCE_PITCH_OFFSET 0 @@ -291,6 +291,32 @@ #ifndef APPCONF_BALANCE_ROLL_FAULT #define APPCONF_BALANCE_ROLL_FAULT 45 #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_STARTUP_PITCH +#define APPCONF_BALANCE_STARTUP_PITCH 30.0 +#endif +#ifndef APPCONF_BALANCE_STARTUP_ROLL +#define APPCONF_BALANCE_STARTUP_ROLL 5.0 +#endif +#ifndef APPCONF_BALANCE_STARTUP_SPEED +#define APPCONF_BALANCE_STARTUP_SPEED 40.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 #endif /* APPCONF_DEFAULT_H_ */ diff --git a/applications/app.h b/applications/app.h index a458a13a..5990df5a 100644 --- a/applications/app.h +++ b/applications/app.h @@ -60,10 +60,9 @@ void app_balance_configure(balance_config *conf); float app_balance_get_pid_output(void); float app_balance_get_pitch(void); float app_balance_get_roll(void); -float app_balance_get_diff_time(void); +uint32_t app_balance_get_diff_time(void); float app_balance_get_motor_current(void); float app_balance_get_motor_position(void); -void app_balance_terminal_rpy(int argc, const char **argv); // Custom apps void app_custom_start(void); diff --git a/applications/app_balance.c b/applications/app_balance.c index 9252d8c1..0f10da59 100644 --- a/applications/app_balance.c +++ b/applications/app_balance.c @@ -27,6 +27,8 @@ #include "commands.h" #include "imu/imu.h" #include "imu/ahrs.h" +#include "utils.h" + #include @@ -34,13 +36,14 @@ typedef enum { CALIBRATING = 0, RUNNING, - FAULT + FAULT, + DEAD } BalanceState; typedef enum { - SLOW = 40, - FAST = 5 -} SetpointAdjustmentSpeed; + STARTUP = 0, + TILTBACK +} SetpointAdjustmentType; // Example thread static THD_FUNCTION(example_thread, arg); @@ -49,8 +52,6 @@ static THD_WORKING_AREA(example_thread_wa, 2048); // 2kb stack for this thread static volatile balance_config config; static thread_t *app_thread; -static bool registered_terminal = false; - // Values used in loop static BalanceState state; static double pitch, roll; @@ -58,99 +59,108 @@ static double proportional, integral, derivative; static double last_proportional; static double pid_value; static double setpoint, setpoint_target; -static SetpointAdjustmentSpeed setpointAdjustmentSpeed; +static SetpointAdjustmentType setpointAdjustmentType; +static double startup_step_size, tiltback_step_size; static systime_t current_time, last_time, diff_time; static systime_t cal_start_time, cal_diff_time; -static systime_t last_setpoint_step_time; // Values read to pass in app data to GUI static double motor_current; static double motor_position; -void app_balance_terminal_rpy(int argc, const char **argv) { - if (argc != 4){ - commands_printf("PID values NOT set!"); - return; - } -} - void app_balance_configure(balance_config *conf) { - config = *conf; - - if(!registered_terminal){ - terminal_register_command_callback("euc_pid", "Sets PID values!", 0, app_balance_terminal_rpy); - registered_terminal = true; - } + config = *conf; } void app_balance_start(void) { - // Set the UART TX pin as an input with pulldown - // palSetPadMode(HW_UART_TX_PORT, HW_UART_TX_PIN, PAL_MODE_INPUT_PULLDOWN); - commands_printf("Custom app start"); - hw_stop_i2c(); - hw_start_i2c(); - imu_init(); - // Reset all Values - state = CALIBRATING; - pitch = 0; - roll = 0; - proportional = 0; - integral = 0; - derivative = 0; - last_proportional = 0; - pid_value = 0; - setpoint = 0; - setpoint_target = 0; - setpointAdjustmentSpeed = SLOW; - current_time = NULL; - last_time = NULL; - diff_time = NULL; - cal_start_time = NULL; - cal_diff_time = NULL; - last_setpoint_step_time = NULL; + // Reset IMU + hw_stop_i2c(); + hw_start_i2c(); + imu_init(); + + // Reset all Values + state = CALIBRATING; + pitch = 0; + roll = 0; + proportional = 0; + integral = 0; + derivative = 0; + last_proportional = 0; + pid_value = 0; + setpoint = 0; + setpoint_target = 0; + setpointAdjustmentType = STARTUP; + startup_step_size = (config.startup_speed / 1000) * config.loop_delay; + tiltback_step_size = (config.tiltback_speed / 1000) * config.loop_delay; + current_time = NULL; + last_time = NULL; + diff_time = NULL; + cal_start_time = NULL; + cal_diff_time = NULL; // Start the example thread app_thread = chThdCreateStatic(example_thread_wa, sizeof(example_thread_wa), NORMALPRIO, example_thread, NULL); } void app_balance_stop(void) { - commands_printf("Custom app stop"); chThdTerminate(app_thread); - mc_interface_set_current(0); - hw_stop_i2c(); - hw_start_i2c(); - imu_init(); + mc_interface_set_current(0); + hw_stop_i2c(); + hw_start_i2c(); + imu_init(); } float app_balance_get_pid_output(void) { - return pid_value; + return pid_value; } float app_balance_get_pitch(void) { - return pitch; + return pitch; } float app_balance_get_roll(void) { - return roll; + return roll; } uint32_t app_balance_get_diff_time(void) { - if(diff_time != NULL){ - return ST2US(diff_time); - }else{ - return 0; - } + if(diff_time != NULL){ + return ST2US(diff_time); + }else{ + return 0; + } } float app_balance_get_motor_current(void) { - return motor_current; + return motor_current; } float app_balance_get_motor_position(void) { - return motor_position; + return motor_position; +} + +double get_setpoint_adjustment_step_size(){ + switch(setpointAdjustmentType){ + case (STARTUP): + return startup_step_size; + case (TILTBACK): + return tiltback_step_size; + } + return 0; +} + +double apply_deadzone(double error){ + if(config.deadzone == 0){ + return error; + } + + if(error < config.deadzone && error > -config.deadzone){ + return 0; + } else if(error > config.deadzone){ + return error - config.deadzone; + } else { + return error + config.deadzone; + } } static THD_FUNCTION(example_thread, arg) { (void)arg; - chRegSetThreadName("APP_EXAMPLE"); - commands_printf("Custom app thread"); -// chThdSleepSeconds(config.start_delay); + chRegSetThreadName("APP_BALANCE"); while (!chThdShouldTerminateX()) { // Update times @@ -163,7 +173,6 @@ static THD_FUNCTION(example_thread, arg) { // Read values for GUI motor_current = mc_interface_get_tot_current_directional_filtered(); -// motor_current = mc_interface_get_duty_cycle_now(); motor_position = mc_interface_get_pid_pos_now(); // Read gyro values @@ -183,54 +192,59 @@ static THD_FUNCTION(example_thread, arg) { ahrs_set_madgwick_beta(config.cal_m_b); } cal_diff_time = current_time - cal_start_time; + + // Calibration is done if(ST2MS(cal_diff_time) > config.cal_delay){ + + // Set gyro config to be running config + ahrs_set_madgwick_acc_confidence_decay(config.m_acd); + ahrs_set_madgwick_beta(config.m_b); + + // Set fault and wait for valid startup condition state = FAULT; cal_start_time = NULL; cal_diff_time = NULL; - - ahrs_set_madgwick_acc_confidence_decay(config.m_acd); - ahrs_set_madgwick_beta(config.m_b); } break; case (RUNNING): + // Check for overspeed + if(mc_interface_get_duty_cycle_now() > config.overspeed_duty || mc_interface_get_duty_cycle_now() < -config.overspeed_duty){ + state = DEAD; + } + // Check for fault if(pitch > config.pitch_fault || pitch < -config.pitch_fault || roll > config.roll_fault || roll < -config.roll_fault){ state = FAULT; } // Over speed tilt back safety - if(mc_interface_get_duty_cycle_now() > .75){ - setpoint_target = 15; - setpointAdjustmentSpeed = SLOW; - } else if(mc_interface_get_duty_cycle_now() < -.75){ - setpoint_target = -15; - setpointAdjustmentSpeed = SLOW; + if(mc_interface_get_duty_cycle_now() > config.tiltback_duty){ + setpoint_target = config.tiltback_angle; + setpointAdjustmentType = TILTBACK; + } else if(mc_interface_get_duty_cycle_now() < -config.tiltback_duty){ + setpoint_target = -config.tiltback_angle; + setpointAdjustmentType = TILTBACK; }else{ setpoint_target = 0; } // Adjust setpoint if(setpoint != setpoint_target){ - // If it is time to take a step - if(last_setpoint_step_time == NULL || ST2MS(current_time - last_setpoint_step_time) > setpointAdjustmentSpeed){ - // If we are less than one step size away, go all the way - if(fabs(setpoint_target - setpoint) < 0.2){ - setpoint = setpoint_target; - }else if (setpoint_target - setpoint > 0){ - setpoint += 0.2; - }else{ - setpoint -= 0.2; - } - - // Update step time - last_setpoint_step_time = current_time; - - + // If we are less than one step size away, go all the way + if(fabs(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 - pitch; + // Apply deadzone + proportional = apply_deadzone(proportional); + // Resume real PID maths integral = integral + proportional; derivative = proportional - last_proportional; @@ -238,6 +252,13 @@ static THD_FUNCTION(example_thread, arg) { last_proportional = proportional; + // Apply current boost + if(pid_value > 0){ + pid_value += config.current_boost; + }else if(pid_value < 0){ + pid_value -= config.current_boost; + } + // Reset the timeout timeout_reset(); @@ -245,24 +266,28 @@ static THD_FUNCTION(example_thread, arg) { mc_interface_set_current(pid_value); break; case (FAULT): - // Check for running - if(pitch < 30.0 && pitch > -30.0 && roll < 5.0 && roll > -5.0){ + // Check for valid startup position + if(pitch < config.startup_pitch && pitch > -config.startup_pitch && roll < config.startup_roll && roll > -config.startup_roll){ setpoint = pitch; setpoint_target = 0; - setpointAdjustmentSpeed = FAST; + setpointAdjustmentType = STARTUP; state = RUNNING; break; } + // Disable output + mc_interface_set_current(0); + break; + case (DEAD): // Disable output mc_interface_set_current(0); break; } - - // Delay between loops chThdSleepMilliseconds(config.loop_delay); } - mc_interface_set_current(0); + + // Disable output + mc_interface_set_current(0); } diff --git a/commands.c b/commands.c index 58d9da3b..27a4ca9a 100644 --- a/commands.c +++ b/commands.c @@ -509,7 +509,8 @@ void commands_process_packet(unsigned char *data, unsigned int len, 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_pitch() * 1000000.0), &ind); buffer_append_int32(send_buffer, (int32_t)(app_balance_get_roll() * 1000000.0), &ind); - buffer_append_int32(send_buffer, app_balance_get_diff_time(), &ind); +// buffer_append_uint32(send_buffer, (uint32_t)123456789, &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); reply_func(send_buffer, ind); diff --git a/datatypes.h b/datatypes.h index f1addc2f..fdd44cee 100644 --- a/datatypes.h +++ b/datatypes.h @@ -498,6 +498,15 @@ typedef struct { float roll_offset; float pitch_fault; float roll_fault; + float overspeed_duty; + float tiltback_duty; + float tiltback_angle; + float tiltback_speed; + float startup_pitch; + float startup_roll; + float startup_speed; + float deadzone; + float current_boost; } balance_config; // CAN status modes From 2c41dda63801340321601185e656a4bbc2d0e3ad Mon Sep 17 00:00:00 2001 From: Mitch Lustig Date: Thu, 8 Aug 2019 23:39:14 -0700 Subject: [PATCH 07/31] Add confgenerator fix bugs --- applications/app_balance.c | 17 ++++++++++++----- confgenerator.c | 27 +++++++++++++++++++++++++++ confgenerator.h | 2 +- 3 files changed, 40 insertions(+), 6 deletions(-) diff --git a/applications/app_balance.c b/applications/app_balance.c index 0f10da59..bac5cc6d 100644 --- a/applications/app_balance.c +++ b/applications/app_balance.c @@ -91,8 +91,8 @@ void app_balance_start(void) { setpoint = 0; setpoint_target = 0; setpointAdjustmentType = STARTUP; - startup_step_size = (config.startup_speed / 1000) * config.loop_delay; - tiltback_step_size = (config.tiltback_speed / 1000) * config.loop_delay; + startup_step_size = 0; + tiltback_step_size = 0; current_time = NULL; last_time = NULL; diff_time = NULL; @@ -162,6 +162,13 @@ static THD_FUNCTION(example_thread, arg) { (void)arg; chRegSetThreadName("APP_BALANCE"); + // Do one off config + startup_step_size = (config.startup_speed / 1000) * config.loop_delay; + tiltback_step_size = (config.tiltback_speed / 1000) * config.loop_delay; + + state = CALIBRATING; + setpointAdjustmentType = STARTUP; + while (!chThdShouldTerminateX()) { // Update times current_time = chVTGetSystemTimeX(); @@ -231,12 +238,12 @@ static THD_FUNCTION(example_thread, arg) { // Adjust setpoint if(setpoint != setpoint_target){ // If we are less than one step size away, go all the way - if(fabs(setpoint_target - setpoint) < get_setpoint_adjustment_step_size){ + if(fabs(setpoint_target - setpoint) < get_setpoint_adjustment_step_size()){ setpoint = setpoint_target; }else if (setpoint_target - setpoint > 0){ - setpoint += get_setpoint_adjustment_step_size; + setpoint += get_setpoint_adjustment_step_size(); }else{ - setpoint -= get_setpoint_adjustment_step_size; + setpoint -= get_setpoint_adjustment_step_size(); } } diff --git a/confgenerator.c b/confgenerator.c index 9dbd8caa..e377a8de 100644 --- a/confgenerator.c +++ b/confgenerator.c @@ -230,6 +230,15 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration buffer_append_float32_auto(buffer, conf->app_balance_conf.roll_offset, &ind); buffer_append_float32_auto(buffer, conf->app_balance_conf.pitch_fault, &ind); buffer_append_float32_auto(buffer, conf->app_balance_conf.roll_fault, &ind); + 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.startup_pitch, &ind); + buffer_append_float32_auto(buffer, conf->app_balance_conf.startup_roll, &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); return ind; } @@ -466,6 +475,15 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration conf->app_balance_conf.roll_offset = buffer_get_float32_auto(buffer, &ind); conf->app_balance_conf.pitch_fault = buffer_get_float32_auto(buffer, &ind); conf->app_balance_conf.roll_fault = buffer_get_float32_auto(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.startup_pitch = buffer_get_float32_auto(buffer, &ind); + conf->app_balance_conf.startup_roll = 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); return true; } @@ -686,4 +704,13 @@ void confgenerator_set_defaults_appconf(app_configuration *conf) { conf->app_balance_conf.roll_offset = APPCONF_BALANCE_ROLL_OFFSET; conf->app_balance_conf.pitch_fault = APPCONF_BALANCE_PITCH_FAULT; conf->app_balance_conf.roll_fault = APPCONF_BALANCE_ROLL_FAULT; + 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.startup_pitch = APPCONF_BALANCE_STARTUP_PITCH; + conf->app_balance_conf.startup_roll = APPCONF_BALANCE_STARTUP_ROLL; + 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; } diff --git a/confgenerator.h b/confgenerator.h index 03c0ea8e..ddeb6e84 100644 --- a/confgenerator.h +++ b/confgenerator.h @@ -9,7 +9,7 @@ // Constants #define MCCONF_SIGNATURE 503309878 -#define APPCONF_SIGNATURE 384122670 +#define APPCONF_SIGNATURE 2685639154 // Functions int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf); From 7cd25a1252ff5c3e2d11116aab38b7e6501255d7 Mon Sep 17 00:00:00 2001 From: Mitch Lustig Date: Fri, 9 Aug 2019 20:55:58 -0700 Subject: [PATCH 08/31] Send app state --- applications/app.h | 1 + applications/app_balance.c | 3 +++ commands.c | 2 +- 3 files changed, 5 insertions(+), 1 deletion(-) diff --git a/applications/app.h b/applications/app.h index 5990df5a..0ce4a299 100644 --- a/applications/app.h +++ b/applications/app.h @@ -63,6 +63,7 @@ float app_balance_get_roll(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); // Custom apps void app_custom_start(void); diff --git a/applications/app_balance.c b/applications/app_balance.c index bac5cc6d..bdb98831 100644 --- a/applications/app_balance.c +++ b/applications/app_balance.c @@ -133,6 +133,9 @@ float app_balance_get_motor_current(void) { float app_balance_get_motor_position(void) { return motor_position; } +uint16_t app_balance_get_state(void) { + return state; +} double get_setpoint_adjustment_step_size(){ switch(setpointAdjustmentType){ diff --git a/commands.c b/commands.c index 27a4ca9a..c8d600b9 100644 --- a/commands.c +++ b/commands.c @@ -509,10 +509,10 @@ void commands_process_packet(unsigned char *data, unsigned int len, 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_pitch() * 1000000.0), &ind); buffer_append_int32(send_buffer, (int32_t)(app_balance_get_roll() * 1000000.0), &ind); -// buffer_append_uint32(send_buffer, (uint32_t)123456789, &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); reply_func(send_buffer, ind); } break; From 62da9e5b83703384fd0ec0fdd1d37a11e4bb32b9 Mon Sep 17 00:00:00 2001 From: Mitch Lustig Date: Fri, 9 Aug 2019 23:16:20 -0700 Subject: [PATCH 09/31] Better peripheral gyro management --- appconf/appconf_default.h | 3 +++ applications/app_balance.c | 24 +++++++++++++++++------- confgenerator.c | 3 +++ confgenerator.h | 2 +- datatypes.h | 1 + hwconf/hw_60.h | 6 +++--- imu/imu.c | 12 +++++++++--- imu/imu.h | 2 +- main.c | 2 +- 9 files changed, 39 insertions(+), 16 deletions(-) diff --git a/appconf/appconf_default.h b/appconf/appconf_default.h index 5b00d396..a018d68d 100644 --- a/appconf/appconf_default.h +++ b/appconf/appconf_default.h @@ -285,6 +285,9 @@ #ifndef APPCONF_BALANCE_ROLL_OFFSET #define APPCONF_BALANCE_ROLL_OFFSET 0 #endif +#ifndef APPCONF_BALANCE_USE_PERIPHERAL +#define APPCONF_BALANCE_USE_PERIPHERAL false +#endif #ifndef APPCONF_BALANCE_PITCH_FAULT #define APPCONF_BALANCE_PITCH_FAULT 45 #endif diff --git a/applications/app_balance.c b/applications/app_balance.c index bdb98831..c367a39d 100644 --- a/applications/app_balance.c +++ b/applications/app_balance.c @@ -75,9 +75,11 @@ void app_balance_configure(balance_config *conf) { void app_balance_start(void) { // Reset IMU - hw_stop_i2c(); - hw_start_i2c(); - imu_init(); + if(config.use_peripheral){ + hw_stop_i2c(); + hw_start_i2c(); + imu_init(true); + } // Reset all Values state = CALIBRATING; @@ -106,9 +108,13 @@ void app_balance_start(void) { void app_balance_stop(void) { chThdTerminate(app_thread); mc_interface_set_current(0); - hw_stop_i2c(); - hw_start_i2c(); - imu_init(); + + // Reset IMU + if(config.use_peripheral){ + imu_init(false); + hw_stop_i2c(); + hw_start_i2c(); + } } float app_balance_get_pid_output(void) { @@ -273,7 +279,11 @@ static THD_FUNCTION(example_thread, arg) { timeout_reset(); // Output to motor - mc_interface_set_current(pid_value); + if(fabs(pid_value) < 1){ + mc_interface_release_motor(); + }else { + mc_interface_set_current(pid_value); + } break; case (FAULT): // Check for valid startup position diff --git a/confgenerator.c b/confgenerator.c index e377a8de..4ed09f49 100644 --- a/confgenerator.c +++ b/confgenerator.c @@ -228,6 +228,7 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration buffer_append_float32_auto(buffer, conf->app_balance_conf.cal_m_b, &ind); buffer_append_float32_auto(buffer, conf->app_balance_conf.pitch_offset, &ind); buffer_append_float32_auto(buffer, conf->app_balance_conf.roll_offset, &ind); + buffer[ind++] = conf->app_balance_conf.use_peripheral; buffer_append_float32_auto(buffer, conf->app_balance_conf.pitch_fault, &ind); buffer_append_float32_auto(buffer, conf->app_balance_conf.roll_fault, &ind); buffer_append_float32_auto(buffer, conf->app_balance_conf.overspeed_duty, &ind); @@ -473,6 +474,7 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration conf->app_balance_conf.cal_m_b = buffer_get_float32_auto(buffer, &ind); conf->app_balance_conf.pitch_offset = buffer_get_float32_auto(buffer, &ind); conf->app_balance_conf.roll_offset = buffer_get_float32_auto(buffer, &ind); + conf->app_balance_conf.use_peripheral = buffer[ind++]; conf->app_balance_conf.pitch_fault = buffer_get_float32_auto(buffer, &ind); conf->app_balance_conf.roll_fault = buffer_get_float32_auto(buffer, &ind); conf->app_balance_conf.overspeed_duty = buffer_get_float32_auto(buffer, &ind); @@ -702,6 +704,7 @@ void confgenerator_set_defaults_appconf(app_configuration *conf) { conf->app_balance_conf.cal_m_b = APPCONF_BALANCE_CAL_M_B; conf->app_balance_conf.pitch_offset = APPCONF_BALANCE_PITCH_OFFSET; conf->app_balance_conf.roll_offset = APPCONF_BALANCE_ROLL_OFFSET; + conf->app_balance_conf.use_peripheral = APPCONF_BALANCE_USE_PERIPHERAL; conf->app_balance_conf.pitch_fault = APPCONF_BALANCE_PITCH_FAULT; conf->app_balance_conf.roll_fault = APPCONF_BALANCE_ROLL_FAULT; conf->app_balance_conf.overspeed_duty = APPCONF_BALANCE_OVERSPEED_DUTY; diff --git a/confgenerator.h b/confgenerator.h index ddeb6e84..012feb35 100644 --- a/confgenerator.h +++ b/confgenerator.h @@ -9,7 +9,7 @@ // Constants #define MCCONF_SIGNATURE 503309878 -#define APPCONF_SIGNATURE 2685639154 +#define APPCONF_SIGNATURE 3625267250 // Functions int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf); diff --git a/datatypes.h b/datatypes.h index fdd44cee..44b85f98 100644 --- a/datatypes.h +++ b/datatypes.h @@ -496,6 +496,7 @@ typedef struct { float cal_m_b; float pitch_offset; float roll_offset; + bool use_peripheral; float pitch_fault; float roll_fault; float overspeed_duty; diff --git a/hwconf/hw_60.h b/hwconf/hw_60.h index 10f4b5cb..b43ad0eb 100644 --- a/hwconf/hw_60.h +++ b/hwconf/hw_60.h @@ -218,9 +218,9 @@ // MPU9250 #define MPU9X50_SDA_GPIO GPIOB -#define MPU9X50_SDA_PIN 11 -#define MPU9X50_SCL_GPIO GPIOB -#define MPU9X50_SCL_PIN 10 +#define MPU9X50_SDA_PIN 2 +#define MPU9X50_SCL_GPIO GPIOA +#define MPU9X50_SCL_PIN 15 #define IMU_FLIP // Measurement macros diff --git a/imu/imu.c b/imu/imu.c index 14b28f17..f0524f00 100644 --- a/imu/imu.c +++ b/imu/imu.c @@ -40,19 +40,25 @@ static ICM20948_STATE m_icm20948_state; static void imu_read_callback(float *accel, float *gyro, float *mag); static void terminal_rpy(int argc, const char **argv); -void imu_init(void) { +void imu_init(bool use_peripheral) { ahrs_init_attitude_info(&m_att); + if(use_peripheral){ + imu_init_mpu9x50(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN, + HW_I2C_SCL_PORT, HW_I2C_SCL_PIN); + } else { + #ifdef MPU9X50_SDA_GPIO - imu_init_mpu9x50(MPU9X50_SDA_GPIO, MPU9X50_SDA_PIN, + imu_init_mpu9x50(MPU9X50_SDA_GPIO, MPU9X50_SDA_PIN, MPU9X50_SCL_GPIO, MPU9X50_SCL_PIN); #endif #ifdef ICM20948_SDA_GPIO - imu_init_icm20948(ICM20948_SDA_GPIO, ICM20948_SDA_PIN, + imu_init_icm20948(ICM20948_SDA_GPIO, ICM20948_SDA_PIN, ICM20948_SCL_GPIO, ICM20948_SCL_PIN, ICM20948_AD0_VAL); #endif + } terminal_register_command_callback( "imu_rpy", "Print 100 roll/pitch/yaw samples at 10 Hz", diff --git a/imu/imu.h b/imu/imu.h index 1fa1932a..0d57366c 100644 --- a/imu/imu.h +++ b/imu/imu.h @@ -24,7 +24,7 @@ #include "hal.h" #include "i2c_bb.h" -void imu_init(void); +void imu_init(bool use_peripheral); i2c_bb_state *imu_get_i2c(void); void imu_init_mpu9x50(stm32_gpio_t *sda_gpio, int sda_pin, stm32_gpio_t *scl_gpio, int scl_pin); diff --git a/main.c b/main.c index 271a2ce2..0fed5dd1 100644 --- a/main.c +++ b/main.c @@ -325,7 +325,7 @@ int main(void) { timeout_init(); timeout_configure(appconf.timeout_msec, appconf.timeout_brake_current); - imu_init(); + imu_init(false); #if HAS_BLACKMAGIC bm_init(); From 971957a0d1166c300cb5d9c82137b6d680bf2ba8 Mon Sep 17 00:00:00 2001 From: Mitch Lustig Date: Sat, 10 Aug 2019 00:16:48 -0700 Subject: [PATCH 10/31] Fix all compiler warnings --- applications/app_balance.c | 56 ++++++++++++++++++-------------------- imu/ahrs.c | 18 ++++++------ imu/ahrs.h | 22 ++++++--------- 3 files changed, 43 insertions(+), 53 deletions(-) diff --git a/applications/app_balance.c b/applications/app_balance.c index c367a39d..f0c2314b 100644 --- a/applications/app_balance.c +++ b/applications/app_balance.c @@ -54,19 +54,19 @@ static thread_t *app_thread; // Values used in loop static BalanceState state; -static double pitch, roll; -static double proportional, integral, derivative; -static double last_proportional; -static double pid_value; -static double setpoint, setpoint_target; +static float pitch, roll; +static float proportional, integral, derivative; +static float last_proportional; +static float pid_value; +static float setpoint, setpoint_target; static SetpointAdjustmentType setpointAdjustmentType; -static double startup_step_size, tiltback_step_size; +static float startup_step_size, tiltback_step_size; static systime_t current_time, last_time, diff_time; static systime_t cal_start_time, cal_diff_time; // Values read to pass in app data to GUI -static double motor_current; -static double motor_position; +static float motor_current; +static float motor_position; void app_balance_configure(balance_config *conf) { config = *conf; @@ -95,11 +95,11 @@ void app_balance_start(void) { setpointAdjustmentType = STARTUP; startup_step_size = 0; tiltback_step_size = 0; - current_time = NULL; - last_time = NULL; - diff_time = NULL; - cal_start_time = NULL; - cal_diff_time = NULL; + current_time = 0; + last_time = 0; + diff_time = 0; + cal_start_time = 0; + cal_diff_time = 0; // Start the example thread app_thread = chThdCreateStatic(example_thread_wa, sizeof(example_thread_wa), NORMALPRIO, example_thread, NULL); @@ -127,11 +127,7 @@ float app_balance_get_roll(void) { return roll; } uint32_t app_balance_get_diff_time(void) { - if(diff_time != NULL){ - return ST2US(diff_time); - }else{ - return 0; - } + return ST2US(diff_time); } float app_balance_get_motor_current(void) { return motor_current; @@ -143,7 +139,7 @@ uint16_t app_balance_get_state(void) { return state; } -double get_setpoint_adjustment_step_size(){ +float get_setpoint_adjustment_step_size(void){ switch(setpointAdjustmentType){ case (STARTUP): return startup_step_size; @@ -153,7 +149,7 @@ double get_setpoint_adjustment_step_size(){ return 0; } -double apply_deadzone(double error){ +float apply_deadzone(float error){ if(config.deadzone == 0){ return error; } @@ -181,7 +177,7 @@ static THD_FUNCTION(example_thread, arg) { while (!chThdShouldTerminateX()) { // Update times current_time = chVTGetSystemTimeX(); - if(last_time == NULL){ + if(last_time == 0){ last_time = current_time; } diff_time = current_time - last_time; @@ -192,17 +188,17 @@ static THD_FUNCTION(example_thread, arg) { motor_position = mc_interface_get_pid_pos_now(); // Read gyro values - pitch = (double)(imu_get_pitch() * 180.0 / M_PI); - roll = (double)(imu_get_roll() * 180.0 / M_PI); + pitch = imu_get_pitch() * 180.0f / M_PI; + roll = imu_get_roll() * 180.0f / M_PI; // Apply offsets - pitch = fmod(((pitch + 180.0) + config.pitch_offset), 360.0) - 180.0; - roll = fmod(((roll + 180.0) + config.roll_offset), 360.0) - 180.0; + pitch = fmodf(((pitch + 180.0f) + config.pitch_offset), 360.0f) - 180.0f; + roll = fmodf(((roll + 180.0f) + config.roll_offset), 360.0f) - 180.0f; // State based logic switch(state){ case (CALIBRATING): - if(cal_start_time == NULL){ + if(cal_start_time == 0){ cal_start_time = current_time; ahrs_set_madgwick_acc_confidence_decay(config.cal_m_acd); ahrs_set_madgwick_beta(config.cal_m_b); @@ -218,8 +214,8 @@ static THD_FUNCTION(example_thread, arg) { // Set fault and wait for valid startup condition state = FAULT; - cal_start_time = NULL; - cal_diff_time = NULL; + cal_start_time = 0; + cal_diff_time = 0; } break; case (RUNNING): @@ -247,7 +243,7 @@ static THD_FUNCTION(example_thread, arg) { // Adjust setpoint if(setpoint != setpoint_target){ // If we are less than one step size away, go all the way - if(fabs(setpoint_target - setpoint) < get_setpoint_adjustment_step_size()){ + 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(); @@ -279,7 +275,7 @@ static THD_FUNCTION(example_thread, arg) { timeout_reset(); // Output to motor - if(fabs(pid_value) < 1){ + if(pid_value == 0){ mc_interface_release_motor(); }else { mc_interface_set_current(pid_value); diff --git a/imu/ahrs.c b/imu/ahrs.c index 070c1b2c..96d25756 100644 --- a/imu/ahrs.c +++ b/imu/ahrs.c @@ -18,11 +18,11 @@ #include "utils.h" #include -//// Settings -//#define TWO_KP (2.0f * 0.3f) // 2 * proportional gain -//#define TWO_KI (2.0f * 0.0f) // 2 * integral gain -//#define ACC_CONFIDENCE_DECAY (1.0) -//#define BETA (0.1f) // 2 * proportional gain?? +// Settings +static float mahonyKp = (2.0f * 0.3f); // 2 * proportional gain +static float mahonyKi = (2.0f * 0.0f); // 2 * integral gain +static float madgwickAccConfidenceDecay = (1.0); +static float madgwickBeta = (0.5f); // 2 * proportional gain?? // Private functions static float invSqrt(float x); @@ -573,16 +573,16 @@ float ahrs_get_madgwick_beta(){ return madgwickBeta; } -float ahrs_set_mahony_kp(float kp){ +void ahrs_set_mahony_kp(float kp){ mahonyKp = kp; } -float ahrs_set_mahony_ki(float ki){ +void ahrs_set_mahony_ki(float ki){ mahonyKi = ki; } -float ahrs_set_madgwick_acc_confidence_decay(float accConfidenceDecay){ +void ahrs_set_madgwick_acc_confidence_decay(float accConfidenceDecay){ madgwickAccConfidenceDecay = accConfidenceDecay; } -float ahrs_set_madgwick_beta(float beta){ +void ahrs_set_madgwick_beta(float beta){ madgwickBeta = beta; } diff --git a/imu/ahrs.h b/imu/ahrs.h index cfc99bb3..febd23b8 100644 --- a/imu/ahrs.h +++ b/imu/ahrs.h @@ -15,12 +15,6 @@ #include "conf_general.h" -// Settings -static float mahonyKp = (2.0f * 0.3f); // 2 * proportional gain -static float mahonyKi = (2.0f * 0.0f); // 2 * integral gain -static float madgwickAccConfidenceDecay = (1.0); -static float madgwickBeta = (0.5f); // 2 * proportional gain?? - // Function declarations void ahrs_init_attitude_info(ATTITUDE_INFO *att); void ahrs_update_initial_orientation(float *accelXYZ, float *magXYZ, ATTITUDE_INFO *att); @@ -36,14 +30,14 @@ float ahrs_get_pitch(ATTITUDE_INFO *att); float ahrs_get_yaw(ATTITUDE_INFO *att); void ahrs_get_roll_pitch_yaw(float *rpy, ATTITUDE_INFO *att); -float ahrs_get_mahony_kp(); -float ahrs_get_mahony_ki(); -float ahrs_get_madgwick_acc_confidence_decay(); -float ahrs_get_madgwick_beta(); +float ahrs_get_mahony_kp(void); +float ahrs_get_mahony_ki(void); +float ahrs_get_madgwick_acc_confidence_decay(void); +float ahrs_get_madgwick_beta(void); -float ahrs_set_mahony_kp(float mahony_kp); -float ahrs_set_mahony_ki(float mahony_ki); -float ahrs_set_madgwick_acc_confidence_decay(float madgwick_acc_confidence_decay); -float ahrs_set_madgwick_beta(float madgwick_beta); +void ahrs_set_mahony_kp(float mahony_kp); +void ahrs_set_mahony_ki(float mahony_ki); +void ahrs_set_madgwick_acc_confidence_decay(float madgwick_acc_confidence_decay); +void ahrs_set_madgwick_beta(float madgwick_beta); #endif From c3cf302ba3944b5e8042699fb097e1b1462cf72a Mon Sep 17 00:00:00 2001 From: Mitch Lustig Date: Sat, 10 Aug 2019 21:52:21 -0700 Subject: [PATCH 11/31] FIx thread stopping synchronization. FIx gyro config not being applied. --- applications/app.c | 4 +++- applications/app_balance.c | 9 ++++----- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/applications/app.c b/applications/app.c index 5ee013a4..81442633 100644 --- a/applications/app.c +++ b/applications/app.c @@ -65,6 +65,9 @@ void app_set_configuration(app_configuration *conf) { app_custom_stop(); #endif + // Configure balance app before starting it. + app_balance_configure(&appconf.app_balance_conf); + switch (appconf.app_to_use) { case APP_PPM: app_ppm_start(); @@ -121,7 +124,6 @@ void app_set_configuration(app_configuration *conf) { app_adc_configure(&appconf.app_adc_conf); app_uartcomm_configure(appconf.app_uart_baudrate, appconf.permanent_uart_enabled); app_nunchuk_configure(&appconf.app_chuk_conf); - app_balance_configure(&appconf.app_balance_conf); #ifdef APP_CUSTOM_TO_USE app_custom_configure(&appconf); diff --git a/applications/app_balance.c b/applications/app_balance.c index f0c2314b..7ff41341 100644 --- a/applications/app_balance.c +++ b/applications/app_balance.c @@ -76,8 +76,6 @@ void app_balance_start(void) { // Reset IMU if(config.use_peripheral){ - hw_stop_i2c(); - hw_start_i2c(); imu_init(true); } @@ -106,14 +104,15 @@ void app_balance_start(void) { } void app_balance_stop(void) { - chThdTerminate(app_thread); + if(app_thread != NULL){ + chThdTerminate(app_thread); + chThdWait(app_thread); + } mc_interface_set_current(0); // Reset IMU if(config.use_peripheral){ imu_init(false); - hw_stop_i2c(); - hw_start_i2c(); } } From bf27a308ab040c5a8273bd51c9f2882cbb392133 Mon Sep 17 00:00:00 2001 From: Mitch Lustig Date: Sat, 10 Aug 2019 22:00:00 -0700 Subject: [PATCH 12/31] Rename thread --- applications/app_balance.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/applications/app_balance.c b/applications/app_balance.c index 7ff41341..1aa515ed 100644 --- a/applications/app_balance.c +++ b/applications/app_balance.c @@ -45,9 +45,9 @@ typedef enum { TILTBACK } SetpointAdjustmentType; -// Example thread -static THD_FUNCTION(example_thread, arg); -static THD_WORKING_AREA(example_thread_wa, 2048); // 2kb stack for this thread +// 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 config; static thread_t *app_thread; @@ -99,8 +99,8 @@ void app_balance_start(void) { cal_start_time = 0; cal_diff_time = 0; - // Start the example thread - app_thread = chThdCreateStatic(example_thread_wa, sizeof(example_thread_wa), NORMALPRIO, example_thread, NULL); + // Start the balance thread + app_thread = chThdCreateStatic(balance_thread_wa, sizeof(balance_thread_wa), NORMALPRIO, balance_thread, NULL); } void app_balance_stop(void) { @@ -162,7 +162,7 @@ float apply_deadzone(float error){ } } -static THD_FUNCTION(example_thread, arg) { +static THD_FUNCTION(balance_thread, arg) { (void)arg; chRegSetThreadName("APP_BALANCE"); From 05d81a6e66418855d3c61a8265546015c33fcb6d Mon Sep 17 00:00:00 2001 From: Mitch Lustig Date: Sat, 10 Aug 2019 22:01:29 -0700 Subject: [PATCH 13/31] Remove unused config --- hwconf/hw_410.h | 7 ------- 1 file changed, 7 deletions(-) diff --git a/hwconf/hw_410.h b/hwconf/hw_410.h index 9d3ee6d2..b9c4bfa3 100644 --- a/hwconf/hw_410.h +++ b/hwconf/hw_410.h @@ -132,13 +132,6 @@ #define HW_I2C_SDA_PORT GPIOB #define HW_I2C_SDA_PIN 11 -// MPU9250 -#define MPU9X50_SDA_GPIO GPIOB -#define MPU9X50_SDA_PIN 11 -#define MPU9X50_SCL_GPIO GPIOB -#define MPU9X50_SCL_PIN 10 -#define IMU_FLIP - // Hall/encoder pins #define HW_HALL_ENC_GPIO1 GPIOB #define HW_HALL_ENC_PIN1 6 From a2231593b200211098a5e08662f473ea3199e8c0 Mon Sep 17 00:00:00 2001 From: Mitch Lustig Date: Sun, 25 Aug 2019 23:49:33 -0700 Subject: [PATCH 14/31] Start on better gyro config --- appconf/appconf_default.h | 65 ++++++++++++++++++++++++-------------- applications/app.c | 5 ++- applications/app.h | 2 +- applications/app_balance.c | 65 ++++++++++++++++++++------------------ confgenerator.c | 57 +++++++++++++++++++++------------ confgenerator.h | 2 +- datatypes.h | 30 ++++++++++++------ imu/imu.c | 51 +++++++++++++++--------------- imu/imu.h | 2 +- imu/mpu9150.c | 13 ++++---- imu/mpu9150.h | 2 +- main.c | 1 - 12 files changed, 172 insertions(+), 123 deletions(-) diff --git a/appconf/appconf_default.h b/appconf/appconf_default.h index a018d68d..596afaa5 100644 --- a/appconf/appconf_default.h +++ b/appconf/appconf_default.h @@ -261,23 +261,8 @@ #ifndef APPCONF_BALANCE_KD #define APPCONF_BALANCE_KD 0.0 #endif -#ifndef APPCONF_BALANCE_LOOP_DELAY -#define APPCONF_BALANCE_LOOP_DELAY 5 -#endif -#ifndef APPCONF_BALANCE_M_ACD -#define APPCONF_BALANCE_M_ACD 1.0 -#endif -#ifndef APPCONF_BALANCE_M_B -#define APPCONF_BALANCE_M_B 0.1 -#endif -#ifndef APPCONF_BALANCE_CAL_DELAY -#define APPCONF_BALANCE_CAL_DELAY 1500 -#endif -#ifndef APPCONF_BALANCE_CAL_M_ACD -#define APPCONF_BALANCE_CAL_M_ACD 1.0 -#endif -#ifndef APPCONF_BALANCE_CAL_M_B -#define APPCONF_BALANCE_CAL_M_B 2.0 +#ifndef APPCONF_BALANCE_HERTZ +#define APPCONF_BALANCE_HERTZ 1000 #endif #ifndef APPCONF_BALANCE_PITCH_OFFSET #define APPCONF_BALANCE_PITCH_OFFSET 0 @@ -285,11 +270,8 @@ #ifndef APPCONF_BALANCE_ROLL_OFFSET #define APPCONF_BALANCE_ROLL_OFFSET 0 #endif -#ifndef APPCONF_BALANCE_USE_PERIPHERAL -#define APPCONF_BALANCE_USE_PERIPHERAL false -#endif #ifndef APPCONF_BALANCE_PITCH_FAULT -#define APPCONF_BALANCE_PITCH_FAULT 45 +#define APPCONF_BALANCE_PITCH_FAULT 20 #endif #ifndef APPCONF_BALANCE_ROLL_FAULT #define APPCONF_BALANCE_ROLL_FAULT 45 @@ -307,13 +289,13 @@ #define APPCONF_BALANCE_TILTBACK_SPEED 5.0 #endif #ifndef APPCONF_BALANCE_STARTUP_PITCH -#define APPCONF_BALANCE_STARTUP_PITCH 30.0 +#define APPCONF_BALANCE_STARTUP_PITCH 20.0 #endif #ifndef APPCONF_BALANCE_STARTUP_ROLL -#define APPCONF_BALANCE_STARTUP_ROLL 5.0 +#define APPCONF_BALANCE_STARTUP_ROLL 8.0 #endif #ifndef APPCONF_BALANCE_STARTUP_SPEED -#define APPCONF_BALANCE_STARTUP_SPEED 40.0 +#define APPCONF_BALANCE_STARTUP_SPEED 30.0 #endif #ifndef APPCONF_BALANCE_DEADZONE #define APPCONF_BALANCE_DEADZONE 0.0 @@ -322,4 +304,39 @@ #define APPCONF_BALANCE_CURRENT_BOOST 0.0 #endif +// IMU conf +#ifndef APPCONF_IMU_USE_PERIPHERAL +#define APPCONF_IMU_USE_PERIPHERAL false +#endif +#ifndef APPCONF_IMU_PITCH_AXIS +#define APPCONF_IMU_PITCH_AXIS 0 +#endif +#ifndef APPCONF_IMU_ROLL_AXIS +#define APPCONF_IMU_ROLL_AXIS 1 +#endif +#ifndef APPCONF_IMU_YAW_AXIS +#define APPCONF_IMU_YAW_AXIS 2 +#endif +#ifndef APPCONF_IMU_FLIP +#define APPCONF_IMU_FLIP true +#endif +#ifndef APPCONF_IMU_HERTZ +#define APPCONF_IMU_HERTZ 200 +#endif +#ifndef APPCONF_IMU_M_ACD +#define APPCONF_IMU_M_ACD 1.0 +#endif +#ifndef APPCONF_IMU_M_B +#define APPCONF_IMU_M_B 0.1 +#endif +#ifndef APPCONF_IMU_CAL_DELAY +#define APPCONF_IMU_CAL_DELAY 1500 +#endif +#ifndef APPCONF_IMU_CAL_M_ACD +#define APPCONF_IMU_CAL_M_ACD 1.0 +#endif +#ifndef APPCONF_IMU_CAL_M_B +#define APPCONF_IMU_CAL_M_B 2.0 +#endif + #endif /* APPCONF_DEFAULT_H_ */ diff --git a/applications/app.c b/applications/app.c index 81442633..0712ca2b 100644 --- a/applications/app.c +++ b/applications/app.c @@ -65,8 +65,11 @@ void app_set_configuration(app_configuration *conf) { app_custom_stop(); #endif + // Start Gyro + imu_init(&appconf.app_imu_conf); + // Configure balance app before starting it. - app_balance_configure(&appconf.app_balance_conf); + app_balance_configure(&appconf.app_balance_conf, &appconf.app_imu_conf); switch (appconf.app_to_use) { case APP_PPM: diff --git a/applications/app.h b/applications/app.h index 0ce4a299..c366b3d5 100644 --- a/applications/app.h +++ b/applications/app.h @@ -56,7 +56,7 @@ 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); +void app_balance_configure(balance_config *conf, imu_config *conf2); float app_balance_get_pid_output(void); float app_balance_get_pitch(void); float app_balance_get_roll(void); diff --git a/applications/app_balance.c b/applications/app_balance.c index 1aa515ed..98eaadae 100644 --- a/applications/app_balance.c +++ b/applications/app_balance.c @@ -50,6 +50,7 @@ static THD_FUNCTION(balance_thread, arg); static THD_WORKING_AREA(balance_thread_wa, 2048); // 2kb stack for this thread static volatile balance_config config; +static volatile imu_config imu_conf; static thread_t *app_thread; // Values used in loop @@ -68,16 +69,17 @@ static systime_t cal_start_time, cal_diff_time; static float motor_current; static float motor_position; -void app_balance_configure(balance_config *conf) { +void app_balance_configure(balance_config *conf, imu_config *conf2) { config = *conf; + imu_conf = *conf2; } void app_balance_start(void) { - // Reset IMU - if(config.use_peripheral){ - imu_init(true); - } +// // Reset IMU +// if(config.use_peripheral){ +// imu_init(true); +// } // Reset all Values state = CALIBRATING; @@ -110,10 +112,10 @@ void app_balance_stop(void) { } mc_interface_set_current(0); - // Reset IMU - if(config.use_peripheral){ - imu_init(false); - } +// // Reset IMU +// if(config.use_peripheral){ +// imu_init(false); +// } } float app_balance_get_pid_output(void) { @@ -167,8 +169,8 @@ static THD_FUNCTION(balance_thread, arg) { chRegSetThreadName("APP_BALANCE"); // Do one off config - startup_step_size = (config.startup_speed / 1000) * config.loop_delay; - tiltback_step_size = (config.tiltback_speed / 1000) * config.loop_delay; + startup_step_size = config.startup_speed / config.hertz; + tiltback_step_size = config.tiltback_speed / config.hertz; state = CALIBRATING; setpointAdjustmentType = STARTUP; @@ -197,25 +199,26 @@ static THD_FUNCTION(balance_thread, arg) { // State based logic switch(state){ case (CALIBRATING): - if(cal_start_time == 0){ - cal_start_time = current_time; - ahrs_set_madgwick_acc_confidence_decay(config.cal_m_acd); - ahrs_set_madgwick_beta(config.cal_m_b); - } - cal_diff_time = current_time - cal_start_time; - - // Calibration is done - if(ST2MS(cal_diff_time) > config.cal_delay){ - - // Set gyro config to be running config - ahrs_set_madgwick_acc_confidence_decay(config.m_acd); - ahrs_set_madgwick_beta(config.m_b); - - // Set fault and wait for valid startup condition - state = FAULT; - cal_start_time = 0; - cal_diff_time = 0; - } +// if(cal_start_time == 0){ +// cal_start_time = current_time; +// ahrs_set_madgwick_acc_confidence_decay(config.cal_m_acd); +// ahrs_set_madgwick_beta(config.cal_m_b); +// } +// cal_diff_time = current_time - cal_start_time; +// +// // Calibration is done +// if(ST2MS(cal_diff_time) > config.cal_delay){ +// +// // Set gyro config to be running config +// ahrs_set_madgwick_acc_confidence_decay(config.m_acd); +// ahrs_set_madgwick_beta(config.m_b); +// +// // Set fault and wait for valid startup condition +// state = FAULT; +// cal_start_time = 0; +// cal_diff_time = 0; +// } + state = FAULT; break; case (RUNNING): // Check for overspeed @@ -300,7 +303,7 @@ static THD_FUNCTION(balance_thread, arg) { } // Delay between loops - chThdSleepMilliseconds(config.loop_delay); + chThdSleepMicroseconds((int)((1000.0 / config.hertz) * 1000.0)); } // Disable output diff --git a/confgenerator.c b/confgenerator.c index 4ed09f49..e58fd69e 100644 --- a/confgenerator.c +++ b/confgenerator.c @@ -220,15 +220,9 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration 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[ind++] = (uint8_t)conf->app_balance_conf.loop_delay; - buffer_append_float32_auto(buffer, conf->app_balance_conf.m_acd, &ind); - buffer_append_float32_auto(buffer, conf->app_balance_conf.m_b, &ind); - buffer_append_uint32(buffer, conf->app_balance_conf.cal_delay, &ind); - buffer_append_float32_auto(buffer, conf->app_balance_conf.cal_m_acd, &ind); - buffer_append_float32_auto(buffer, conf->app_balance_conf.cal_m_b, &ind); + buffer_append_uint16(buffer, conf->app_balance_conf.hertz, &ind); buffer_append_float32_auto(buffer, conf->app_balance_conf.pitch_offset, &ind); buffer_append_float32_auto(buffer, conf->app_balance_conf.roll_offset, &ind); - buffer[ind++] = conf->app_balance_conf.use_peripheral; buffer_append_float32_auto(buffer, conf->app_balance_conf.pitch_fault, &ind); buffer_append_float32_auto(buffer, conf->app_balance_conf.roll_fault, &ind); buffer_append_float32_auto(buffer, conf->app_balance_conf.overspeed_duty, &ind); @@ -240,6 +234,17 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration 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->app_imu_conf.use_peripheral; + buffer[ind++] = (uint8_t)conf->app_imu_conf.pitch_axis; + buffer[ind++] = (uint8_t)conf->app_imu_conf.roll_axis; + buffer[ind++] = (uint8_t)conf->app_imu_conf.yaw_axis; + buffer[ind++] = conf->app_imu_conf.flip; + buffer_append_uint16(buffer, conf->app_imu_conf.hertz, &ind); + buffer_append_float32_auto(buffer, conf->app_imu_conf.m_acd, &ind); + buffer_append_float32_auto(buffer, conf->app_imu_conf.m_b, &ind); + buffer_append_uint32(buffer, conf->app_imu_conf.cal_delay, &ind); + buffer_append_float32_auto(buffer, conf->app_imu_conf.cal_m_acd, &ind); + buffer_append_float32_auto(buffer, conf->app_imu_conf.cal_m_b, &ind); return ind; } @@ -466,15 +471,9 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration 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.loop_delay = buffer[ind++]; - conf->app_balance_conf.m_acd = buffer_get_float32_auto(buffer, &ind); - conf->app_balance_conf.m_b = buffer_get_float32_auto(buffer, &ind); - conf->app_balance_conf.cal_delay = buffer_get_uint32(buffer, &ind); - conf->app_balance_conf.cal_m_acd = buffer_get_float32_auto(buffer, &ind); - conf->app_balance_conf.cal_m_b = buffer_get_float32_auto(buffer, &ind); + conf->app_balance_conf.hertz = buffer_get_uint16(buffer, &ind); conf->app_balance_conf.pitch_offset = buffer_get_float32_auto(buffer, &ind); conf->app_balance_conf.roll_offset = buffer_get_float32_auto(buffer, &ind); - conf->app_balance_conf.use_peripheral = buffer[ind++]; conf->app_balance_conf.pitch_fault = buffer_get_float32_auto(buffer, &ind); conf->app_balance_conf.roll_fault = buffer_get_float32_auto(buffer, &ind); conf->app_balance_conf.overspeed_duty = buffer_get_float32_auto(buffer, &ind); @@ -486,6 +485,17 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration 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->app_imu_conf.use_peripheral = buffer[ind++]; + conf->app_imu_conf.pitch_axis = buffer[ind++]; + conf->app_imu_conf.roll_axis = buffer[ind++]; + conf->app_imu_conf.yaw_axis = buffer[ind++]; + conf->app_imu_conf.flip = buffer[ind++]; + conf->app_imu_conf.hertz = buffer_get_uint16(buffer, &ind); + conf->app_imu_conf.m_acd = buffer_get_float32_auto(buffer, &ind); + conf->app_imu_conf.m_b = buffer_get_float32_auto(buffer, &ind); + conf->app_imu_conf.cal_delay = buffer_get_uint32(buffer, &ind); + conf->app_imu_conf.cal_m_acd = buffer_get_float32_auto(buffer, &ind); + conf->app_imu_conf.cal_m_b = buffer_get_float32_auto(buffer, &ind); return true; } @@ -696,15 +706,9 @@ void confgenerator_set_defaults_appconf(app_configuration *conf) { 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.loop_delay = APPCONF_BALANCE_LOOP_DELAY; - conf->app_balance_conf.m_acd = APPCONF_BALANCE_M_ACD; - conf->app_balance_conf.m_b = APPCONF_BALANCE_M_B; - conf->app_balance_conf.cal_delay = APPCONF_BALANCE_CAL_DELAY; - conf->app_balance_conf.cal_m_acd = APPCONF_BALANCE_CAL_M_ACD; - conf->app_balance_conf.cal_m_b = APPCONF_BALANCE_CAL_M_B; + conf->app_balance_conf.hertz = APPCONF_BALANCE_HERTZ; conf->app_balance_conf.pitch_offset = APPCONF_BALANCE_PITCH_OFFSET; conf->app_balance_conf.roll_offset = APPCONF_BALANCE_ROLL_OFFSET; - conf->app_balance_conf.use_peripheral = APPCONF_BALANCE_USE_PERIPHERAL; conf->app_balance_conf.pitch_fault = APPCONF_BALANCE_PITCH_FAULT; conf->app_balance_conf.roll_fault = APPCONF_BALANCE_ROLL_FAULT; conf->app_balance_conf.overspeed_duty = APPCONF_BALANCE_OVERSPEED_DUTY; @@ -716,4 +720,15 @@ void confgenerator_set_defaults_appconf(app_configuration *conf) { 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->app_imu_conf.use_peripheral = APPCONF_IMU_USE_PERIPHERAL; + conf->app_imu_conf.pitch_axis = APPCONF_IMU_PITCH_AXIS; + conf->app_imu_conf.roll_axis = APPCONF_IMU_ROLL_AXIS; + conf->app_imu_conf.yaw_axis = APPCONF_IMU_ROLL_AXIS; + conf->app_imu_conf.flip = APPCONF_IMU_FLIP; + conf->app_imu_conf.hertz = APPCONF_IMU_HERTZ; + conf->app_imu_conf.m_acd = APPCONF_IMU_M_ACD; + conf->app_imu_conf.m_b = APPCONF_IMU_M_B; + conf->app_imu_conf.cal_delay = APPCONF_IMU_CAL_DELAY; + conf->app_imu_conf.cal_m_acd = APPCONF_IMU_CAL_M_ACD; + conf->app_imu_conf.cal_m_b = APPCONF_IMU_CAL_M_B; } diff --git a/confgenerator.h b/confgenerator.h index 012feb35..6c8691ce 100644 --- a/confgenerator.h +++ b/confgenerator.h @@ -9,7 +9,7 @@ // Constants #define MCCONF_SIGNATURE 503309878 -#define APPCONF_SIGNATURE 3625267250 +#define APPCONF_SIGNATURE 4207459108 // Functions int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf); diff --git a/datatypes.h b/datatypes.h index 44b85f98..f6763f59 100644 --- a/datatypes.h +++ b/datatypes.h @@ -483,20 +483,13 @@ typedef struct { bool send_crc_ack; } nrf_config; -// Balance Datatypes typedef struct { float kp; float ki; float kd; - uint8_t loop_delay; - float m_acd; - float m_b; - uint32_t cal_delay; - float cal_m_acd; - float cal_m_b; + uint16_t hertz; float pitch_offset; float roll_offset; - bool use_peripheral; float pitch_fault; float roll_fault; float overspeed_duty; @@ -510,6 +503,20 @@ typedef struct { float current_boost; } balance_config; +typedef struct { + bool use_peripheral; + uint8_t pitch_axis; + uint8_t roll_axis; + uint8_t yaw_axis; + bool flip; + uint16_t hertz; + float m_acd; + float m_b; + uint16_t cal_delay; + float cal_m_acd; + float cal_m_b; +} imu_config; + // CAN status modes typedef enum { CAN_STATUS_DISABLED = 0, @@ -553,8 +560,11 @@ typedef struct { // NRF application settings nrf_config app_nrf_conf; - // Balance application settings - balance_config app_balance_conf; + // Balance application settings + balance_config app_balance_conf; + + // Balance application settings + imu_config app_imu_conf; } app_configuration; // Communication commands diff --git a/imu/imu.c b/imu/imu.c index f0524f00..4376074a 100644 --- a/imu/imu.c +++ b/imu/imu.c @@ -35,19 +35,20 @@ static float m_accel[3], m_gyro[3], m_mag[3]; static stkalign_t m_thd_work_area[THD_WORKING_AREA_SIZE(2048) / sizeof(stkalign_t)]; static i2c_bb_state m_i2c_bb; static ICM20948_STATE m_icm20948_state; +static imu_config config; // Private functions static void imu_read_callback(float *accel, float *gyro, float *mag); static void terminal_rpy(int argc, const char **argv); -void imu_init(bool use_peripheral) { +void imu_init(imu_config *conf) { + config = *conf; ahrs_init_attitude_info(&m_att); - if(use_peripheral){ + if(config.use_peripheral){ imu_init_mpu9x50(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN, HW_I2C_SCL_PORT, HW_I2C_SCL_PIN); } else { - #ifdef MPU9X50_SDA_GPIO imu_init_mpu9x50(MPU9X50_SDA_GPIO, MPU9X50_SDA_PIN, MPU9X50_SCL_GPIO, MPU9X50_SCL_PIN); @@ -73,7 +74,7 @@ i2c_bb_state *imu_get_i2c(void) { void imu_init_mpu9x50(stm32_gpio_t *sda_gpio, int sda_pin, stm32_gpio_t *scl_gpio, int scl_pin) { - mpu9150_init(sda_gpio, sda_pin, + mpu9150_init(&config, sda_gpio, sda_pin, scl_gpio, scl_pin, m_thd_work_area, sizeof(m_thd_work_area)); mpu9150_set_read_callback(imu_read_callback); @@ -160,31 +161,31 @@ static void imu_read_callback(float *accel, float *gyro, float *mag) { float dt = timer_seconds_elapsed_since(last_time); last_time = timer_time_now(); -#ifdef IMU_FLIP - m_accel[0] = -accel[0]; - m_accel[1] = accel[1]; - m_accel[2] = -accel[2]; + if(config.flip){ + m_accel[0] = -accel[config.pitch_axis]; + m_accel[1] = accel[config.roll_axis]; + m_accel[2] = -accel[config.yaw_axis]; - m_gyro[0] = -gyro[0]; - m_gyro[1] = gyro[1]; - m_gyro[2] = -gyro[2]; + m_gyro[0] = -gyro[config.pitch_axis]; + m_gyro[1] = gyro[config.roll_axis]; + m_gyro[2] = -gyro[config.yaw_axis]; - m_mag[0] = -mag[0]; - m_mag[1] = mag[1]; - m_mag[2] = -mag[2]; -#else - m_accel[0] = accel[0]; - m_accel[1] = accel[1]; - m_accel[2] = accel[2]; + m_mag[0] = -mag[config.pitch_axis]; + m_mag[1] = mag[config.roll_axis]; + m_mag[2] = -mag[config.yaw_axis]; + } else { + m_accel[0] = accel[config.pitch_axis]; + m_accel[1] = accel[config.roll_axis]; + m_accel[2] = accel[config.yaw_axis]; - m_gyro[0] = gyro[0]; - m_gyro[1] = gyro[1]; - m_gyro[2] = gyro[2]; + m_gyro[0] = gyro[config.pitch_axis]; + m_gyro[1] = gyro[config.roll_axis]; + m_gyro[2] = gyro[config.yaw_axis]; - m_mag[0] = mag[0]; - m_mag[1] = mag[1]; - m_mag[2] = mag[2]; -#endif + m_mag[0] = mag[config.pitch_axis]; + m_mag[1] = mag[config.roll_axis]; + m_mag[2] = mag[config.yaw_axis]; + } float gyro_rad[3]; gyro_rad[0] = m_gyro[0] * M_PI / 180.0; diff --git a/imu/imu.h b/imu/imu.h index 0d57366c..f03fc06b 100644 --- a/imu/imu.h +++ b/imu/imu.h @@ -24,7 +24,7 @@ #include "hal.h" #include "i2c_bb.h" -void imu_init(bool use_peripheral); +void imu_init(imu_config *conf); i2c_bb_state *imu_get_i2c(void); void imu_init_mpu9x50(stm32_gpio_t *sda_gpio, int sda_pin, stm32_gpio_t *scl_gpio, int scl_pin); diff --git a/imu/mpu9150.c b/imu/mpu9150.c index 2ddea1d6..1551acf6 100644 --- a/imu/mpu9150.c +++ b/imu/mpu9150.c @@ -37,9 +37,7 @@ #define USE_MAGNETOMETER 1 #define MPU_I2C_TIMEOUT 10 #define MAG_DIV 10 -#define ITERATION_TIME_US 5000 #define FAIL_DELAY_US 1000 -#define MIN_ITERATION_DELAY_US 500 #define MAX_IDENTICAL_READS 5 #define MPU_ADDR1 0x68 #define MPU_ADDR2 0x69 @@ -59,6 +57,7 @@ static volatile bool is_mpu9250; static i2c_bb_state i2cs; static volatile int16_t mpu9150_gyro_offsets[3]; static volatile bool mpu_found; +static imu_config config; // Private functions static int reset_init_mpu(void); @@ -75,10 +74,12 @@ static thread_t *mpu_tp = 0; // Function pointers static void(*read_callback)(float *accel, float *gyro, float *mag) = 0; -void mpu9150_init(stm32_gpio_t *sda_gpio, int sda_pin, +void mpu9150_init(imu_config *conf, stm32_gpio_t *sda_gpio, int sda_pin, stm32_gpio_t *scl_gpio, int scl_pin, stkalign_t *work_area, size_t work_area_size) { + config = *conf; + failed_reads = 0; failed_mag_reads = 0; read_callback = 0; @@ -111,7 +112,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) { chThdCreateStatic(work_area, work_area_size, NORMALPRIO, mpu_thread, NULL); @@ -338,12 +339,12 @@ static THD_FUNCTION(mpu_thread, arg) { iteration_timer = chVTGetSystemTime(); } - iteration_timer += US2ST(ITERATION_TIME_US); + iteration_timer += US2ST((int)((1000.0 / config.hertz) * 1000.0)); systime_t time_start = chVTGetSystemTime(); if (iteration_timer > time_start) { chThdSleep(iteration_timer - time_start); } else { - chThdSleepMicroseconds(MIN_ITERATION_DELAY_US); + chThdSleepMicroseconds((int)(((1000.0 / config.hertz) * 1000.0)/2)); iteration_timer = chVTGetSystemTime(); } } diff --git a/imu/mpu9150.h b/imu/mpu9150.h index a05ae5e6..7414e03c 100644 --- a/imu/mpu9150.h +++ b/imu/mpu9150.h @@ -24,7 +24,7 @@ #include "hal.h" // Functions -void mpu9150_init(stm32_gpio_t *sda_gpio, int sda_pin, +void mpu9150_init(imu_config *conf, stm32_gpio_t *sda_gpio, int sda_pin, stm32_gpio_t *scl_gpio, int scl_pin, stkalign_t *work_area, size_t work_area_size); bool mpu9150_is_mpu9250(void); diff --git a/main.c b/main.c index 0fed5dd1..f22ecb10 100644 --- a/main.c +++ b/main.c @@ -325,7 +325,6 @@ int main(void) { timeout_init(); timeout_configure(appconf.timeout_msec, appconf.timeout_brake_current); - imu_init(false); #if HAS_BLACKMAGIC bm_init(); From f257f775845cd13f09c5ac2a0c9ca34785bd8761 Mon Sep 17 00:00:00 2001 From: Mitch Lustig Date: Tue, 27 Aug 2019 09:10:44 -0700 Subject: [PATCH 15/31] Implement calibration --- applications/app_balance.c | 86 +++++++++++++++----------------------- confgenerator.c | 2 +- imu/imu.c | 53 +++++++++++++---------- 3 files changed, 67 insertions(+), 74 deletions(-) diff --git a/applications/app_balance.c b/applications/app_balance.c index 98eaadae..5b28bfc1 100644 --- a/applications/app_balance.c +++ b/applications/app_balance.c @@ -49,7 +49,7 @@ typedef enum { static THD_FUNCTION(balance_thread, arg); static THD_WORKING_AREA(balance_thread_wa, 2048); // 2kb stack for this thread -static volatile balance_config config; +static volatile balance_config balance_conf; static volatile imu_config imu_conf; static thread_t *app_thread; @@ -70,17 +70,12 @@ static float motor_current; static float motor_position; void app_balance_configure(balance_config *conf, imu_config *conf2) { - config = *conf; + balance_conf = *conf; imu_conf = *conf2; } void app_balance_start(void) { -// // Reset IMU -// if(config.use_peripheral){ -// imu_init(true); -// } - // Reset all Values state = CALIBRATING; pitch = 0; @@ -111,11 +106,6 @@ void app_balance_stop(void) { chThdWait(app_thread); } mc_interface_set_current(0); - -// // Reset IMU -// if(config.use_peripheral){ -// imu_init(false); -// } } float app_balance_get_pid_output(void) { @@ -151,16 +141,16 @@ float get_setpoint_adjustment_step_size(void){ } float apply_deadzone(float error){ - if(config.deadzone == 0){ + if(balance_conf.deadzone == 0){ return error; } - if(error < config.deadzone && error > -config.deadzone){ + if(error < balance_conf.deadzone && error > -balance_conf.deadzone){ return 0; - } else if(error > config.deadzone){ - return error - config.deadzone; + } else if(error > balance_conf.deadzone){ + return error - balance_conf.deadzone; } else { - return error + config.deadzone; + return error + balance_conf.deadzone; } } @@ -169,8 +159,8 @@ static THD_FUNCTION(balance_thread, arg) { chRegSetThreadName("APP_BALANCE"); // Do one off config - startup_step_size = config.startup_speed / config.hertz; - tiltback_step_size = config.tiltback_speed / config.hertz; + startup_step_size = balance_conf.startup_speed / balance_conf.hertz; + tiltback_step_size = balance_conf.tiltback_speed / balance_conf.hertz; state = CALIBRATING; setpointAdjustmentType = STARTUP; @@ -193,50 +183,42 @@ static THD_FUNCTION(balance_thread, arg) { roll = imu_get_roll() * 180.0f / M_PI; // Apply offsets - pitch = fmodf(((pitch + 180.0f) + config.pitch_offset), 360.0f) - 180.0f; - roll = fmodf(((roll + 180.0f) + config.roll_offset), 360.0f) - 180.0f; + pitch = fmodf(((pitch + 180.0f) + balance_conf.pitch_offset), 360.0f) - 180.0f; + roll = fmodf(((roll + 180.0f) + balance_conf.roll_offset), 360.0f) - 180.0f; // State based logic switch(state){ case (CALIBRATING): -// if(cal_start_time == 0){ -// cal_start_time = current_time; -// ahrs_set_madgwick_acc_confidence_decay(config.cal_m_acd); -// ahrs_set_madgwick_beta(config.cal_m_b); -// } -// cal_diff_time = current_time - cal_start_time; -// -// // Calibration is done -// if(ST2MS(cal_diff_time) > config.cal_delay){ -// -// // Set gyro config to be running config -// ahrs_set_madgwick_acc_confidence_decay(config.m_acd); -// ahrs_set_madgwick_beta(config.m_b); -// -// // Set fault and wait for valid startup condition -// state = FAULT; -// cal_start_time = 0; -// cal_diff_time = 0; -// } - state = FAULT; + if(cal_start_time == 0){ + cal_start_time = current_time; + } + cal_diff_time = current_time - cal_start_time; + + // Calibration is done + if(ST2MS(cal_diff_time) > imu_conf.cal_delay){ + // Set fault and wait for valid startup condition + state = FAULT; + cal_start_time = 0; + cal_diff_time = 0; + } break; case (RUNNING): // Check for overspeed - if(mc_interface_get_duty_cycle_now() > config.overspeed_duty || mc_interface_get_duty_cycle_now() < -config.overspeed_duty){ + if(fabsf(mc_interface_get_duty_cycle_now()) > balance_conf.overspeed_duty){ state = DEAD; } // Check for fault - if(pitch > config.pitch_fault || pitch < -config.pitch_fault || roll > config.roll_fault || roll < -config.roll_fault){ + if(fabsf(pitch) > balance_conf.pitch_fault || fabsf(roll) > balance_conf.roll_fault){ state = FAULT; } // Over speed tilt back safety - if(mc_interface_get_duty_cycle_now() > config.tiltback_duty){ - setpoint_target = config.tiltback_angle; + if(mc_interface_get_duty_cycle_now() > balance_conf.tiltback_duty){ + setpoint_target = balance_conf.tiltback_angle; setpointAdjustmentType = TILTBACK; - } else if(mc_interface_get_duty_cycle_now() < -config.tiltback_duty){ - setpoint_target = -config.tiltback_angle; + } else if(mc_interface_get_duty_cycle_now() < -balance_conf.tiltback_duty){ + setpoint_target = -balance_conf.tiltback_angle; setpointAdjustmentType = TILTBACK; }else{ setpoint_target = 0; @@ -262,15 +244,15 @@ static THD_FUNCTION(balance_thread, arg) { integral = integral + proportional; derivative = proportional - last_proportional; - pid_value = (config.kp * proportional) + (config.ki * integral) + (config.kd * derivative); + 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 += config.current_boost; + pid_value += balance_conf.current_boost; }else if(pid_value < 0){ - pid_value -= config.current_boost; + pid_value -= balance_conf.current_boost; } // Reset the timeout @@ -285,7 +267,7 @@ static THD_FUNCTION(balance_thread, arg) { break; case (FAULT): // Check for valid startup position - if(pitch < config.startup_pitch && pitch > -config.startup_pitch && roll < config.startup_roll && roll > -config.startup_roll){ + if(fabsf(pitch) < balance_conf.startup_pitch && fabsf(roll) < balance_conf.startup_roll){ setpoint = pitch; setpoint_target = 0; setpointAdjustmentType = STARTUP; @@ -303,7 +285,7 @@ static THD_FUNCTION(balance_thread, arg) { } // Delay between loops - chThdSleepMicroseconds((int)((1000.0 / config.hertz) * 1000.0)); + chThdSleepMicroseconds((int)((1000.0 / balance_conf.hertz) * 1000.0)); } // Disable output diff --git a/confgenerator.c b/confgenerator.c index e58fd69e..49b371b8 100644 --- a/confgenerator.c +++ b/confgenerator.c @@ -723,7 +723,7 @@ void confgenerator_set_defaults_appconf(app_configuration *conf) { conf->app_imu_conf.use_peripheral = APPCONF_IMU_USE_PERIPHERAL; conf->app_imu_conf.pitch_axis = APPCONF_IMU_PITCH_AXIS; conf->app_imu_conf.roll_axis = APPCONF_IMU_ROLL_AXIS; - conf->app_imu_conf.yaw_axis = APPCONF_IMU_ROLL_AXIS; + conf->app_imu_conf.yaw_axis = APPCONF_IMU_YAW_AXIS; conf->app_imu_conf.flip = APPCONF_IMU_FLIP; conf->app_imu_conf.hertz = APPCONF_IMU_HERTZ; conf->app_imu_conf.m_acd = APPCONF_IMU_M_ACD; diff --git a/imu/imu.c b/imu/imu.c index 4376074a..7d891fa9 100644 --- a/imu/imu.c +++ b/imu/imu.c @@ -36,6 +36,7 @@ static stkalign_t m_thd_work_area[THD_WORKING_AREA_SIZE(2048) / sizeof(stkalign_ static i2c_bb_state m_i2c_bb; static ICM20948_STATE m_icm20948_state; static imu_config config; +static systime_t init_time; // Private functions static void imu_read_callback(float *accel, float *gyro, float *mag); @@ -65,6 +66,10 @@ void imu_init(imu_config *conf) { "Print 100 roll/pitch/yaw samples at 10 Hz", 0, terminal_rpy); + + init_time = chVTGetSystemTimeX(); + ahrs_set_madgwick_acc_confidence_decay(config.cal_m_acd); + ahrs_set_madgwick_beta(config.cal_m_b); } i2c_bb_state *imu_get_i2c(void) { @@ -161,37 +166,43 @@ 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(config.flip){ - m_accel[0] = -accel[config.pitch_axis]; - m_accel[1] = accel[config.roll_axis]; - m_accel[2] = -accel[config.yaw_axis]; + if(config.flip){ + m_accel[0] = -accel[config.pitch_axis]; + m_accel[1] = accel[config.roll_axis]; + m_accel[2] = -accel[config.yaw_axis]; - m_gyro[0] = -gyro[config.pitch_axis]; - m_gyro[1] = gyro[config.roll_axis]; - m_gyro[2] = -gyro[config.yaw_axis]; + m_gyro[0] = -gyro[config.pitch_axis]; + m_gyro[1] = gyro[config.roll_axis]; + m_gyro[2] = -gyro[config.yaw_axis]; - m_mag[0] = -mag[config.pitch_axis]; - m_mag[1] = mag[config.roll_axis]; - m_mag[2] = -mag[config.yaw_axis]; - } else { - m_accel[0] = accel[config.pitch_axis]; - m_accel[1] = accel[config.roll_axis]; - m_accel[2] = accel[config.yaw_axis]; + m_mag[0] = -mag[config.pitch_axis]; + m_mag[1] = mag[config.roll_axis]; + m_mag[2] = -mag[config.yaw_axis]; + } else { + m_accel[0] = accel[config.pitch_axis]; + m_accel[1] = accel[config.roll_axis]; + m_accel[2] = accel[config.yaw_axis]; - m_gyro[0] = gyro[config.pitch_axis]; - m_gyro[1] = gyro[config.roll_axis]; - m_gyro[2] = gyro[config.yaw_axis]; + m_gyro[0] = gyro[config.pitch_axis] -0; + m_gyro[1] = gyro[config.roll_axis] -18.0; + m_gyro[2] = gyro[config.yaw_axis] -3.0; - m_mag[0] = mag[config.pitch_axis]; - m_mag[1] = mag[config.roll_axis]; - m_mag[2] = mag[config.yaw_axis]; - } + m_mag[0] = mag[config.pitch_axis]; + m_mag[1] = mag[config.roll_axis]; + m_mag[2] = mag[config.yaw_axis]; + } float gyro_rad[3]; gyro_rad[0] = m_gyro[0] * M_PI / 180.0; gyro_rad[1] = m_gyro[1] * M_PI / 180.0; gyro_rad[2] = m_gyro[2] * M_PI / 180.0; + if(init_time != 0 && ST2MS(chVTGetSystemTimeX() - init_time) > config.cal_delay){ + ahrs_set_madgwick_acc_confidence_decay(config.m_acd); + ahrs_set_madgwick_beta(config.m_b); + init_time = 0; + } + ahrs_update_madgwick_imu(gyro_rad, m_accel, dt, (ATTITUDE_INFO*)&m_att); } From 7a47053ca6d8aa87fea13ce608293c29afb96e19 Mon Sep 17 00:00:00 2001 From: Mitch Lustig Date: Tue, 27 Aug 2019 19:57:35 -0700 Subject: [PATCH 16/31] Add startup calibration sequence --- appconf/appconf_default.h | 15 ++++++++----- applications/app_balance.c | 36 ++++++++++++++--------------- confgenerator.c | 21 +++++++++-------- confgenerator.h | 2 +- datatypes.h | 13 ++++++++--- imu/imu.c | 46 ++++++++++++++++++++++++++++++-------- 6 files changed, 87 insertions(+), 46 deletions(-) diff --git a/appconf/appconf_default.h b/appconf/appconf_default.h index 596afaa5..90795c6d 100644 --- a/appconf/appconf_default.h +++ b/appconf/appconf_default.h @@ -329,14 +329,17 @@ #ifndef APPCONF_IMU_M_B #define APPCONF_IMU_M_B 0.1 #endif -#ifndef APPCONF_IMU_CAL_DELAY -#define APPCONF_IMU_CAL_DELAY 1500 +#ifndef APPCONF_IMU_STARTUP_TIME +#define APPCONF_IMU_STARTUP_TIME 1500 #endif -#ifndef APPCONF_IMU_CAL_M_ACD -#define APPCONF_IMU_CAL_M_ACD 1.0 +#ifndef APPCONF_IMU_STARTUP_M_ACD +#define APPCONF_IMU_STARTUP_M_ACD 1.0 #endif -#ifndef APPCONF_IMU_CAL_M_B -#define APPCONF_IMU_CAL_M_B 2.0 +#ifndef APPCONF_IMU_STARTUP_M_B +#define APPCONF_IMU_STARTUP_M_B 2.0 +#endif +#ifndef APPCONF_IMU_CAL_TYPE +#define APPCONF_IMU_CAL_TYPE IMU_CAL_NONE #endif #endif /* APPCONF_DEFAULT_H_ */ diff --git a/applications/app_balance.c b/applications/app_balance.c index 5b28bfc1..0963a79f 100644 --- a/applications/app_balance.c +++ b/applications/app_balance.c @@ -34,14 +34,14 @@ // Data type typedef enum { - CALIBRATING = 0, + STARTUP = 0, RUNNING, FAULT, DEAD } BalanceState; typedef enum { - STARTUP = 0, + CENTERING = 0, TILTBACK } SetpointAdjustmentType; @@ -63,7 +63,7 @@ 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 cal_start_time, cal_diff_time; +static systime_t startup_start_time, startup_diff_time; // Values read to pass in app data to GUI static float motor_current; @@ -77,7 +77,7 @@ void app_balance_configure(balance_config *conf, imu_config *conf2) { void app_balance_start(void) { // Reset all Values - state = CALIBRATING; + state = STARTUP; pitch = 0; roll = 0; proportional = 0; @@ -87,14 +87,14 @@ void app_balance_start(void) { pid_value = 0; setpoint = 0; setpoint_target = 0; - setpointAdjustmentType = STARTUP; + setpointAdjustmentType = CENTERING; startup_step_size = 0; tiltback_step_size = 0; current_time = 0; last_time = 0; diff_time = 0; - cal_start_time = 0; - cal_diff_time = 0; + startup_start_time = 0; + startup_diff_time = 0; // Start the balance thread app_thread = chThdCreateStatic(balance_thread_wa, sizeof(balance_thread_wa), NORMALPRIO, balance_thread, NULL); @@ -132,7 +132,7 @@ uint16_t app_balance_get_state(void) { float get_setpoint_adjustment_step_size(void){ switch(setpointAdjustmentType){ - case (STARTUP): + case (CENTERING): return startup_step_size; case (TILTBACK): return tiltback_step_size; @@ -162,8 +162,8 @@ static THD_FUNCTION(balance_thread, arg) { startup_step_size = balance_conf.startup_speed / balance_conf.hertz; tiltback_step_size = balance_conf.tiltback_speed / balance_conf.hertz; - state = CALIBRATING; - setpointAdjustmentType = STARTUP; + state = STARTUP; + setpointAdjustmentType = CENTERING; while (!chThdShouldTerminateX()) { // Update times @@ -188,18 +188,18 @@ static THD_FUNCTION(balance_thread, arg) { // State based logic switch(state){ - case (CALIBRATING): - if(cal_start_time == 0){ - cal_start_time = current_time; + case (STARTUP): + if(startup_start_time == 0){ + startup_start_time = current_time; } - cal_diff_time = current_time - cal_start_time; + startup_diff_time = current_time - startup_start_time; // Calibration is done - if(ST2MS(cal_diff_time) > imu_conf.cal_delay){ + if(ST2MS(startup_diff_time) > imu_conf.startup_time){ // Set fault and wait for valid startup condition state = FAULT; - cal_start_time = 0; - cal_diff_time = 0; + startup_start_time = 0; + startup_diff_time = 0; } break; case (RUNNING): @@ -270,7 +270,7 @@ static THD_FUNCTION(balance_thread, arg) { if(fabsf(pitch) < balance_conf.startup_pitch && fabsf(roll) < balance_conf.startup_roll){ setpoint = pitch; setpoint_target = 0; - setpointAdjustmentType = STARTUP; + setpointAdjustmentType = CENTERING; state = RUNNING; break; } diff --git a/confgenerator.c b/confgenerator.c index 49b371b8..3ba9617d 100644 --- a/confgenerator.c +++ b/confgenerator.c @@ -242,9 +242,10 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration buffer_append_uint16(buffer, conf->app_imu_conf.hertz, &ind); buffer_append_float32_auto(buffer, conf->app_imu_conf.m_acd, &ind); buffer_append_float32_auto(buffer, conf->app_imu_conf.m_b, &ind); - buffer_append_uint32(buffer, conf->app_imu_conf.cal_delay, &ind); - buffer_append_float32_auto(buffer, conf->app_imu_conf.cal_m_acd, &ind); - buffer_append_float32_auto(buffer, conf->app_imu_conf.cal_m_b, &ind); + buffer_append_uint32(buffer, conf->app_imu_conf.startup_time, &ind); + buffer_append_float32_auto(buffer, conf->app_imu_conf.startup_m_acd, &ind); + buffer_append_float32_auto(buffer, conf->app_imu_conf.startup_m_b, &ind); + buffer[ind++] = conf->app_imu_conf.cal_type; return ind; } @@ -493,9 +494,10 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration conf->app_imu_conf.hertz = buffer_get_uint16(buffer, &ind); conf->app_imu_conf.m_acd = buffer_get_float32_auto(buffer, &ind); conf->app_imu_conf.m_b = buffer_get_float32_auto(buffer, &ind); - conf->app_imu_conf.cal_delay = buffer_get_uint32(buffer, &ind); - conf->app_imu_conf.cal_m_acd = buffer_get_float32_auto(buffer, &ind); - conf->app_imu_conf.cal_m_b = buffer_get_float32_auto(buffer, &ind); + conf->app_imu_conf.startup_time = buffer_get_uint32(buffer, &ind); + conf->app_imu_conf.startup_m_acd = buffer_get_float32_auto(buffer, &ind); + conf->app_imu_conf.startup_m_b = buffer_get_float32_auto(buffer, &ind); + conf->app_imu_conf.cal_type = buffer[ind++]; return true; } @@ -728,7 +730,8 @@ void confgenerator_set_defaults_appconf(app_configuration *conf) { conf->app_imu_conf.hertz = APPCONF_IMU_HERTZ; conf->app_imu_conf.m_acd = APPCONF_IMU_M_ACD; conf->app_imu_conf.m_b = APPCONF_IMU_M_B; - conf->app_imu_conf.cal_delay = APPCONF_IMU_CAL_DELAY; - conf->app_imu_conf.cal_m_acd = APPCONF_IMU_CAL_M_ACD; - conf->app_imu_conf.cal_m_b = APPCONF_IMU_CAL_M_B; + conf->app_imu_conf.startup_time = APPCONF_IMU_STARTUP_TIME; + conf->app_imu_conf.startup_m_acd = APPCONF_IMU_STARTUP_M_ACD; + conf->app_imu_conf.startup_m_b = APPCONF_IMU_STARTUP_M_B; + conf->app_imu_conf.cal_type = APPCONF_IMU_CAL_TYPE; } diff --git a/confgenerator.h b/confgenerator.h index 6c8691ce..55d648dd 100644 --- a/confgenerator.h +++ b/confgenerator.h @@ -9,7 +9,7 @@ // Constants #define MCCONF_SIGNATURE 503309878 -#define APPCONF_SIGNATURE 4207459108 +#define APPCONF_SIGNATURE 1147089268 // Functions int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf); diff --git a/datatypes.h b/datatypes.h index f6763f59..5f2f76df 100644 --- a/datatypes.h +++ b/datatypes.h @@ -503,6 +503,12 @@ typedef struct { float current_boost; } balance_config; +typedef enum { + IMU_CAL_NONE = 0, + IMU_CAL_STARTUP, + IMU_CAL_REALTIME +} IMU_CAL; + typedef struct { bool use_peripheral; uint8_t pitch_axis; @@ -512,9 +518,10 @@ typedef struct { uint16_t hertz; float m_acd; float m_b; - uint16_t cal_delay; - float cal_m_acd; - float cal_m_b; + uint16_t startup_time; + float startup_m_acd; + float startup_m_b; + IMU_CAL cal_type; } imu_config; // CAN status modes diff --git a/imu/imu.c b/imu/imu.c index 7d891fa9..13e7fd5a 100644 --- a/imu/imu.c +++ b/imu/imu.c @@ -37,6 +37,9 @@ static i2c_bb_state m_i2c_bb; static ICM20948_STATE m_icm20948_state; static imu_config config; static systime_t init_time; +static float gyro_offset_0, gyro_offset_1, gyro_offset_2; +static float gyro_cal_storage[3][400]; +static int gyro_cal_count; // Private functions static void imu_read_callback(float *accel, float *gyro, float *mag); @@ -68,8 +71,13 @@ void imu_init(imu_config *conf) { terminal_rpy); init_time = chVTGetSystemTimeX(); - ahrs_set_madgwick_acc_confidence_decay(config.cal_m_acd); - ahrs_set_madgwick_beta(config.cal_m_b); + ahrs_set_madgwick_acc_confidence_decay(config.startup_m_acd); + ahrs_set_madgwick_beta(config.startup_m_b); + + gyro_cal_count = 0; + gyro_offset_0 = 0; + gyro_offset_1 = 0; + gyro_offset_2 = 0; } i2c_bb_state *imu_get_i2c(void) { @@ -171,9 +179,9 @@ static void imu_read_callback(float *accel, float *gyro, float *mag) { m_accel[1] = accel[config.roll_axis]; m_accel[2] = -accel[config.yaw_axis]; - m_gyro[0] = -gyro[config.pitch_axis]; - m_gyro[1] = gyro[config.roll_axis]; - m_gyro[2] = -gyro[config.yaw_axis]; + m_gyro[0] = -gyro[config.pitch_axis] - gyro_offset_0; + m_gyro[1] = gyro[config.roll_axis] - gyro_offset_1; + m_gyro[2] = -gyro[config.yaw_axis] - gyro_offset_2; m_mag[0] = -mag[config.pitch_axis]; m_mag[1] = mag[config.roll_axis]; @@ -183,9 +191,9 @@ static void imu_read_callback(float *accel, float *gyro, float *mag) { m_accel[1] = accel[config.roll_axis]; m_accel[2] = accel[config.yaw_axis]; - m_gyro[0] = gyro[config.pitch_axis] -0; - m_gyro[1] = gyro[config.roll_axis] -18.0; - m_gyro[2] = gyro[config.yaw_axis] -3.0; + m_gyro[0] = gyro[config.pitch_axis] - gyro_offset_0; + m_gyro[1] = gyro[config.roll_axis] - gyro_offset_1; + m_gyro[2] = gyro[config.yaw_axis] - gyro_offset_2; m_mag[0] = mag[config.pitch_axis]; m_mag[1] = mag[config.roll_axis]; @@ -197,12 +205,32 @@ static void imu_read_callback(float *accel, float *gyro, float *mag) { gyro_rad[1] = m_gyro[1] * M_PI / 180.0; gyro_rad[2] = m_gyro[2] * M_PI / 180.0; - if(init_time != 0 && ST2MS(chVTGetSystemTimeX() - init_time) > config.cal_delay){ + if(init_time != 0 && ST2MS(chVTGetSystemTimeX() - init_time) > config.startup_time){ ahrs_set_madgwick_acc_confidence_decay(config.m_acd); ahrs_set_madgwick_beta(config.m_b); init_time = 0; } + if(config.cal_type == IMU_CAL_STARTUP){ + if(gyro_cal_count < 400){ + gyro_cal_storage[0][gyro_cal_count] = m_gyro[0]; + gyro_cal_storage[1][gyro_cal_count] = m_gyro[1]; + gyro_cal_storage[2][gyro_cal_count] = m_gyro[2]; + gyro_cal_count ++; + }else if(gyro_cal_count == 400){ + for(int i = 0; i < 400; i++){ + gyro_offset_0 += gyro_cal_storage[0][i]; + gyro_offset_1 += gyro_cal_storage[1][i]; + gyro_offset_2 += gyro_cal_storage[2][i]; + } + gyro_offset_0 = gyro_offset_0/400; + gyro_offset_1 = gyro_offset_1/400; + gyro_offset_2 = gyro_offset_2/400; + + gyro_cal_count ++; + } + } + ahrs_update_madgwick_imu(gyro_rad, m_accel, dt, (ATTITUDE_INFO*)&m_att); } From 91900a0e63f718eae43f0bd0768a4391539682ee Mon Sep 17 00:00:00 2001 From: Mitch Lustig Date: Wed, 28 Aug 2019 01:41:47 -0700 Subject: [PATCH 17/31] Update conf generator --- confgenerator.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/confgenerator.h b/confgenerator.h index 55d648dd..a32d6ebc 100644 --- a/confgenerator.h +++ b/confgenerator.h @@ -9,7 +9,7 @@ // Constants #define MCCONF_SIGNATURE 503309878 -#define APPCONF_SIGNATURE 1147089268 +#define APPCONF_SIGNATURE 4068834242 // Functions int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf); From 474d8c1691c64ce52a76cdf716da207e62048adc Mon Sep 17 00:00:00 2001 From: Mitch Lustig Date: Sun, 8 Sep 2019 12:14:59 -0700 Subject: [PATCH 18/31] Little more cleanup --- applications/app.h | 1 - imu/ahrs.h | 10 ---------- imu/mpu9150.c | 7 +++---- imu/mpu9150.h | 2 +- 4 files changed, 4 insertions(+), 16 deletions(-) diff --git a/applications/app.h b/applications/app.h index c366b3d5..4b245afe 100644 --- a/applications/app.h +++ b/applications/app.h @@ -69,6 +69,5 @@ uint16_t app_balance_get_state(void); void app_custom_start(void); void app_custom_stop(void); void app_custom_configure(app_configuration *conf); -void app_custom_terminal_rpy(int argc, const char **argv); #endif /* APP_H_ */ diff --git a/imu/ahrs.h b/imu/ahrs.h index 756825a6..874c5829 100644 --- a/imu/ahrs.h +++ b/imu/ahrs.h @@ -31,14 +31,4 @@ float ahrs_get_pitch(ATTITUDE_INFO *att); float ahrs_get_yaw(ATTITUDE_INFO *att); void ahrs_get_roll_pitch_yaw(float *rpy, ATTITUDE_INFO *att); -float ahrs_get_mahony_kp(void); -float ahrs_get_mahony_ki(void); -float ahrs_get_madgwick_acc_confidence_decay(void); -float ahrs_get_madgwick_beta(void); - -void ahrs_set_mahony_kp(float mahony_kp); -void ahrs_set_mahony_ki(float mahony_ki); -void ahrs_set_madgwick_acc_confidence_decay(float madgwick_acc_confidence_decay); -void ahrs_set_madgwick_beta(float madgwick_beta); - #endif diff --git a/imu/mpu9150.c b/imu/mpu9150.c index 4e112dc0..51d49cd3 100644 --- a/imu/mpu9150.c +++ b/imu/mpu9150.c @@ -38,6 +38,7 @@ #define MPU_I2C_TIMEOUT 10 #define MAG_DIV 10 #define FAIL_DELAY_US 1000 +#define MIN_ITERATION_DELAY_US 500 #define MAX_IDENTICAL_READS 5 #define MPU_ADDR1 0x68 #define MPU_ADDR2 0x69 @@ -76,12 +77,10 @@ static thread_t *mpu_tp = 0; // Function pointers static void(*read_callback)(float *accel, float *gyro, float *mag) = 0; -void mpu9150_init(imu_config *conf, stm32_gpio_t *sda_gpio, int sda_pin, +void mpu9150_init(stm32_gpio_t *sda_gpio, int sda_pin, stm32_gpio_t *scl_gpio, int scl_pin, stkalign_t *work_area, size_t work_area_size) { - config = *conf; - failed_reads = 0; failed_mag_reads = 0; read_callback = 0; @@ -369,7 +368,7 @@ static THD_FUNCTION(mpu_thread, arg) { if (iteration_timer > time_start) { chThdSleep(iteration_timer - time_start); } else { - chThdSleepMicroseconds((int)(((1000.0 / config.hertz) * 1000.0)/2)); + chThdSleepMicroseconds(MIN_ITERATION_DELAY_US); iteration_timer = chVTGetSystemTime(); } } diff --git a/imu/mpu9150.h b/imu/mpu9150.h index d4a6bf31..62b57cca 100644 --- a/imu/mpu9150.h +++ b/imu/mpu9150.h @@ -24,7 +24,7 @@ #include "hal.h" // Functions -void mpu9150_init(imu_config *conf, stm32_gpio_t *sda_gpio, int sda_pin, +void mpu9150_init(stm32_gpio_t *sda_gpio, int sda_pin, stm32_gpio_t *scl_gpio, int scl_pin, stkalign_t *work_area, size_t work_area_size); void mpu9150_stop(void); From 72e9c24bd75a3311f622acb72f1924338e175f7b Mon Sep 17 00:00:00 2001 From: Mitch Lustig Date: Sun, 8 Sep 2019 18:32:53 -0700 Subject: [PATCH 19/31] Make axes configurable, remove offset management --- appconf/appconf_default.h | 24 ++++---- applications/app.c | 2 +- applications/app.h | 2 +- applications/app_balance.c | 55 +++++++++++------ confgenerator.c | 122 ++++++++++++++++++++++++++++++++++--- confgenerator.h | 2 +- datatypes.h | 19 ++++-- imu/imu.c | 2 +- 8 files changed, 179 insertions(+), 49 deletions(-) diff --git a/appconf/appconf_default.h b/appconf/appconf_default.h index a16981dd..1045bf25 100644 --- a/appconf/appconf_default.h +++ b/appconf/appconf_default.h @@ -276,17 +276,17 @@ #ifndef APPCONF_BALANCE_HERTZ #define APPCONF_BALANCE_HERTZ 1000 #endif -#ifndef APPCONF_BALANCE_PITCH_OFFSET -#define APPCONF_BALANCE_PITCH_OFFSET 0 +#ifndef APPCONF_BALANCE_M_AXIS +#define APPCONF_BALANCE_M_AXIS 0 #endif -#ifndef APPCONF_BALANCE_ROLL_OFFSET -#define APPCONF_BALANCE_ROLL_OFFSET 0 +#ifndef APPCONF_BALANCE_C_AXIS +#define APPCONF_BALANCE_C_AXIS 1 #endif -#ifndef APPCONF_BALANCE_PITCH_FAULT -#define APPCONF_BALANCE_PITCH_FAULT 20 +#ifndef APPCONF_BALANCE_M_FAULT +#define APPCONF_BALANCE_M_FAULT 20 #endif -#ifndef APPCONF_BALANCE_ROLL_FAULT -#define APPCONF_BALANCE_ROLL_FAULT 45 +#ifndef APPCONF_BALANCE_C_FAULT +#define APPCONF_BALANCE_C_FAULT 45 #endif #ifndef APPCONF_BALANCE_OVERSPEED_DUTY #define APPCONF_BALANCE_OVERSPEED_DUTY 0.9 @@ -300,11 +300,11 @@ #ifndef APPCONF_BALANCE_TILTBACK_SPEED #define APPCONF_BALANCE_TILTBACK_SPEED 5.0 #endif -#ifndef APPCONF_BALANCE_STARTUP_PITCH -#define APPCONF_BALANCE_STARTUP_PITCH 20.0 +#ifndef APPCONF_BALANCE_STARTUP_M_TOLERANCE +#define APPCONF_BALANCE_STARTUP_M_TOLERANCE 20.0 #endif -#ifndef APPCONF_BALANCE_STARTUP_ROLL -#define APPCONF_BALANCE_STARTUP_ROLL 8.0 +#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 diff --git a/applications/app.c b/applications/app.c index 5745d59f..b5752214 100644 --- a/applications/app.c +++ b/applications/app.c @@ -69,7 +69,7 @@ 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.app_imu_conf); + app_balance_configure(&appconf.app_balance_conf); switch (appconf.app_to_use) { case APP_PPM: diff --git a/applications/app.h b/applications/app.h index 4b245afe..29815a64 100644 --- a/applications/app.h +++ b/applications/app.h @@ -56,7 +56,7 @@ 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); +void app_balance_configure(balance_config *conf); float app_balance_get_pid_output(void); float app_balance_get_pitch(void); float app_balance_get_roll(void); diff --git a/applications/app_balance.c b/applications/app_balance.c index 0963a79f..c84fbca0 100644 --- a/applications/app_balance.c +++ b/applications/app_balance.c @@ -28,6 +28,7 @@ #include "imu/imu.h" #include "imu/ahrs.h" #include "utils.h" +#include "datatypes.h" #include @@ -50,12 +51,11 @@ 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 pitch, roll; +static float m_angle, c_angle; static float proportional, integral, derivative; static float last_proportional; static float pid_value; @@ -69,17 +69,16 @@ static systime_t startup_start_time, startup_diff_time; static float motor_current; static float motor_position; -void app_balance_configure(balance_config *conf, imu_config *conf2) { +void app_balance_configure(balance_config *conf) { balance_conf = *conf; - imu_conf = *conf2; } void app_balance_start(void) { // Reset all Values state = STARTUP; - pitch = 0; - roll = 0; + m_angle = 0; + c_angle = 0; proportional = 0; integral = 0; derivative = 0; @@ -112,10 +111,10 @@ float app_balance_get_pid_output(void) { return pid_value; } float app_balance_get_pitch(void) { - return pitch; + return m_angle; } float app_balance_get_roll(void) { - return roll; + return c_angle; } uint32_t app_balance_get_diff_time(void) { return ST2US(diff_time); @@ -178,13 +177,29 @@ static THD_FUNCTION(balance_thread, arg) { motor_current = mc_interface_get_tot_current_directional_filtered(); motor_position = mc_interface_get_pid_pos_now(); - // Read gyro values - pitch = imu_get_pitch() * 180.0f / M_PI; - roll = imu_get_roll() * 180.0f / M_PI; - - // Apply offsets - pitch = fmodf(((pitch + 180.0f) + balance_conf.pitch_offset), 360.0f) - 180.0f; - roll = fmodf(((roll + 180.0f) + balance_conf.roll_offset), 360.0f) - 180.0f; + // 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; + } // State based logic switch(state){ @@ -195,7 +210,7 @@ static THD_FUNCTION(balance_thread, arg) { startup_diff_time = current_time - startup_start_time; // Calibration is done - if(ST2MS(startup_diff_time) > imu_conf.startup_time){ + if(ST2MS(startup_diff_time) > 1000){ // Set fault and wait for valid startup condition state = FAULT; startup_start_time = 0; @@ -209,7 +224,7 @@ static THD_FUNCTION(balance_thread, arg) { } // Check for fault - if(fabsf(pitch) > balance_conf.pitch_fault || fabsf(roll) > balance_conf.roll_fault){ + if(fabsf(m_angle) > balance_conf.m_fault || fabsf(c_angle) > balance_conf.c_fault){ state = FAULT; } @@ -237,7 +252,7 @@ static THD_FUNCTION(balance_thread, arg) { } // Do PID maths - proportional = setpoint - pitch; + proportional = setpoint - m_angle; // Apply deadzone proportional = apply_deadzone(proportional); // Resume real PID maths @@ -267,8 +282,8 @@ static THD_FUNCTION(balance_thread, arg) { break; case (FAULT): // Check for valid startup position - if(fabsf(pitch) < balance_conf.startup_pitch && fabsf(roll) < balance_conf.startup_roll){ - setpoint = pitch; + if(fabsf(m_angle) < balance_conf.startup_m_tolerance && fabsf(c_angle) < balance_conf.startup_c_tolerance){ + setpoint = m_angle; setpoint_target = 0; setpointAdjustmentType = CENTERING; state = RUNNING; diff --git a/confgenerator.c b/confgenerator.c index d0407827..f69255ad 100644 --- a/confgenerator.c +++ b/confgenerator.c @@ -1,7 +1,7 @@ // This file is autogenerated by VESC Tool #include "buffer.h" -#include "conf_mc_app_default.h" +#include "conf_general.h" #include "confgenerator.h" int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf) { @@ -153,6 +153,7 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration buffer[ind++] = conf->can_baud_rate; buffer[ind++] = conf->pairing_done; buffer[ind++] = conf->permanent_uart_enabled; + buffer[ind++] = conf->shutdown_mode; buffer[ind++] = conf->uavcan_enable; buffer[ind++] = (uint8_t)conf->uavcan_esc_index; buffer[ind++] = conf->app_to_use; @@ -220,8 +221,43 @@ 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; -==== BASE ==== -==== BASE ==== + 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_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.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); + buffer_append_float32_auto(buffer, conf->imu_conf.accel_confidence_decay, &ind); + buffer_append_float32_auto(buffer, conf->imu_conf.mahony_kp, &ind); + buffer_append_float32_auto(buffer, conf->imu_conf.mahony_ki, &ind); + buffer_append_float32_auto(buffer, conf->imu_conf.madgwick_beta, &ind); + buffer_append_float32_auto(buffer, conf->imu_conf.rot_roll, &ind); + buffer_append_float32_auto(buffer, conf->imu_conf.rot_pitch, &ind); + buffer_append_float32_auto(buffer, conf->imu_conf.rot_yaw, &ind); + buffer_append_float32_auto(buffer, conf->imu_conf.accel_offsets[0], &ind); + buffer_append_float32_auto(buffer, conf->imu_conf.accel_offsets[1], &ind); + buffer_append_float32_auto(buffer, conf->imu_conf.accel_offsets[2], &ind); + buffer_append_float32_auto(buffer, conf->imu_conf.gyro_offsets[0], &ind); + buffer_append_float32_auto(buffer, conf->imu_conf.gyro_offsets[1], &ind); + buffer_append_float32_auto(buffer, conf->imu_conf.gyro_offsets[2], &ind); + buffer_append_float32_auto(buffer, conf->imu_conf.gyro_offset_comp_fact[0], &ind); + buffer_append_float32_auto(buffer, conf->imu_conf.gyro_offset_comp_fact[1], &ind); + buffer_append_float32_auto(buffer, conf->imu_conf.gyro_offset_comp_fact[2], &ind); + buffer_append_float32_auto(buffer, conf->imu_conf.gyro_offset_comp_clamp, &ind); return ind; } @@ -381,6 +417,7 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration conf->can_baud_rate = buffer[ind++]; conf->pairing_done = buffer[ind++]; conf->permanent_uart_enabled = buffer[ind++]; + conf->shutdown_mode = buffer[ind++]; conf->uavcan_enable = buffer[ind++]; conf->uavcan_esc_index = buffer[ind++]; conf->app_to_use = buffer[ind++]; @@ -448,8 +485,43 @@ 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++]; -==== BASE ==== -==== BASE ==== + 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.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.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); + conf->imu_conf.accel_confidence_decay = buffer_get_float32_auto(buffer, &ind); + conf->imu_conf.mahony_kp = buffer_get_float32_auto(buffer, &ind); + conf->imu_conf.mahony_ki = buffer_get_float32_auto(buffer, &ind); + conf->imu_conf.madgwick_beta = buffer_get_float32_auto(buffer, &ind); + conf->imu_conf.rot_roll = buffer_get_float32_auto(buffer, &ind); + conf->imu_conf.rot_pitch = buffer_get_float32_auto(buffer, &ind); + conf->imu_conf.rot_yaw = buffer_get_float32_auto(buffer, &ind); + conf->imu_conf.accel_offsets[0] = buffer_get_float32_auto(buffer, &ind); + conf->imu_conf.accel_offsets[1] = buffer_get_float32_auto(buffer, &ind); + conf->imu_conf.accel_offsets[2] = buffer_get_float32_auto(buffer, &ind); + conf->imu_conf.gyro_offsets[0] = buffer_get_float32_auto(buffer, &ind); + conf->imu_conf.gyro_offsets[1] = buffer_get_float32_auto(buffer, &ind); + conf->imu_conf.gyro_offsets[2] = buffer_get_float32_auto(buffer, &ind); + conf->imu_conf.gyro_offset_comp_fact[0] = buffer_get_float32_auto(buffer, &ind); + conf->imu_conf.gyro_offset_comp_fact[1] = buffer_get_float32_auto(buffer, &ind); + conf->imu_conf.gyro_offset_comp_fact[2] = buffer_get_float32_auto(buffer, &ind); + conf->imu_conf.gyro_offset_comp_clamp = buffer_get_float32_auto(buffer, &ind); return true; } @@ -593,6 +665,7 @@ void confgenerator_set_defaults_appconf(app_configuration *conf) { conf->can_baud_rate = APPCONF_CAN_BAUD_RATE; conf->pairing_done = APPCONF_PAIRING_DONE; conf->permanent_uart_enabled = APPCONF_PERMANENT_UART_ENABLED; + conf->shutdown_mode = APPCONF_SHUTDOWN_MODE; conf->uavcan_enable = APPCONF_UAVCAN_ENABLE; conf->uavcan_esc_index = APPCONF_UAVCAN_ESC_INDEX; conf->app_to_use = APPCONF_APP_TO_USE; @@ -660,6 +733,41 @@ 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; -==== BASE ==== -==== BASE ==== + 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.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.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; + conf->imu_conf.accel_confidence_decay = APPCONF_IMU_ACCEL_CONFIDENCE_DECAY; + conf->imu_conf.mahony_kp = APPCONF_IMU_MAHONY_KP; + conf->imu_conf.mahony_ki = APPCONF_IMU_MAHONY_KI; + conf->imu_conf.madgwick_beta = APPCONF_IMU_MADGWICK_BETA; + conf->imu_conf.rot_roll = APPCONF_IMU_ROT_ROLL; + conf->imu_conf.rot_pitch = APPCONF_IMU_ROT_PITCH; + conf->imu_conf.rot_yaw = APPCONF_IMU_ROT_YAW; + conf->imu_conf.accel_offsets[0] = APPCONF_IMU_A_OFFSET_0; + conf->imu_conf.accel_offsets[1] = APPCONF_IMU_A_OFFSET_1; + conf->imu_conf.accel_offsets[2] = APPCONF_IMU_A_OFFSET_2; + conf->imu_conf.gyro_offsets[0] = APPCONF_IMU_G_OFFSET_0; + conf->imu_conf.gyro_offsets[1] = APPCONF_IMU_G_OFFSET_1; + conf->imu_conf.gyro_offsets[2] = APPCONF_IMU_G_OFFSET_2; + conf->imu_conf.gyro_offset_comp_fact[0] = APPCONF_IMU_G_OFFSET_COMP_FACT_0; + conf->imu_conf.gyro_offset_comp_fact[1] = APPCONF_IMU_G_OFFSET_COMP_FACT_1; + conf->imu_conf.gyro_offset_comp_fact[2] = APPCONF_IMU_G_OFFSET_COMP_FACT_2; + conf->imu_conf.gyro_offset_comp_clamp = APPCONF_IMU_G_OFFSET_COMP_CLAMP; } diff --git a/confgenerator.h b/confgenerator.h index bf161a7d..c9cd9cb1 100644 --- a/confgenerator.h +++ b/confgenerator.h @@ -9,7 +9,7 @@ // Constants #define MCCONF_SIGNATURE 503309878 -#define APPCONF_SIGNATURE 543202478 +#define APPCONF_SIGNATURE 3772026662 // Functions int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf); diff --git a/datatypes.h b/datatypes.h index a8f62c76..ddf311d5 100644 --- a/datatypes.h +++ b/datatypes.h @@ -486,21 +486,28 @@ 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; - float pitch_offset; - float roll_offset; - float pitch_fault; - float roll_fault; + AXIS m_axis; + AXIS c_axis; + float m_fault; + float c_fault; float overspeed_duty; float tiltback_duty; float tiltback_angle; float tiltback_speed; - float startup_pitch; - float startup_roll; + float startup_m_tolerance; + float startup_c_tolerance; float startup_speed; float deadzone; float current_boost; diff --git a/imu/imu.c b/imu/imu.c index 5e9f8eb0..83d5a403 100644 --- a/imu/imu.c +++ b/imu/imu.c @@ -102,7 +102,7 @@ void imu_init_mpu9x50(stm32_gpio_t *sda_gpio, int sda_pin, stm32_gpio_t *scl_gpio, int scl_pin) { imu_stop(); - mpu9150_init(&config, sda_gpio, sda_pin, + mpu9150_init(sda_gpio, sda_pin, scl_gpio, scl_pin, m_thd_work_area, sizeof(m_thd_work_area)); mpu9150_set_read_callback(imu_read_callback); From a4de5eb2b5183182b3c0cdc43ef3a48c2227932a Mon Sep 17 00:00:00 2001 From: Mitch Lustig Date: Sun, 8 Sep 2019 22:23:31 -0700 Subject: [PATCH 20/31] Cleanup --- applications/app.h | 4 ++-- applications/app_balance.c | 4 ++-- commands.c | 4 ++-- datatypes.h | 2 +- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/applications/app.h b/applications/app.h index 29815a64..edac85e1 100644 --- a/applications/app.h +++ b/applications/app.h @@ -58,8 +58,8 @@ void app_balance_start(void); void app_balance_stop(void); void app_balance_configure(balance_config *conf); float app_balance_get_pid_output(void); -float app_balance_get_pitch(void); -float app_balance_get_roll(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); diff --git a/applications/app_balance.c b/applications/app_balance.c index c84fbca0..a7084d57 100644 --- a/applications/app_balance.c +++ b/applications/app_balance.c @@ -110,10 +110,10 @@ void app_balance_stop(void) { float app_balance_get_pid_output(void) { return pid_value; } -float app_balance_get_pitch(void) { +float app_balance_get_m_angle(void) { return m_angle; } -float app_balance_get_roll(void) { +float app_balance_get_c_angle(void) { return c_angle; } uint32_t app_balance_get_diff_time(void) { diff --git a/commands.c b/commands.c index 8c8bbd1b..96637707 100644 --- a/commands.c +++ b/commands.c @@ -515,8 +515,8 @@ void commands_process_packet(unsigned char *data, unsigned int len, 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_pitch() * 1000000.0), &ind); - buffer_append_int32(send_buffer, (int32_t)(app_balance_get_roll() * 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); diff --git a/datatypes.h b/datatypes.h index ddf311d5..9a5ac667 100644 --- a/datatypes.h +++ b/datatypes.h @@ -643,7 +643,6 @@ typedef enum { COMM_GET_DECODED_PPM, COMM_GET_DECODED_ADC, COMM_GET_DECODED_CHUK, - COMM_GET_DECODED_BALANCE, COMM_FORWARD_CAN, COMM_SET_CHUCK_DATA, COMM_CUSTOM_APP_DATA, @@ -689,6 +688,7 @@ typedef enum { COMM_PLOT_DATA, COMM_PLOT_ADD_GRAPH, COMM_PLOT_SET_GRAPH, + COMM_GET_DECODED_BALANCE, } COMM_PACKET_ID; // CAN commands From 1cc9a9b14c5cbaf8fa4494362ca813cde19bd3ba Mon Sep 17 00:00:00 2001 From: Mitch Lustig Date: Sun, 8 Sep 2019 22:38:09 -0700 Subject: [PATCH 21/31] Re-implement startup AHRS config override So my EUC stops trying to kill me when i boot it up laying down --- applications/app.c | 2 +- applications/app.h | 2 +- applications/app_balance.c | 8 +++++++- 3 files changed, 9 insertions(+), 3 deletions(-) diff --git a/applications/app.c b/applications/app.c index b5752214..7aa765d8 100644 --- a/applications/app.c +++ b/applications/app.c @@ -69,7 +69,7 @@ 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); + app_balance_configure(&appconf.app_balance_conf, &appconf.imu_conf); switch (appconf.app_to_use) { case APP_PPM: diff --git a/applications/app.h b/applications/app.h index edac85e1..e86ec00b 100644 --- a/applications/app.h +++ b/applications/app.h @@ -56,7 +56,7 @@ 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); +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); diff --git a/applications/app_balance.c b/applications/app_balance.c index a7084d57..7fb09442 100644 --- a/applications/app_balance.c +++ b/applications/app_balance.c @@ -51,6 +51,7 @@ 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 @@ -69,8 +70,9 @@ static systime_t startup_start_time, startup_diff_time; static float motor_current; static float motor_position; -void app_balance_configure(balance_config *conf) { +void app_balance_configure(balance_config *conf, imu_config *conf2) { balance_conf = *conf; + imu_conf = *conf2; } void app_balance_start(void) { @@ -206,11 +208,15 @@ static THD_FUNCTION(balance_thread, arg) { case (STARTUP): if(startup_start_time == 0){ startup_start_time = current_time; + // Overwrite AHRS config + ahrs_update_all_parameters(1.00, imu_conf.mahony_kp, imu_conf.mahony_ki, 2.00); } startup_diff_time = current_time - startup_start_time; // Calibration is done if(ST2MS(startup_diff_time) > 1000){ + // Restore AHRS config + ahrs_update_all_parameters(imu_conf.accel_confidence_decay, imu_conf.mahony_kp, imu_conf.mahony_ki, imu_conf.madgwick_beta); // Set fault and wait for valid startup condition state = FAULT; startup_start_time = 0; From 67a4653bd89c7c607899ddbe9c49818cb9f8af08 Mon Sep 17 00:00:00 2001 From: Mitch Lustig Date: Tue, 10 Sep 2019 01:07:05 -0700 Subject: [PATCH 22/31] Add support for dual fault switches --- appconf/appconf_default.h | 7 +++++-- applications/app.h | 1 + applications/app_balance.c | 34 +++++++++++++++++++++++++++++++--- commands.c | 1 + confgenerator.c | 3 +++ confgenerator.h | 2 +- datatypes.h | 1 + 7 files changed, 43 insertions(+), 6 deletions(-) diff --git a/appconf/appconf_default.h b/appconf/appconf_default.h index 1045bf25..3a528d7f 100644 --- a/appconf/appconf_default.h +++ b/appconf/appconf_default.h @@ -283,10 +283,13 @@ #define APPCONF_BALANCE_C_AXIS 1 #endif #ifndef APPCONF_BALANCE_M_FAULT -#define APPCONF_BALANCE_M_FAULT 20 +#define APPCONF_BALANCE_M_FAULT 20 #endif #ifndef APPCONF_BALANCE_C_FAULT -#define APPCONF_BALANCE_C_FAULT 45 +#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 diff --git a/applications/app.h b/applications/app.h index e86ec00b..d1932c93 100644 --- a/applications/app.h +++ b/applications/app.h @@ -64,6 +64,7 @@ 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); diff --git a/applications/app_balance.c b/applications/app_balance.c index 7fb09442..7c29da7e 100644 --- a/applications/app_balance.c +++ b/applications/app_balance.c @@ -65,6 +65,7 @@ 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; @@ -81,6 +82,7 @@ void app_balance_start(void) { state = STARTUP; m_angle = 0; c_angle = 0; + switches_value = 0; proportional = 0; integral = 0; derivative = 0; @@ -97,6 +99,12 @@ void app_balance_start(void) { startup_start_time = 0; startup_diff_time = 0; + // 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); + } + // Start the balance thread app_thread = chThdCreateStatic(balance_thread_wa, sizeof(balance_thread_wa), NORMALPRIO, balance_thread, NULL); } @@ -130,6 +138,9 @@ float app_balance_get_motor_position(void) { 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){ @@ -203,6 +214,18 @@ static THD_FUNCTION(balance_thread, arg) { break; } + if(!balance_conf.use_switches){ + switches_value = 2; + }else{ + switches_value = 0; + 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; + } + } + // State based logic switch(state){ case (STARTUP): @@ -230,7 +253,12 @@ static THD_FUNCTION(balance_thread, arg) { } // Check for fault - if(fabsf(m_angle) > balance_conf.m_fault || fabsf(c_angle) > balance_conf.c_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; } @@ -287,8 +315,8 @@ static THD_FUNCTION(balance_thread, arg) { } break; case (FAULT): - // Check for valid startup position - if(fabsf(m_angle) < balance_conf.startup_m_tolerance && fabsf(c_angle) < balance_conf.startup_c_tolerance){ + // 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; diff --git a/commands.c b/commands.c index 96637707..94fd0be9 100644 --- a/commands.c +++ b/commands.c @@ -521,6 +521,7 @@ void commands_process_packet(unsigned char *data, unsigned int len, 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; diff --git a/confgenerator.c b/confgenerator.c index f69255ad..ff8615dc 100644 --- a/confgenerator.c +++ b/confgenerator.c @@ -229,6 +229,7 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration 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); @@ -493,6 +494,7 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration 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); @@ -741,6 +743,7 @@ void confgenerator_set_defaults_appconf(app_configuration *conf) { 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; diff --git a/confgenerator.h b/confgenerator.h index c9cd9cb1..0ff856c7 100644 --- a/confgenerator.h +++ b/confgenerator.h @@ -9,7 +9,7 @@ // Constants #define MCCONF_SIGNATURE 503309878 -#define APPCONF_SIGNATURE 3772026662 +#define APPCONF_SIGNATURE 4290437352 // Functions int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf); diff --git a/datatypes.h b/datatypes.h index 9a5ac667..dd6fae46 100644 --- a/datatypes.h +++ b/datatypes.h @@ -502,6 +502,7 @@ typedef struct { AXIS c_axis; float m_fault; float c_fault; + bool use_switches; float overspeed_duty; float tiltback_duty; float tiltback_angle; From fe53fdf716c33cdb9361dc07264555e2abb2267c Mon Sep 17 00:00:00 2001 From: Mitch Lustig Date: Tue, 10 Sep 2019 01:14:32 -0700 Subject: [PATCH 23/31] Update conf gen BUT WHY IS IT 0 THO?!? --- confgenerator.c | 342 ------------------------------------------------ confgenerator.h | 2 +- 2 files changed, 1 insertion(+), 343 deletions(-) diff --git a/confgenerator.c b/confgenerator.c index ff8615dc..4ef70fb4 100644 --- a/confgenerator.c +++ b/confgenerator.c @@ -145,120 +145,6 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration buffer_append_uint32(buffer, APPCONF_SIGNATURE, &ind); - buffer[ind++] = (uint8_t)conf->controller_id; - buffer_append_uint32(buffer, conf->timeout_msec, &ind); - buffer_append_float32_auto(buffer, conf->timeout_brake_current, &ind); - buffer[ind++] = conf->send_can_status; - buffer_append_uint16(buffer, conf->send_can_status_rate_hz, &ind); - buffer[ind++] = conf->can_baud_rate; - buffer[ind++] = conf->pairing_done; - buffer[ind++] = conf->permanent_uart_enabled; - buffer[ind++] = conf->shutdown_mode; - buffer[ind++] = conf->uavcan_enable; - buffer[ind++] = (uint8_t)conf->uavcan_esc_index; - buffer[ind++] = conf->app_to_use; - buffer[ind++] = conf->app_ppm_conf.ctrl_type; - buffer_append_float32_auto(buffer, conf->app_ppm_conf.pid_max_erpm, &ind); - buffer_append_float32_auto(buffer, conf->app_ppm_conf.hyst, &ind); - buffer_append_float32_auto(buffer, conf->app_ppm_conf.pulse_start, &ind); - buffer_append_float32_auto(buffer, conf->app_ppm_conf.pulse_end, &ind); - buffer_append_float32_auto(buffer, conf->app_ppm_conf.pulse_center, &ind); - buffer[ind++] = conf->app_ppm_conf.median_filter; - buffer[ind++] = conf->app_ppm_conf.safe_start; - buffer_append_float32_auto(buffer, conf->app_ppm_conf.throttle_exp, &ind); - buffer_append_float32_auto(buffer, conf->app_ppm_conf.throttle_exp_brake, &ind); - buffer[ind++] = conf->app_ppm_conf.throttle_exp_mode; - buffer_append_float32_auto(buffer, conf->app_ppm_conf.ramp_time_pos, &ind); - buffer_append_float32_auto(buffer, conf->app_ppm_conf.ramp_time_neg, &ind); - buffer[ind++] = conf->app_ppm_conf.multi_esc; - buffer[ind++] = conf->app_ppm_conf.tc; - buffer_append_float32_auto(buffer, conf->app_ppm_conf.tc_max_diff, &ind); - buffer_append_float32_auto(buffer, conf->app_ppm_conf.max_erpm_for_dir, &ind); - buffer[ind++] = conf->app_adc_conf.ctrl_type; - buffer_append_float32_auto(buffer, conf->app_adc_conf.hyst, &ind); - buffer_append_float32_auto(buffer, conf->app_adc_conf.voltage_start, &ind); - buffer_append_float32_auto(buffer, conf->app_adc_conf.voltage_end, &ind); - buffer_append_float32_auto(buffer, conf->app_adc_conf.voltage_center, &ind); - buffer_append_float32_auto(buffer, conf->app_adc_conf.voltage2_start, &ind); - buffer_append_float32_auto(buffer, conf->app_adc_conf.voltage2_end, &ind); - buffer[ind++] = conf->app_adc_conf.use_filter; - buffer[ind++] = conf->app_adc_conf.safe_start; - buffer[ind++] = conf->app_adc_conf.cc_button_inverted; - buffer[ind++] = conf->app_adc_conf.rev_button_inverted; - buffer[ind++] = conf->app_adc_conf.voltage_inverted; - buffer[ind++] = conf->app_adc_conf.voltage2_inverted; - buffer_append_float32_auto(buffer, conf->app_adc_conf.throttle_exp, &ind); - buffer_append_float32_auto(buffer, conf->app_adc_conf.throttle_exp_brake, &ind); - buffer[ind++] = conf->app_adc_conf.throttle_exp_mode; - buffer_append_float32_auto(buffer, conf->app_adc_conf.ramp_time_pos, &ind); - buffer_append_float32_auto(buffer, conf->app_adc_conf.ramp_time_neg, &ind); - buffer[ind++] = conf->app_adc_conf.multi_esc; - buffer[ind++] = conf->app_adc_conf.tc; - buffer_append_float32_auto(buffer, conf->app_adc_conf.tc_max_diff, &ind); - buffer_append_uint16(buffer, conf->app_adc_conf.update_rate_hz, &ind); - buffer_append_uint32(buffer, conf->app_uart_baudrate, &ind); - buffer[ind++] = conf->app_chuk_conf.ctrl_type; - buffer_append_float32_auto(buffer, conf->app_chuk_conf.hyst, &ind); - buffer_append_float32_auto(buffer, conf->app_chuk_conf.ramp_time_pos, &ind); - buffer_append_float32_auto(buffer, conf->app_chuk_conf.ramp_time_neg, &ind); - buffer_append_float32_auto(buffer, conf->app_chuk_conf.stick_erpm_per_s_in_cc, &ind); - buffer_append_float32_auto(buffer, conf->app_chuk_conf.throttle_exp, &ind); - buffer_append_float32_auto(buffer, conf->app_chuk_conf.throttle_exp_brake, &ind); - buffer[ind++] = conf->app_chuk_conf.throttle_exp_mode; - buffer[ind++] = conf->app_chuk_conf.multi_esc; - buffer[ind++] = conf->app_chuk_conf.tc; - buffer_append_float32_auto(buffer, conf->app_chuk_conf.tc_max_diff, &ind); - buffer[ind++] = conf->app_chuk_conf.use_smart_rev; - buffer_append_float32_auto(buffer, conf->app_chuk_conf.smart_rev_max_duty, &ind); - buffer_append_float32_auto(buffer, conf->app_chuk_conf.smart_rev_ramp_time, &ind); - buffer[ind++] = conf->app_nrf_conf.speed; - buffer[ind++] = conf->app_nrf_conf.power; - buffer[ind++] = conf->app_nrf_conf.crc_type; - buffer[ind++] = conf->app_nrf_conf.retry_delay; - buffer[ind++] = (uint8_t)conf->app_nrf_conf.retries; - buffer[ind++] = (uint8_t)conf->app_nrf_conf.channel; - buffer[ind++] = (uint8_t)conf->app_nrf_conf.address[0]; - 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.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); - buffer_append_float32_auto(buffer, conf->imu_conf.accel_confidence_decay, &ind); - buffer_append_float32_auto(buffer, conf->imu_conf.mahony_kp, &ind); - buffer_append_float32_auto(buffer, conf->imu_conf.mahony_ki, &ind); - buffer_append_float32_auto(buffer, conf->imu_conf.madgwick_beta, &ind); - buffer_append_float32_auto(buffer, conf->imu_conf.rot_roll, &ind); - buffer_append_float32_auto(buffer, conf->imu_conf.rot_pitch, &ind); - buffer_append_float32_auto(buffer, conf->imu_conf.rot_yaw, &ind); - buffer_append_float32_auto(buffer, conf->imu_conf.accel_offsets[0], &ind); - buffer_append_float32_auto(buffer, conf->imu_conf.accel_offsets[1], &ind); - buffer_append_float32_auto(buffer, conf->imu_conf.accel_offsets[2], &ind); - buffer_append_float32_auto(buffer, conf->imu_conf.gyro_offsets[0], &ind); - buffer_append_float32_auto(buffer, conf->imu_conf.gyro_offsets[1], &ind); - buffer_append_float32_auto(buffer, conf->imu_conf.gyro_offsets[2], &ind); - buffer_append_float32_auto(buffer, conf->imu_conf.gyro_offset_comp_fact[0], &ind); - buffer_append_float32_auto(buffer, conf->imu_conf.gyro_offset_comp_fact[1], &ind); - buffer_append_float32_auto(buffer, conf->imu_conf.gyro_offset_comp_fact[2], &ind); - buffer_append_float32_auto(buffer, conf->imu_conf.gyro_offset_comp_clamp, &ind); return ind; } @@ -410,120 +296,6 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration return false; } - conf->controller_id = buffer[ind++]; - conf->timeout_msec = buffer_get_uint32(buffer, &ind); - conf->timeout_brake_current = buffer_get_float32_auto(buffer, &ind); - conf->send_can_status = buffer[ind++]; - conf->send_can_status_rate_hz = buffer_get_uint16(buffer, &ind); - conf->can_baud_rate = buffer[ind++]; - conf->pairing_done = buffer[ind++]; - conf->permanent_uart_enabled = buffer[ind++]; - conf->shutdown_mode = buffer[ind++]; - conf->uavcan_enable = buffer[ind++]; - conf->uavcan_esc_index = buffer[ind++]; - conf->app_to_use = buffer[ind++]; - conf->app_ppm_conf.ctrl_type = buffer[ind++]; - conf->app_ppm_conf.pid_max_erpm = buffer_get_float32_auto(buffer, &ind); - conf->app_ppm_conf.hyst = buffer_get_float32_auto(buffer, &ind); - conf->app_ppm_conf.pulse_start = buffer_get_float32_auto(buffer, &ind); - conf->app_ppm_conf.pulse_end = buffer_get_float32_auto(buffer, &ind); - conf->app_ppm_conf.pulse_center = buffer_get_float32_auto(buffer, &ind); - conf->app_ppm_conf.median_filter = buffer[ind++]; - conf->app_ppm_conf.safe_start = buffer[ind++]; - conf->app_ppm_conf.throttle_exp = buffer_get_float32_auto(buffer, &ind); - conf->app_ppm_conf.throttle_exp_brake = buffer_get_float32_auto(buffer, &ind); - conf->app_ppm_conf.throttle_exp_mode = buffer[ind++]; - conf->app_ppm_conf.ramp_time_pos = buffer_get_float32_auto(buffer, &ind); - conf->app_ppm_conf.ramp_time_neg = buffer_get_float32_auto(buffer, &ind); - conf->app_ppm_conf.multi_esc = buffer[ind++]; - conf->app_ppm_conf.tc = buffer[ind++]; - conf->app_ppm_conf.tc_max_diff = buffer_get_float32_auto(buffer, &ind); - conf->app_ppm_conf.max_erpm_for_dir = buffer_get_float32_auto(buffer, &ind); - conf->app_adc_conf.ctrl_type = buffer[ind++]; - conf->app_adc_conf.hyst = buffer_get_float32_auto(buffer, &ind); - conf->app_adc_conf.voltage_start = buffer_get_float32_auto(buffer, &ind); - conf->app_adc_conf.voltage_end = buffer_get_float32_auto(buffer, &ind); - conf->app_adc_conf.voltage_center = buffer_get_float32_auto(buffer, &ind); - conf->app_adc_conf.voltage2_start = buffer_get_float32_auto(buffer, &ind); - conf->app_adc_conf.voltage2_end = buffer_get_float32_auto(buffer, &ind); - conf->app_adc_conf.use_filter = buffer[ind++]; - conf->app_adc_conf.safe_start = buffer[ind++]; - conf->app_adc_conf.cc_button_inverted = buffer[ind++]; - conf->app_adc_conf.rev_button_inverted = buffer[ind++]; - conf->app_adc_conf.voltage_inverted = buffer[ind++]; - conf->app_adc_conf.voltage2_inverted = buffer[ind++]; - conf->app_adc_conf.throttle_exp = buffer_get_float32_auto(buffer, &ind); - conf->app_adc_conf.throttle_exp_brake = buffer_get_float32_auto(buffer, &ind); - conf->app_adc_conf.throttle_exp_mode = buffer[ind++]; - conf->app_adc_conf.ramp_time_pos = buffer_get_float32_auto(buffer, &ind); - conf->app_adc_conf.ramp_time_neg = buffer_get_float32_auto(buffer, &ind); - conf->app_adc_conf.multi_esc = buffer[ind++]; - conf->app_adc_conf.tc = buffer[ind++]; - conf->app_adc_conf.tc_max_diff = buffer_get_float32_auto(buffer, &ind); - conf->app_adc_conf.update_rate_hz = buffer_get_uint16(buffer, &ind); - conf->app_uart_baudrate = buffer_get_uint32(buffer, &ind); - conf->app_chuk_conf.ctrl_type = buffer[ind++]; - conf->app_chuk_conf.hyst = buffer_get_float32_auto(buffer, &ind); - conf->app_chuk_conf.ramp_time_pos = buffer_get_float32_auto(buffer, &ind); - conf->app_chuk_conf.ramp_time_neg = buffer_get_float32_auto(buffer, &ind); - conf->app_chuk_conf.stick_erpm_per_s_in_cc = buffer_get_float32_auto(buffer, &ind); - conf->app_chuk_conf.throttle_exp = buffer_get_float32_auto(buffer, &ind); - conf->app_chuk_conf.throttle_exp_brake = buffer_get_float32_auto(buffer, &ind); - conf->app_chuk_conf.throttle_exp_mode = buffer[ind++]; - conf->app_chuk_conf.multi_esc = buffer[ind++]; - conf->app_chuk_conf.tc = buffer[ind++]; - conf->app_chuk_conf.tc_max_diff = buffer_get_float32_auto(buffer, &ind); - conf->app_chuk_conf.use_smart_rev = buffer[ind++]; - conf->app_chuk_conf.smart_rev_max_duty = buffer_get_float32_auto(buffer, &ind); - conf->app_chuk_conf.smart_rev_ramp_time = buffer_get_float32_auto(buffer, &ind); - conf->app_nrf_conf.speed = buffer[ind++]; - conf->app_nrf_conf.power = buffer[ind++]; - conf->app_nrf_conf.crc_type = buffer[ind++]; - conf->app_nrf_conf.retry_delay = buffer[ind++]; - conf->app_nrf_conf.retries = (int8_t)buffer[ind++]; - conf->app_nrf_conf.channel = (int8_t)buffer[ind++]; - conf->app_nrf_conf.address[0] = buffer[ind++]; - 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.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); - conf->imu_conf.accel_confidence_decay = buffer_get_float32_auto(buffer, &ind); - conf->imu_conf.mahony_kp = buffer_get_float32_auto(buffer, &ind); - conf->imu_conf.mahony_ki = buffer_get_float32_auto(buffer, &ind); - conf->imu_conf.madgwick_beta = buffer_get_float32_auto(buffer, &ind); - conf->imu_conf.rot_roll = buffer_get_float32_auto(buffer, &ind); - conf->imu_conf.rot_pitch = buffer_get_float32_auto(buffer, &ind); - conf->imu_conf.rot_yaw = buffer_get_float32_auto(buffer, &ind); - conf->imu_conf.accel_offsets[0] = buffer_get_float32_auto(buffer, &ind); - conf->imu_conf.accel_offsets[1] = buffer_get_float32_auto(buffer, &ind); - conf->imu_conf.accel_offsets[2] = buffer_get_float32_auto(buffer, &ind); - conf->imu_conf.gyro_offsets[0] = buffer_get_float32_auto(buffer, &ind); - conf->imu_conf.gyro_offsets[1] = buffer_get_float32_auto(buffer, &ind); - conf->imu_conf.gyro_offsets[2] = buffer_get_float32_auto(buffer, &ind); - conf->imu_conf.gyro_offset_comp_fact[0] = buffer_get_float32_auto(buffer, &ind); - conf->imu_conf.gyro_offset_comp_fact[1] = buffer_get_float32_auto(buffer, &ind); - conf->imu_conf.gyro_offset_comp_fact[2] = buffer_get_float32_auto(buffer, &ind); - conf->imu_conf.gyro_offset_comp_clamp = buffer_get_float32_auto(buffer, &ind); return true; } @@ -659,118 +431,4 @@ void confgenerator_set_defaults_mcconf(mc_configuration *conf) { } void confgenerator_set_defaults_appconf(app_configuration *conf) { - conf->controller_id = HW_DEFAULT_ID; - conf->timeout_msec = APPCONF_TIMEOUT_MSEC; - conf->timeout_brake_current = APPCONF_TIMEOUT_BRAKE_CURRENT; - conf->send_can_status = APPCONF_SEND_CAN_STATUS; - conf->send_can_status_rate_hz = APPCONF_SEND_CAN_STATUS_RATE_HZ; - conf->can_baud_rate = APPCONF_CAN_BAUD_RATE; - conf->pairing_done = APPCONF_PAIRING_DONE; - conf->permanent_uart_enabled = APPCONF_PERMANENT_UART_ENABLED; - conf->shutdown_mode = APPCONF_SHUTDOWN_MODE; - conf->uavcan_enable = APPCONF_UAVCAN_ENABLE; - conf->uavcan_esc_index = APPCONF_UAVCAN_ESC_INDEX; - conf->app_to_use = APPCONF_APP_TO_USE; - conf->app_ppm_conf.ctrl_type = APPCONF_PPM_CTRL_TYPE; - conf->app_ppm_conf.pid_max_erpm = APPCONF_PPM_PID_MAX_ERPM; - conf->app_ppm_conf.hyst = APPCONF_PPM_HYST; - conf->app_ppm_conf.pulse_start = APPCONF_PPM_PULSE_START; - conf->app_ppm_conf.pulse_end = APPCONF_PPM_PULSE_END; - conf->app_ppm_conf.pulse_center = APPCONF_PPM_PULSE_CENTER; - conf->app_ppm_conf.median_filter = APPCONF_PPM_MEDIAN_FILTER; - conf->app_ppm_conf.safe_start = APPCONF_PPM_SAFE_START; - conf->app_ppm_conf.throttle_exp = APPCONF_PPM_THROTTLE_EXP; - conf->app_ppm_conf.throttle_exp_brake = APPCONF_PPM_THROTTLE_EXP_BRAKE; - conf->app_ppm_conf.throttle_exp_mode = APPCONF_PPM_THROTTLE_EXP_MODE; - conf->app_ppm_conf.ramp_time_pos = APPCONF_PPM_RAMP_TIME_POS; - conf->app_ppm_conf.ramp_time_neg = APPCONF_PPM_RAMP_TIME_NEG; - 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_ppm_conf.max_erpm_for_dir = APPCONF_PPM_MAX_ERPM_FOR_DIR; - 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; - conf->app_adc_conf.voltage_end = APPCONF_ADC_VOLTAGE_END; - conf->app_adc_conf.voltage_center = APPCONF_ADC_VOLTAGE_CENTER; - conf->app_adc_conf.voltage2_start = APPCONF_ADC_VOLTAGE2_START; - conf->app_adc_conf.voltage2_end = APPCONF_ADC_VOLTAGE2_END; - conf->app_adc_conf.use_filter = APPCONF_ADC_USE_FILTER; - conf->app_adc_conf.safe_start = APPCONF_ADC_SAFE_START; - conf->app_adc_conf.cc_button_inverted = APPCONF_ADC_CC_BUTTON_INVERTED; - conf->app_adc_conf.rev_button_inverted = APPCONF_ADC_REV_BUTTON_INVERTED; - conf->app_adc_conf.voltage_inverted = APPCONF_ADC_VOLTAGE_INVERTED; - conf->app_adc_conf.voltage2_inverted = APPCONF_ADC_VOLTAGE2_INVERTED; - conf->app_adc_conf.throttle_exp = APPCONF_ADC_THROTTLE_EXP; - conf->app_adc_conf.throttle_exp_brake = APPCONF_ADC_THROTTLE_EXP_BRAKE; - conf->app_adc_conf.throttle_exp_mode = APPCONF_ADC_THROTTLE_EXP_MODE; - conf->app_adc_conf.ramp_time_pos = APPCONF_ADC_RAMP_TIME_POS; - conf->app_adc_conf.ramp_time_neg = APPCONF_ADC_RAMP_TIME_NEG; - conf->app_adc_conf.multi_esc = APPCONF_ADC_MULTI_ESC; - conf->app_adc_conf.tc = APPCONF_ADC_TC; - conf->app_adc_conf.tc_max_diff = APPCONF_ADC_TC_MAX_DIFF; - conf->app_adc_conf.update_rate_hz = APPCONF_ADC_UPDATE_RATE_HZ; - conf->app_uart_baudrate = APPCONF_UART_BAUDRATE; - conf->app_chuk_conf.ctrl_type = APPCONF_CHUK_CTRL_TYPE; - conf->app_chuk_conf.hyst = APPCONF_CHUK_HYST; - conf->app_chuk_conf.ramp_time_pos = APPCONF_CHUK_RAMP_TIME_POS; - conf->app_chuk_conf.ramp_time_neg = APPCONF_CHUK_RAMP_TIME_NEG; - conf->app_chuk_conf.stick_erpm_per_s_in_cc = APPCONF_STICK_ERPM_PER_S_IN_CC; - conf->app_chuk_conf.throttle_exp = APPCONF_CHUK_THROTTLE_EXP; - conf->app_chuk_conf.throttle_exp_brake = APPCONF_CHUK_THROTTLE_EXP_BRAKE; - conf->app_chuk_conf.throttle_exp_mode = APPCONF_CHUK_THROTTLE_EXP_MODE; - 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_chuk_conf.use_smart_rev = APPCONF_CHUK_USE_SMART_REV; - conf->app_chuk_conf.smart_rev_max_duty = APPCONF_CHUK_SMART_REV_MAX_DUTY; - conf->app_chuk_conf.smart_rev_ramp_time = APPCONF_CHUK_SMART_REV_RAMP_TIME; - 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; - 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.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; - conf->imu_conf.accel_confidence_decay = APPCONF_IMU_ACCEL_CONFIDENCE_DECAY; - conf->imu_conf.mahony_kp = APPCONF_IMU_MAHONY_KP; - conf->imu_conf.mahony_ki = APPCONF_IMU_MAHONY_KI; - conf->imu_conf.madgwick_beta = APPCONF_IMU_MADGWICK_BETA; - conf->imu_conf.rot_roll = APPCONF_IMU_ROT_ROLL; - conf->imu_conf.rot_pitch = APPCONF_IMU_ROT_PITCH; - conf->imu_conf.rot_yaw = APPCONF_IMU_ROT_YAW; - conf->imu_conf.accel_offsets[0] = APPCONF_IMU_A_OFFSET_0; - conf->imu_conf.accel_offsets[1] = APPCONF_IMU_A_OFFSET_1; - conf->imu_conf.accel_offsets[2] = APPCONF_IMU_A_OFFSET_2; - conf->imu_conf.gyro_offsets[0] = APPCONF_IMU_G_OFFSET_0; - conf->imu_conf.gyro_offsets[1] = APPCONF_IMU_G_OFFSET_1; - conf->imu_conf.gyro_offsets[2] = APPCONF_IMU_G_OFFSET_2; - conf->imu_conf.gyro_offset_comp_fact[0] = APPCONF_IMU_G_OFFSET_COMP_FACT_0; - conf->imu_conf.gyro_offset_comp_fact[1] = APPCONF_IMU_G_OFFSET_COMP_FACT_1; - conf->imu_conf.gyro_offset_comp_fact[2] = APPCONF_IMU_G_OFFSET_COMP_FACT_2; - conf->imu_conf.gyro_offset_comp_clamp = APPCONF_IMU_G_OFFSET_COMP_CLAMP; } diff --git a/confgenerator.h b/confgenerator.h index 0ff856c7..59e1dfdc 100644 --- a/confgenerator.h +++ b/confgenerator.h @@ -9,7 +9,7 @@ // Constants #define MCCONF_SIGNATURE 503309878 -#define APPCONF_SIGNATURE 4290437352 +#define APPCONF_SIGNATURE 0 // Functions int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf); From 5d3f829e69a9dac133ce9df066456444894b1ff8 Mon Sep 17 00:00:00 2001 From: Mitch Lustig Date: Tue, 10 Sep 2019 01:38:03 -0700 Subject: [PATCH 24/31] Fix conf gen --- confgenerator.c | 342 ++++++++++++++++++++++++++++++++++++++++++++++++ confgenerator.h | 2 +- 2 files changed, 343 insertions(+), 1 deletion(-) diff --git a/confgenerator.c b/confgenerator.c index 4ef70fb4..ff8615dc 100644 --- a/confgenerator.c +++ b/confgenerator.c @@ -145,6 +145,120 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration buffer_append_uint32(buffer, APPCONF_SIGNATURE, &ind); + buffer[ind++] = (uint8_t)conf->controller_id; + buffer_append_uint32(buffer, conf->timeout_msec, &ind); + buffer_append_float32_auto(buffer, conf->timeout_brake_current, &ind); + buffer[ind++] = conf->send_can_status; + buffer_append_uint16(buffer, conf->send_can_status_rate_hz, &ind); + buffer[ind++] = conf->can_baud_rate; + buffer[ind++] = conf->pairing_done; + buffer[ind++] = conf->permanent_uart_enabled; + buffer[ind++] = conf->shutdown_mode; + buffer[ind++] = conf->uavcan_enable; + buffer[ind++] = (uint8_t)conf->uavcan_esc_index; + buffer[ind++] = conf->app_to_use; + buffer[ind++] = conf->app_ppm_conf.ctrl_type; + buffer_append_float32_auto(buffer, conf->app_ppm_conf.pid_max_erpm, &ind); + buffer_append_float32_auto(buffer, conf->app_ppm_conf.hyst, &ind); + buffer_append_float32_auto(buffer, conf->app_ppm_conf.pulse_start, &ind); + buffer_append_float32_auto(buffer, conf->app_ppm_conf.pulse_end, &ind); + buffer_append_float32_auto(buffer, conf->app_ppm_conf.pulse_center, &ind); + buffer[ind++] = conf->app_ppm_conf.median_filter; + buffer[ind++] = conf->app_ppm_conf.safe_start; + buffer_append_float32_auto(buffer, conf->app_ppm_conf.throttle_exp, &ind); + buffer_append_float32_auto(buffer, conf->app_ppm_conf.throttle_exp_brake, &ind); + buffer[ind++] = conf->app_ppm_conf.throttle_exp_mode; + buffer_append_float32_auto(buffer, conf->app_ppm_conf.ramp_time_pos, &ind); + buffer_append_float32_auto(buffer, conf->app_ppm_conf.ramp_time_neg, &ind); + buffer[ind++] = conf->app_ppm_conf.multi_esc; + buffer[ind++] = conf->app_ppm_conf.tc; + buffer_append_float32_auto(buffer, conf->app_ppm_conf.tc_max_diff, &ind); + buffer_append_float32_auto(buffer, conf->app_ppm_conf.max_erpm_for_dir, &ind); + buffer[ind++] = conf->app_adc_conf.ctrl_type; + buffer_append_float32_auto(buffer, conf->app_adc_conf.hyst, &ind); + buffer_append_float32_auto(buffer, conf->app_adc_conf.voltage_start, &ind); + buffer_append_float32_auto(buffer, conf->app_adc_conf.voltage_end, &ind); + buffer_append_float32_auto(buffer, conf->app_adc_conf.voltage_center, &ind); + buffer_append_float32_auto(buffer, conf->app_adc_conf.voltage2_start, &ind); + buffer_append_float32_auto(buffer, conf->app_adc_conf.voltage2_end, &ind); + buffer[ind++] = conf->app_adc_conf.use_filter; + buffer[ind++] = conf->app_adc_conf.safe_start; + buffer[ind++] = conf->app_adc_conf.cc_button_inverted; + buffer[ind++] = conf->app_adc_conf.rev_button_inverted; + buffer[ind++] = conf->app_adc_conf.voltage_inverted; + buffer[ind++] = conf->app_adc_conf.voltage2_inverted; + buffer_append_float32_auto(buffer, conf->app_adc_conf.throttle_exp, &ind); + buffer_append_float32_auto(buffer, conf->app_adc_conf.throttle_exp_brake, &ind); + buffer[ind++] = conf->app_adc_conf.throttle_exp_mode; + buffer_append_float32_auto(buffer, conf->app_adc_conf.ramp_time_pos, &ind); + buffer_append_float32_auto(buffer, conf->app_adc_conf.ramp_time_neg, &ind); + buffer[ind++] = conf->app_adc_conf.multi_esc; + buffer[ind++] = conf->app_adc_conf.tc; + buffer_append_float32_auto(buffer, conf->app_adc_conf.tc_max_diff, &ind); + buffer_append_uint16(buffer, conf->app_adc_conf.update_rate_hz, &ind); + buffer_append_uint32(buffer, conf->app_uart_baudrate, &ind); + buffer[ind++] = conf->app_chuk_conf.ctrl_type; + buffer_append_float32_auto(buffer, conf->app_chuk_conf.hyst, &ind); + buffer_append_float32_auto(buffer, conf->app_chuk_conf.ramp_time_pos, &ind); + buffer_append_float32_auto(buffer, conf->app_chuk_conf.ramp_time_neg, &ind); + buffer_append_float32_auto(buffer, conf->app_chuk_conf.stick_erpm_per_s_in_cc, &ind); + buffer_append_float32_auto(buffer, conf->app_chuk_conf.throttle_exp, &ind); + buffer_append_float32_auto(buffer, conf->app_chuk_conf.throttle_exp_brake, &ind); + buffer[ind++] = conf->app_chuk_conf.throttle_exp_mode; + buffer[ind++] = conf->app_chuk_conf.multi_esc; + buffer[ind++] = conf->app_chuk_conf.tc; + buffer_append_float32_auto(buffer, conf->app_chuk_conf.tc_max_diff, &ind); + buffer[ind++] = conf->app_chuk_conf.use_smart_rev; + buffer_append_float32_auto(buffer, conf->app_chuk_conf.smart_rev_max_duty, &ind); + buffer_append_float32_auto(buffer, conf->app_chuk_conf.smart_rev_ramp_time, &ind); + buffer[ind++] = conf->app_nrf_conf.speed; + buffer[ind++] = conf->app_nrf_conf.power; + buffer[ind++] = conf->app_nrf_conf.crc_type; + buffer[ind++] = conf->app_nrf_conf.retry_delay; + buffer[ind++] = (uint8_t)conf->app_nrf_conf.retries; + buffer[ind++] = (uint8_t)conf->app_nrf_conf.channel; + buffer[ind++] = (uint8_t)conf->app_nrf_conf.address[0]; + 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.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); + buffer_append_float32_auto(buffer, conf->imu_conf.accel_confidence_decay, &ind); + buffer_append_float32_auto(buffer, conf->imu_conf.mahony_kp, &ind); + buffer_append_float32_auto(buffer, conf->imu_conf.mahony_ki, &ind); + buffer_append_float32_auto(buffer, conf->imu_conf.madgwick_beta, &ind); + buffer_append_float32_auto(buffer, conf->imu_conf.rot_roll, &ind); + buffer_append_float32_auto(buffer, conf->imu_conf.rot_pitch, &ind); + buffer_append_float32_auto(buffer, conf->imu_conf.rot_yaw, &ind); + buffer_append_float32_auto(buffer, conf->imu_conf.accel_offsets[0], &ind); + buffer_append_float32_auto(buffer, conf->imu_conf.accel_offsets[1], &ind); + buffer_append_float32_auto(buffer, conf->imu_conf.accel_offsets[2], &ind); + buffer_append_float32_auto(buffer, conf->imu_conf.gyro_offsets[0], &ind); + buffer_append_float32_auto(buffer, conf->imu_conf.gyro_offsets[1], &ind); + buffer_append_float32_auto(buffer, conf->imu_conf.gyro_offsets[2], &ind); + buffer_append_float32_auto(buffer, conf->imu_conf.gyro_offset_comp_fact[0], &ind); + buffer_append_float32_auto(buffer, conf->imu_conf.gyro_offset_comp_fact[1], &ind); + buffer_append_float32_auto(buffer, conf->imu_conf.gyro_offset_comp_fact[2], &ind); + buffer_append_float32_auto(buffer, conf->imu_conf.gyro_offset_comp_clamp, &ind); return ind; } @@ -296,6 +410,120 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration return false; } + conf->controller_id = buffer[ind++]; + conf->timeout_msec = buffer_get_uint32(buffer, &ind); + conf->timeout_brake_current = buffer_get_float32_auto(buffer, &ind); + conf->send_can_status = buffer[ind++]; + conf->send_can_status_rate_hz = buffer_get_uint16(buffer, &ind); + conf->can_baud_rate = buffer[ind++]; + conf->pairing_done = buffer[ind++]; + conf->permanent_uart_enabled = buffer[ind++]; + conf->shutdown_mode = buffer[ind++]; + conf->uavcan_enable = buffer[ind++]; + conf->uavcan_esc_index = buffer[ind++]; + conf->app_to_use = buffer[ind++]; + conf->app_ppm_conf.ctrl_type = buffer[ind++]; + conf->app_ppm_conf.pid_max_erpm = buffer_get_float32_auto(buffer, &ind); + conf->app_ppm_conf.hyst = buffer_get_float32_auto(buffer, &ind); + conf->app_ppm_conf.pulse_start = buffer_get_float32_auto(buffer, &ind); + conf->app_ppm_conf.pulse_end = buffer_get_float32_auto(buffer, &ind); + conf->app_ppm_conf.pulse_center = buffer_get_float32_auto(buffer, &ind); + conf->app_ppm_conf.median_filter = buffer[ind++]; + conf->app_ppm_conf.safe_start = buffer[ind++]; + conf->app_ppm_conf.throttle_exp = buffer_get_float32_auto(buffer, &ind); + conf->app_ppm_conf.throttle_exp_brake = buffer_get_float32_auto(buffer, &ind); + conf->app_ppm_conf.throttle_exp_mode = buffer[ind++]; + conf->app_ppm_conf.ramp_time_pos = buffer_get_float32_auto(buffer, &ind); + conf->app_ppm_conf.ramp_time_neg = buffer_get_float32_auto(buffer, &ind); + conf->app_ppm_conf.multi_esc = buffer[ind++]; + conf->app_ppm_conf.tc = buffer[ind++]; + conf->app_ppm_conf.tc_max_diff = buffer_get_float32_auto(buffer, &ind); + conf->app_ppm_conf.max_erpm_for_dir = buffer_get_float32_auto(buffer, &ind); + conf->app_adc_conf.ctrl_type = buffer[ind++]; + conf->app_adc_conf.hyst = buffer_get_float32_auto(buffer, &ind); + conf->app_adc_conf.voltage_start = buffer_get_float32_auto(buffer, &ind); + conf->app_adc_conf.voltage_end = buffer_get_float32_auto(buffer, &ind); + conf->app_adc_conf.voltage_center = buffer_get_float32_auto(buffer, &ind); + conf->app_adc_conf.voltage2_start = buffer_get_float32_auto(buffer, &ind); + conf->app_adc_conf.voltage2_end = buffer_get_float32_auto(buffer, &ind); + conf->app_adc_conf.use_filter = buffer[ind++]; + conf->app_adc_conf.safe_start = buffer[ind++]; + conf->app_adc_conf.cc_button_inverted = buffer[ind++]; + conf->app_adc_conf.rev_button_inverted = buffer[ind++]; + conf->app_adc_conf.voltage_inverted = buffer[ind++]; + conf->app_adc_conf.voltage2_inverted = buffer[ind++]; + conf->app_adc_conf.throttle_exp = buffer_get_float32_auto(buffer, &ind); + conf->app_adc_conf.throttle_exp_brake = buffer_get_float32_auto(buffer, &ind); + conf->app_adc_conf.throttle_exp_mode = buffer[ind++]; + conf->app_adc_conf.ramp_time_pos = buffer_get_float32_auto(buffer, &ind); + conf->app_adc_conf.ramp_time_neg = buffer_get_float32_auto(buffer, &ind); + conf->app_adc_conf.multi_esc = buffer[ind++]; + conf->app_adc_conf.tc = buffer[ind++]; + conf->app_adc_conf.tc_max_diff = buffer_get_float32_auto(buffer, &ind); + conf->app_adc_conf.update_rate_hz = buffer_get_uint16(buffer, &ind); + conf->app_uart_baudrate = buffer_get_uint32(buffer, &ind); + conf->app_chuk_conf.ctrl_type = buffer[ind++]; + conf->app_chuk_conf.hyst = buffer_get_float32_auto(buffer, &ind); + conf->app_chuk_conf.ramp_time_pos = buffer_get_float32_auto(buffer, &ind); + conf->app_chuk_conf.ramp_time_neg = buffer_get_float32_auto(buffer, &ind); + conf->app_chuk_conf.stick_erpm_per_s_in_cc = buffer_get_float32_auto(buffer, &ind); + conf->app_chuk_conf.throttle_exp = buffer_get_float32_auto(buffer, &ind); + conf->app_chuk_conf.throttle_exp_brake = buffer_get_float32_auto(buffer, &ind); + conf->app_chuk_conf.throttle_exp_mode = buffer[ind++]; + conf->app_chuk_conf.multi_esc = buffer[ind++]; + conf->app_chuk_conf.tc = buffer[ind++]; + conf->app_chuk_conf.tc_max_diff = buffer_get_float32_auto(buffer, &ind); + conf->app_chuk_conf.use_smart_rev = buffer[ind++]; + conf->app_chuk_conf.smart_rev_max_duty = buffer_get_float32_auto(buffer, &ind); + conf->app_chuk_conf.smart_rev_ramp_time = buffer_get_float32_auto(buffer, &ind); + conf->app_nrf_conf.speed = buffer[ind++]; + conf->app_nrf_conf.power = buffer[ind++]; + conf->app_nrf_conf.crc_type = buffer[ind++]; + conf->app_nrf_conf.retry_delay = buffer[ind++]; + conf->app_nrf_conf.retries = (int8_t)buffer[ind++]; + conf->app_nrf_conf.channel = (int8_t)buffer[ind++]; + conf->app_nrf_conf.address[0] = buffer[ind++]; + 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.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); + conf->imu_conf.accel_confidence_decay = buffer_get_float32_auto(buffer, &ind); + conf->imu_conf.mahony_kp = buffer_get_float32_auto(buffer, &ind); + conf->imu_conf.mahony_ki = buffer_get_float32_auto(buffer, &ind); + conf->imu_conf.madgwick_beta = buffer_get_float32_auto(buffer, &ind); + conf->imu_conf.rot_roll = buffer_get_float32_auto(buffer, &ind); + conf->imu_conf.rot_pitch = buffer_get_float32_auto(buffer, &ind); + conf->imu_conf.rot_yaw = buffer_get_float32_auto(buffer, &ind); + conf->imu_conf.accel_offsets[0] = buffer_get_float32_auto(buffer, &ind); + conf->imu_conf.accel_offsets[1] = buffer_get_float32_auto(buffer, &ind); + conf->imu_conf.accel_offsets[2] = buffer_get_float32_auto(buffer, &ind); + conf->imu_conf.gyro_offsets[0] = buffer_get_float32_auto(buffer, &ind); + conf->imu_conf.gyro_offsets[1] = buffer_get_float32_auto(buffer, &ind); + conf->imu_conf.gyro_offsets[2] = buffer_get_float32_auto(buffer, &ind); + conf->imu_conf.gyro_offset_comp_fact[0] = buffer_get_float32_auto(buffer, &ind); + conf->imu_conf.gyro_offset_comp_fact[1] = buffer_get_float32_auto(buffer, &ind); + conf->imu_conf.gyro_offset_comp_fact[2] = buffer_get_float32_auto(buffer, &ind); + conf->imu_conf.gyro_offset_comp_clamp = buffer_get_float32_auto(buffer, &ind); return true; } @@ -431,4 +659,118 @@ void confgenerator_set_defaults_mcconf(mc_configuration *conf) { } void confgenerator_set_defaults_appconf(app_configuration *conf) { + conf->controller_id = HW_DEFAULT_ID; + conf->timeout_msec = APPCONF_TIMEOUT_MSEC; + conf->timeout_brake_current = APPCONF_TIMEOUT_BRAKE_CURRENT; + conf->send_can_status = APPCONF_SEND_CAN_STATUS; + conf->send_can_status_rate_hz = APPCONF_SEND_CAN_STATUS_RATE_HZ; + conf->can_baud_rate = APPCONF_CAN_BAUD_RATE; + conf->pairing_done = APPCONF_PAIRING_DONE; + conf->permanent_uart_enabled = APPCONF_PERMANENT_UART_ENABLED; + conf->shutdown_mode = APPCONF_SHUTDOWN_MODE; + conf->uavcan_enable = APPCONF_UAVCAN_ENABLE; + conf->uavcan_esc_index = APPCONF_UAVCAN_ESC_INDEX; + conf->app_to_use = APPCONF_APP_TO_USE; + conf->app_ppm_conf.ctrl_type = APPCONF_PPM_CTRL_TYPE; + conf->app_ppm_conf.pid_max_erpm = APPCONF_PPM_PID_MAX_ERPM; + conf->app_ppm_conf.hyst = APPCONF_PPM_HYST; + conf->app_ppm_conf.pulse_start = APPCONF_PPM_PULSE_START; + conf->app_ppm_conf.pulse_end = APPCONF_PPM_PULSE_END; + conf->app_ppm_conf.pulse_center = APPCONF_PPM_PULSE_CENTER; + conf->app_ppm_conf.median_filter = APPCONF_PPM_MEDIAN_FILTER; + conf->app_ppm_conf.safe_start = APPCONF_PPM_SAFE_START; + conf->app_ppm_conf.throttle_exp = APPCONF_PPM_THROTTLE_EXP; + conf->app_ppm_conf.throttle_exp_brake = APPCONF_PPM_THROTTLE_EXP_BRAKE; + conf->app_ppm_conf.throttle_exp_mode = APPCONF_PPM_THROTTLE_EXP_MODE; + conf->app_ppm_conf.ramp_time_pos = APPCONF_PPM_RAMP_TIME_POS; + conf->app_ppm_conf.ramp_time_neg = APPCONF_PPM_RAMP_TIME_NEG; + 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_ppm_conf.max_erpm_for_dir = APPCONF_PPM_MAX_ERPM_FOR_DIR; + 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; + conf->app_adc_conf.voltage_end = APPCONF_ADC_VOLTAGE_END; + conf->app_adc_conf.voltage_center = APPCONF_ADC_VOLTAGE_CENTER; + conf->app_adc_conf.voltage2_start = APPCONF_ADC_VOLTAGE2_START; + conf->app_adc_conf.voltage2_end = APPCONF_ADC_VOLTAGE2_END; + conf->app_adc_conf.use_filter = APPCONF_ADC_USE_FILTER; + conf->app_adc_conf.safe_start = APPCONF_ADC_SAFE_START; + conf->app_adc_conf.cc_button_inverted = APPCONF_ADC_CC_BUTTON_INVERTED; + conf->app_adc_conf.rev_button_inverted = APPCONF_ADC_REV_BUTTON_INVERTED; + conf->app_adc_conf.voltage_inverted = APPCONF_ADC_VOLTAGE_INVERTED; + conf->app_adc_conf.voltage2_inverted = APPCONF_ADC_VOLTAGE2_INVERTED; + conf->app_adc_conf.throttle_exp = APPCONF_ADC_THROTTLE_EXP; + conf->app_adc_conf.throttle_exp_brake = APPCONF_ADC_THROTTLE_EXP_BRAKE; + conf->app_adc_conf.throttle_exp_mode = APPCONF_ADC_THROTTLE_EXP_MODE; + conf->app_adc_conf.ramp_time_pos = APPCONF_ADC_RAMP_TIME_POS; + conf->app_adc_conf.ramp_time_neg = APPCONF_ADC_RAMP_TIME_NEG; + conf->app_adc_conf.multi_esc = APPCONF_ADC_MULTI_ESC; + conf->app_adc_conf.tc = APPCONF_ADC_TC; + conf->app_adc_conf.tc_max_diff = APPCONF_ADC_TC_MAX_DIFF; + conf->app_adc_conf.update_rate_hz = APPCONF_ADC_UPDATE_RATE_HZ; + conf->app_uart_baudrate = APPCONF_UART_BAUDRATE; + conf->app_chuk_conf.ctrl_type = APPCONF_CHUK_CTRL_TYPE; + conf->app_chuk_conf.hyst = APPCONF_CHUK_HYST; + conf->app_chuk_conf.ramp_time_pos = APPCONF_CHUK_RAMP_TIME_POS; + conf->app_chuk_conf.ramp_time_neg = APPCONF_CHUK_RAMP_TIME_NEG; + conf->app_chuk_conf.stick_erpm_per_s_in_cc = APPCONF_STICK_ERPM_PER_S_IN_CC; + conf->app_chuk_conf.throttle_exp = APPCONF_CHUK_THROTTLE_EXP; + conf->app_chuk_conf.throttle_exp_brake = APPCONF_CHUK_THROTTLE_EXP_BRAKE; + conf->app_chuk_conf.throttle_exp_mode = APPCONF_CHUK_THROTTLE_EXP_MODE; + 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_chuk_conf.use_smart_rev = APPCONF_CHUK_USE_SMART_REV; + conf->app_chuk_conf.smart_rev_max_duty = APPCONF_CHUK_SMART_REV_MAX_DUTY; + conf->app_chuk_conf.smart_rev_ramp_time = APPCONF_CHUK_SMART_REV_RAMP_TIME; + 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; + 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.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; + conf->imu_conf.accel_confidence_decay = APPCONF_IMU_ACCEL_CONFIDENCE_DECAY; + conf->imu_conf.mahony_kp = APPCONF_IMU_MAHONY_KP; + conf->imu_conf.mahony_ki = APPCONF_IMU_MAHONY_KI; + conf->imu_conf.madgwick_beta = APPCONF_IMU_MADGWICK_BETA; + conf->imu_conf.rot_roll = APPCONF_IMU_ROT_ROLL; + conf->imu_conf.rot_pitch = APPCONF_IMU_ROT_PITCH; + conf->imu_conf.rot_yaw = APPCONF_IMU_ROT_YAW; + conf->imu_conf.accel_offsets[0] = APPCONF_IMU_A_OFFSET_0; + conf->imu_conf.accel_offsets[1] = APPCONF_IMU_A_OFFSET_1; + conf->imu_conf.accel_offsets[2] = APPCONF_IMU_A_OFFSET_2; + conf->imu_conf.gyro_offsets[0] = APPCONF_IMU_G_OFFSET_0; + conf->imu_conf.gyro_offsets[1] = APPCONF_IMU_G_OFFSET_1; + conf->imu_conf.gyro_offsets[2] = APPCONF_IMU_G_OFFSET_2; + conf->imu_conf.gyro_offset_comp_fact[0] = APPCONF_IMU_G_OFFSET_COMP_FACT_0; + conf->imu_conf.gyro_offset_comp_fact[1] = APPCONF_IMU_G_OFFSET_COMP_FACT_1; + conf->imu_conf.gyro_offset_comp_fact[2] = APPCONF_IMU_G_OFFSET_COMP_FACT_2; + conf->imu_conf.gyro_offset_comp_clamp = APPCONF_IMU_G_OFFSET_COMP_CLAMP; } diff --git a/confgenerator.h b/confgenerator.h index 59e1dfdc..0ff856c7 100644 --- a/confgenerator.h +++ b/confgenerator.h @@ -9,7 +9,7 @@ // Constants #define MCCONF_SIGNATURE 503309878 -#define APPCONF_SIGNATURE 0 +#define APPCONF_SIGNATURE 4290437352 // Functions int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf); From 8638036750b2a1da0435d022c386fe5734b4bee4 Mon Sep 17 00:00:00 2001 From: Mitch Lustig Date: Tue, 10 Sep 2019 02:04:04 -0700 Subject: [PATCH 25/31] Fix wheel spin-up on boot By waiting for gyro to be initialized --- applications/app_balance.c | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/applications/app_balance.c b/applications/app_balance.c index 7c29da7e..7e620de0 100644 --- a/applications/app_balance.c +++ b/applications/app_balance.c @@ -230,6 +230,13 @@ static THD_FUNCTION(balance_thread, arg) { switch(state){ case (STARTUP): if(startup_start_time == 0){ + // Loop and wait for gyro to start returning actual values + float acc_values[3]; + imu_get_accel(acc_values); + while(acc_values[0] == 0 && acc_values[1] == 0 && acc_values[1] == 0){ + chThdSleepMilliseconds(20); + imu_get_accel(acc_values); + } startup_start_time = current_time; // Overwrite AHRS config ahrs_update_all_parameters(1.00, imu_conf.mahony_kp, imu_conf.mahony_ki, 2.00); From 22d4f7c99a4264db65b6ac5e4b48e9b8b66a7cad Mon Sep 17 00:00:00 2001 From: Mitch Lustig Date: Tue, 10 Sep 2019 02:25:13 -0700 Subject: [PATCH 26/31] Fix the index, fix the build --- applications/app_balance.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/applications/app_balance.c b/applications/app_balance.c index 7e620de0..a7a428f2 100644 --- a/applications/app_balance.c +++ b/applications/app_balance.c @@ -233,7 +233,7 @@ static THD_FUNCTION(balance_thread, arg) { // Loop and wait for gyro to start returning actual values float acc_values[3]; imu_get_accel(acc_values); - while(acc_values[0] == 0 && acc_values[1] == 0 && acc_values[1] == 0){ + while(acc_values[0] == 0 && acc_values[1] == 0 && acc_values[2] == 0){ chThdSleepMilliseconds(20); imu_get_accel(acc_values); } From 518776d04e72977ca929f1180e87205016f74bc2 Mon Sep 17 00:00:00 2001 From: Mitch Lustig Date: Tue, 10 Sep 2019 19:29:44 -0700 Subject: [PATCH 27/31] Add basic low voltage tiltback --- appconf/appconf_default.h | 3 +++ applications/app_balance.c | 12 +++++++----- confgenerator.c | 3 +++ confgenerator.h | 2 +- datatypes.h | 1 + 5 files changed, 15 insertions(+), 6 deletions(-) diff --git a/appconf/appconf_default.h b/appconf/appconf_default.h index 3a528d7f..10f36a09 100644 --- a/appconf/appconf_default.h +++ b/appconf/appconf_default.h @@ -303,6 +303,9 @@ #ifndef APPCONF_BALANCE_TILTBACK_SPEED #define APPCONF_BALANCE_TILTBACK_SPEED 5.0 #endif +#ifndef APPCONF_BALANCE_TILTBACK_VOLTAGE +#define APPCONF_BALANCE_TILTBACK_VOLTAGE 0.0 +#endif #ifndef APPCONF_BALANCE_STARTUP_M_TOLERANCE #define APPCONF_BALANCE_STARTUP_M_TOLERANCE 20.0 #endif diff --git a/applications/app_balance.c b/applications/app_balance.c index a7a428f2..44557fd2 100644 --- a/applications/app_balance.c +++ b/applications/app_balance.c @@ -270,11 +270,13 @@ static THD_FUNCTION(balance_thread, arg) { } // Over speed tilt back safety - if(mc_interface_get_duty_cycle_now() > balance_conf.tiltback_duty){ - setpoint_target = balance_conf.tiltback_angle; - setpointAdjustmentType = TILTBACK; - } else if(mc_interface_get_duty_cycle_now() < -balance_conf.tiltback_duty){ - setpoint_target = -balance_conf.tiltback_angle; + 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_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; diff --git a/confgenerator.c b/confgenerator.c index ff8615dc..e40f1c6b 100644 --- a/confgenerator.c +++ b/confgenerator.c @@ -234,6 +234,7 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration 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_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); @@ -499,6 +500,7 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration 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_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); @@ -748,6 +750,7 @@ void confgenerator_set_defaults_appconf(app_configuration *conf) { 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_voltage = APPCONF_BALANCE_TILTBACK_VOLTAGE; 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; diff --git a/confgenerator.h b/confgenerator.h index 0ff856c7..2fef2bbc 100644 --- a/confgenerator.h +++ b/confgenerator.h @@ -9,7 +9,7 @@ // Constants #define MCCONF_SIGNATURE 503309878 -#define APPCONF_SIGNATURE 4290437352 +#define APPCONF_SIGNATURE 1344252731 // Functions int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf); diff --git a/datatypes.h b/datatypes.h index dd6fae46..c09917ad 100644 --- a/datatypes.h +++ b/datatypes.h @@ -507,6 +507,7 @@ typedef struct { float tiltback_duty; float tiltback_angle; float tiltback_speed; + float tiltback_voltage; float startup_m_tolerance; float startup_c_tolerance; float startup_speed; From 95483cff41b48315a531ef0c2d511896c4fbc373 Mon Sep 17 00:00:00 2001 From: Mitch Lustig Date: Wed, 11 Sep 2019 23:08:28 -0700 Subject: [PATCH 28/31] Centralise IMU startup sequence --- applications/app_balance.c | 27 +++++---------------------- imu/imu.c | 21 +++++++++++++++++++-- imu/imu.h | 1 + 3 files changed, 25 insertions(+), 24 deletions(-) diff --git a/applications/app_balance.c b/applications/app_balance.c index 44557fd2..c827ee66 100644 --- a/applications/app_balance.c +++ b/applications/app_balance.c @@ -229,29 +229,12 @@ static THD_FUNCTION(balance_thread, arg) { // State based logic switch(state){ case (STARTUP): - if(startup_start_time == 0){ - // Loop and wait for gyro to start returning actual values - float acc_values[3]; - imu_get_accel(acc_values); - while(acc_values[0] == 0 && acc_values[1] == 0 && acc_values[2] == 0){ - chThdSleepMilliseconds(20); - imu_get_accel(acc_values); - } - startup_start_time = current_time; - // Overwrite AHRS config - ahrs_update_all_parameters(1.00, imu_conf.mahony_kp, imu_conf.mahony_ki, 2.00); - } - startup_diff_time = current_time - startup_start_time; - - // Calibration is done - if(ST2MS(startup_diff_time) > 1000){ - // Restore AHRS config - ahrs_update_all_parameters(imu_conf.accel_confidence_decay, imu_conf.mahony_kp, imu_conf.mahony_ki, imu_conf.madgwick_beta); - // Set fault and wait for valid startup condition - state = FAULT; - startup_start_time = 0; - startup_diff_time = 0; + while(!imu_startup_done()){ + chThdSleepMilliseconds(50); } + state = FAULT; + startup_start_time = 0; + startup_diff_time = 0; break; case (RUNNING): // Check for overspeed 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); From 9f263b8c1a816bdbb4861d4f0141d15cbc1a0bf7 Mon Sep 17 00:00:00 2001 From: Mitch Lustig Date: Wed, 11 Sep 2019 23:18:12 -0700 Subject: [PATCH 29/31] Reorder app_use enum --- confgenerator.h | 2 +- datatypes.h | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/confgenerator.h b/confgenerator.h index 2fef2bbc..e713ebe0 100644 --- a/confgenerator.h +++ b/confgenerator.h @@ -9,7 +9,7 @@ // Constants #define MCCONF_SIGNATURE 503309878 -#define APPCONF_SIGNATURE 1344252731 +#define APPCONF_SIGNATURE 3223366057 // Functions int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf); diff --git a/datatypes.h b/datatypes.h index c09917ad..5541babb 100644 --- a/datatypes.h +++ b/datatypes.h @@ -317,8 +317,8 @@ typedef enum { APP_ADC_UART, APP_NUNCHUK, APP_NRF, - APP_BALANCE, - APP_CUSTOM + APP_CUSTOM, + APP_BALANCE } app_use; // Throttle curve mode From 3c31d264fd8ca223ad756d48cec3747a28c3c271 Mon Sep 17 00:00:00 2001 From: Mitch Lustig Date: Thu, 12 Sep 2019 00:07:26 -0700 Subject: [PATCH 30/31] Add high voltage tiltback --- appconf/appconf_default.h | 7 +++++-- applications/app_balance.c | 3 ++- confgenerator.c | 9 ++++++--- confgenerator.h | 2 +- datatypes.h | 3 ++- 5 files changed, 16 insertions(+), 8 deletions(-) diff --git a/appconf/appconf_default.h b/appconf/appconf_default.h index 10f36a09..7314dd17 100644 --- a/appconf/appconf_default.h +++ b/appconf/appconf_default.h @@ -303,8 +303,11 @@ #ifndef APPCONF_BALANCE_TILTBACK_SPEED #define APPCONF_BALANCE_TILTBACK_SPEED 5.0 #endif -#ifndef APPCONF_BALANCE_TILTBACK_VOLTAGE -#define APPCONF_BALANCE_TILTBACK_VOLTAGE 0.0 +#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 diff --git a/applications/app_balance.c b/applications/app_balance.c index c827ee66..51a24bfb 100644 --- a/applications/app_balance.c +++ b/applications/app_balance.c @@ -254,7 +254,8 @@ static THD_FUNCTION(balance_thread, arg) { // 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_voltage)){ + (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 { diff --git a/confgenerator.c b/confgenerator.c index e40f1c6b..747cb512 100644 --- a/confgenerator.c +++ b/confgenerator.c @@ -234,7 +234,8 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration 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_voltage, &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); @@ -500,7 +501,8 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration 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_voltage = 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); @@ -750,7 +752,8 @@ void confgenerator_set_defaults_appconf(app_configuration *conf) { 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_voltage = APPCONF_BALANCE_TILTBACK_VOLTAGE; + 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; diff --git a/confgenerator.h b/confgenerator.h index e713ebe0..051356f1 100644 --- a/confgenerator.h +++ b/confgenerator.h @@ -9,7 +9,7 @@ // Constants #define MCCONF_SIGNATURE 503309878 -#define APPCONF_SIGNATURE 3223366057 +#define APPCONF_SIGNATURE 183781183 // Functions int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf); diff --git a/datatypes.h b/datatypes.h index 5541babb..a9ba40f9 100644 --- a/datatypes.h +++ b/datatypes.h @@ -507,7 +507,8 @@ typedef struct { float tiltback_duty; float tiltback_angle; float tiltback_speed; - float tiltback_voltage; + float tiltback_high_voltage; + float tiltback_low_voltage; float startup_m_tolerance; float startup_c_tolerance; float startup_speed; From b53102d6fd9129a6546a35cf74800e91e104257b Mon Sep 17 00:00:00 2001 From: Mitch Lustig Date: Thu, 12 Sep 2019 23:08:56 -0700 Subject: [PATCH 31/31] Fix build for versions without HW_SPI --- applications/app_balance.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/applications/app_balance.c b/applications/app_balance.c index 51a24bfb..e6d960eb 100644 --- a/applications/app_balance.c +++ b/applications/app_balance.c @@ -99,11 +99,13 @@ void app_balance_start(void) { 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); @@ -218,12 +220,14 @@ static THD_FUNCTION(balance_thread, arg) { 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