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 <= THROTTLE_MANUAL_TILT_COMPENSATED ) { // 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; }
// 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 if (control_mode == STABILIZE){ motors.stab_throttle = true; } else { motors.stab_throttle = false; }
// allow swash collective to move if we are in manual throttle modes, even if disarmed if( !motors.armed() ) {
if ( !(throttle_mode == THROTTLE_MANUAL) && !(throttle_mode == THROTTLE_MANUAL_TILT_COMPENSATED)){ throttle_accel_deactivate(); // do not allow the accel based throttle to override our command return; } }
#else // 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 // 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.coll_out); #else update_throttle_cruise(pilot_throttle_scaled); #endif //HELI_FRAME
// check if we've taken off yet
if (!ap.takeoff_complete && motors.armed()) {
if (pilot_throttle_scaled > g.throttle_cruise) { // we must be in the air by now set_takeoff_complete(true); } } }
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.coll_out); #else update_throttle_cruise(pilot_throttle_scaled); #endif //HELI_FRAME
if (!ap.takeoff_complete && motors.armed()) {
if (pilot_throttle_scaled > g.throttle_cruise) { // we must be in the air by now set_takeoff_complete(true);
} } }
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); // 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; } }
// 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; }
// get_target_alt_for_reporting - returns target altitude in cm for reporting purposes (logs and gcs) static float get_target_alt_for_reporting() {
return target_alt_for_reporting; }
static void read_AHRS(void) {
// Perform IMU calculations and get attitude info //----------------------------------------------- #if HIL_MODE != HIL_MODE_DISABLED // update hil before ahrs update gcs_check_input(); #endif
ahrs.update();
omega = ins.get_gyro(); }
static void update_trig(void){ Vector2f yawvector;
const Matrix3f &temp = ahrs.get_dcm_matrix();
yawvector.x = temp.a.x; // sin yawvector.y = temp.b.x; // cos yawvector.normalize();
cos_pitch_x = safe_sqrt(1 - (temp.c.x * temp.c.x)); // level = 1
cos_roll_x = temp.c.z / cos_pitch_x; // level = 1
cos_pitch_x = constrain_float(cos_pitch_x, 0, 1.0);
// this relies on constrain_float() of infinity doing the right thing, // which it does do in avr-libc
cos_roll_x = constrain_float(cos_roll_x, -1.0, 1.0);
sin_yaw = constrain_float(yawvector.y, -1.0, 1.0); cos_yaw = constrain_float(yawvector.x, -1.0, 1.0);
// added to convert earth frame to body frame for rate controllers sin_pitch = -temp.c.x;
sin_roll = temp.c.y / cos_pitch_x;
// update wp_nav controller with trig values
wp_nav.set_cos_sin_yaw(cos_yaw, sin_yaw, cos_pitch_x);
//flat:
// 0 ° = cos_yaw: 1.00, sin_yaw: 0.00, // 90° = cos_yaw: 0.00, sin_yaw: 1.00, // 180 = cos_yaw: -1.00, sin_yaw: 0.00, // 270 = cos_yaw: 0.00, sin_yaw: -1.00, }
// read baro and sonar altitude at 20hz static void update_altitude() {
#if HIL_MODE == HIL_MODE_ATTITUDE
// we are in the SIM, fake out the baro and Sonar baro_alt = g_gps->altitude_cm;
if(g.sonar_enabled) {
sonar_alt = baro_alt; } #else
// read in baro altitude
baro_alt = read_barometer();
// read in sonar altitude
sonar_alt = read_sonar(); #endif // HIL_MODE == HIL_MODE_ATTITUDE
// write altitude info to dataflash logs
if ((g.log_bitmask & MASK_LOG_CTUN) && motors.armed()) { Log_Write_Control_Tuning(); } }
static void tuning(){
// exit immediately when radio failsafe is invoked so tuning values are not set to zero if (failsafe.radio || failsafe.radio_counter != 0) { return; }
tuning_value = (float)g.rc_6.control_in / 1000.0f;
g.rc_6.set_range(g.radio_tuning_low,g.radio_tuning_high); // 0 to 1
switch(g.radio_tuning) {
// Roll, Pitch tuning
case CH6_STABILIZE_ROLL_PITCH_KP:
g.pi_stabilize_roll.kP(tuning_value); g.pi_stabilize_pitch.kP(tuning_value); break;
case CH6_RATE_ROLL_PITCH_KP:
g.pid_rate_roll.kP(tuning_value); g.pid_rate_pitch.kP(tuning_value); break;
case CH6_RATE_ROLL_PITCH_KI:
g.pid_rate_roll.kI(tuning_value); g.pid_rate_pitch.kI(tuning_value); break;