From 3cbef57c64563f1facd5856bd8ee314ec1108186 Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Fri, 23 Nov 2012 15:57:49 +0900 Subject: [PATCH] ArduCopter: first merge of leonard's accel based altitude controller plus other changes from Randy Changes include: New low-level get_throttle_accel function takes target acceleration and compares vs earth-frame Z accelerometer values to produce output to motors. Higher level throttle controllers modified to call new get_throttle_accel controller Throttle_rate_stabilized controller added which maintains a desired climb/descent rate Throttle_land controller added - descends using normal auto throttle controller to 10m then descends at 50cm/s Multiple throttle modes added including landing mode Land flight mode no longer needs GPS Throttle cruise maintenance moved to update_throttle_cruise function --- ArduCopter/ArduCopter.pde | 210 +++++++------ ArduCopter/Attitude.pde | 556 ++++++++++++++++++++++++---------- ArduCopter/Parameters.h | 3 + ArduCopter/Parameters.pde | 2 + ArduCopter/commands_logic.pde | 76 +---- ArduCopter/config.h | 32 +- ArduCopter/defines.h | 12 +- ArduCopter/flip.pde | 6 +- ArduCopter/navigation.pde | 36 ++- ArduCopter/system.pde | 50 ++- 10 files changed, 607 insertions(+), 376 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 84fee25203..4403819302 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -609,6 +609,15 @@ static int32_t roll_rate_target_bf = 0; // body frame roll rate target static int32_t pitch_rate_target_bf = 0; // body frame pitch rate target static int32_t yaw_rate_target_bf = 0; // body frame yaw rate target +//////////////////////////////////////////////////////////////////////////////// +// Throttle variables +//////////////////////////////////////////////////////////////////////////////// +static int16_t throttle_accel_target_ef = 0; // earth frame throttle acceleration target +static bool throttle_accel_controller_active = false; // true when accel based throttle controller is active, false when higher level throttle controllers are providing throttle output directly +static float z_accel_meas = 0; // filtered throttle acceleration +static float throttle_avg; // g.throttle_cruise as a float + + //////////////////////////////////////////////////////////////////////////////// // ACRO Mode //////////////////////////////////////////////////////////////////////////////// @@ -714,8 +723,6 @@ static byte throttle_mode; //////////////////////////////////////////////////////////////////////////////// // An additional throttle added to keep the copter at the same altitude when banking static int16_t angle_boost; -// Push copter down for clean landing -static int16_t landing_boost; // for controlling the landing throttle curve //verifies landings static int16_t ground_detector; @@ -942,6 +949,11 @@ AP_Limit_Geofence geofence_limit(FENCE_START_BYTE, FENCE_WP_SIZE, MAX_FENC AP_Limit_Altitude altitude_limit(¤t_loc); #endif +//////////////////////////////////////////////////////////////////////////////// +// function definitions to keep compiler from complaining about undeclared functions +//////////////////////////////////////////////////////////////////////////////// +void get_throttle_althold(int32_t target_alt, int16_t max_climb_rate = ALTHOLD_MAX_CLIMB_RATE); + //////////////////////////////////////////////////////////////////////////////// // Top-level logic //////////////////////////////////////////////////////////////////////////////// @@ -1657,29 +1669,20 @@ void update_simple_mode(void) g.rc_2.control_in = _pitch; } -#define THROTTLE_FILTER_SIZE 2 - // 50 hz update rate // controls all throttle behavior void update_throttle_mode(void) { + int16_t pilot_climb_rate; + if(ap.do_flip) // this is pretty bad but needed to flip in AP modes. return; - int16_t throttle_out; - -#if AUTO_THROTTLE_HOLD != 0 - static float throttle_avg = 0; // this is initialised to g.throttle_cruise later -#endif - -#if FRAME_CONFIG != HELI_FRAME - // calculate angle boost - if(throttle_mode == THROTTLE_MANUAL) { - angle_boost = get_angle_boost(g.rc_3.control_in); - }else{ - angle_boost = get_angle_boost(g.throttle_cruise); + // do not run throttle controllers if motors disarmed + if( !motors.armed() ) { + set_throttle_out(0); + return; } -#endif #if FRAME_CONFIG == HELI_FRAME if (roll_pitch_mode == ROLL_PITCH_STABLE){ @@ -1690,109 +1693,118 @@ void update_throttle_mode(void) #endif // HELI_FRAME switch(throttle_mode) { + case THROTTLE_MANUAL: - if (g.rc_3.control_in > 0) { -#if FRAME_CONFIG == HELI_FRAME - g.rc_3.servo_out = g.rc_3.control_in; -#else - if (control_mode == ACRO) { - g.rc_3.servo_out = g.rc_3.control_in; - }else{ - g.rc_3.servo_out = g.rc_3.control_in + angle_boost; - } -#endif + // completely manual throttle + if(g.rc_3.control_in <= 0){ + set_throttle_out(0); + }else{ + // send pilot's output directly to motors + set_throttle_out(g.rc_3.control_in); -#if AUTO_THROTTLE_HOLD != 0 - // ensure throttle_avg has been initialised - if( throttle_avg == 0 ) { - throttle_avg = g.throttle_cruise; - } - // calc average throttle - if ((g.rc_3.control_in > g.throttle_min) && (abs(climb_rate) < 60) && (g_gps->ground_speed < 200)) { - throttle_avg = throttle_avg * .99 + (float)g.rc_3.control_in * .01; - g.throttle_cruise = throttle_avg; - } -#endif + // update estimate of throttle cruise + update_throttle_cruise(g.rc_3.control_in); - if (false == ap.takeoff_complete && motors.armed()) { + // check if we've taken off yet + if (!ap.takeoff_complete && motors.armed()) { if (g.rc_3.control_in > g.throttle_cruise) { // we must be in the air by now set_takeoff_complete(true); } } + } + break; + case THROTTLE_MANUAL_TILT_COMPENSATED: + // manual throttle but with angle boost + if (g.rc_3.control_in <= 0) { + set_throttle_out(0); }else{ + // TO-DO: combine multicopter and tradheli angle boost functions +#if FRAME_CONFIG == HELI_FRAME + set_throttle_out(heli_get_angle_boost(g.rc_3.control_in)); +#else + angle_boost = get_angle_boost(g.rc_3.control_in); + set_throttle_out(g.rc_3.control_in + angle_boost); +#endif - // make sure we also request 0 throttle out - // so the props stop ... properly - // ---------------------------------------- - g.rc_3.servo_out = 0; + // update estimate of throttle cruise + update_throttle_cruise(g.rc_3.control_in); + + if (!ap.takeoff_complete && motors.armed()) { + if (g.rc_3.control_in > g.throttle_cruise) { + // we must be in the air by now + set_takeoff_complete(true); + } + } + } + break; + + case THROTTLE_ACCELERATION: + // pilot inputs the desired acceleration + if(g.rc_3.control_in <= 0){ + set_throttle_out(0); + }else{ + int16_t desired_acceleration = get_pilot_desired_acceleration(g.rc_3.control_in); + set_throttle_accel_target(desired_acceleration); + } + + case THROTTLE_RATE: + // pilot inputs the desired climb rate. Note this is the unstabilized rate controller + if(g.rc_3.control_in <= 0){ + set_throttle_out(0); + }else{ + pilot_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in); + get_throttle_rate(pilot_climb_rate); + } + break; + + case THROTTLE_STABILIZED_RATE: + // pilot inputs the desired climb rate. Note this is the unstabilized rate controller + if(g.rc_3.control_in <= 0){ + set_throttle_out(0); + }else{ + pilot_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in); + get_throttle_rate_stabilized(pilot_climb_rate); + } + break; + + case THROTTLE_DIRECT_ALT: + // pilot inputs a desired altitude from 0 ~ 10 meters + if(g.rc_3.control_in <= 0){ + set_throttle_out(0); + }else{ + // To-Do: this should update the global desired altitude variable next_WP.alt + int32_t desired_alt = get_pilot_desired_direct_alt(g.rc_3.control_in); + get_throttle_althold(desired_alt); } break; case THROTTLE_HOLD: - // allow interactive changing of atitude - if(g.rc_3.radio_in < (g.rc_3.radio_min + 200)){ - int16_t _rate = 180 - (((g.rc_3.radio_in - g.rc_3.radio_min) * 12) / 20); - reset_throttle_counter = 150; - nav_throttle = get_throttle_rate(-_rate); - g.rc_3.servo_out = g.throttle_cruise + nav_throttle + angle_boost; - break; - }else if(g.rc_3.radio_in > (g.rc_3.radio_max - 200)){ - int16_t _rate = 300 - ((g.rc_3.radio_max - g.rc_3.radio_in) * 18) / 20; - reset_throttle_counter = 150; - nav_throttle = get_throttle_rate(_rate); - g.rc_3.servo_out = g.throttle_cruise + nav_throttle + angle_boost; - break; + // alt hold plus pilot input of climb rate + pilot_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in); + + // check for pilot override + if( pilot_climb_rate != 0 ) { + get_throttle_rate_stabilized(pilot_climb_rate); + }else{ + get_throttle_rate_stabilized(0); + force_new_altitude(current_loc.alt); //TO-DO: this should be set to stabilized target } - - - // allow 1 second of slow down after pilot moves throttle back into deadzone - if(reset_throttle_counter > 0) { - reset_throttle_counter--; - // if 1 second has passed set the target altitude to the current altitude - if(reset_throttle_counter == 0) { - force_new_altitude(max(current_loc.alt, 100)); - }else{ - nav_throttle = get_throttle_rate(0); - g.rc_3.servo_out = g.throttle_cruise + nav_throttle + angle_boost; - break; - } - } - - // else fall through + break; case THROTTLE_AUTO: - + // auto pilot altitude controller with target altitude held in next_WP.alt if(motors.auto_armed() == true) { - - // how far off are we - altitude_error = get_altitude_error(); - - //int16_t desired_climb_rate; - if(alt_change_flag == REACHED_ALT) { // we are at or above the target alt - desired_climb_rate = g.pi_alt_hold.get_p(altitude_error); // calculate desired speed from lon error - update_throttle_cruise(g.pi_alt_hold.get_i(altitude_error, .02)); - desired_climb_rate = constrain(desired_climb_rate, -250, 250); - nav_throttle = get_throttle_rate(desired_climb_rate); - }else{ - desired_climb_rate = get_desired_climb_rate(); - nav_throttle = get_throttle_rate(desired_climb_rate); - } + get_throttle_althold(next_WP.alt); + // TO-DO: need to somehow set nav_throttle } + // TO-DO: what if auto_armed is not true?! throttle stuck at unknown position? + break; - // hack to remove the influence of the ground effect - if(g.sonar_enabled && current_loc.alt < 100 && landing_boost != 0) { - nav_throttle = min(nav_throttle, 0); - } - -#if FRAME_CONFIG == HELI_FRAME - throttle_out = g.throttle_cruise + nav_throttle - landing_boost; -#else - throttle_out = g.throttle_cruise + nav_throttle + angle_boost - landing_boost; -#endif - - g.rc_3.servo_out = throttle_out; + case THROTTLE_LAND: + // landing throttle controller + get_throttle_land(); break; } } diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 30cf67ca1b..398f3ee278 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -291,6 +291,11 @@ run_rate_controllers() g.rc_2.servo_out = get_rate_pitch(pitch_rate_target_bf); g.rc_4.servo_out = get_rate_yaw(yaw_rate_target_bf); #endif + + // run throttle controller if an accel based target has been provided + if( throttle_accel_controller_active ) { + g.rc_3.servo_out = get_throttle_accel(throttle_accel_target_ef); + } } #if FRAME_CONFIG == HELI_FRAME @@ -584,11 +589,321 @@ get_rate_yaw(int32_t target_rate) } #endif // !HELI_FRAME +// calculate modified roll/pitch depending upon optical flow calculated position +static int32_t +get_of_roll(int32_t input_roll) +{ +#ifdef OPTFLOW_ENABLED + static float tot_x_cm = 0; // total distance from target + static uint32_t last_of_roll_update = 0; + int32_t new_roll = 0; + int32_t p,i,d; + + // check if new optflow data available + if( optflow.last_update != last_of_roll_update) { + last_of_roll_update = optflow.last_update; + + // add new distance moved + tot_x_cm += optflow.x_cm; + + // only stop roll if caller isn't modifying roll + if( input_roll == 0 && current_loc.alt < 1500) { + p = g.pid_optflow_roll.get_p(-tot_x_cm); + i = g.pid_optflow_roll.get_i(-tot_x_cm,1.0); // we could use the last update time to calculate the time change + d = g.pid_optflow_roll.get_d(-tot_x_cm,1.0); + new_roll = p+i+d; + }else{ + g.pid_optflow_roll.reset_I(); + tot_x_cm = 0; + p = 0; // for logging + i = 0; + d = 0; + } + // limit amount of change and maximum angle + of_roll = constrain(new_roll, (of_roll-20), (of_roll+20)); + + #if LOGGING_ENABLED == ENABLED + static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash + // log output if PID logging is on and we are tuning the rate P, I or D gains + if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_OPTFLOW_KP || g.radio_tuning == CH6_OPTFLOW_KI || g.radio_tuning == CH6_OPTFLOW_KD) ) { + log_counter++; + if( log_counter >= 5 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10 + log_counter = 0; + Log_Write_PID(CH6_OPTFLOW_KP, tot_x_cm, p, i, d, of_roll, tuning_value); + } + } + #endif // LOGGING_ENABLED == ENABLED + } + + // limit max angle + of_roll = constrain(of_roll, -1000, 1000); + + return input_roll+of_roll; +#else + return input_roll; +#endif +} + +static int32_t +get_of_pitch(int32_t input_pitch) +{ +#ifdef OPTFLOW_ENABLED + static float tot_y_cm = 0; // total distance from target + static uint32_t last_of_pitch_update = 0; + int32_t new_pitch = 0; + int32_t p,i,d; + + // check if new optflow data available + if( optflow.last_update != last_of_pitch_update ) { + last_of_pitch_update = optflow.last_update; + + // add new distance moved + tot_y_cm += optflow.y_cm; + + // only stop roll if caller isn't modifying pitch + if( input_pitch == 0 && current_loc.alt < 1500 ) { + p = g.pid_optflow_pitch.get_p(tot_y_cm); + i = g.pid_optflow_pitch.get_i(tot_y_cm, 1.0); // we could use the last update time to calculate the time change + d = g.pid_optflow_pitch.get_d(tot_y_cm, 1.0); + new_pitch = p + i + d; + }else{ + tot_y_cm = 0; + g.pid_optflow_pitch.reset_I(); + p = 0; // for logging + i = 0; + d = 0; + } + + // limit amount of change + of_pitch = constrain(new_pitch, (of_pitch-20), (of_pitch+20)); + + #if LOGGING_ENABLED == ENABLED + static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash + // log output if PID logging is on and we are tuning the rate P, I or D gains + if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_OPTFLOW_KP || g.radio_tuning == CH6_OPTFLOW_KI || g.radio_tuning == CH6_OPTFLOW_KD) ) { + log_counter++; + if( log_counter >= 5 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10 + log_counter = 0; + Log_Write_PID(CH6_OPTFLOW_KP+100, tot_y_cm, p, i, d, of_pitch, tuning_value); + } + } + #endif // LOGGING_ENABLED == ENABLED + } + + // limit max angle + of_pitch = constrain(of_pitch, -1000, 1000); + + return input_pitch+of_pitch; +#else + return input_pitch; +#endif +} + + +/************************************************************* + * throttle control + ****************************************************************/ + +// update_throttle_cruise - update throttle cruise if necessary +static void update_throttle_cruise(int16_t throttle) +{ + // ensure throttle_avg has been initialised + if( throttle_avg == 0 ) { + throttle_avg = g.throttle_cruise; + } + // calc average throttle + if ((throttle > g.throttle_min) && (abs(climb_rate) < 60) && (g_gps->ground_speed < 200)) { + throttle_avg = throttle_avg * .99 + (float)throttle * .01; + g.throttle_cruise = throttle_avg; + } +} + +static int16_t get_angle_boost(int16_t value) +{ + float temp = cos_pitch_x * cos_roll_x; + temp = constrain(temp, .75, 1.0); + return ((float)(value + 80) / temp) - (value + 80); +} + +#if FRAME_CONFIG == HELI_FRAME +// heli_angle_boost - adds a boost depending on roll/pitch values +// equivalent of quad's angle_boost function +// throttle value should be 0 ~ 1000 +static int16_t heli_get_angle_boost(int16_t throttle) +{ + float angle_boost_factor = cos_pitch_x * cos_roll_x; + angle_boost_factor = 1.0 - constrain(angle_boost_factor, .5, 1.0); + int16_t throttle_above_mid = max(throttle - motors.throttle_mid,0); + return throttle + throttle_above_mid*angle_boost_factor; + +} +#endif // HELI_FRAME + + // set_throttle_out - to be called by upper throttle controllers when they wish to provide throttle output directly to motors (instead of using accel based throttle controller) + // provide 0 to cut motors +void set_throttle_out( int16_t throttle_out ) +{ + g.rc_3.servo_out = throttle_out; + throttle_accel_controller_active = false; +} + +// set_throttle_accel_target - to be called by upper throttle controllers to set desired vertical acceleration in earth frame +void set_throttle_accel_target( int16_t desired_acceleration ) +{ + throttle_accel_target_ef = desired_acceleration; + throttle_accel_controller_active = true; +} + +// get_throttle_accel - accelerometer based throttle controller +// returns an actual throttle output (0 ~ 1000) to be sent to the motors static int16_t +get_throttle_accel(int16_t z_target_accel) +{ + static float accel_one_g = -980; // filtered estimate of 1G in cm/s/s + int32_t p,i,d; // used to capture pid values for logging + int16_t z_accel_error, output; + float z_accel_meas_temp; + + Vector3f accel = ins.get_accel(); + Matrix3f dcm_matrix = ahrs.get_dcm_matrix(); + + // Calculate Earth Frame Z acceleration + z_accel_meas_temp = (dcm_matrix.c.x * accel.x + dcm_matrix.c.y * accel.y + dcm_matrix.c.z * accel.z)* 100.0; + + // Filter Earth Frame Z acceleration with fc = 0.01 Hz to find 1 g + accel_one_g = accel_one_g + 0.00062792 * (z_accel_meas_temp - accel_one_g); + + z_accel_meas_temp = z_accel_meas_temp - accel_one_g; + + // Filter Earth Frame Z acceleration with fc = 1 Hz + z_accel_meas = z_accel_meas + 0.059117 * (z_accel_meas_temp - z_accel_meas); + + // calculate accel error + z_accel_error = constrain(z_target_accel + z_accel_meas, -32000, 32000); + + // separately calculate p, i, d values for logging + p = g.pid_throttle_accel.get_p(z_accel_error); + // freeze I term if we've breached throttle limits + if( motors.reached_limit(AP_MOTOR_THROTTLE_LIMIT) ) { + i = g.pid_throttle_accel.get_integrator(); + }else{ + i = g.pid_throttle_accel.get_i(z_accel_error, .02); + } + d = g.pid_throttle_accel.get_d(z_accel_error, .02); + + // + // limit the rate + output = constrain(p+i+d+g.throttle_cruise, g.throttle_min, g.throttle_max); + //Serial.printf("ThAccel 1 z_target_accel:%4.2f z_accel_meas:%4.2f z_accel_error:%4.2f output:%4.2f p:%4.2f i:%4.2f d:%4.2f \n", 1.0*z_target_accel, 1.0*z_accel_meas, 1.0*z_accel_error, 1.0*output, 1.0*p, 1.0*i, 1.0*d); + //Serial.printf("motors.reached_limit:%4.2f reset_accel_throttle_counter:%4.2f output:%4.2f p:%4.2f i:%4.2f d:%4.2f \n", 1.0*motors.reached_limit(0xff), 1.0*reset_accel_throttle_counter, 1.0*output, 1.0*p, 1.0*i, 1.0*d); + //Serial.printf("One G: z_target_accel:%4.2f z_accel_meas:%4.2f accel_one_g:%4.2f \n", 1.0*z_target_accel, 1.0*z_accel_meas, 1.0*accel_one_g); + +#if LOGGING_ENABLED == ENABLED + static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash + // log output if PID loggins is on and we are tuning the yaw + if( g.log_bitmask & MASK_LOG_PID && g.radio_tuning == CH6_THROTTLE_KP ) { + log_counter++; + if( log_counter >= 10 ) { // (update rate / desired output rate) = (50hz / 10hz) = 5hz + log_counter = 0; + Log_Write_PID(CH6_THROTTLE_KP, z_accel_error, p, i, d, output, tuning_value); + } + } +#endif + + return output; +} + +// get_pilot_desired_climb_rate - transform pilot's throttle input to +// climb rate in cm/s. we use radio_in instead of control_in to get the full range +// without any deadzone at the bottom +#define THROTTLE_IN_MIDDLE 500 // the throttle mid point +#define THROTTLE_IN_DEADBAND 100 // the throttle input channel's deadband in PWM +#define THROTTLE_IN_DEADBAND_TOP (THROTTLE_IN_MIDDLE+THROTTLE_IN_DEADBAND) // top of the deadband +#define THROTTLE_IN_DEADBAND_BOTTOM (THROTTLE_IN_MIDDLE-THROTTLE_IN_DEADBAND) // bottom of the deadband +static int16_t get_pilot_desired_climb_rate(int16_t throttle_control) +{ + int16_t desired_rate = 0; + + // throttle failsafe check + if( ap.failsafe ) { + return 0; + } + + // ensure a reasonable throttle value + throttle_control = constrain(throttle_control,0,1000); + + // check throttle is above, below or in the deadband + if (throttle_control < THROTTLE_IN_DEADBAND_BOTTOM) { + // below the deadband + desired_rate = (int32_t)THROTTLE_IN_MIDDLE * (throttle_control-THROTTLE_IN_DEADBAND_BOTTOM) / (THROTTLE_IN_MIDDLE - THROTTLE_IN_DEADBAND); + }else if (throttle_control > THROTTLE_IN_DEADBAND_TOP) { + // above the deadband + desired_rate = (int32_t)THROTTLE_IN_MIDDLE * (throttle_control-THROTTLE_IN_DEADBAND_TOP) / (THROTTLE_IN_MIDDLE - THROTTLE_IN_DEADBAND); + }else{ + // must be in the deadband + desired_rate = 0; + } + + return desired_rate; +} + +// get_pilot_desired_acceleration - transform pilot's throttle input to a desired acceleration +// default upper and lower bounds are 500 cm/s/s (roughly 1/2 a G) +// returns acceleration in cm/s/s +static int16_t get_pilot_desired_acceleration(int16_t throttle_control) +{ + int32_t desired_accel = 0; + + // throttle failsafe check + if( ap.failsafe ) { + return 0; + } + + // ensure a reasonable throttle value + throttle_control = constrain(throttle_control,0,1000); + + // check throttle is above, below or in the deadband + if (throttle_control < THROTTLE_IN_DEADBAND_BOTTOM) { + // below the deadband + desired_accel = (int32_t)THROTTLE_IN_MIDDLE * (throttle_control-THROTTLE_IN_DEADBAND_BOTTOM) / (THROTTLE_IN_MIDDLE - THROTTLE_IN_DEADBAND); + }else if(throttle_control > THROTTLE_IN_DEADBAND_TOP) { + // above the deadband + desired_accel = (int32_t)THROTTLE_IN_MIDDLE * (throttle_control-THROTTLE_IN_DEADBAND_TOP) / (THROTTLE_IN_MIDDLE - THROTTLE_IN_DEADBAND); + }else{ + // must be in the deadband + desired_accel = 0; + } + + return desired_accel; +} + +// get_pilot_desired_direct_alt - transform pilot's throttle input to a desired altitude +// return altitude in cm between 0 to 10m +static int32_t get_pilot_desired_direct_alt(int16_t throttle_control) +{ + int32_t desired_alt = 0; + + // throttle failsafe check + if( ap.failsafe ) { + return 0; + } + + // ensure a reasonable throttle value + throttle_control = constrain(throttle_control,0,1000); + + desired_alt = throttle_control; + + return desired_alt; +} + +// get_throttle_rate - calculates desired accel required to achieve desired z_target_speed +// sets accel based throttle controller target +static void get_throttle_rate(int16_t z_target_speed) { int32_t p,i,d; // used to capture pid values for logging - int16_t z_rate_error, output = 0; + int16_t z_rate_error; + int16_t target_accel; // calculate rate error #if INERTIAL_NAV == ENABLED @@ -597,12 +912,6 @@ get_throttle_rate(int16_t z_target_speed) z_rate_error = z_target_speed - climb_rate; // calc the speed error #endif -//////////////////////////////////////////////////////////////////////////////////////// -// Commenting this out because 'tmp' is not being used anymore? -// Robert Lefebvre 21/11/2012 -// int16_t tmp = ((int32_t)z_target_speed * (int32_t)g.throttle_cruise) / 280; -// tmp = min(tmp, 500); - // separately calculate p, i, d values for logging p = g.pid_throttle.get_p(z_rate_error); @@ -614,54 +923,109 @@ get_throttle_rate(int16_t z_target_speed) } d = g.pid_throttle.get_d(z_rate_error, .02); - // - // limit the rate - output += constrain(p+i+d, THROTTLE_RATE_CONSTRAIN_NEGATIVE, THROTTLE_RATE_CONSTRAIN_POSITIVE); + // consolidate target acceleration + target_accel = p+i+d; #if LOGGING_ENABLED == ENABLED - static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash + static uint8_t log_counter = 0; // used to slow down logging of PID values to dataflash // log output if PID loggins is on and we are tuning the yaw if( g.log_bitmask & MASK_LOG_PID && g.radio_tuning == CH6_THROTTLE_KP ) { log_counter++; if( log_counter >= 10 ) { // (update rate / desired output rate) = (50hz / 10hz) = 5hz log_counter = 0; - Log_Write_PID(CH6_THROTTLE_KP, z_rate_error, p, i, d, output, tuning_value); + Log_Write_PID(CH6_THROTTLE_KP, z_rate_error, p, i, d, target_accel, tuning_value); } } #endif - return output; + // set target for accel based throttle controller + set_throttle_accel_target(target_accel); } -// Keeps old data out of our calculation / logs -static void reset_nav_params(void) +// get_throttle_rate_stabilized - rate controller with additional 'stabilizer' +// 'stabilizer' ensure desired rate is being met +// calls normal throttle rate controller which updates accel based throttle controller targets +static void +get_throttle_rate_stabilized(int16_t target_rate) { - nav_throttle = 0; + static float alt_desired = 0; // The desired altitude in cm. + static float alt_rate = 0; // the desired climb rate in cm/s. + static uint32_t last_call_ms = 0; - // always start Circle mode at same angle - circle_angle = 0; + float altitude_error; + int16_t desired_rate; + int16_t alt_error_max; + uint32_t now = millis(); - // We must be heading to a new WP, so XTrack must be 0 - crosstrack_error = 0; + // reset target altitude if this controller has just been engaged + if( now - last_call_ms > 1000 ) { + alt_desired = current_loc.alt; + } + last_call_ms = millis(); - // Will be set by new command - target_bearing = 0; + // Limit acceleration of the desired altitude to +-5 m/s^2 + alt_rate += constrain(target_rate-alt_rate, -10, 10); + alt_desired += alt_rate * 0.02; - // Will be set by new command - wp_distance = 0; + alt_error_max = constrain(600-abs(target_rate),100,600); + altitude_error = constrain(alt_desired - current_loc.alt, -alt_error_max, alt_error_max); + alt_desired = current_loc.alt + altitude_error; - // Will be set by new command, used by loiter - long_error = 0; - lat_error = 0; - nav_lon = 0; - nav_lat = 0; - nav_roll = 0; - nav_pitch = 0; - auto_roll = 0; - auto_pitch = 0; + if(altitude_error > 0){ + desired_rate = g.pi_alt_hold.get_p(10*sqrt(altitude_error)); + }else if(altitude_error < 0){ + desired_rate = g.pi_alt_hold.get_p(-10*sqrt(abs(altitude_error))); + }else{ + desired_rate = g.pi_alt_hold.get_p(0); + } + desired_rate = constrain(desired_rate, -200, 200) + target_rate; - // make sure we stick to Nav yaw on takeoff - auto_yaw = nav_yaw; + // call rate based throttle controller which will update accel based throttle controller targets + get_throttle_rate(desired_rate); +} + +// get_throttle_althold - hold at the desired altitude in cm +// updates accel based throttle controller targets +// Note: max_climb_rate is an optional parameter to allow reuse of this function by landing controller +static void +get_throttle_althold(int32_t target_alt, int16_t max_climb_rate) +{ + int32_t altitude_error; + int16_t desired_rate; + + altitude_error = target_alt - current_loc.alt; + if(altitude_error > 0){ + desired_rate = g.pi_alt_hold.get_p(10*sqrt(altitude_error)); + }else if(altitude_error < 0){ + desired_rate = g.pi_alt_hold.get_p(-10*sqrt(abs(altitude_error))); + }else{ + desired_rate = g.pi_alt_hold.get_p(0); + } + desired_rate = constrain(desired_rate, ALTHOLD_MIN_CLIMB_RATE, max_climb_rate); + + // call rate based throttle controller which will update accel based throttle controller targets + get_throttle_rate(desired_rate); + + // TO-DO: enabled PID logging for this controller + //Log_Write_PID(1, (int32_t)target_alt, (int32_t)current_loc.alt, (int32_t)climb_rate, (int32_t)altitude_error, (int32_t)desired_rate, (float)desired_accel); + //Log_Write_PID(1, target_alt, current_loc.alt, climb_rate, altitude_error, desired_rate, desired_accel); +} + +// get_throttle_land - high level landing logic +// sends the desired acceleration in the accel based throttle controller +// called at 50hz +#define LAND_START_ALT 1000 // altitude in cm where land controller switches to slow rate of descent +#define LAND_DESCENT_SPEED -50 // minimum descent speed in cm/s +static void +get_throttle_land() +{ + // if we are above 10m perform regular alt hold descent + if (current_loc.alt >= LAND_START_ALT) { + get_throttle_althold(LAND_START_ALT, LAND_DESCENT_SPEED); + }else{ + get_throttle_rate_stabilized(LAND_DESCENT_SPEED); + } + // TO-DO: add detection of whether we've landed or not and set ap.land_complete } /* @@ -723,125 +1087,3 @@ static void reset_stability_I(void) g.pi_stabilize_roll.reset_I(); g.pi_stabilize_pitch.reset_I(); } - - -/************************************************************* - * throttle control - ****************************************************************/ - -static int16_t get_angle_boost(int16_t value) -{ - float temp = cos_pitch_x * cos_roll_x; - temp = constrain(temp, .75, 1.0); - return ((float)(value + 80) / temp) - (value + 80); -} - -// calculate modified roll/pitch depending upon optical flow calculated position -static int32_t -get_of_roll(int32_t input_roll) -{ -#if OPTFLOW == ENABLED - static float tot_x_cm = 0; // total distance from target - static uint32_t last_of_roll_update = 0; - int32_t new_roll = 0; - int32_t p,i,d; - - // check if new optflow data available - if( optflow.last_update != last_of_roll_update) { - last_of_roll_update = optflow.last_update; - - // add new distance moved - tot_x_cm += optflow.x_cm; - - // only stop roll if caller isn't modifying roll - if( input_roll == 0 && current_loc.alt < 1500) { - p = g.pid_optflow_roll.get_p(-tot_x_cm); - i = g.pid_optflow_roll.get_i(-tot_x_cm,1.0); // we could use the last update time to calculate the time change - d = g.pid_optflow_roll.get_d(-tot_x_cm,1.0); - new_roll = p+i+d; - }else{ - g.pid_optflow_roll.reset_I(); - tot_x_cm = 0; - p = 0; // for logging - i = 0; - d = 0; - } - // limit amount of change and maximum angle - of_roll = constrain(new_roll, (of_roll-20), (of_roll+20)); - - #if LOGGING_ENABLED == ENABLED - static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash - // log output if PID logging is on and we are tuning the rate P, I or D gains - if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_OPTFLOW_KP || g.radio_tuning == CH6_OPTFLOW_KI || g.radio_tuning == CH6_OPTFLOW_KD) ) { - log_counter++; - if( log_counter >= 5 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10 - log_counter = 0; - Log_Write_PID(CH6_OPTFLOW_KP, tot_x_cm, p, i, d, of_roll, tuning_value); - } - } - #endif // LOGGING_ENABLED == ENABLED - } - - // limit max angle - of_roll = constrain(of_roll, -1000, 1000); - - return input_roll+of_roll; -#else - return input_roll; -#endif // OPTFLOW == ENABLED -} - -static int32_t -get_of_pitch(int32_t input_pitch) -{ -#if OPTFLOW == ENABLED - static float tot_y_cm = 0; // total distance from target - static uint32_t last_of_pitch_update = 0; - int32_t new_pitch = 0; - int32_t p,i,d; - - // check if new optflow data available - if( optflow.last_update != last_of_pitch_update ) { - last_of_pitch_update = optflow.last_update; - - // add new distance moved - tot_y_cm += optflow.y_cm; - - // only stop roll if caller isn't modifying pitch - if( input_pitch == 0 && current_loc.alt < 1500 ) { - p = g.pid_optflow_pitch.get_p(tot_y_cm); - i = g.pid_optflow_pitch.get_i(tot_y_cm, 1.0); // we could use the last update time to calculate the time change - d = g.pid_optflow_pitch.get_d(tot_y_cm, 1.0); - new_pitch = p + i + d; - }else{ - tot_y_cm = 0; - g.pid_optflow_pitch.reset_I(); - p = 0; // for logging - i = 0; - d = 0; - } - - // limit amount of change - of_pitch = constrain(new_pitch, (of_pitch-20), (of_pitch+20)); - - #if LOGGING_ENABLED == ENABLED - static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash - // log output if PID logging is on and we are tuning the rate P, I or D gains - if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_OPTFLOW_KP || g.radio_tuning == CH6_OPTFLOW_KI || g.radio_tuning == CH6_OPTFLOW_KD) ) { - log_counter++; - if( log_counter >= 5 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10 - log_counter = 0; - Log_Write_PID(CH6_OPTFLOW_KP+100, tot_y_cm, p, i, d, of_pitch, tuning_value); - } - } - #endif // LOGGING_ENABLED == ENABLED - } - - // limit max angle - of_pitch = constrain(of_pitch, -1000, 1000); - - return input_pitch+of_pitch; -#else - return input_pitch; -#endif // OPTFLOW == ENABLED -} diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 7733fde9b3..479e21ca16 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -227,6 +227,7 @@ public: k_param_pid_optflow_pitch, k_param_acro_balance_roll, k_param_acro_balance_pitch, + k_param_pid_throttle_accel, // 241 // 254,255: reserved }; @@ -368,6 +369,7 @@ public: AC_PID pid_nav_lon; AC_PID pid_throttle; + AC_PID pid_throttle_accel; AC_PID pid_optflow_roll; AC_PID pid_optflow_pitch; @@ -416,6 +418,7 @@ public: pid_nav_lon (NAV_P, NAV_I, NAV_D, NAV_IMAX * 100), pid_throttle (THROTTLE_P, THROTTLE_I, THROTTLE_D, THROTTLE_IMAX), + pid_throttle_accel (THROTTLE_ACCEL_P, THROTTLE_ACCEL_I, THROTTLE_ACCEL_D, THROTTLE_ACCEL_IMAX), pid_optflow_roll (OPTFLOW_ROLL_P, OPTFLOW_ROLL_I, OPTFLOW_ROLL_D, OPTFLOW_IMAX * 100), pid_optflow_pitch (OPTFLOW_PITCH_P, OPTFLOW_PITCH_I, OPTFLOW_PITCH_D, OPTFLOW_IMAX * 100), diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 0f764d61cb..8d447115e4 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -371,6 +371,8 @@ const AP_Param::Info var_info[] PROGMEM = { GGROUP(pid_nav_lon, "NAV_LON_", AC_PID), GGROUP(pid_throttle, "THR_RATE_", AC_PID), + GGROUP(pid_throttle_accel,"THR_ACCEL_", AC_PID), + GGROUP(pid_optflow_roll, "OF_RLL_", AC_PID), GGROUP(pid_optflow_pitch, "OF_PIT_", AC_PID), diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 3edf384074..ad9bf0dbbc 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -148,11 +148,7 @@ static bool verify_must() break; case MAV_CMD_NAV_LAND: - if(g.sonar_enabled == true) { - return verify_land_sonar(); - }else{ - return verify_land_baro(); - } + return verify_land(); break; case MAV_CMD_NAV_LOITER_UNLIM: @@ -276,14 +272,11 @@ static void do_nav_wp() static void do_land() { wp_control = LOITER_MODE; + throttle_mode = THROTTLE_LAND; // just to make sure set_land_complete(false); - // landing boost lowers the main throttle to mimmick - // the effect of a user's hand - landing_boost = 0; - // A counter that goes up if our climb rate stalls out. ground_detector = 0; @@ -361,74 +354,21 @@ static bool verify_takeoff() return (current_loc.alt > next_WP.alt); } -// called at 10hz -static bool verify_land_sonar() +// verify_land - returns true if landing has been completed +static bool verify_land() { + // loiter above 3m if(current_loc.alt > 300) { wp_control = LOITER_MODE; - ground_detector = 0; - }else{ - // begin to pull down on the throttle - landing_boost++; - landing_boost = min(landing_boost, 40); - } - - if(current_loc.alt < 200 ) { - wp_control = NO_NAV_MODE; - } - - if(current_loc.alt < 150 ) { - // if we are low or don't seem to be decending much, increment ground detector - if(current_loc.alt < 80 || abs(climb_rate) < 20) { - landing_boost++; // reduce the throttle at twice the normal rate - - if(ground_detector < 30) { - ground_detector++; - }else if (ground_detector == 30) { - set_land_complete(true); - if(g.rc_3.control_in == 0) { - ground_detector++; - init_disarm_motors(); - } - return true; - } - } - } - return false; -} - -static bool verify_land_baro() -{ - if(current_loc.alt > 300) { - wp_control = LOITER_MODE; - ground_detector = 0; - }else{ - // begin to pull down on the throttle - landing_boost++; - landing_boost = min(landing_boost, 40); } + // turn off loiter below 1m if(current_loc.alt < 100 ) { wp_control = NO_NAV_MODE; } - if(current_loc.alt < 200 ) { - if(abs(climb_rate) < 40) { - landing_boost++; - - if(ground_detector < 30) { - ground_detector++; - }else if (ground_detector == 30) { - set_land_complete(true); - if(g.rc_3.control_in == 0) { - ground_detector++; - init_disarm_motors(); - } - return true; - } - } - } - return false; + // rely on THROTTLE_LAND mode to correctly update landing status + return ap.land_complete; } static bool verify_nav_wp() diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 2033b370bc..2bd3f202ba 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -871,10 +871,10 @@ #endif #ifndef ALT_HOLD_P - # define ALT_HOLD_P 0.3 // .5 + # define ALT_HOLD_P 0.5 #endif #ifndef ALT_HOLD_I - # define ALT_HOLD_I 0.04 + # define ALT_HOLD_I 0.0 #endif #ifndef ALT_HOLD_IMAX # define ALT_HOLD_IMAX 300 @@ -882,10 +882,10 @@ // RATE control #ifndef THROTTLE_P - # define THROTTLE_P 0.3 // .25 + # define THROTTLE_P 1.0 #endif #ifndef THROTTLE_I - # define THROTTLE_I 0.03 + # define THROTTLE_I 0.0 #endif #ifndef THROTTLE_D # define THROTTLE_D 0.0 @@ -894,15 +894,27 @@ # define THROTTLE_IMAX 300 #endif -#ifndef THROTTLE_RATE_CONSTRAIN_POSITIVE - # define THROTTLE_RATE_CONSTRAIN_POSITIVE 200 +// minimum and maximum climb rates while in alt hold mode +#ifndef ALTHOLD_MAX_CLIMB_RATE + # define ALTHOLD_MAX_CLIMB_RATE 500 +#endif +#ifndef ALTHOLD_MIN_CLIMB_RATE + # define ALTHOLD_MIN_CLIMB_RATE -ALTHOLD_MAX_CLIMB_RATE #endif -#ifndef THROTTLE_RATE_CONSTRAIN_NEGATIVE - # define THROTTLE_RATE_CONSTRAIN_NEGATIVE -150 +// Throttle Accel control +#ifndef THROTTLE_ACCEL_P + # define THROTTLE_ACCEL_P 0.5 +#endif +#ifndef THROTTLE_ACCEL_I + # define THROTTLE_ACCEL_I 1 +#endif +#ifndef THROTTLE_ACCEL_D + # define THROTTLE_ACCEL_D 0.0 +#endif +#ifndef THROTTLE_ACCEL_IMAX + # define THROTTLE_ACCEL_IMAX 500 #endif - - ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 43465df477..4dfe2e7320 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -28,9 +28,15 @@ #define ROLL_PITCH_TOY 4 // THOR This is the Roll and Pitch // mode -#define THROTTLE_MANUAL 0 -#define THROTTLE_HOLD 1 -#define THROTTLE_AUTO 2 +#define THROTTLE_MANUAL 0 // manual throttle mode - pilot input goes directly to motors +#define THROTTLE_MANUAL_TILT_COMPENSATED 1 // mostly manual throttle but with some tilt compensation +#define THROTTLE_ACCELERATION 2 // pilot inputs the desired acceleration +#define THROTTLE_RATE 3 // pilot inputs the desired climb rate. Note: this uses the unstabilized rate controller +#define THROTTLE_STABILIZED_RATE 4 // pilot inputs the desired climb rate. Uses stabilized rate controller +#define THROTTLE_DIRECT_ALT 5 // pilot inputs a desired altitude from 0 ~ 10 meters +#define THROTTLE_HOLD 6 // alt hold plus pilot input of climb rate +#define THROTTLE_AUTO 7 // auto pilot altitude controller with target altitude held in next_WP.alt +#define THROTTLE_LAND 8 // landing throttle controller // active altitude sensor diff --git a/ArduCopter/flip.pde b/ArduCopter/flip.pde index e80732190d..1ed9a9d86b 100644 --- a/ArduCopter/flip.pde +++ b/ArduCopter/flip.pde @@ -39,7 +39,7 @@ void roll_flip() if (roll < 4500) { // Roll control g.rc_1.servo_out = AAP_ROLL_OUT * flip_dir; - g.rc_3.servo_out = g.rc_3.control_in + AAP_THR_INC; + set_throttle_out(g.rc_3.control_in + AAP_THR_INC); }else{ flip_state++; } @@ -52,7 +52,7 @@ void roll_flip() #else g.rc_1.servo_out = get_rate_roll(40000 * flip_dir); #endif // HELI_FRAME - g.rc_3.servo_out = g.rc_3.control_in - AAP_THR_DEC; + set_throttle_out(g.rc_3.control_in - AAP_THR_DEC); }else{ flip_state++; flip_timer = 0; @@ -63,7 +63,7 @@ void roll_flip() if (flip_timer < 100) { //g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in); get_stabilize_roll(g.rc_1.control_in); - g.rc_3.servo_out = g.rc_3.control_in + AAP_THR_INC; + set_throttle_out(g.rc_3.control_in + AAP_THR_INC); flip_timer++; }else{ Log_Write_Event(DATA_END_FLIP); diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 2efbe0b18c..2ff4b85263 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -278,11 +278,7 @@ static void run_navigation_contollers() break; case LAND: - if(g.sonar_enabled) - verify_land_sonar(); - else - verify_land_baro(); - + verify_land(); // calculates the desired Roll and Pitch update_nav_wp(); break; @@ -654,6 +650,36 @@ static void verify_altitude() } } +// Keeps old data out of our calculation / logs +static void reset_nav_params(void) +{ + nav_throttle = 0; + + // always start Circle mode at same angle + circle_angle = 0; + + // We must be heading to a new WP, so XTrack must be 0 + crosstrack_error = 0; + + // Will be set by new command + target_bearing = 0; + + // Will be set by new command + wp_distance = 0; + + // Will be set by new command, used by loiter + long_error = 0; + lat_error = 0; + nav_lon = 0; + nav_lat = 0; + nav_roll = 0; + nav_pitch = 0; + auto_roll = 0; + auto_pitch = 0; + + // make sure we stick to Nav yaw on takeoff + auto_yaw = nav_yaw; +} static int32_t wrap_360(int32_t error) { diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index ba391d71c0..8afd6304bb 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -390,21 +390,16 @@ static void startup_ground(void) static void set_mode(byte mode) { - // if we don't have GPS lock - if(false == ap.home_is_set) { - // THOR - // We don't care about Home if we don't have lock yet in Toy mode - if(mode == TOY_A || mode == TOY_M || mode == OF_LOITER) { - // nothing - }else if (mode > ALT_HOLD) { + // Switch to stabilize mode if requested mode requires a GPS lock + if(!ap.home_is_set) { + if (mode > ALT_HOLD && mode != TOY_A && mode != TOY_M && mode != OF_LOITER && mode != LAND) { mode = STABILIZE; } } - // nothing but OF_LOITER for OptFlow only - if (g.optflow_enabled && g_gps->status() != GPS::GPS_OK) { - if (mode > ALT_HOLD && mode != OF_LOITER) - mode = STABILIZE; + // Switch to stabilize if OF_LOITER requested but no optical flow sensor + if (mode == OF_LOITER && !g.optflow_enabled ) { + mode = STABILIZE; } control_mode = mode; @@ -418,9 +413,6 @@ static void set_mode(byte mode) // clearing value used in interactive alt hold reset_throttle_counter = 0; - // clearing value used to force the copter down in landing mode - landing_boost = 0; - // do not auto_land if we are leaving RTL loiter_timer = 0; @@ -458,7 +450,7 @@ static void set_mode(byte mode) ap.manual_attitude = true; yaw_mode = YAW_HOLD; roll_pitch_mode = ROLL_PITCH_STABLE; - throttle_mode = THROTTLE_MANUAL; + throttle_mode = THROTTLE_MANUAL_TILT_COMPENSATED; break; case ALT_HOLD: @@ -521,11 +513,19 @@ static void set_mode(byte mode) break; case LAND: + if( ap.home_is_set ) { + // switch to loiter if we have gps + ap.manual_attitude = false; + yaw_mode = LOITER_YAW; + roll_pitch_mode = LOITER_RP; + }else{ + // otherwise remain with stabilize roll and pitch + ap.manual_attitude = true; + yaw_mode = YAW_HOLD; + roll_pitch_mode = ROLL_PITCH_STABLE; + } ap.manual_throttle = false; - ap.manual_attitude = false; - yaw_mode = LOITER_YAW; - roll_pitch_mode = LOITER_RP; - throttle_mode = THROTTLE_AUTO; + throttle_mode = THROTTLE_LAND; do_land(); break; @@ -615,18 +615,6 @@ init_simple_bearing() Log_Write_Data(DATA_INIT_SIMPLE_BEARING, initial_simple_bearing); } -static void update_throttle_cruise(int16_t tmp) -{ - if(tmp != 0) { - g.throttle_cruise += tmp; - reset_throttle_I(); - } - - // recalc kp - //g.pid_throttle.kP((float)g.throttle_cruise.get() / 981.0); - //cliSerial->printf("kp:%1.4f\n",kp); -} - #if CLI_SLIDER_ENABLED == ENABLED && CLI_ENABLED == ENABLED static boolean check_startup_for_CLI()