/* recalculate the flight_stage */ void Plane::update_flight_stage(void) { // Update the speed & height controller states if (auto_throttle_mode && !throttle_suppressed) { if (control_mode==AUTO) { if (auto_state.takeoff_complete == false) { set_flight_stage(AP_SpdHgtControl::FLIGHT_TAKEOFF); } else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND && auto_state.land_complete == true) { set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_FINAL); } else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) { set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_APPROACH); } else { set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL); } } else { set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL); } SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(), target_airspeed_cm, flight_stage, auto_state.takeoff_pitch_cd, throttle_nudge, tecs_hgt_afe(), aerodynamic_load_factor); if (should_log(MASK_LOG_TECS)) { Log_Write_TECS_Tuning(); } } // tell AHRS the airspeed to true airspeed ratio airspeed.set_EAS2TAS(barometer.get_EAS2TAS()); }
/* recalculate the flight_stage */ void Plane::update_flight_stage(void) { // Update the speed & height controller states if (auto_throttle_mode && !throttle_suppressed) { if (control_mode==AUTO) { if (quadplane.in_vtol_auto()) { set_flight_stage(AP_SpdHgtControl::FLIGHT_VTOL); } else if (auto_state.takeoff_complete == false) { set_flight_stage(AP_SpdHgtControl::FLIGHT_TAKEOFF); } else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) { if ((g.land_abort_throttle_enable && channel_throttle->control_in > 95) || auto_state.commanded_go_around || flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT){ // abort mode is sticky, it must complete while executing NAV_LAND set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_ABORT); } else if (auto_state.land_complete == true) { set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_FINAL); } else if (flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) { float path_progress = location_path_proportion(current_loc, prev_WP_loc, next_WP_loc); bool lined_up = abs(nav_controller->bearing_error_cd()) < 1000; bool below_prev_WP = current_loc.alt < prev_WP_loc.alt; if ((path_progress > 0.15f && lined_up && below_prev_WP) || path_progress > 0.5f) { set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_APPROACH); } else { set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL); } } } else if (quadplane.in_assisted_flight()) { set_flight_stage(AP_SpdHgtControl::FLIGHT_VTOL); } else { set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL); } } else { set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL); } SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(), target_airspeed_cm, flight_stage, auto_state.takeoff_pitch_cd, throttle_nudge, tecs_hgt_afe(), aerodynamic_load_factor); if (should_log(MASK_LOG_TECS)) { Log_Write_TECS_Tuning(); } } else if (control_mode == QSTABILIZE || control_mode == QHOVER || control_mode == QLOITER || quadplane.in_assisted_flight()) { set_flight_stage(AP_SpdHgtControl::FLIGHT_VTOL); } else { set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL); } // tell AHRS the airspeed to true airspeed ratio airspeed.set_EAS2TAS(barometer.get_EAS2TAS()); }
void Plane::update_alt() { barometer.update(); if (should_log(MASK_LOG_IMU)) { Log_Write_Baro(); } // calculate the sink rate. float sink_rate; Vector3f vel; if (ahrs.get_velocity_NED(vel)) { sink_rate = vel.z; } else if (gps.status() >= AP_GPS::GPS_OK_FIX_3D && gps.have_vertical_velocity()) { sink_rate = gps.velocity().z; } else { sink_rate = -barometer.get_climb_rate(); } // low pass the sink rate to take some of the noise out auto_state.sink_rate = 0.8f * auto_state.sink_rate + 0.2f*sink_rate; geofence_check(true); update_flight_stage(); if (auto_throttle_mode && !throttle_suppressed) { bool is_doing_auto_land = false; float distance_beyond_land_wp = 0; switch (flight_stage) { case AP_SpdHgtControl::FLIGHT_LAND_APPROACH: case AP_SpdHgtControl::FLIGHT_LAND_PREFLARE: case AP_SpdHgtControl::FLIGHT_LAND_FINAL: is_doing_auto_land = true; if (location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) { distance_beyond_land_wp = get_distance(current_loc, next_WP_loc); } break; default: break; } SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(), target_airspeed_cm, flight_stage, is_doing_auto_land, distance_beyond_land_wp, get_takeoff_pitch_min_cd(), throttle_nudge, tecs_hgt_afe(), aerodynamic_load_factor); if (should_log(MASK_LOG_TECS)) { Log_Write_TECS_Tuning(); } } }
void Plane::update_alt() { barometer.update(); // calculate the sink rate. float sink_rate; Vector3f vel; if (ahrs.get_velocity_NED(vel)) { sink_rate = vel.z; } else if (gps.status() >= AP_GPS::GPS_OK_FIX_3D && gps.have_vertical_velocity()) { sink_rate = gps.velocity().z; } else { sink_rate = -barometer.get_climb_rate(); } // low pass the sink rate to take some of the noise out auto_state.sink_rate = 0.8f * auto_state.sink_rate + 0.2f*sink_rate; geofence_check(true); update_flight_stage(); if (auto_throttle_mode && !throttle_suppressed) { float distance_beyond_land_wp = 0; if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND && location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) { distance_beyond_land_wp = get_distance(current_loc, next_WP_loc); } bool soaring_active = false; #if SOARING_ENABLED == ENABLED if (g2.soaring_controller.is_active() && g2.soaring_controller.get_throttle_suppressed()) { soaring_active = true; } #endif SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(), target_airspeed_cm, flight_stage, distance_beyond_land_wp, get_takeoff_pitch_min_cd(), throttle_nudge, tecs_hgt_afe(), aerodynamic_load_factor, soaring_active); } }
void Plane::update_alt() { barometer.update(); if (should_log(MASK_LOG_IMU)) { Log_Write_Baro(); } // calculate the sink rate. float sink_rate; Vector3f vel; if (ahrs.get_velocity_NED(vel)) { sink_rate = vel.z; } else if (gps.status() >= AP_GPS::GPS_OK_FIX_3D && gps.have_vertical_velocity()) { sink_rate = gps.velocity().z; } else { sink_rate = -barometer.get_climb_rate(); } // low pass the sink rate to take some of the noise out auto_state.sink_rate = 0.8f * auto_state.sink_rate + 0.2f*sink_rate; geofence_check(true); update_flight_stage(); if (auto_throttle_mode && !throttle_suppressed) { float distance_beyond_land_wp = 0; if (auto_state.land_in_progress && location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) { distance_beyond_land_wp = get_distance(current_loc, next_WP_loc); } SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(), target_airspeed_cm, flight_stage, auto_state.land_in_progress, distance_beyond_land_wp, get_takeoff_pitch_min_cd(), throttle_nudge, tecs_hgt_afe(), aerodynamic_load_factor); } }