From 223c6fd4de2d206d86c8a7f10d753a2feced45fb Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 3 Feb 2014 14:06:08 +0900 Subject: [PATCH] Copter: remove deadwood, update_thr_cruise always runs --- ArduCopter/ArduCopter.pde | 356 +------------------- ArduCopter/Attitude.pde | 665 +------------------------------------- 2 files changed, 18 insertions(+), 1003 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index a5a8d89034..13e8461d02 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -882,6 +882,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { { auto_trim, 40, 14 }, { update_altitude, 40, 100 }, { run_nav_updates, 40, 80 }, + { update_thr_cruise, 40, 10 }, { three_hz_loop, 133, 9 }, { compass_accumulate, 8, 42 }, { barometer_accumulate, 8, 25 }, @@ -936,6 +937,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { { auto_trim, 10, 140 }, { update_altitude, 10, 1000 }, { run_nav_updates, 10, 800 }, + { update_thr_cruise, 1, 50 }, { three_hz_loop, 33, 90 }, { compass_accumulate, 2, 420 }, { barometer_accumulate, 2, 250 }, @@ -1146,6 +1148,8 @@ static void update_batt_compass(void) #if HIL_MODE != HIL_MODE_ATTITUDE // don't execute in HIL mode if(g.compass_enabled) { + // update compass with throttle value - used for compassmot + compass.set_throttle((float)g.rc_3.servo_out/1000.0f); if (compass.read()) { compass.null_offsets(); } @@ -1365,56 +1369,6 @@ static void update_GPS(void) failsafe_gps_check(); } -// set_roll_pitch_mode - update roll/pitch mode and initialise any variables as required -bool set_roll_pitch_mode(uint8_t new_roll_pitch_mode) -{ - // boolean to ensure proper initialisation of throttle modes - bool roll_pitch_initialised = false; - - // return immediately if no change - if( new_roll_pitch_mode == roll_pitch_mode ) { - return true; - } - - switch( new_roll_pitch_mode ) { - case ROLL_PITCH_STABLE: - reset_roll_pitch_in_filters(g.rc_1.control_in, g.rc_2.control_in); - roll_pitch_initialised = true; - break; - case ROLL_PITCH_ACRO: - // reset acro level rates - acro_roll_rate = 0; - acro_pitch_rate = 0; - roll_pitch_initialised = true; - break; - case ROLL_PITCH_STABLE_OF: - case ROLL_PITCH_DRIFT: - reset_roll_pitch_in_filters(g.rc_1.control_in, g.rc_2.control_in); - roll_pitch_initialised = true; - break; - case ROLL_PITCH_AUTO: - case ROLL_PITCH_LOITER: - case ROLL_PITCH_SPORT: - roll_pitch_initialised = true; - break; - reset_roll_pitch_in_filters(g.rc_1.control_in, g.rc_2.control_in); - } - - // if initialisation has been successful update the yaw mode - if( roll_pitch_initialised ) { - exit_roll_pitch_mode(roll_pitch_mode); - roll_pitch_mode = new_roll_pitch_mode; - } - - // return success or failure - return roll_pitch_initialised; -} - -// exit_roll_pitch_mode - peforms any code required when exiting the current roll-pitch mode -void exit_roll_pitch_mode(uint8_t old_roll_pitch_mode) -{ -} - // update_flight_mode - calls the appropriate attitude controllers based on flight mode // called at 100hz or more static void update_flight_mode() @@ -1488,101 +1442,6 @@ static void update_flight_mode() } } -// update_roll_pitch_mode - run high level roll and pitch controllers -// 100hz update rate -void update_roll_pitch_mode(void) -{ - switch(roll_pitch_mode) { - case ROLL_PITCH_ACRO: - // copy user input for reporting purposes - control_roll = g.rc_1.control_in; - control_pitch = g.rc_2.control_in; - -#if FRAME_CONFIG == HELI_FRAME - // ACRO does not get SIMPLE mode ability - if (motors.has_flybar()) { - g.rc_1.servo_out = g.rc_1.control_in; - g.rc_2.servo_out = g.rc_2.control_in; - }else{ - acro_level_mix = constrain_float(1-max(max(abs(g.rc_1.control_in), abs(g.rc_2.control_in)), abs(g.rc_4.control_in))/4500.0, 0, 1)*ahrs.cos_pitch(); - get_roll_rate_stabilized_bf(g.rc_1.control_in); - get_pitch_rate_stabilized_bf(g.rc_2.control_in); - get_acro_level_rates(); - } -#else // !HELI_FRAME - acro_level_mix = constrain_float(1-max(max(abs(g.rc_1.control_in), abs(g.rc_2.control_in)), abs(g.rc_4.control_in))/4500.0, 0, 1)*ahrs.cos_pitch(); - get_roll_rate_stabilized_bf(g.rc_1.control_in); - get_pitch_rate_stabilized_bf(g.rc_2.control_in); - get_acro_level_rates(); -#endif // HELI_FRAME - break; - - case ROLL_PITCH_AUTO: - // copy latest output from nav controller to stabilize controller - control_roll = wp_nav.get_roll(); - control_pitch = wp_nav.get_pitch(); - get_stabilize_roll(control_roll); - get_stabilize_pitch(control_pitch); - break; - - case ROLL_PITCH_STABLE_OF: - // apply SIMPLE mode transform - update_simple_mode(); - - // convert pilot input to lean angles - get_pilot_desired_lean_angles(g.rc_1.control_in, g.rc_2.control_in, control_roll, control_pitch); - - // mix in user control with optical flow - control_roll = get_of_roll(control_roll); - control_pitch = get_of_pitch(control_pitch); - - // call stabilize controller - get_stabilize_roll(control_roll); - get_stabilize_pitch(control_pitch); - break; - - case ROLL_PITCH_DRIFT: - get_roll_pitch_drift(); - break; - - case ROLL_PITCH_LOITER: - // apply SIMPLE mode transform - update_simple_mode(); - - // update loiter target from user controls - wp_nav.set_pilot_desired_acceleration(g.rc_1.control_in, g.rc_2.control_in); - - // copy latest output from nav controller to stabilize controller - control_roll = wp_nav.get_roll(); - control_pitch = wp_nav.get_pitch(); - - get_stabilize_roll(control_roll); - get_stabilize_pitch(control_pitch); - break; - - case ROLL_PITCH_SPORT: - // apply SIMPLE mode transform - update_simple_mode(); - - // copy user input for reporting purposes - control_roll = g.rc_1.control_in; - control_pitch = g.rc_2.control_in; - get_roll_rate_stabilized_ef(g.rc_1.control_in); - get_pitch_rate_stabilized_ef(g.rc_2.control_in); - break; - } - - #if FRAME_CONFIG != HELI_FRAME - if(g.rc_3.control_in == 0 && control_mode <= ACRO) { - reset_rate_I(); - } - #endif //HELI_FRAME - - if(ap.new_radio_frame) { - // clear new radio frame info - ap.new_radio_frame = false; - } -} static void init_simple_bearing() @@ -1643,213 +1502,6 @@ void update_super_simple_bearing(bool force_update) } } -// throttle_mode_manual - returns true if the throttle is directly controlled by the pilot -bool throttle_mode_manual(uint8_t thr_mode) -{ - return (thr_mode == THROTTLE_MANUAL || thr_mode == THROTTLE_MANUAL_TILT_COMPENSATED || thr_mode == THROTTLE_MANUAL_HELI); -} - -// set_throttle_mode - sets the throttle mode and initialises any variables as required -bool set_throttle_mode( uint8_t new_throttle_mode ) -{ - // boolean to ensure proper initialisation of throttle modes - bool throttle_initialised = false; - - // return immediately if no change - if( new_throttle_mode == throttle_mode ) { - return true; - } - - // initialise any variables required for the new throttle mode - switch(new_throttle_mode) { - case THROTTLE_MANUAL: - case THROTTLE_MANUAL_TILT_COMPENSATED: - throttle_accel_deactivate(); // this controller does not use accel based throttle controller - throttle_initialised = true; - break; - - case THROTTLE_HOLD: - case THROTTLE_AUTO: - controller_desired_alt = get_initial_alt_hold(current_loc.alt, climb_rate); // reset controller desired altitude to current altitude - wp_nav.set_desired_alt(controller_desired_alt); // same as above but for loiter controller - if (throttle_mode_manual(throttle_mode)) { // reset the alt hold I terms if previous throttle mode was manual - reset_throttle_I(); - set_accel_throttle_I_from_pilot_throttle(get_pilot_desired_throttle(g.rc_3.control_in)); - } - throttle_initialised = true; - break; - - case THROTTLE_LAND: - reset_land_detector(); // initialise land detector - controller_desired_alt = get_initial_alt_hold(current_loc.alt, climb_rate); // reset controller desired altitude to current altitude - throttle_initialised = true; - break; - -#if FRAME_CONFIG == HELI_FRAME - case THROTTLE_MANUAL_HELI: - throttle_accel_deactivate(); // this controller does not use accel based throttle controller - throttle_initialised = true; - break; -#endif - } - - // update the throttle mode - if( throttle_initialised ) { - throttle_mode = new_throttle_mode; - - // reset some variables used for logging - desired_climb_rate = 0; - nav_throttle = 0; - } - - // return success or failure - return throttle_initialised; -} - -// update_throttle_mode - run high level throttle controllers -// 50 hz update rate -void update_throttle_mode(void) -{ - int16_t pilot_climb_rate; - int16_t pilot_throttle_scaled; - - if(ap.do_flip) // this is pretty bad but needed to flip in AP modes. - return; - -#if FRAME_CONFIG != HELI_FRAME - // do not run throttle controllers if motors disarmed - if( !motors.armed() ) { - set_throttle_out(0, false); - throttle_accel_deactivate(); // do not allow the accel based throttle to override our command - set_target_alt_for_reporting(0); - return; - } -#endif // FRAME_CONFIG != HELI_FRAME - - switch(throttle_mode) { - - case THROTTLE_MANUAL: - // completely manual throttle - if(g.rc_3.control_in <= 0){ - set_throttle_out(0, false); - }else{ - // send pilot's output directly to motors - pilot_throttle_scaled = get_pilot_desired_throttle(g.rc_3.control_in); - set_throttle_out(pilot_throttle_scaled, false); - - // update estimate of throttle cruise - #if FRAME_CONFIG == HELI_FRAME - update_throttle_cruise(motors.get_collective_out()); - #else - update_throttle_cruise(pilot_throttle_scaled); - #endif //HELI_FRAME - } - set_target_alt_for_reporting(0); - break; - - case THROTTLE_MANUAL_TILT_COMPENSATED: - // manual throttle but with angle boost - if (g.rc_3.control_in <= 0) { - set_throttle_out(0, false); // no need for angle boost with zero throttle - }else{ - pilot_throttle_scaled = get_pilot_desired_throttle(g.rc_3.control_in); - set_throttle_out(pilot_throttle_scaled, true); - - // update estimate of throttle cruise - #if FRAME_CONFIG == HELI_FRAME - update_throttle_cruise(motors.get_collective_out()); - #else - update_throttle_cruise(pilot_throttle_scaled); - #endif //HELI_FRAME - } - set_target_alt_for_reporting(0); - break; - - case THROTTLE_HOLD: - if(ap.auto_armed) { - // alt hold plus pilot input of climb rate - pilot_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in); - - // special handling if we have landed - if (ap.land_complete) { - if (pilot_climb_rate > 0) { - // indicate we are taking off - set_land_complete(false); - // clear i term when we're taking off - set_throttle_takeoff(); - }else{ - // move throttle to minimum to keep us on the ground - set_throttle_out(0, false); - // deactivate accel based throttle controller (it will be automatically re-enabled when alt-hold controller next runs) - throttle_accel_deactivate(); - } - } - // check land_complete flag again in case it was changed above - if (!ap.land_complete) { - if( sonar_alt_health >= SONAR_ALT_HEALTH_MAX ) { - // if sonar is ok, use surface tracking - get_throttle_surface_tracking(pilot_climb_rate,0.02f); // this function calls set_target_alt_for_reporting for us - }else{ - // if no sonar fall back stabilize rate controller - get_throttle_rate_stabilized(pilot_climb_rate); // this function calls set_target_alt_for_reporting for us - } - } - }else{ - // pilot's throttle must be at zero so keep motors off - set_throttle_out(0, false); - // deactivate accel based throttle controller - throttle_accel_deactivate(); - set_target_alt_for_reporting(0); - } - break; - - case THROTTLE_AUTO: - // auto pilot altitude controller with target altitude held in wp_nav.get_desired_alt() - if(ap.auto_armed) { - // special handling if we are just taking off - if (ap.land_complete) { - // tell motors to do a slow start. - motors.slow_start(true); - } - get_throttle_althold_with_slew(wp_nav.get_desired_alt(), -wp_nav.get_descent_velocity(), wp_nav.get_climb_velocity()); - set_target_alt_for_reporting(wp_nav.get_desired_alt()); // To-Do: return get_destination_alt if we are flying to a waypoint - }else{ - // pilot's throttle must be at zero so keep motors off - set_throttle_out(0, false); - // deactivate accel based throttle controller - throttle_accel_deactivate(); - set_target_alt_for_reporting(0); - } - break; - - case THROTTLE_LAND: - // landing throttle controller - get_throttle_land(); - set_target_alt_for_reporting(0); - break; - -#if FRAME_CONFIG == HELI_FRAME - case THROTTLE_MANUAL_HELI: - // trad heli manual throttle controller - // send pilot's output directly to swash plate - pilot_throttle_scaled = get_pilot_desired_collective(g.rc_3.control_in); - set_throttle_out(pilot_throttle_scaled, false); - - // update estimate of throttle cruise - update_throttle_cruise(motors.get_collective_out()); - set_target_alt_for_reporting(0); - break; -#endif // HELI_FRAME - - } -} - -// set_target_alt_for_reporting - set target altitude in cm for reporting purposes (logs and gcs) -static void set_target_alt_for_reporting(float alt_cm) -{ - target_alt_for_reporting = alt_cm; -} - static void read_AHRS(void) { // Perform IMU calculations and get attitude info diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 43bdbd76c3..c7873d8ecd 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -69,438 +69,10 @@ static float get_pilot_desired_yaw_rate(int16_t stick_angle) return stick_angle * g.acro_yaw_p; } -static void -get_stabilize_roll(int32_t target_angle) -{ - // angle error - target_angle = wrap_180_cd(target_angle - ahrs.roll_sensor); - - // limit the error we're feeding to the PID - target_angle = constrain_int32(target_angle, -aparm.angle_max, aparm.angle_max); - - // convert to desired rate - int32_t target_rate = g.pi_stabilize_roll.kP() * target_angle; - - // constrain the target rate - if (!ap.disable_stab_rate_limit) { - target_rate = constrain_int32(target_rate, -g.angle_rate_max, g.angle_rate_max); - } - - // set targets for rate controller - set_roll_rate_target(target_rate, EARTH_FRAME); -} - -static void -get_stabilize_pitch(int32_t target_angle) -{ - // angle error - target_angle = wrap_180_cd(target_angle - ahrs.pitch_sensor); - - // limit the error we're feeding to the PID - target_angle = constrain_int32(target_angle, -aparm.angle_max, aparm.angle_max); - - // convert to desired rate - int32_t target_rate = g.pi_stabilize_pitch.kP() * target_angle; - - // constrain the target rate - if (!ap.disable_stab_rate_limit) { - target_rate = constrain_int32(target_rate, -g.angle_rate_max, g.angle_rate_max); - } - - // set targets for rate controller - set_pitch_rate_target(target_rate, EARTH_FRAME); -} - -static void -get_stabilize_yaw(int32_t target_angle) -{ - int32_t target_rate; - int32_t angle_error; - - // angle error - angle_error = wrap_180_cd(target_angle - ahrs.yaw_sensor); - - // limit the error we're feeding to the PID - angle_error = constrain_int32(angle_error, -4500, 4500); - - // convert angle error to desired Rate: - target_rate = g.pi_stabilize_yaw.kP() * angle_error; - - // do not use rate controllers for helicotpers with external gyros -#if FRAME_CONFIG == HELI_FRAME - if(motors.tail_type() == AP_MOTORS_HELI_TAILTYPE_SERVO_EXTGYRO) { - g.rc_4.servo_out = constrain_int32(target_rate, -4500, 4500); - } -#endif - - // set targets for rate controller - set_yaw_rate_target(target_rate, EARTH_FRAME); -} - -// get_acro_level_rates - calculate earth frame rate corrections to pull the copter back to level while in ACRO mode -static void -get_acro_level_rates() -{ - // zero earth frame leveling if trainer is disabled - if (g.acro_trainer == ACRO_TRAINER_DISABLED) { - set_roll_rate_target(0, BODY_EARTH_FRAME); - set_pitch_rate_target(0, BODY_EARTH_FRAME); - set_yaw_rate_target(0, BODY_EARTH_FRAME); - return; - } - - // Calculate trainer mode earth frame rate command for roll - int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor); - int32_t target_rate = 0; - - if (g.acro_trainer == ACRO_TRAINER_LIMITED) { - if (roll_angle > aparm.angle_max){ - target_rate = g.pi_stabilize_roll.get_p(aparm.angle_max-roll_angle); - }else if (roll_angle < -aparm.angle_max) { - target_rate = g.pi_stabilize_roll.get_p(-aparm.angle_max-roll_angle); - } - } - roll_angle = constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE); - target_rate -= roll_angle * g.acro_balance_roll; - - // add earth frame targets for roll rate controller - set_roll_rate_target(target_rate, BODY_EARTH_FRAME); - - // Calculate trainer mode earth frame rate command for pitch - int32_t pitch_angle = wrap_180_cd(ahrs.pitch_sensor); - target_rate = 0; - - if (g.acro_trainer == ACRO_TRAINER_LIMITED) { - if (pitch_angle > aparm.angle_max){ - target_rate = g.pi_stabilize_pitch.get_p(aparm.angle_max-pitch_angle); - }else if (pitch_angle < -aparm.angle_max) { - target_rate = g.pi_stabilize_pitch.get_p(-aparm.angle_max-pitch_angle); - } - } - pitch_angle = constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE); - target_rate -= pitch_angle * g.acro_balance_pitch; - - // add earth frame targets for pitch rate controller - set_pitch_rate_target(target_rate, BODY_EARTH_FRAME); - - // add earth frame targets for yaw rate controller - set_yaw_rate_target(0, BODY_EARTH_FRAME); -} - -// Roll with rate input and stabilized in the body frame -static void -get_roll_rate_stabilized_bf(int32_t stick_angle) -{ - static float angle_error = 0; - - // convert the input to the desired body frame roll rate - int32_t rate_request = stick_angle * g.acro_rp_p; - - if (g.acro_trainer == ACRO_TRAINER_LIMITED) { - rate_request += acro_roll_rate; - }else{ - // Scale pitch leveling by stick input - acro_roll_rate = (float)acro_roll_rate*acro_level_mix; - - // Calculate rate limit to prevent change of rate through inverted - int32_t rate_limit = labs(labs(rate_request)-labs(acro_roll_rate)); - - rate_request += acro_roll_rate; - rate_request = constrain_int32(rate_request, -rate_limit, rate_limit); - } - - // add automatic correction - int32_t rate_correction = g.pi_stabilize_roll.get_p(angle_error); - - // set body frame targets for rate controller - set_roll_rate_target(rate_request+rate_correction, BODY_FRAME); - - // Calculate integrated body frame rate error - angle_error += (rate_request - (omega.x * DEGX100)) * G_Dt; - - // don't let angle error grow too large - angle_error = constrain_float(angle_error, -MAX_ROLL_OVERSHOOT, MAX_ROLL_OVERSHOOT); - -#if FRAME_CONFIG == HELI_FRAME - if (!motors.motor_runup_complete()) { - angle_error = 0; - } -#else - if (!motors.armed() || g.rc_3.servo_out == 0) { - angle_error = 0; - } -#endif // HELI_FRAME -} - -// Pitch with rate input and stabilized in the body frame -static void -get_pitch_rate_stabilized_bf(int32_t stick_angle) -{ - static float angle_error = 0; - - // convert the input to the desired body frame pitch rate - int32_t rate_request = stick_angle * g.acro_rp_p; - - if (g.acro_trainer == ACRO_TRAINER_LIMITED) { - rate_request += acro_pitch_rate; - }else{ - // Scale pitch leveling by stick input - acro_pitch_rate = (float)acro_pitch_rate*acro_level_mix; - - // Calculate rate limit to prevent change of rate through inverted - int32_t rate_limit = labs(labs(rate_request)-labs(acro_pitch_rate)); - - rate_request += acro_pitch_rate; - rate_request = constrain_int32(rate_request, -rate_limit, rate_limit); - } - - // add automatic correction - int32_t rate_correction = g.pi_stabilize_pitch.get_p(angle_error); - - // set body frame targets for rate controller - set_pitch_rate_target(rate_request+rate_correction, BODY_FRAME); - - // Calculate integrated body frame rate error - angle_error += (rate_request - (omega.y * DEGX100)) * G_Dt; - - // don't let angle error grow too large - angle_error = constrain_float(angle_error, -MAX_PITCH_OVERSHOOT, MAX_PITCH_OVERSHOOT); - -#if FRAME_CONFIG == HELI_FRAME - if (!motors.motor_runup_complete()) { - angle_error = 0; - } -#else - if (!motors.armed() || g.rc_3.servo_out == 0) { - angle_error = 0; - } -#endif // HELI_FRAME -} - -// Yaw with rate input and stabilized in the body frame -static void -get_yaw_rate_stabilized_bf(int32_t stick_angle) -{ - static float angle_error = 0; - - // convert the input to the desired body frame yaw rate - int32_t rate_request = stick_angle * g.acro_yaw_p; - - if (g.acro_trainer == ACRO_TRAINER_LIMITED) { - rate_request += acro_yaw_rate; - }else{ - // Scale pitch leveling by stick input - acro_yaw_rate = (float)acro_yaw_rate*acro_level_mix; - - // Calculate rate limit to prevent change of rate through inverted - int32_t rate_limit = labs(labs(rate_request)-labs(acro_yaw_rate)); - - rate_request += acro_yaw_rate; - rate_request = constrain_int32(rate_request, -rate_limit, rate_limit); - } - - // add automatic correction - int32_t rate_correction = g.pi_stabilize_yaw.get_p(angle_error); - - // set body frame targets for rate controller - set_yaw_rate_target(rate_request+rate_correction, BODY_FRAME); - - // Calculate integrated body frame rate error - angle_error += (rate_request - (omega.z * DEGX100)) * G_Dt; - - // don't let angle error grow too large - angle_error = constrain_float(angle_error, -MAX_YAW_OVERSHOOT, MAX_YAW_OVERSHOOT); - -#if FRAME_CONFIG == HELI_FRAME - if (!motors.motor_runup_complete()) { - angle_error = 0; - } -#else - if (!motors.armed() || g.rc_3.servo_out == 0) { - angle_error = 0; - } -#endif // HELI_FRAME -} - -// Roll with rate input and stabilized in the earth frame -static void -get_roll_rate_stabilized_ef(int32_t stick_angle) -{ - int32_t angle_error = 0; - - // convert the input to the desired roll rate - int32_t target_rate = stick_angle * g.acro_rp_p - (acro_roll * g.acro_balance_roll); - - // convert the input to the desired roll rate - acro_roll += target_rate * G_Dt; - acro_roll = wrap_180_cd(acro_roll); - - // ensure that we don't reach gimbal lock - if (labs(acro_roll) > aparm.angle_max) { - acro_roll = constrain_int32(acro_roll, -aparm.angle_max, aparm.angle_max); - angle_error = wrap_180_cd(acro_roll - ahrs.roll_sensor); - } else { - // angle error with maximum of +- max_angle_overshoot - angle_error = wrap_180_cd(acro_roll - ahrs.roll_sensor); - angle_error = constrain_int32(angle_error, -MAX_ROLL_OVERSHOOT, MAX_ROLL_OVERSHOOT); - } - -#if FRAME_CONFIG == HELI_FRAME - if (!motors.motor_runup_complete()) { - angle_error = 0; - } -#else - // reset target angle to current angle if motors not spinning - if (!motors.armed() || g.rc_3.servo_out == 0) { - angle_error = 0; - } -#endif // HELI_FRAME - - // update acro_roll to be within max_angle_overshoot of our current heading - acro_roll = wrap_180_cd(angle_error + ahrs.roll_sensor); - - // set earth frame targets for rate controller - set_roll_rate_target(g.pi_stabilize_roll.get_p(angle_error) + target_rate, EARTH_FRAME); -} - -// Pitch with rate input and stabilized in the earth frame -static void -get_pitch_rate_stabilized_ef(int32_t stick_angle) -{ - int32_t angle_error = 0; - - // convert the input to the desired pitch rate - int32_t target_rate = stick_angle * g.acro_rp_p - (acro_pitch * g.acro_balance_pitch); - - // convert the input to the desired pitch rate - acro_pitch += target_rate * G_Dt; - acro_pitch = wrap_180_cd(acro_pitch); - - // ensure that we don't reach gimbal lock - if (labs(acro_pitch) > aparm.angle_max) { - acro_pitch = constrain_int32(acro_pitch, -aparm.angle_max, aparm.angle_max); - angle_error = wrap_180_cd(acro_pitch - ahrs.pitch_sensor); - } else { - // angle error with maximum of +- max_angle_overshoot - angle_error = wrap_180_cd(acro_pitch - ahrs.pitch_sensor); - angle_error = constrain_int32(angle_error, -MAX_PITCH_OVERSHOOT, MAX_PITCH_OVERSHOOT); - } - -#if FRAME_CONFIG == HELI_FRAME - if (!motors.motor_runup_complete()) { - angle_error = 0; - } -#else - // reset target angle to current angle if motors not spinning - if (!motors.armed() || g.rc_3.servo_out == 0) { - angle_error = 0; - } -#endif // HELI_FRAME - - // update acro_pitch to be within max_angle_overshoot of our current heading - acro_pitch = wrap_180_cd(angle_error + ahrs.pitch_sensor); - - // set earth frame targets for rate controller - set_pitch_rate_target(g.pi_stabilize_pitch.get_p(angle_error) + target_rate, EARTH_FRAME); -} - -// Yaw with rate input and stabilized in the earth frame -static void -get_yaw_rate_stabilized_ef(int32_t stick_angle) -{ - - int32_t angle_error = 0; - - // convert the input to the desired yaw rate - int32_t target_rate = stick_angle * g.acro_yaw_p; - - // convert the input to the desired yaw rate - control_yaw += target_rate * G_Dt; - control_yaw = wrap_360_cd(control_yaw); - - // calculate difference between desired heading and current heading - angle_error = wrap_180_cd(control_yaw - ahrs.yaw_sensor); - - // limit the maximum overshoot - angle_error = constrain_int32(angle_error, -MAX_YAW_OVERSHOOT, MAX_YAW_OVERSHOOT); - -#if FRAME_CONFIG == HELI_FRAME - if (!motors.motor_runup_complete()) { - angle_error = 0; - } -#else - // reset target angle to current heading if motors not spinning - if (!motors.armed() || g.rc_3.servo_out == 0) { - angle_error = 0; - } -#endif // HELI_FRAME - - // update control_yaw to be within max_angle_overshoot of our current heading - control_yaw = wrap_360_cd(angle_error + ahrs.yaw_sensor); - - // set earth frame targets for rate controller - set_yaw_rate_target(g.pi_stabilize_yaw.get_p(angle_error)+target_rate, EARTH_FRAME); -} - -// set_roll_rate_target - to be called by upper controllers to set roll rate targets in the earth frame -void set_roll_rate_target( int32_t desired_rate, uint8_t earth_or_body_frame ) { - rate_targets_frame = earth_or_body_frame; - if( earth_or_body_frame == BODY_FRAME ) { - roll_rate_target_bf = desired_rate; - }else{ - roll_rate_target_ef = desired_rate; - } -} - -// set_pitch_rate_target - to be called by upper controllers to set pitch rate targets in the earth frame -void set_pitch_rate_target( int32_t desired_rate, uint8_t earth_or_body_frame ) { - rate_targets_frame = earth_or_body_frame; - if( earth_or_body_frame == BODY_FRAME ) { - pitch_rate_target_bf = desired_rate; - }else{ - pitch_rate_target_ef = desired_rate; - } -} - -// set_yaw_rate_target - to be called by upper controllers to set yaw rate targets in the earth frame -void set_yaw_rate_target( int32_t desired_rate, uint8_t earth_or_body_frame ) { - rate_targets_frame = earth_or_body_frame; - if( earth_or_body_frame == BODY_FRAME ) { - yaw_rate_target_bf = desired_rate; - }else{ - yaw_rate_target_ef = desired_rate; - } -} - - /************************************************************* * yaw controllers *************************************************************/ - // get_look_at_yaw - updates bearing to look at center of circle or do a panorama -// should be called at 100hz -static void get_circle_yaw() -{ - static uint8_t look_at_yaw_counter = 0; // used to reduce update rate to 10hz - - // if circle radius is zero do panorama - if( g.circle_radius == 0 ) { - // slew yaw towards circle angle - control_yaw = get_yaw_slew(control_yaw, ToDeg(circle_angle)*100, AUTO_YAW_SLEW_RATE); - }else{ - look_at_yaw_counter++; - if( look_at_yaw_counter >= 10 ) { - look_at_yaw_counter = 0; - yaw_look_at_WP_bearing = pv_get_bearing_cd(inertial_nav.get_position(), yaw_look_at_WP); - } - // slew yaw - control_yaw = get_yaw_slew(control_yaw, yaw_look_at_WP_bearing, AUTO_YAW_SLEW_RATE); - } - - // call stabilize yaw controller - get_stabilize_yaw(control_yaw); -} - // get_look_at_yaw - updates bearing to location held in look_at_yaw_WP and calls stabilize yaw controller // should be called at 100hz static float get_look_at_yaw() @@ -529,89 +101,32 @@ static float get_look_ahead_yaw() * throttle control ****************************************************************/ -// update_throttle_cruise - update throttle cruise if necessary -static void update_throttle_cruise(int16_t throttle) +// update_thr_cruise - update throttle cruise if necessary +// should be called at 100hz +static void update_thr_cruise() { // ensure throttle_avg has been initialised if( throttle_avg == 0 ) { throttle_avg = g.throttle_cruise; + // update position controller + pos_control.set_throttle_hover(throttle_avg); } + + // if not armed or landed exit + if (!motors.armed() || ap.land_complete) { + return; + } + + // get throttle output + int16_t throttle = g.rc_3.servo_out; + // calc average throttle if we are in a level hover if (throttle > g.throttle_min && abs(climb_rate) < 60 && labs(ahrs.roll_sensor) < 500 && labs(ahrs.pitch_sensor) < 500) { throttle_avg = throttle_avg * 0.99f + (float)throttle * 0.01f; g.throttle_cruise = throttle_avg; - } // update position controller pos_control.set_throttle_hover(throttle_avg); -} - -#if FRAME_CONFIG == HELI_FRAME -// get_angle_boost - returns a throttle including compensation for roll/pitch angle -// throttle value should be 0 ~ 1000 -// for traditional helicopters -static int16_t get_angle_boost(int16_t throttle) -{ - float angle_boost_factor = ahrs.cos_pitch() * ahrs.cos_roll(); - angle_boost_factor = 1.0f - constrain_float(angle_boost_factor, .5f, 1.0f); - int16_t throttle_above_mid = max(throttle - motors.get_collective_mid(),0); - - // to allow logging of angle boost - angle_boost = throttle_above_mid*angle_boost_factor; - - return throttle + angle_boost; -} -#else // all multicopters -// get_angle_boost - returns a throttle including compensation for roll/pitch angle -// throttle value should be 0 ~ 1000 -static int16_t get_angle_boost(int16_t throttle) -{ - float temp = ahrs.cos_pitch() * ahrs.cos_roll(); - int16_t throttle_out; - - temp = constrain_float(temp, 0.5f, 1.0f); - - // reduce throttle if we go inverted - temp = constrain_float(9000-max(labs(ahrs.roll_sensor),labs(ahrs.pitch_sensor)), 0, 3000) / (3000 * temp); - - // apply scale and constrain throttle - throttle_out = constrain_float((float)(throttle-g.throttle_min) * temp + g.throttle_min, g.throttle_min, 1000); - - // to allow logging of angle boost - angle_boost = throttle_out - throttle; - - return throttle_out; -} -#endif // FRAME_CONFIG == HELI_FRAME - - // set_throttle_out - to be called by upper throttle controllers when they wish to provide throttle output directly to motors - // provide 0 to cut motors -void set_throttle_out( int16_t throttle_out, bool apply_angle_boost ) -{ - if( apply_angle_boost ) { - g.rc_3.servo_out = get_angle_boost(throttle_out); - }else{ - g.rc_3.servo_out = throttle_out; - // clear angle_boost for logging purposes - angle_boost = 0; } - - // update compass with throttle value - compass.set_throttle((float)g.rc_3.servo_out/1000.0f); -} - -// 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; -} - -// disable_throttle_accel - disables the accel based throttle controller -// it will be re-enasbled on the next set_throttle_accel_target -// required when we wish to set motors to zero when pilot inputs zero throttle -void throttle_accel_deactivate() -{ - throttle_accel_controller_active = false; } // set_throttle_takeoff - allows parents to tell throttle controller we are taking off so I terms can be cleared @@ -692,158 +207,6 @@ static int16_t get_pilot_desired_climb_rate(int16_t throttle_control) return desired_rate; } -// get_initial_alt_hold - get new target altitude based on current altitude and climb rate -static int32_t -get_initial_alt_hold( int32_t alt_cm, int16_t climb_rate_cms) -{ - int32_t target_alt; - int32_t linear_distance; // half the distace we swap between linear and sqrt and the distace we offset sqrt. - int32_t linear_velocity; // the velocity we swap between linear and sqrt. - - linear_velocity = ALT_HOLD_ACCEL_MAX/g.pi_alt_hold.kP(); - - if (abs(climb_rate_cms) < linear_velocity) { - target_alt = alt_cm + climb_rate_cms/g.pi_alt_hold.kP(); - } else { - linear_distance = ALT_HOLD_ACCEL_MAX/(2*g.pi_alt_hold.kP()*g.pi_alt_hold.kP()); - if (climb_rate_cms > 0){ - target_alt = alt_cm + linear_distance + (int32_t)climb_rate_cms*(int32_t)climb_rate_cms/(2*ALT_HOLD_ACCEL_MAX); - } else { - target_alt = alt_cm - ( linear_distance + (int32_t)climb_rate_cms*(int32_t)climb_rate_cms/(2*ALT_HOLD_ACCEL_MAX) ); - } - } - return constrain_int32(target_alt, alt_cm - ALT_HOLD_INIT_MAX_OVERSHOOT, alt_cm + ALT_HOLD_INIT_MAX_OVERSHOOT); -} - -// get_throttle_rate - calculates desired accel required to achieve desired z_target_speed -// sets accel based throttle controller target -static void -get_throttle_rate(float z_target_speed) -{ - static uint32_t last_call_ms = 0; - static float z_rate_error = 0; // The velocity error in cm. - static float z_target_speed_filt = 0; // The filtered requested speed - float z_target_speed_delta; // The change in requested speed - int32_t p; // used to capture pid values for logging - int32_t output; // the target acceleration if the accel based throttle is enabled, otherwise the output to be sent to the motors - uint32_t now = millis(); - - // reset target altitude if this controller has just been engaged - if( now - last_call_ms > 100 ) { - // Reset Filter - z_rate_error = 0; - z_target_speed_filt = z_target_speed; - output = 0; - } else { - // calculate rate error and filter with cut off frequency of 2 Hz - z_rate_error = z_rate_error + 0.20085f * ((z_target_speed - climb_rate) - z_rate_error); - // feed forward acceleration based on change in the filtered desired speed. - z_target_speed_delta = 0.20085f * (z_target_speed - z_target_speed_filt); - z_target_speed_filt = z_target_speed_filt + z_target_speed_delta; - output = z_target_speed_delta * 50.0f; // To-Do: replace 50 with dt - } - last_call_ms = now; - - // calculate p - p = g.pid_throttle_rate.kP() * z_rate_error; - - // consolidate and constrain target acceleration - output += p; - output = constrain_int32(output, -32000, 32000); - - // set target for accel based throttle controller - set_throttle_accel_target(output); - - // update throttle cruise - // TO-DO: this may not be correct because g.rc_3.servo_out has not been updated for this iteration - if( z_target_speed == 0 ) { - update_throttle_cruise(g.rc_3.servo_out); - } -} - -// 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 min_climb_rate, int16_t max_climb_rate) -{ - int32_t alt_error; - float desired_rate; - int32_t linear_distance; // half the distace we swap between linear and sqrt and the distace we offset sqrt. - - // calculate altitude error - alt_error = target_alt - current_loc.alt; - - // check kP to avoid division by zero - if( g.pi_alt_hold.kP() != 0 ) { - linear_distance = ALT_HOLD_ACCEL_MAX/(2*g.pi_alt_hold.kP()*g.pi_alt_hold.kP()); - if( alt_error > 2*linear_distance ) { - desired_rate = safe_sqrt(2*ALT_HOLD_ACCEL_MAX*(alt_error-linear_distance)); - }else if( alt_error < -2*linear_distance ) { - desired_rate = -safe_sqrt(2*ALT_HOLD_ACCEL_MAX*(-alt_error-linear_distance)); - }else{ - desired_rate = g.pi_alt_hold.get_p(alt_error); - } - }else{ - desired_rate = 0; - } - - desired_rate = constrain_float(desired_rate, 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 -} - -// get_throttle_althold_with_slew - altitude controller with slew to avoid step changes in altitude target -// calls normal althold controller which updates accel based throttle controller targets -static void -get_throttle_althold_with_slew(int32_t target_alt, int16_t min_climb_rate, int16_t max_climb_rate) -{ - float alt_change = target_alt-controller_desired_alt; - // adjust desired alt if motors have not hit their limits - if ((alt_change<0 && !motors.limit.throttle_lower) || (alt_change>0 && !motors.limit.throttle_upper)) { - controller_desired_alt += constrain_float(alt_change, min_climb_rate*0.02f, max_climb_rate*0.02f); - } - - // do not let target altitude get too far from current altitude - controller_desired_alt = constrain_float(controller_desired_alt,current_loc.alt-750,current_loc.alt+750); - - get_throttle_althold(controller_desired_alt, min_climb_rate-250, max_climb_rate+250); // 250 is added to give head room to alt hold controller -} - -// 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) -{ - // adjust desired alt if motors have not hit their limits - if ((target_rate<0 && !motors.limit.throttle_lower) || (target_rate>0 && !motors.limit.throttle_upper)) { - controller_desired_alt += target_rate * 0.02f; - } - - // do not let target altitude get too far from current altitude - controller_desired_alt = constrain_float(controller_desired_alt,current_loc.alt-750,current_loc.alt+750); - -#if AC_FENCE == ENABLED - // do not let target altitude be too close to the fence - // To-Do: add this to other altitude controllers - if((fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) { - float alt_limit = fence.get_safe_alt() * 100.0f; - if (controller_desired_alt > alt_limit) { - controller_desired_alt = alt_limit; - } - } -#endif - - // update target altitude for reporting purposes - set_target_alt_for_reporting(controller_desired_alt); - - get_throttle_althold(controller_desired_alt, -g.pilot_velocity_z_max-250, g.pilot_velocity_z_max+250); // 250 is added to give head room to alt hold controller -} - // get_throttle_surface_tracking - hold copter at the desired distance above the ground // returns climb rate (in cm/s) which should be passed to the position controller static float get_throttle_surface_tracking(int16_t target_rate, float dt)