Skip to content

Commit

Permalink
Implement nose angle adjustment
Browse files Browse the repository at this point in the history
  • Loading branch information
@Mitchlol
Mitchlol committed Feb 23, 2020
1 parent 2c07512 commit d4f6036
Show file tree
Hide file tree
Showing 4 changed files with 30 additions and 22 deletions.
7 changes: 2 additions & 5 deletions appconf/appconf_default.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -349,11 +349,8 @@
#ifndef APPCONF_BALANCE_FAULT_DELAY
#define APPCONF_BALANCE_FAULT_DELAY 0
#endif
#ifndef APPCONF_BALANCE_NOSE_ANGLE
#define APPCONF_BALANCE_NOSE_ANGLE 0.0
#endif
#ifndef APPCONF_BALANCE_NOSE_ANGLE_SPEED
#define APPCONF_BALANCE_NOSE_ANGLE_SPEED 0.0
#ifndef APPCONF_BALANCE_TILTBACK_CONSTANT
#define APPCONF_BALANCE_TILTBACK_CONSTANT 0.0
#endif

// IMU
Expand Down
33 changes: 24 additions & 9 deletions applications/app_balance.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,7 @@ static thread_t *app_thread;
static BalanceState state;
static float pitch_angle, roll_angle;
static float gyro[3];
static float duty_cycle, abs_duty_cycle;
static float proportional, integral, derivative;
static float last_proportional;
static float pid_value;
Expand Down Expand Up @@ -90,6 +91,11 @@ void app_balance_start(void) {
state = STARTUP;
pitch_angle = 0;
roll_angle = 0;
gyro[0] = 0;
gyro[1] = 0;
gyro[2] = 0;
duty_cycle = 0;
abs_duty_cycle = 0;
switches_value = 0;
proportional = 0;
integral = 0;
Expand Down Expand Up @@ -218,7 +224,6 @@ static THD_FUNCTION(balance_thread, arg) {
tiltback_step_size = balance_conf.tiltback_speed / balance_conf.hertz;

state = STARTUP;
setpointAdjustmentType = CENTERING;

while (!chThdShouldTerminateX()) {
// Update times
Expand All @@ -237,6 +242,8 @@ static THD_FUNCTION(balance_thread, arg) {
pitch_angle = imu_get_pitch() * 180.0f / M_PI;
roll_angle = imu_get_roll() * 180.0f / M_PI;
imu_get_gyro(gyro);
duty_cycle = mc_interface_get_duty_cycle_now();
abs_duty_cycle = fabsf(duty_cycle);

if(!balance_conf.use_switches){
switches_value = 2;
Expand Down Expand Up @@ -267,7 +274,7 @@ static THD_FUNCTION(balance_thread, arg) {
break;
case (RUNNING):
// Check for overspeed
if(fabsf(mc_interface_get_duty_cycle_now()) > balance_conf.overspeed_duty){
if(abs_duty_cycle > balance_conf.overspeed_duty){
if(ST2MS(current_time - dead_start_time) > balance_conf.overspeed_delay){
state = DEAD;
}
Expand All @@ -280,7 +287,7 @@ static THD_FUNCTION(balance_thread, arg) {
fabsf(pitch_angle) > balance_conf.pitch_fault || // Balnce axis tip over
fabsf(roll_angle) > balance_conf.roll_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
(app_balance_get_switch_value() == 1 && abs_duty_cycle < 0.003) // Switch partially open and stopped
){
if(ST2MS(current_time - fault_start_time) > balance_conf.fault_delay){
state = FAULT;
Expand All @@ -290,15 +297,23 @@ 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_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){
if(abs_duty_cycle > balance_conf.tiltback_duty ||
(abs_duty_cycle > 0.05 && GET_INPUT_VOLTAGE() > balance_conf.tiltback_high_voltage) ||
(abs_duty_cycle > 0.05 && GET_INPUT_VOLTAGE() < balance_conf.tiltback_low_voltage)){
if(duty_cycle > 0){
setpoint_target = balance_conf.tiltback_angle;
} else {
setpoint_target = -balance_conf.tiltback_angle;
}
setpointAdjustmentType = TILTBACK;
}else if(abs_duty_cycle > 0.03){
// Nose angle adjustment
if(duty_cycle > 0){
setpoint_target = balance_conf.tiltback_constant;
} else {
setpoint_target = -balance_conf.tiltback_constant;
}
setpointAdjustmentType = TILTBACK;
}else{
setpoint_target = 0;
}
Expand Down Expand Up @@ -336,9 +351,9 @@ static THD_FUNCTION(balance_thread, arg) {

if(balance_conf.multi_esc){
// Do PID maths
if(fabsf(mc_interface_get_duty_cycle_now()) < .02){
if(abs_duty_cycle < .02){
yaw_proportional = 0 - gyro[2];
} else if(mc_interface_get_duty_cycle_now() < 0){
} else if(duty_cycle < 0){
yaw_proportional = (balance_conf.roll_steer_kp * roll_angle) - gyro[2];
} else{
yaw_proportional = (-balance_conf.roll_steer_kp * roll_angle) - gyro[2];
Expand Down
9 changes: 3 additions & 6 deletions confgenerator.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -260,8 +260,7 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration
buffer_append_float32_auto(buffer, conf->app_balance_conf.brake_current, &ind);
buffer_append_uint16(buffer, conf->app_balance_conf.overspeed_delay, &ind);
buffer_append_uint16(buffer, conf->app_balance_conf.fault_delay, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.nose_angle, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.nose_angle_speed, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.tiltback_constant, &ind);
buffer[ind++] = conf->imu_conf.type;
buffer[ind++] = conf->imu_conf.mode;
buffer_append_uint16(buffer, conf->imu_conf.sample_rate_hz, &ind);
Expand Down Expand Up @@ -548,8 +547,7 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration
conf->app_balance_conf.brake_current = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.overspeed_delay = buffer_get_uint16(buffer, &ind);
conf->app_balance_conf.fault_delay = buffer_get_uint16(buffer, &ind);
conf->app_balance_conf.nose_angle = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.nose_angle_speed = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.tiltback_constant = 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);
Expand Down Expand Up @@ -820,8 +818,7 @@ void confgenerator_set_defaults_appconf(app_configuration *conf) {
conf->app_balance_conf.brake_current = APPCONF_BALANCE_BRAKE_CURRENT;
conf->app_balance_conf.overspeed_delay = APPCONF_BALANCE_OVERSPEED_DELAY;
conf->app_balance_conf.fault_delay = APPCONF_BALANCE_FAULT_DELAY;
conf->app_balance_conf.nose_angle = APPCONF_BALANCE_NOSE_ANGLE;
conf->app_balance_conf.nose_angle_speed = APPCONF_BALANCE_NOSE_ANGLE_SPEED;
conf->app_balance_conf.tiltback_constant = APPCONF_BALANCE_TILTBACK_CONSTANT;
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;
Expand Down
3 changes: 1 addition & 2 deletions datatypes.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -565,8 +565,7 @@ typedef struct {
float brake_current;
uint16_t overspeed_delay;
uint16_t fault_delay;
float nose_angle;
float nose_angle_speed;
float tiltback_constant;
} balance_config;

// CAN status modes
Expand Down

0 comments on commit d4f6036

Please sign in to comment.