} } }
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;
case CH6_RATE_ROLL_PITCH_KD:
g.pid_rate_roll.kD(tuning_value); g.pid_rate_pitch.kD(tuning_value); break;
// Yaw tuning
case CH6_STABILIZE_YAW_KP:
g.pi_stabilize_yaw.kP(tuning_value); break;
case CH6_YAW_RATE_KP:
g.pid_rate_yaw.kP(tuning_value); break;
case CH6_YAW_RATE_KD:
g.pid_rate_yaw.kD(tuning_value); break;
// Altitude and throttle tuning case CH6_ALTITUDE_HOLD_KP:
g.pi_alt_hold.kP(tuning_value); break;
case CH6_THROTTLE_RATE_KP:
g.pid_throttle_rate.kP(tuning_value); break;
case CH6_THROTTLE_RATE_KD:
g.pid_throttle_rate.kD(tuning_value); break;
case CH6_THROTTLE_ACCEL_KP:
g.pid_throttle_accel.kP(tuning_value); break;
case CH6_THROTTLE_ACCEL_KI:
g.pid_throttle_accel.kI(tuning_value); break;
case CH6_THROTTLE_ACCEL_KD:
g.pid_throttle_accel.kD(tuning_value); break;
// Loiter and navigation tuning case CH6_LOITER_POSITION_KP:
g.pi_loiter_lat.kP(tuning_value); g.pi_loiter_lon.kP(tuning_value); break;
case CH6_LOITER_RATE_KP:
g.pid_loiter_rate_lon.kP(tuning_value); g.pid_loiter_rate_lat.kP(tuning_value); break;
case CH6_LOITER_RATE_KI:
g.pid_loiter_rate_lon.kI(tuning_value); g.pid_loiter_rate_lat.kI(tuning_value); break;
case CH6_LOITER_RATE_KD:
g.pid_loiter_rate_lon.kD(tuning_value); g.pid_loiter_rate_lat.kD(tuning_value);