2019-07-27 21:25:56 -07:00
|
|
|
/*
|
|
|
|
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 <http://www.gnu.org/licenses/>.
|
|
|
|
*/
|
|
|
|
|
|
|
|
#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"
|
2019-08-04 11:47:23 -07:00
|
|
|
#include "imu/ahrs.h"
|
2022-03-16 10:40:51 -07:00
|
|
|
#include "utils_math.h"
|
|
|
|
#include "utils_sys.h"
|
2019-09-08 18:32:53 -07:00
|
|
|
#include "datatypes.h"
|
2020-02-23 00:22:21 -08:00
|
|
|
#include "comm_can.h"
|
2021-06-01 03:20:00 -07:00
|
|
|
#include "terminal.h"
|
2022-09-27 21:30:15 -07:00
|
|
|
#include "digital_filter.h"
|
2019-08-07 00:22:51 -07:00
|
|
|
|
2019-07-27 21:25:56 -07:00
|
|
|
|
|
|
|
#include <math.h>
|
2021-06-01 03:20:00 -07:00
|
|
|
#include <stdio.h>
|
2019-07-27 21:25:56 -07:00
|
|
|
|
2020-02-23 00:22:21 -08:00
|
|
|
// Can
|
|
|
|
#define MAX_CAN_AGE 0.1
|
|
|
|
|
2021-05-16 14:53:03 -07:00
|
|
|
// Data type (Value 5 was removed, and can be reused at a later date, but i wanted to preserve the current value's numbers for UIs)
|
2019-08-04 11:47:23 -07:00
|
|
|
typedef enum {
|
2019-08-27 19:57:35 -07:00
|
|
|
STARTUP = 0,
|
2021-05-16 14:53:03 -07:00
|
|
|
RUNNING = 1,
|
|
|
|
RUNNING_TILTBACK_DUTY = 2,
|
|
|
|
RUNNING_TILTBACK_HIGH_VOLTAGE = 3,
|
|
|
|
RUNNING_TILTBACK_LOW_VOLTAGE = 4,
|
|
|
|
FAULT_ANGLE_PITCH = 6,
|
|
|
|
FAULT_ANGLE_ROLL = 7,
|
|
|
|
FAULT_SWITCH_HALF = 8,
|
|
|
|
FAULT_SWITCH_FULL = 9,
|
|
|
|
FAULT_DUTY = 10,
|
|
|
|
FAULT_STARTUP = 11
|
2019-08-04 11:47:23 -07:00
|
|
|
} BalanceState;
|
|
|
|
|
2019-08-04 23:13:35 -07:00
|
|
|
typedef enum {
|
2019-08-27 19:57:35 -07:00
|
|
|
CENTERING = 0,
|
2021-07-29 17:09:31 -07:00
|
|
|
TILTBACK_DUTY,
|
|
|
|
TILTBACK_HV,
|
|
|
|
TILTBACK_LV,
|
|
|
|
TILTBACK_NONE
|
2019-08-07 00:22:51 -07:00
|
|
|
} SetpointAdjustmentType;
|
2019-08-04 23:13:35 -07:00
|
|
|
|
2020-02-23 13:34:51 -08:00
|
|
|
typedef enum {
|
|
|
|
OFF = 0,
|
|
|
|
HALF,
|
|
|
|
ON
|
|
|
|
} SwitchState;
|
|
|
|
|
2019-08-10 22:00:00 -07:00
|
|
|
// Balance thread
|
|
|
|
static THD_FUNCTION(balance_thread, arg);
|
2022-01-16 12:57:12 -08:00
|
|
|
static THD_WORKING_AREA(balance_thread_wa, 1024); // 2kb stack for this thread
|
2019-07-27 21:25:56 -07:00
|
|
|
|
2020-06-19 01:00:37 -07:00
|
|
|
static thread_t *app_thread;
|
|
|
|
|
|
|
|
// Config values
|
2019-08-27 09:10:44 -07:00
|
|
|
static volatile balance_config balance_conf;
|
2019-09-08 22:38:09 -07:00
|
|
|
static volatile imu_config imu_conf;
|
2021-06-06 14:01:42 -07:00
|
|
|
static systime_t loop_time;
|
2021-07-29 17:09:31 -07:00
|
|
|
static float startup_step_size;
|
|
|
|
static float tiltback_duty_step_size, tiltback_hv_step_size, tiltback_lv_step_size, tiltback_return_step_size;
|
|
|
|
static float torquetilt_on_step_size, torquetilt_off_step_size, turntilt_step_size;
|
|
|
|
static float tiltback_variable, tiltback_variable_max_erpm, noseangling_step_size;
|
2019-07-27 21:25:56 -07:00
|
|
|
|
2020-06-19 01:00:37 -07:00
|
|
|
// Runtime values read from elsewhere
|
2022-04-21 00:35:15 -07:00
|
|
|
static float pitch_angle, last_pitch_angle, roll_angle, abs_roll_angle, abs_roll_angle_sin, last_gyro_y;
|
2020-02-23 00:22:21 -08:00
|
|
|
static float gyro[3];
|
2020-02-23 10:23:19 -08:00
|
|
|
static float duty_cycle, abs_duty_cycle;
|
2020-03-15 23:18:34 -07:00
|
|
|
static float erpm, abs_erpm, avg_erpm;
|
2020-06-19 01:00:37 -07:00
|
|
|
static float motor_current;
|
|
|
|
static float motor_position;
|
|
|
|
static float adc1, adc2;
|
|
|
|
static SwitchState switch_state;
|
|
|
|
|
|
|
|
// Rumtime state values
|
|
|
|
static BalanceState state;
|
2022-04-21 00:35:15 -07:00
|
|
|
static float proportional, integral, derivative, proportional2, integral2, derivative2;
|
2021-03-27 14:00:55 -07:00
|
|
|
static float last_proportional, abs_proportional;
|
2019-08-10 00:16:48 -07:00
|
|
|
static float pid_value;
|
2021-04-06 02:18:32 -07:00
|
|
|
static float setpoint, setpoint_target, setpoint_target_interpolated;
|
2021-06-30 16:17:57 -07:00
|
|
|
static float noseangling_interpolated;
|
2021-04-06 02:18:32 -07:00
|
|
|
static float torquetilt_filtered_current, torquetilt_target, torquetilt_interpolated;
|
2021-06-01 03:20:00 -07:00
|
|
|
static Biquad torquetilt_current_biquad;
|
2021-04-06 02:18:32 -07:00
|
|
|
static float turntilt_target, turntilt_interpolated;
|
2019-08-07 00:22:51 -07:00
|
|
|
static SetpointAdjustmentType setpointAdjustmentType;
|
2020-06-19 01:00:37 -07:00
|
|
|
static float yaw_proportional, yaw_integral, yaw_derivative, yaw_last_proportional, yaw_pid_value, yaw_setpoint;
|
2021-06-06 14:01:42 -07:00
|
|
|
static systime_t current_time, last_time, diff_time, loop_overshoot;
|
|
|
|
static float filtered_loop_overshoot, loop_overshoot_alpha, filtered_diff_time;
|
2020-06-19 01:28:52 -07:00
|
|
|
static systime_t fault_angle_pitch_timer, fault_angle_roll_timer, fault_switch_timer, fault_switch_half_timer, fault_duty_timer;
|
2021-05-16 14:53:03 -07:00
|
|
|
static float d_pt1_lowpass_state, d_pt1_lowpass_k, d_pt1_highpass_state, d_pt1_highpass_k;
|
2021-05-23 14:58:47 -07:00
|
|
|
static float motor_timeout;
|
2021-06-05 16:05:19 -07:00
|
|
|
static systime_t brake_timeout;
|
2019-07-29 00:31:48 -07:00
|
|
|
|
2021-06-01 03:20:00 -07:00
|
|
|
// Debug values
|
|
|
|
static int debug_render_1, debug_render_2;
|
|
|
|
static int debug_sample_field, debug_sample_count, debug_sample_index;
|
2021-06-05 17:14:23 -07:00
|
|
|
static int debug_experiment_1, debug_experiment_2, debug_experiment_3, debug_experiment_4, debug_experiment_5, debug_experiment_6;
|
2021-06-01 03:20:00 -07:00
|
|
|
|
2021-05-23 14:58:47 -07:00
|
|
|
// Function Prototypes
|
|
|
|
static void set_current(float current, float yaw_current);
|
2021-06-01 03:20:00 -07:00
|
|
|
static void terminal_render(int argc, const char **argv);
|
|
|
|
static void terminal_sample(int argc, const char **argv);
|
2021-06-05 17:14:23 -07:00
|
|
|
static void terminal_experiment(int argc, const char **argv);
|
2021-06-01 03:20:00 -07:00
|
|
|
static float app_balance_get_debug(int index);
|
|
|
|
static void app_balance_sample_debug(void);
|
2021-06-05 17:14:23 -07:00
|
|
|
static void app_balance_experiment(void);
|
2021-06-01 03:20:00 -07:00
|
|
|
|
2021-05-23 14:58:47 -07:00
|
|
|
// Exposed Functions
|
2019-09-08 22:38:09 -07:00
|
|
|
void app_balance_configure(balance_config *conf, imu_config *conf2) {
|
2019-08-27 09:10:44 -07:00
|
|
|
balance_conf = *conf;
|
2019-09-08 22:38:09 -07:00
|
|
|
imu_conf = *conf2;
|
2020-06-19 01:00:37 -07:00
|
|
|
// Set calculated values from config
|
2021-06-06 14:01:42 -07:00
|
|
|
loop_time = US2ST((int)((1000.0 / balance_conf.hertz) * 1000.0));
|
|
|
|
|
2021-05-23 14:58:47 -07:00
|
|
|
motor_timeout = ((1000.0 / balance_conf.hertz)/1000.0) * 20; // Times 20 for a nice long grace period
|
2021-06-01 03:20:00 -07:00
|
|
|
|
|
|
|
startup_step_size = balance_conf.startup_speed / balance_conf.hertz;
|
2021-08-18 00:45:51 -07:00
|
|
|
tiltback_duty_step_size = balance_conf.tiltback_duty_speed / balance_conf.hertz;
|
2021-07-29 17:09:31 -07:00
|
|
|
tiltback_hv_step_size = balance_conf.tiltback_hv_speed / balance_conf.hertz;
|
|
|
|
tiltback_lv_step_size = balance_conf.tiltback_lv_speed / balance_conf.hertz;
|
|
|
|
tiltback_return_step_size = balance_conf.tiltback_return_speed / balance_conf.hertz;
|
2021-06-01 03:20:00 -07:00
|
|
|
torquetilt_on_step_size = balance_conf.torquetilt_on_speed / balance_conf.hertz;
|
|
|
|
torquetilt_off_step_size = balance_conf.torquetilt_off_speed / balance_conf.hertz;
|
|
|
|
turntilt_step_size = balance_conf.turntilt_speed / balance_conf.hertz;
|
2021-07-29 17:09:31 -07:00
|
|
|
noseangling_step_size = balance_conf.noseangling_speed / balance_conf.hertz;
|
2021-06-01 03:20:00 -07:00
|
|
|
|
|
|
|
// Init Filters
|
2021-06-06 14:01:42 -07:00
|
|
|
if(balance_conf.loop_time_filter > 0){
|
|
|
|
loop_overshoot_alpha = 2*M_PI*((float)1/balance_conf.hertz)*balance_conf.loop_time_filter/(2*M_PI*((float)1/balance_conf.hertz)*balance_conf.loop_time_filter+1);
|
|
|
|
}
|
2021-05-16 14:53:03 -07:00
|
|
|
if(balance_conf.kd_pt1_lowpass_frequency > 0){
|
2020-07-03 20:33:15 -07:00
|
|
|
float dT = 1.0 / balance_conf.hertz;
|
2021-05-16 14:53:03 -07:00
|
|
|
float RC = 1.0 / ( 2.0 * M_PI * balance_conf.kd_pt1_lowpass_frequency);
|
|
|
|
d_pt1_lowpass_k = dT / (RC + dT);
|
|
|
|
}
|
|
|
|
if(balance_conf.kd_pt1_highpass_frequency > 0){
|
|
|
|
float dT = 1.0 / balance_conf.hertz;
|
|
|
|
float RC = 1.0 / ( 2.0 * M_PI * balance_conf.kd_pt1_highpass_frequency);
|
|
|
|
d_pt1_highpass_k = dT / (RC + dT);
|
2020-07-03 20:33:15 -07:00
|
|
|
}
|
2021-06-01 03:20:00 -07:00
|
|
|
if(balance_conf.torquetilt_filter > 0){ // Torquetilt Current Biquad
|
|
|
|
float Fc = balance_conf.torquetilt_filter / balance_conf.hertz;
|
2021-07-06 11:12:05 -07:00
|
|
|
biquad_config(&torquetilt_current_biquad, BQ_LOWPASS, Fc);
|
2021-06-01 03:20:00 -07:00
|
|
|
}
|
2021-06-06 14:01:42 -07:00
|
|
|
|
2021-06-30 12:46:52 -07:00
|
|
|
// Variable nose angle adjustment / tiltback (setting is per 1000erpm, convert to per erpm)
|
2021-06-25 21:41:41 -07:00
|
|
|
tiltback_variable = balance_conf.tiltback_variable / 1000;
|
2022-01-03 10:50:41 -08:00
|
|
|
if (tiltback_variable > 0) {
|
|
|
|
tiltback_variable_max_erpm = fabsf(balance_conf.tiltback_variable_max / tiltback_variable);
|
|
|
|
} else {
|
|
|
|
tiltback_variable_max_erpm = 100000;
|
|
|
|
}
|
2021-06-25 21:41:41 -07:00
|
|
|
|
2021-06-06 14:01:42 -07:00
|
|
|
// Reset loop time variables
|
|
|
|
last_time = 0;
|
|
|
|
filtered_loop_overshoot = 0;
|
2019-07-27 21:25:56 -07:00
|
|
|
}
|
|
|
|
|
|
|
|
void app_balance_start(void) {
|
2020-06-19 01:00:37 -07:00
|
|
|
// First start only, override state to startup
|
2019-08-27 19:57:35 -07:00
|
|
|
state = STARTUP;
|
2021-06-01 03:20:00 -07:00
|
|
|
// Register terminal commands
|
|
|
|
terminal_register_command_callback(
|
|
|
|
"app_balance_render",
|
|
|
|
"Render debug values on the balance real time data graph",
|
|
|
|
"[Field Number] [Plot (Optional 1 or 2)]",
|
|
|
|
terminal_render);
|
|
|
|
terminal_register_command_callback(
|
2021-06-05 17:14:23 -07:00
|
|
|
"app_balance_sample",
|
|
|
|
"Output real time values to the terminal",
|
|
|
|
"[Field Number] [Sample Count]",
|
|
|
|
terminal_sample);
|
|
|
|
terminal_register_command_callback(
|
|
|
|
"app_balance_experiment",
|
|
|
|
"Output real time values to the experiments graph",
|
|
|
|
"[Field Number] [Plot 1-6]",
|
|
|
|
terminal_experiment);
|
2020-06-19 01:00:37 -07:00
|
|
|
// Start the balance thread
|
|
|
|
app_thread = chThdCreateStatic(balance_thread_wa, sizeof(balance_thread_wa), NORMALPRIO, balance_thread, NULL);
|
|
|
|
}
|
|
|
|
|
2021-05-23 14:58:47 -07:00
|
|
|
void app_balance_stop(void) {
|
|
|
|
if(app_thread != NULL){
|
|
|
|
chThdTerminate(app_thread);
|
|
|
|
chThdWait(app_thread);
|
|
|
|
}
|
|
|
|
set_current(0, 0);
|
2021-06-01 03:20:00 -07:00
|
|
|
terminal_unregister_callback(terminal_render);
|
|
|
|
terminal_unregister_callback(terminal_sample);
|
2019-07-27 21:25:56 -07:00
|
|
|
}
|
|
|
|
|
2019-07-29 00:31:48 -07:00
|
|
|
float app_balance_get_pid_output(void) {
|
2019-08-07 00:22:51 -07:00
|
|
|
return pid_value;
|
2019-07-29 00:31:48 -07:00
|
|
|
}
|
2019-09-21 01:07:31 -07:00
|
|
|
float app_balance_get_pitch_angle(void) {
|
2021-04-07 22:42:21 -07:00
|
|
|
return pitch_angle;
|
2019-07-29 00:31:48 -07:00
|
|
|
}
|
2019-09-21 01:07:31 -07:00
|
|
|
float app_balance_get_roll_angle(void) {
|
2021-04-07 22:42:21 -07:00
|
|
|
return roll_angle;
|
2019-07-30 00:27:23 -07:00
|
|
|
}
|
|
|
|
uint32_t app_balance_get_diff_time(void) {
|
2019-08-10 00:16:48 -07:00
|
|
|
return ST2US(diff_time);
|
2019-07-29 00:31:48 -07:00
|
|
|
}
|
|
|
|
float app_balance_get_motor_current(void) {
|
2019-08-07 00:22:51 -07:00
|
|
|
return motor_current;
|
2019-07-27 21:25:56 -07:00
|
|
|
}
|
2019-08-09 20:55:58 -07:00
|
|
|
uint16_t app_balance_get_state(void) {
|
|
|
|
return state;
|
|
|
|
}
|
2020-02-23 13:34:51 -08:00
|
|
|
uint16_t app_balance_get_switch_state(void) {
|
|
|
|
return switch_state;
|
|
|
|
}
|
|
|
|
float app_balance_get_adc1(void) {
|
|
|
|
return adc1;
|
|
|
|
}
|
|
|
|
float app_balance_get_adc2(void) {
|
|
|
|
return adc2;
|
2019-09-10 01:07:05 -07:00
|
|
|
}
|
2021-06-01 03:20:00 -07:00
|
|
|
float app_balance_get_debug1(void) {
|
|
|
|
return app_balance_get_debug(debug_render_1);
|
|
|
|
}
|
|
|
|
float app_balance_get_debug2(void) {
|
|
|
|
return app_balance_get_debug(debug_render_2);
|
|
|
|
}
|
2019-08-07 00:22:51 -07:00
|
|
|
|
2021-05-23 14:58:47 -07:00
|
|
|
// Internal Functions
|
|
|
|
static void reset_vars(void){
|
|
|
|
// Clear accumulated values.
|
|
|
|
integral = 0;
|
|
|
|
last_proportional = 0;
|
2022-04-21 00:35:15 -07:00
|
|
|
integral2 = 0;
|
2021-05-23 14:58:47 -07:00
|
|
|
yaw_integral = 0;
|
|
|
|
yaw_last_proportional = 0;
|
|
|
|
d_pt1_lowpass_state = 0;
|
|
|
|
d_pt1_highpass_state = 0;
|
|
|
|
// Set values for startup
|
|
|
|
setpoint = pitch_angle;
|
|
|
|
setpoint_target_interpolated = pitch_angle;
|
|
|
|
setpoint_target = 0;
|
2021-06-30 16:17:57 -07:00
|
|
|
noseangling_interpolated = 0;
|
2021-05-23 14:58:47 -07:00
|
|
|
torquetilt_target = 0;
|
|
|
|
torquetilt_interpolated = 0;
|
|
|
|
torquetilt_filtered_current = 0;
|
2021-07-06 11:12:05 -07:00
|
|
|
biquad_reset(&torquetilt_current_biquad);
|
2021-05-23 14:58:47 -07:00
|
|
|
turntilt_target = 0;
|
|
|
|
turntilt_interpolated = 0;
|
|
|
|
setpointAdjustmentType = CENTERING;
|
|
|
|
yaw_setpoint = 0;
|
|
|
|
state = RUNNING;
|
|
|
|
current_time = 0;
|
|
|
|
last_time = 0;
|
|
|
|
diff_time = 0;
|
2021-06-05 16:05:19 -07:00
|
|
|
brake_timeout = 0;
|
2021-05-23 14:58:47 -07:00
|
|
|
}
|
|
|
|
|
|
|
|
static float get_setpoint_adjustment_step_size(void){
|
2019-08-07 00:22:51 -07:00
|
|
|
switch(setpointAdjustmentType){
|
2019-08-27 19:57:35 -07:00
|
|
|
case (CENTERING):
|
2019-08-07 00:22:51 -07:00
|
|
|
return startup_step_size;
|
2021-07-29 17:09:31 -07:00
|
|
|
case (TILTBACK_DUTY):
|
|
|
|
return tiltback_duty_step_size;
|
|
|
|
case (TILTBACK_HV):
|
|
|
|
return tiltback_hv_step_size;
|
|
|
|
case (TILTBACK_LV):
|
|
|
|
return tiltback_lv_step_size;
|
|
|
|
case (TILTBACK_NONE):
|
|
|
|
return tiltback_return_step_size;
|
|
|
|
default:
|
|
|
|
;
|
2019-08-07 00:22:51 -07:00
|
|
|
}
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
2020-06-19 01:00:37 -07:00
|
|
|
// Fault checking order does not really matter. From a UX perspective, switch should be before angle.
|
2021-05-23 14:58:47 -07:00
|
|
|
static bool check_faults(bool ignoreTimers){
|
2020-06-19 01:00:37 -07:00
|
|
|
// Check switch
|
2020-06-19 01:28:52 -07:00
|
|
|
// Switch fully open
|
|
|
|
if(switch_state == OFF){
|
2020-07-02 01:07:54 -07:00
|
|
|
if(ST2MS(current_time - fault_switch_timer) > balance_conf.fault_delay_switch_full || ignoreTimers){
|
2020-07-04 03:35:19 -07:00
|
|
|
state = FAULT_SWITCH_FULL;
|
2020-06-19 01:00:37 -07:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
} else {
|
|
|
|
fault_switch_timer = current_time;
|
|
|
|
}
|
|
|
|
|
2020-06-19 01:28:52 -07:00
|
|
|
// Switch partially open and stopped
|
2022-07-16 13:52:18 -07:00
|
|
|
if(!balance_conf.fault_is_dual_switch) {
|
|
|
|
if((switch_state == HALF || switch_state == OFF) && abs_erpm < balance_conf.fault_adc_half_erpm){
|
|
|
|
if(ST2MS(current_time - fault_switch_half_timer) > balance_conf.fault_delay_switch_half || ignoreTimers){
|
|
|
|
state = FAULT_SWITCH_HALF;
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
} else {
|
|
|
|
fault_switch_half_timer = current_time;
|
2020-06-19 01:28:52 -07:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2020-06-19 01:00:37 -07:00
|
|
|
// Check pitch angle
|
2020-07-02 01:07:54 -07:00
|
|
|
if(fabsf(pitch_angle) > balance_conf.fault_pitch){
|
|
|
|
if(ST2MS(current_time - fault_angle_pitch_timer) > balance_conf.fault_delay_pitch || ignoreTimers){
|
2020-06-19 01:00:37 -07:00
|
|
|
state = FAULT_ANGLE_PITCH;
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
}else{
|
|
|
|
fault_angle_pitch_timer = current_time;
|
|
|
|
}
|
|
|
|
|
|
|
|
// Check roll angle
|
2020-07-02 01:07:54 -07:00
|
|
|
if(fabsf(roll_angle) > balance_conf.fault_roll){
|
|
|
|
if(ST2MS(current_time - fault_angle_roll_timer) > balance_conf.fault_delay_roll || ignoreTimers){
|
2020-07-04 03:35:19 -07:00
|
|
|
state = FAULT_ANGLE_ROLL;
|
2020-06-19 01:00:37 -07:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
}else{
|
|
|
|
fault_angle_roll_timer = current_time;
|
|
|
|
}
|
|
|
|
|
|
|
|
// Check for duty
|
2020-07-02 01:07:54 -07:00
|
|
|
if(abs_duty_cycle > balance_conf.fault_duty){
|
|
|
|
if(ST2MS(current_time - fault_duty_timer) > balance_conf.fault_delay_duty || ignoreTimers){
|
2020-06-19 01:00:37 -07:00
|
|
|
state = FAULT_DUTY;
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
} else {
|
|
|
|
fault_duty_timer = current_time;
|
|
|
|
}
|
|
|
|
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2021-05-23 14:58:47 -07:00
|
|
|
static void calculate_setpoint_target(void){
|
2020-07-02 01:07:54 -07:00
|
|
|
if(setpointAdjustmentType == CENTERING && setpoint_target_interpolated != setpoint_target){
|
|
|
|
// Ignore tiltback during centering sequence
|
2020-07-04 00:11:20 -07:00
|
|
|
state = RUNNING;
|
|
|
|
}else if(abs_duty_cycle > balance_conf.tiltback_duty){
|
2020-07-02 01:07:54 -07:00
|
|
|
if(erpm > 0){
|
2021-08-18 00:45:51 -07:00
|
|
|
setpoint_target = balance_conf.tiltback_duty_angle;
|
2020-07-02 01:07:54 -07:00
|
|
|
} else {
|
2021-08-18 00:45:51 -07:00
|
|
|
setpoint_target = -balance_conf.tiltback_duty_angle;
|
2020-07-02 01:07:54 -07:00
|
|
|
}
|
2021-07-29 17:09:31 -07:00
|
|
|
setpointAdjustmentType = TILTBACK_DUTY;
|
2020-07-04 00:11:20 -07:00
|
|
|
state = RUNNING_TILTBACK_DUTY;
|
2021-08-18 00:45:51 -07:00
|
|
|
}else if(abs_duty_cycle > 0.05 && GET_INPUT_VOLTAGE() > balance_conf.tiltback_hv){
|
2020-07-04 00:11:20 -07:00
|
|
|
if(erpm > 0){
|
2021-07-29 17:09:31 -07:00
|
|
|
setpoint_target = balance_conf.tiltback_hv_angle;
|
2020-07-04 00:11:20 -07:00
|
|
|
} else {
|
2021-07-29 17:09:31 -07:00
|
|
|
setpoint_target = -balance_conf.tiltback_hv_angle;
|
2020-07-04 00:11:20 -07:00
|
|
|
}
|
2021-07-29 17:09:31 -07:00
|
|
|
setpointAdjustmentType = TILTBACK_HV;
|
2020-07-04 00:11:20 -07:00
|
|
|
state = RUNNING_TILTBACK_HIGH_VOLTAGE;
|
2021-08-18 00:45:51 -07:00
|
|
|
}else if(abs_duty_cycle > 0.05 && GET_INPUT_VOLTAGE() < balance_conf.tiltback_lv){
|
2020-07-04 00:11:20 -07:00
|
|
|
if(erpm > 0){
|
2021-07-29 17:09:31 -07:00
|
|
|
setpoint_target = balance_conf.tiltback_lv_angle;
|
2020-07-04 00:11:20 -07:00
|
|
|
} else {
|
2021-07-29 17:09:31 -07:00
|
|
|
setpoint_target = -balance_conf.tiltback_lv_angle;
|
2020-07-04 00:11:20 -07:00
|
|
|
}
|
2021-07-29 17:09:31 -07:00
|
|
|
setpointAdjustmentType = TILTBACK_LV;
|
2020-07-04 00:11:20 -07:00
|
|
|
state = RUNNING_TILTBACK_LOW_VOLTAGE;
|
2020-07-02 01:07:54 -07:00
|
|
|
}else{
|
2021-07-29 17:09:31 -07:00
|
|
|
setpointAdjustmentType = TILTBACK_NONE;
|
2020-07-02 01:07:54 -07:00
|
|
|
setpoint_target = 0;
|
2020-07-04 00:11:20 -07:00
|
|
|
state = RUNNING;
|
2020-07-02 01:07:54 -07:00
|
|
|
}
|
|
|
|
}
|
2020-06-19 01:00:37 -07:00
|
|
|
|
2021-05-23 14:58:47 -07:00
|
|
|
static void calculate_setpoint_interpolated(void){
|
2020-07-02 01:07:54 -07:00
|
|
|
if(setpoint_target_interpolated != setpoint_target){
|
|
|
|
// If we are less than one step size away, go all the way
|
|
|
|
if(fabsf(setpoint_target - setpoint_target_interpolated) < get_setpoint_adjustment_step_size()){
|
|
|
|
setpoint_target_interpolated = setpoint_target;
|
|
|
|
}else if (setpoint_target - setpoint_target_interpolated > 0){
|
|
|
|
setpoint_target_interpolated += get_setpoint_adjustment_step_size();
|
|
|
|
}else{
|
|
|
|
setpoint_target_interpolated -= get_setpoint_adjustment_step_size();
|
|
|
|
}
|
|
|
|
}
|
2020-06-19 01:00:37 -07:00
|
|
|
}
|
|
|
|
|
2021-06-30 16:17:57 -07:00
|
|
|
static void apply_noseangling(void){
|
2021-06-25 21:41:41 -07:00
|
|
|
// Nose angle adjustment, add variable then constant tiltback
|
2021-06-30 16:17:57 -07:00
|
|
|
float noseangling_target = 0;
|
2021-06-25 21:41:41 -07:00
|
|
|
if (fabsf(erpm) > tiltback_variable_max_erpm) {
|
2021-06-30 16:17:57 -07:00
|
|
|
noseangling_target = fabsf(balance_conf.tiltback_variable_max) * SIGN(erpm);
|
2021-06-25 21:41:41 -07:00
|
|
|
} else {
|
2021-06-30 16:17:57 -07:00
|
|
|
noseangling_target = tiltback_variable * erpm;
|
2021-06-25 21:41:41 -07:00
|
|
|
}
|
|
|
|
|
2021-05-16 14:53:03 -07:00
|
|
|
if(erpm > balance_conf.tiltback_constant_erpm){
|
2021-06-30 16:17:57 -07:00
|
|
|
noseangling_target += balance_conf.tiltback_constant;
|
2021-05-16 14:53:03 -07:00
|
|
|
} else if(erpm < -balance_conf.tiltback_constant_erpm){
|
2021-06-30 16:17:57 -07:00
|
|
|
noseangling_target += -balance_conf.tiltback_constant;
|
2021-05-16 14:53:03 -07:00
|
|
|
}
|
|
|
|
|
2021-07-29 17:09:31 -07:00
|
|
|
if(fabsf(noseangling_target - noseangling_interpolated) < noseangling_step_size){
|
2021-06-30 16:17:57 -07:00
|
|
|
noseangling_interpolated = noseangling_target;
|
|
|
|
}else if (noseangling_target - noseangling_interpolated > 0){
|
2021-07-29 17:09:31 -07:00
|
|
|
noseangling_interpolated += noseangling_step_size;
|
2021-05-16 14:53:03 -07:00
|
|
|
}else{
|
2021-07-29 17:09:31 -07:00
|
|
|
noseangling_interpolated -= noseangling_step_size;
|
2021-05-16 14:53:03 -07:00
|
|
|
}
|
2021-06-30 16:17:57 -07:00
|
|
|
setpoint += noseangling_interpolated;
|
2021-05-16 14:53:03 -07:00
|
|
|
}
|
|
|
|
|
2021-05-23 14:58:47 -07:00
|
|
|
static void apply_torquetilt(void){
|
2021-06-01 03:20:00 -07:00
|
|
|
// Filter current (Biquad)
|
|
|
|
if(balance_conf.torquetilt_filter > 0){
|
2021-07-06 11:12:05 -07:00
|
|
|
torquetilt_filtered_current = biquad_process(&torquetilt_current_biquad, motor_current);
|
2021-06-01 03:20:00 -07:00
|
|
|
}else{
|
|
|
|
torquetilt_filtered_current = motor_current;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2021-03-27 20:13:22 -07:00
|
|
|
// Wat is this line O_o
|
|
|
|
// Take abs motor current, subtract start offset, and take the max of that with 0 to get the current above our start threshold (absolute).
|
|
|
|
// Then multiply it by "power" to get our desired angle, and min with the limit to respect boundaries.
|
|
|
|
// Finally multiply it by sign motor current to get directionality back
|
2021-04-17 18:05:24 -07:00
|
|
|
torquetilt_target = fminf(fmaxf((fabsf(torquetilt_filtered_current) - balance_conf.torquetilt_start_current), 0) * balance_conf.torquetilt_strength, balance_conf.torquetilt_angle_limit) * SIGN(torquetilt_filtered_current);
|
2021-03-27 20:13:22 -07:00
|
|
|
|
2021-06-01 03:20:00 -07:00
|
|
|
float step_size;
|
|
|
|
if((torquetilt_interpolated - torquetilt_target > 0 && torquetilt_target > 0) || (torquetilt_interpolated - torquetilt_target < 0 && torquetilt_target < 0)){
|
|
|
|
step_size = torquetilt_off_step_size;
|
|
|
|
}else{
|
|
|
|
step_size = torquetilt_on_step_size;
|
|
|
|
}
|
|
|
|
|
|
|
|
if(fabsf(torquetilt_target - torquetilt_interpolated) < step_size){
|
2021-03-27 20:13:22 -07:00
|
|
|
torquetilt_interpolated = torquetilt_target;
|
|
|
|
}else if (torquetilt_target - torquetilt_interpolated > 0){
|
2021-06-01 03:20:00 -07:00
|
|
|
torquetilt_interpolated += step_size;
|
2021-03-27 20:13:22 -07:00
|
|
|
}else{
|
2021-06-01 03:20:00 -07:00
|
|
|
torquetilt_interpolated -= step_size;
|
2021-03-27 20:13:22 -07:00
|
|
|
}
|
|
|
|
setpoint += torquetilt_interpolated;
|
|
|
|
}
|
|
|
|
|
2021-05-23 14:58:47 -07:00
|
|
|
static void apply_turntilt(void){
|
2021-04-06 02:18:32 -07:00
|
|
|
// Calculate desired angle
|
2021-04-17 18:05:24 -07:00
|
|
|
turntilt_target = abs_roll_angle_sin * balance_conf.turntilt_strength;
|
2021-04-06 02:18:32 -07:00
|
|
|
|
|
|
|
// Apply cutzone
|
2021-04-17 18:05:24 -07:00
|
|
|
if(abs_roll_angle < balance_conf.turntilt_start_angle){
|
2021-04-06 02:18:32 -07:00
|
|
|
turntilt_target = 0;
|
|
|
|
}
|
|
|
|
|
2021-04-17 18:05:24 -07:00
|
|
|
// Disable below erpm threshold otherwise add directionality
|
|
|
|
if(abs_erpm < balance_conf.turntilt_start_erpm){
|
2021-04-06 02:18:32 -07:00
|
|
|
turntilt_target = 0;
|
|
|
|
}else {
|
|
|
|
turntilt_target *= SIGN(erpm);
|
|
|
|
}
|
|
|
|
|
|
|
|
// Apply speed scaling
|
2021-04-17 18:05:24 -07:00
|
|
|
if(abs_erpm < balance_conf.turntilt_erpm_boost_end){
|
|
|
|
turntilt_target *= 1 + ((balance_conf.turntilt_erpm_boost/100.0f) * (abs_erpm / balance_conf.turntilt_erpm_boost_end));
|
|
|
|
}else{
|
|
|
|
turntilt_target *= 1 + (balance_conf.turntilt_erpm_boost/100.0f);
|
2021-04-06 02:18:32 -07:00
|
|
|
}
|
|
|
|
|
2021-04-07 00:30:37 -07:00
|
|
|
// Limit angle to max angle
|
2022-08-30 00:20:01 -07:00
|
|
|
if(turntilt_target > 0){
|
|
|
|
turntilt_target = fminf(turntilt_target, balance_conf.turntilt_angle_limit);
|
|
|
|
}else{
|
|
|
|
turntilt_target = fmaxf(turntilt_target, -balance_conf.turntilt_angle_limit);
|
|
|
|
}
|
2021-04-06 02:18:32 -07:00
|
|
|
|
2021-04-07 00:30:37 -07:00
|
|
|
// Move towards target limited by max speed
|
2021-04-06 02:18:32 -07:00
|
|
|
if(fabsf(turntilt_target - turntilt_interpolated) < turntilt_step_size){
|
|
|
|
turntilt_interpolated = turntilt_target;
|
|
|
|
}else if (turntilt_target - turntilt_interpolated > 0){
|
|
|
|
turntilt_interpolated += turntilt_step_size;
|
|
|
|
}else{
|
|
|
|
turntilt_interpolated -= turntilt_step_size;
|
|
|
|
}
|
|
|
|
setpoint += turntilt_interpolated;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
2021-05-23 14:58:47 -07:00
|
|
|
static float apply_deadzone(float error){
|
2019-08-27 09:10:44 -07:00
|
|
|
if(balance_conf.deadzone == 0){
|
2019-08-07 00:22:51 -07:00
|
|
|
return error;
|
|
|
|
}
|
|
|
|
|
2019-08-27 09:10:44 -07:00
|
|
|
if(error < balance_conf.deadzone && error > -balance_conf.deadzone){
|
2019-08-07 00:22:51 -07:00
|
|
|
return 0;
|
2019-08-27 09:10:44 -07:00
|
|
|
} else if(error > balance_conf.deadzone){
|
|
|
|
return error - balance_conf.deadzone;
|
2019-08-07 00:22:51 -07:00
|
|
|
} else {
|
2019-08-27 09:10:44 -07:00
|
|
|
return error + balance_conf.deadzone;
|
2019-08-07 00:22:51 -07:00
|
|
|
}
|
2019-07-30 00:27:23 -07:00
|
|
|
}
|
2019-07-27 21:25:56 -07:00
|
|
|
|
2021-05-23 14:58:47 -07:00
|
|
|
static void brake(void){
|
2021-06-05 16:05:19 -07:00
|
|
|
// Brake timeout logic
|
|
|
|
if(balance_conf.brake_timeout > 0 && (abs_erpm > 1 || brake_timeout == 0)){
|
|
|
|
brake_timeout = current_time + S2ST(balance_conf.brake_timeout);
|
|
|
|
}
|
|
|
|
if(brake_timeout != 0 && current_time > brake_timeout){
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2020-02-23 00:22:21 -08:00
|
|
|
// Reset the timeout
|
|
|
|
timeout_reset();
|
|
|
|
// Set current
|
|
|
|
mc_interface_set_brake_current(balance_conf.brake_current);
|
|
|
|
if(balance_conf.multi_esc){
|
|
|
|
for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
|
|
|
|
can_status_msg *msg = comm_can_get_status_msg_index(i);
|
|
|
|
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
|
|
|
|
comm_can_set_current_brake(msg->id, balance_conf.brake_current);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2021-05-23 14:58:47 -07:00
|
|
|
static void set_current(float current, float yaw_current){
|
2022-08-30 01:13:42 -07:00
|
|
|
// Limit current output to configured max output (does not account for yaw_current)
|
|
|
|
if(current > 0 && current > mc_interface_get_configuration()->l_current_max){
|
|
|
|
current = mc_interface_get_configuration()->l_current_max;
|
|
|
|
}else if(current < 0 && current < mc_interface_get_configuration()->l_current_min){
|
|
|
|
current = mc_interface_get_configuration()->l_current_min;
|
|
|
|
}
|
2020-02-23 00:22:21 -08:00
|
|
|
// Reset the timeout
|
|
|
|
timeout_reset();
|
|
|
|
// Set current
|
|
|
|
if(balance_conf.multi_esc){
|
2021-05-16 14:53:03 -07:00
|
|
|
// Set the current delay
|
2021-05-23 14:58:47 -07:00
|
|
|
mc_interface_set_current_off_delay(motor_timeout);
|
2021-05-16 14:53:03 -07:00
|
|
|
// Set Current
|
2020-02-23 00:22:21 -08:00
|
|
|
mc_interface_set_current(current + yaw_current);
|
2021-05-16 14:53:03 -07:00
|
|
|
// Can bus
|
2020-02-23 00:22:21 -08:00
|
|
|
for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
|
|
|
|
can_status_msg *msg = comm_can_get_status_msg_index(i);
|
|
|
|
|
|
|
|
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
|
2021-05-23 14:58:47 -07:00
|
|
|
comm_can_set_current_off_delay(msg->id, current - yaw_current, motor_timeout);// Assume 2 motors, i don't know how to steer 3 anyways
|
2020-02-23 00:22:21 -08:00
|
|
|
}
|
|
|
|
}
|
|
|
|
} else {
|
2021-05-16 14:53:03 -07:00
|
|
|
// Set the current delay
|
2021-05-23 14:58:47 -07:00
|
|
|
mc_interface_set_current_off_delay(motor_timeout);
|
2021-05-16 14:53:03 -07:00
|
|
|
// Set Current
|
2020-02-23 00:22:21 -08:00
|
|
|
mc_interface_set_current(current);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2019-08-10 22:00:00 -07:00
|
|
|
static THD_FUNCTION(balance_thread, arg) {
|
2019-07-27 21:25:56 -07:00
|
|
|
(void)arg;
|
2019-08-07 00:22:51 -07:00
|
|
|
chRegSetThreadName("APP_BALANCE");
|
2019-07-27 21:25:56 -07:00
|
|
|
|
|
|
|
while (!chThdShouldTerminateX()) {
|
2019-08-04 11:47:23 -07:00
|
|
|
// Update times
|
|
|
|
current_time = chVTGetSystemTimeX();
|
2019-08-10 00:16:48 -07:00
|
|
|
if(last_time == 0){
|
2019-08-04 11:47:23 -07:00
|
|
|
last_time = current_time;
|
|
|
|
}
|
|
|
|
diff_time = current_time - last_time;
|
2021-06-06 14:01:42 -07:00
|
|
|
filtered_diff_time = 0.03 * diff_time + 0.97 * filtered_diff_time; // Purely a metric
|
2019-08-04 11:47:23 -07:00
|
|
|
last_time = current_time;
|
2021-06-06 14:01:42 -07:00
|
|
|
if(balance_conf.loop_time_filter > 0){
|
|
|
|
loop_overshoot = diff_time - (loop_time - roundf(filtered_loop_overshoot));
|
|
|
|
filtered_loop_overshoot = loop_overshoot_alpha * loop_overshoot + (1-loop_overshoot_alpha)*filtered_loop_overshoot;
|
|
|
|
}
|
2019-08-04 11:47:23 -07:00
|
|
|
|
|
|
|
// Read values for GUI
|
|
|
|
motor_current = mc_interface_get_tot_current_directional_filtered();
|
|
|
|
motor_position = mc_interface_get_pid_pos_now();
|
|
|
|
|
2022-04-21 00:35:15 -07:00
|
|
|
// Set "last" values to previous loops values
|
2021-06-01 03:20:00 -07:00
|
|
|
last_pitch_angle = pitch_angle;
|
2022-04-21 00:35:15 -07:00
|
|
|
last_gyro_y = gyro[1];
|
|
|
|
// Get the values we want
|
2021-10-13 09:13:51 -07:00
|
|
|
pitch_angle = RAD2DEG_f(imu_get_pitch());
|
|
|
|
roll_angle = RAD2DEG_f(imu_get_roll());
|
2021-04-07 22:42:21 -07:00
|
|
|
abs_roll_angle = fabsf(roll_angle);
|
2021-10-13 09:13:51 -07:00
|
|
|
abs_roll_angle_sin = sinf(DEG2RAD_f(abs_roll_angle));
|
2020-02-23 00:22:21 -08:00
|
|
|
imu_get_gyro(gyro);
|
2020-02-23 10:23:19 -08:00
|
|
|
duty_cycle = mc_interface_get_duty_cycle_now();
|
|
|
|
abs_duty_cycle = fabsf(duty_cycle);
|
2020-03-15 23:18:34 -07:00
|
|
|
erpm = mc_interface_get_rpm();
|
|
|
|
abs_erpm = fabsf(erpm);
|
|
|
|
if(balance_conf.multi_esc){
|
|
|
|
avg_erpm = erpm;
|
|
|
|
for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
|
|
|
|
can_status_msg *msg = comm_can_get_status_msg_index(i);
|
|
|
|
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
|
|
|
|
avg_erpm += msg->rpm;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
avg_erpm = avg_erpm/2;// Assume 2 motors, i don't know how to steer 3 anyways
|
|
|
|
}
|
2020-02-23 13:34:51 -08:00
|
|
|
adc1 = (((float)ADC_Value[ADC_IND_EXT])/4095) * V_REG;
|
|
|
|
#ifdef ADC_IND_EXT2
|
|
|
|
adc2 = (((float)ADC_Value[ADC_IND_EXT2])/4095) * V_REG;
|
|
|
|
#else
|
|
|
|
adc2 = 0.0;
|
|
|
|
#endif
|
2019-08-04 11:47:23 -07:00
|
|
|
|
2020-02-23 13:34:51 -08:00
|
|
|
// Calculate switch state from ADC values
|
2020-07-03 20:33:15 -07:00
|
|
|
if(balance_conf.fault_adc1 == 0 && balance_conf.fault_adc2 == 0){ // No Switch
|
2020-02-23 13:34:51 -08:00
|
|
|
switch_state = ON;
|
2020-07-03 20:33:15 -07:00
|
|
|
}else if(balance_conf.fault_adc2 == 0){ // Single switch on ADC1
|
|
|
|
if(adc1 > balance_conf.fault_adc1){
|
2020-02-23 13:34:51 -08:00
|
|
|
switch_state = ON;
|
|
|
|
} else {
|
|
|
|
switch_state = OFF;
|
2019-09-10 01:07:05 -07:00
|
|
|
}
|
2020-07-03 20:33:15 -07:00
|
|
|
}else if(balance_conf.fault_adc1 == 0){ // Single switch on ADC2
|
|
|
|
if(adc2 > balance_conf.fault_adc2){
|
2020-02-23 13:34:51 -08:00
|
|
|
switch_state = ON;
|
|
|
|
} else {
|
|
|
|
switch_state = OFF;
|
|
|
|
}
|
|
|
|
}else{ // Double switch
|
2020-07-03 20:33:15 -07:00
|
|
|
if(adc1 > balance_conf.fault_adc1 && adc2 > balance_conf.fault_adc2){
|
2020-02-23 13:34:51 -08:00
|
|
|
switch_state = ON;
|
2020-07-03 20:33:15 -07:00
|
|
|
}else if(adc1 > balance_conf.fault_adc1 || adc2 > balance_conf.fault_adc2){
|
2022-07-16 13:52:18 -07:00
|
|
|
if (balance_conf.fault_is_dual_switch)
|
|
|
|
switch_state = ON;
|
|
|
|
else
|
|
|
|
switch_state = HALF;
|
2020-02-23 13:34:51 -08:00
|
|
|
}else{
|
|
|
|
switch_state = OFF;
|
2019-09-10 01:07:05 -07:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2020-02-23 13:34:51 -08:00
|
|
|
|
2020-06-19 01:00:37 -07:00
|
|
|
// Control Loop State Logic
|
2019-08-04 11:47:23 -07:00
|
|
|
switch(state){
|
2019-08-27 19:57:35 -07:00
|
|
|
case (STARTUP):
|
2021-06-05 17:50:50 -07:00
|
|
|
// Disable output
|
|
|
|
brake();
|
|
|
|
if(imu_startup_done()){
|
|
|
|
reset_vars();
|
|
|
|
state = FAULT_STARTUP; // Trigger a fault so we need to meet start conditions to start
|
2019-08-27 09:10:44 -07:00
|
|
|
}
|
2020-07-02 01:07:54 -07:00
|
|
|
break;
|
2019-08-04 11:47:23 -07:00
|
|
|
case (RUNNING):
|
2020-06-19 01:00:37 -07:00
|
|
|
case (RUNNING_TILTBACK_DUTY):
|
|
|
|
case (RUNNING_TILTBACK_HIGH_VOLTAGE):
|
|
|
|
case (RUNNING_TILTBACK_LOW_VOLTAGE):
|
2019-08-07 00:22:51 -07:00
|
|
|
|
2020-06-19 01:00:37 -07:00
|
|
|
// Check for faults
|
2020-07-02 01:07:54 -07:00
|
|
|
if(check_faults(false)){
|
2020-06-19 01:00:37 -07:00
|
|
|
break;
|
2019-08-04 23:13:35 -07:00
|
|
|
}
|
|
|
|
|
2020-07-02 01:07:54 -07:00
|
|
|
// Calculate setpoint and interpolation
|
|
|
|
calculate_setpoint_target();
|
|
|
|
calculate_setpoint_interpolated();
|
2021-03-27 12:17:10 -07:00
|
|
|
setpoint = setpoint_target_interpolated;
|
2021-06-30 16:17:57 -07:00
|
|
|
apply_noseangling();
|
2021-04-06 02:18:32 -07:00
|
|
|
apply_torquetilt();
|
|
|
|
apply_turntilt();
|
2019-08-04 23:13:35 -07:00
|
|
|
|
2019-08-04 11:47:23 -07:00
|
|
|
// Do PID maths
|
2019-09-21 01:07:31 -07:00
|
|
|
proportional = setpoint - pitch_angle;
|
2019-08-07 00:22:51 -07:00
|
|
|
// Apply deadzone
|
|
|
|
proportional = apply_deadzone(proportional);
|
|
|
|
// Resume real PID maths
|
2019-08-04 11:47:23 -07:00
|
|
|
integral = integral + proportional;
|
2021-06-01 03:20:00 -07:00
|
|
|
derivative = last_pitch_angle - pitch_angle;
|
2019-08-04 11:47:23 -07:00
|
|
|
|
2022-09-05 18:44:14 -07:00
|
|
|
// Apply I term Filter
|
|
|
|
if(balance_conf.ki_limit > 0 && fabsf(integral * balance_conf.ki) > balance_conf.ki_limit){
|
|
|
|
integral = balance_conf.ki_limit / balance_conf.ki * SIGN(integral);
|
|
|
|
}
|
|
|
|
|
2021-06-01 03:20:00 -07:00
|
|
|
// Apply D term filters
|
2021-05-16 14:53:03 -07:00
|
|
|
if(balance_conf.kd_pt1_lowpass_frequency > 0){
|
|
|
|
d_pt1_lowpass_state = d_pt1_lowpass_state + d_pt1_lowpass_k * (derivative - d_pt1_lowpass_state);
|
|
|
|
derivative = d_pt1_lowpass_state;
|
|
|
|
}
|
|
|
|
if(balance_conf.kd_pt1_highpass_frequency > 0){
|
|
|
|
d_pt1_highpass_state = d_pt1_highpass_state + d_pt1_highpass_k * (derivative - d_pt1_highpass_state);
|
|
|
|
derivative = derivative - d_pt1_highpass_state;
|
2020-07-03 20:33:15 -07:00
|
|
|
}
|
2020-06-19 01:00:37 -07:00
|
|
|
|
2019-08-27 09:10:44 -07:00
|
|
|
pid_value = (balance_conf.kp * proportional) + (balance_conf.ki * integral) + (balance_conf.kd * derivative);
|
2019-08-04 11:47:23 -07:00
|
|
|
|
2022-04-21 00:35:15 -07:00
|
|
|
if(balance_conf.pid_mode == BALANCE_PID_MODE_ANGLE_RATE_CASCADE){
|
|
|
|
proportional2 = pid_value - gyro[1];
|
|
|
|
integral2 = integral2 + proportional2;
|
|
|
|
derivative2 = last_gyro_y - gyro[1];
|
2022-09-05 18:44:14 -07:00
|
|
|
|
|
|
|
// Apply I term Filter
|
|
|
|
if(balance_conf.ki_limit > 0 && fabsf(integral2 * balance_conf.ki2) > balance_conf.ki_limit){
|
|
|
|
integral2 = balance_conf.ki_limit / balance_conf.ki2 * SIGN(integral2);
|
|
|
|
}
|
|
|
|
|
2022-04-21 00:35:15 -07:00
|
|
|
pid_value = (balance_conf.kp2 * proportional2) + (balance_conf.ki2 * integral2) + (balance_conf.kd2 * derivative2);
|
|
|
|
}
|
|
|
|
|
2019-08-04 11:47:23 -07:00
|
|
|
last_proportional = proportional;
|
|
|
|
|
2021-03-27 14:00:55 -07:00
|
|
|
// Apply Booster
|
|
|
|
abs_proportional = fabsf(proportional);
|
|
|
|
if(abs_proportional > balance_conf.booster_angle){
|
|
|
|
if(abs_proportional - balance_conf.booster_angle < balance_conf.booster_ramp){
|
2021-04-17 18:05:24 -07:00
|
|
|
pid_value += (balance_conf.booster_current * SIGN(proportional)) * ((abs_proportional - balance_conf.booster_angle) / balance_conf.booster_ramp);
|
2021-03-27 14:00:55 -07:00
|
|
|
}else{
|
|
|
|
pid_value += balance_conf.booster_current * SIGN(proportional);
|
|
|
|
}
|
2019-08-07 00:22:51 -07:00
|
|
|
}
|
|
|
|
|
2020-02-23 00:22:21 -08:00
|
|
|
if(balance_conf.multi_esc){
|
2020-03-15 23:18:34 -07:00
|
|
|
// Calculate setpoint
|
2020-02-23 10:23:19 -08:00
|
|
|
if(abs_duty_cycle < .02){
|
2020-03-15 23:18:34 -07:00
|
|
|
yaw_setpoint = 0;
|
|
|
|
} else if(avg_erpm < 0){
|
|
|
|
yaw_setpoint = (-balance_conf.roll_steer_kp * roll_angle) + (balance_conf.roll_steer_erpm_kp * roll_angle * avg_erpm);
|
2020-02-23 00:22:21 -08:00
|
|
|
} else{
|
2020-03-15 23:18:34 -07:00
|
|
|
yaw_setpoint = (balance_conf.roll_steer_kp * roll_angle) + (balance_conf.roll_steer_erpm_kp * roll_angle * avg_erpm);
|
2020-02-23 00:22:21 -08:00
|
|
|
}
|
2020-03-15 23:18:34 -07:00
|
|
|
// Do PID maths
|
|
|
|
yaw_proportional = yaw_setpoint - gyro[2];
|
2020-02-23 00:22:21 -08:00
|
|
|
yaw_integral = yaw_integral + yaw_proportional;
|
|
|
|
yaw_derivative = yaw_proportional - yaw_last_proportional;
|
2019-08-04 11:47:23 -07:00
|
|
|
|
2020-02-23 00:22:21 -08:00
|
|
|
yaw_pid_value = (balance_conf.yaw_kp * yaw_proportional) + (balance_conf.yaw_ki * yaw_integral) + (balance_conf.yaw_kd * yaw_derivative);
|
|
|
|
|
2020-03-15 23:18:34 -07:00
|
|
|
if(yaw_pid_value > balance_conf.yaw_current_clamp){
|
|
|
|
yaw_pid_value = balance_conf.yaw_current_clamp;
|
|
|
|
}else if(yaw_pid_value < -balance_conf.yaw_current_clamp){
|
|
|
|
yaw_pid_value = -balance_conf.yaw_current_clamp;
|
|
|
|
}
|
|
|
|
|
2020-02-23 00:22:21 -08:00
|
|
|
yaw_last_proportional = yaw_proportional;
|
2019-08-09 23:16:20 -07:00
|
|
|
}
|
2020-02-23 00:22:21 -08:00
|
|
|
|
|
|
|
// Output to motor
|
|
|
|
set_current(pid_value, yaw_pid_value);
|
2019-08-04 11:47:23 -07:00
|
|
|
break;
|
2020-06-19 01:00:37 -07:00
|
|
|
case (FAULT_ANGLE_PITCH):
|
|
|
|
case (FAULT_ANGLE_ROLL):
|
2020-07-04 03:35:19 -07:00
|
|
|
case (FAULT_SWITCH_HALF):
|
|
|
|
case (FAULT_SWITCH_FULL):
|
|
|
|
case (FAULT_STARTUP):
|
2019-09-10 01:07:05 -07:00
|
|
|
// Check for valid startup position and switch state
|
2020-02-23 13:34:51 -08:00
|
|
|
if(fabsf(pitch_angle) < balance_conf.startup_pitch_tolerance && fabsf(roll_angle) < balance_conf.startup_roll_tolerance && switch_state == ON){
|
2020-06-19 01:00:37 -07:00
|
|
|
reset_vars();
|
2019-08-04 23:13:35 -07:00
|
|
|
break;
|
|
|
|
}
|
2019-08-07 00:22:51 -07:00
|
|
|
// Disable output
|
2020-02-23 00:22:21 -08:00
|
|
|
brake();
|
2019-08-07 00:22:51 -07:00
|
|
|
break;
|
2020-06-19 01:00:37 -07:00
|
|
|
case (FAULT_DUTY):
|
|
|
|
// We need another fault to clear duty fault.
|
|
|
|
// Otherwise duty fault will clear itself as soon as motor pauses, then motor will spool up again.
|
|
|
|
// Rendering this fault useless.
|
2020-07-02 01:07:54 -07:00
|
|
|
check_faults(true);
|
2019-08-04 23:13:35 -07:00
|
|
|
// Disable output
|
2020-02-23 00:22:21 -08:00
|
|
|
brake();
|
2019-08-04 11:47:23 -07:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
2021-06-05 17:14:23 -07:00
|
|
|
// Debug outputs
|
2021-06-01 03:20:00 -07:00
|
|
|
app_balance_sample_debug();
|
2021-06-05 17:14:23 -07:00
|
|
|
app_balance_experiment();
|
2021-06-01 03:20:00 -07:00
|
|
|
|
2019-08-04 11:47:23 -07:00
|
|
|
// Delay between loops
|
2021-06-06 14:01:42 -07:00
|
|
|
chThdSleep(loop_time - roundf(filtered_loop_overshoot));
|
2019-07-27 21:25:56 -07:00
|
|
|
}
|
2019-08-07 00:22:51 -07:00
|
|
|
|
|
|
|
// Disable output
|
2020-02-23 00:22:21 -08:00
|
|
|
brake();
|
2019-07-27 21:25:56 -07:00
|
|
|
}
|
2021-06-01 03:20:00 -07:00
|
|
|
|
|
|
|
// Terminal commands
|
|
|
|
static void terminal_render(int argc, const char **argv) {
|
|
|
|
if (argc == 2 || argc == 3) {
|
|
|
|
int field = 0;
|
|
|
|
int graph = 1;
|
|
|
|
sscanf(argv[1], "%d", &field);
|
|
|
|
if(argc == 3){
|
|
|
|
sscanf(argv[2], "%d", &graph);
|
|
|
|
if(graph < 1 || graph > 2){
|
|
|
|
graph = 1;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
if(graph == 1){
|
|
|
|
debug_render_1 = field;
|
|
|
|
}else{
|
|
|
|
debug_render_2 = field;
|
|
|
|
}
|
|
|
|
} else {
|
|
|
|
commands_printf("This command requires one or two argument(s).\n");
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
static void terminal_sample(int argc, const char **argv) {
|
|
|
|
if (argc == 3) {
|
|
|
|
debug_sample_field = 0;
|
|
|
|
debug_sample_count = 0;
|
|
|
|
sscanf(argv[1], "%d", &debug_sample_field);
|
|
|
|
sscanf(argv[2], "%d", &debug_sample_count);
|
|
|
|
debug_sample_index = 0;
|
|
|
|
} else {
|
|
|
|
commands_printf("This command requires two arguments.\n");
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2021-06-05 17:14:23 -07:00
|
|
|
static void terminal_experiment(int argc, const char **argv) {
|
|
|
|
if (argc == 3) {
|
|
|
|
int field = 0;
|
|
|
|
int graph = 1;
|
|
|
|
sscanf(argv[1], "%d", &field);
|
|
|
|
sscanf(argv[2], "%d", &graph);
|
|
|
|
switch(graph){
|
|
|
|
case (1):
|
|
|
|
debug_experiment_1 = field;
|
|
|
|
break;
|
|
|
|
case (2):
|
|
|
|
debug_experiment_2 = field;
|
|
|
|
break;
|
|
|
|
case (3):
|
|
|
|
debug_experiment_3 = field;
|
|
|
|
break;
|
|
|
|
case (4):
|
|
|
|
debug_experiment_4 = field;
|
|
|
|
break;
|
|
|
|
case (5):
|
|
|
|
debug_experiment_5 = field;
|
|
|
|
break;
|
|
|
|
case (6):
|
|
|
|
debug_experiment_6 = field;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
commands_init_plot("Microseconds", "Balance App Debug Data");
|
|
|
|
commands_plot_add_graph("1");
|
|
|
|
commands_plot_add_graph("2");
|
|
|
|
commands_plot_add_graph("3");
|
|
|
|
commands_plot_add_graph("4");
|
|
|
|
commands_plot_add_graph("5");
|
|
|
|
commands_plot_add_graph("6");
|
|
|
|
} else {
|
|
|
|
commands_printf("This command requires two arguments.\n");
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2021-06-01 03:20:00 -07:00
|
|
|
// Debug functions
|
|
|
|
static float app_balance_get_debug(int index){
|
|
|
|
switch(index){
|
|
|
|
case(1):
|
|
|
|
return motor_position;
|
|
|
|
case(2):
|
|
|
|
return setpoint;
|
|
|
|
case(3):
|
|
|
|
return torquetilt_filtered_current;
|
|
|
|
case(4):
|
|
|
|
return derivative;
|
|
|
|
case(5):
|
|
|
|
return last_pitch_angle - pitch_angle;
|
|
|
|
case(6):
|
|
|
|
return motor_current;
|
2021-06-05 16:05:19 -07:00
|
|
|
case(7):
|
2021-06-06 14:01:42 -07:00
|
|
|
return erpm;
|
|
|
|
case(8):
|
2021-06-05 16:05:19 -07:00
|
|
|
return abs_erpm;
|
2021-06-06 14:01:42 -07:00
|
|
|
case(9):
|
|
|
|
return loop_time;
|
|
|
|
case(10):
|
|
|
|
return diff_time;
|
|
|
|
case(11):
|
|
|
|
return loop_overshoot;
|
|
|
|
case(12):
|
|
|
|
return filtered_loop_overshoot;
|
|
|
|
case(13):
|
|
|
|
return filtered_diff_time;
|
2022-09-05 18:44:14 -07:00
|
|
|
case(14):
|
|
|
|
return integral;
|
|
|
|
case(15):
|
|
|
|
return integral * balance_conf.ki;
|
|
|
|
case(16):
|
|
|
|
return integral2;
|
|
|
|
case(17):
|
|
|
|
return integral2 * balance_conf.ki2;
|
2021-06-01 03:20:00 -07:00
|
|
|
default:
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
static void app_balance_sample_debug(){
|
|
|
|
if(debug_sample_index < debug_sample_count){
|
|
|
|
commands_printf("%f", (double)app_balance_get_debug(debug_sample_field));
|
|
|
|
debug_sample_index += 1;
|
|
|
|
}
|
|
|
|
}
|
2021-06-05 17:14:23 -07:00
|
|
|
static void app_balance_experiment(){
|
|
|
|
if(debug_experiment_1 != 0){
|
|
|
|
commands_plot_set_graph(0);
|
|
|
|
commands_send_plot_points(ST2MS(current_time), app_balance_get_debug(debug_experiment_1));
|
|
|
|
}
|
|
|
|
if(debug_experiment_2 != 0){
|
|
|
|
commands_plot_set_graph(1);
|
|
|
|
commands_send_plot_points(ST2MS(current_time), app_balance_get_debug(debug_experiment_2));
|
|
|
|
}
|
|
|
|
if(debug_experiment_3 != 0){
|
|
|
|
commands_plot_set_graph(2);
|
|
|
|
commands_send_plot_points(ST2MS(current_time), app_balance_get_debug(debug_experiment_3));
|
|
|
|
}
|
|
|
|
if(debug_experiment_4 != 0){
|
|
|
|
commands_plot_set_graph(3);
|
|
|
|
commands_send_plot_points(ST2MS(current_time), app_balance_get_debug(debug_experiment_4));
|
|
|
|
}
|
|
|
|
if(debug_experiment_5 != 0){
|
|
|
|
commands_plot_set_graph(4);
|
|
|
|
commands_send_plot_points(ST2MS(current_time), app_balance_get_debug(debug_experiment_5));
|
|
|
|
}
|
|
|
|
if(debug_experiment_6 != 0){
|
|
|
|
commands_plot_set_graph(5);
|
|
|
|
commands_send_plot_points(ST2MS(current_time), app_balance_get_debug(debug_experiment_6));
|
|
|
|
}
|
|
|
|
}
|