/* main handling for AUTO mode */ void Plane::handle_auto_mode(void) { uint16_t nav_cmd_id; if (mission.state() != AP_Mission::MISSION_RUNNING) { // this should never be reached set_mode(RTL); GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Aircraft in auto without a running mission"); return; } nav_cmd_id = mission.get_current_nav_cmd().id; if (quadplane.in_vtol_auto()) { quadplane.control_auto(next_WP_loc); } else if (nav_cmd_id == MAV_CMD_NAV_TAKEOFF || (nav_cmd_id == MAV_CMD_NAV_LAND && flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT)) { takeoff_calc_roll(); takeoff_calc_pitch(); calc_throttle(); } else if (nav_cmd_id == MAV_CMD_NAV_LAND) { calc_nav_roll(); calc_nav_pitch(); if (auto_state.land_complete) { // during final approach constrain roll to the range // allowed for level flight nav_roll_cd = constrain_int32(nav_roll_cd, -g.level_roll_limit*100UL, g.level_roll_limit*100UL); } calc_throttle(); if (auto_state.land_complete) { // we are in the final stage of a landing - force // zero throttle channel_throttle->set_servo_out(0); } } else { // we are doing normal AUTO flight, the special cases // are for takeoff and landing if (nav_cmd_id != MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT) { steer_state.hold_course_cd = -1; } auto_state.land_complete = false; auto_state.land_pre_flare = false; calc_nav_roll(); calc_nav_pitch(); calc_throttle(); } }
/* main handling for AUTO mode */ void Plane::handle_auto_mode(void) { uint8_t nav_cmd_id; // we should be either running a mission or RTLing home if (mission.state() == AP_Mission::MISSION_RUNNING) { nav_cmd_id = mission.get_current_nav_cmd().id; }else{ nav_cmd_id = auto_rtl_command.id; } switch(nav_cmd_id) { case MAV_CMD_NAV_TAKEOFF: takeoff_calc_roll(); takeoff_calc_pitch(); calc_throttle(); break; case MAV_CMD_NAV_LAND: calc_nav_roll(); calc_nav_pitch(); if (auto_state.land_complete) { // during final approach constrain roll to the range // allowed for level flight nav_roll_cd = constrain_int32(nav_roll_cd, -g.level_roll_limit*100UL, g.level_roll_limit*100UL); } calc_throttle(); if (auto_state.land_complete) { // we are in the final stage of a landing - force // zero throttle channel_throttle->servo_out = 0; } break; default: // we are doing normal AUTO flight, the special cases // are for takeoff and landing steer_state.hold_course_cd = -1; auto_state.land_complete = false; calc_nav_roll(); calc_nav_pitch(); calc_throttle(); break; } }
/* handle speed and height control in FBWB or CRUISE mode. In this mode the elevator is used to change target altitude. The throttle is used to change target airspeed or throttle */ void Plane::update_fbwb_speed_height(void) { static float last_elevator_input; float elevator_input; elevator_input = channel_pitch->control_in / 4500.0f; if (g.flybywire_elev_reverse) { elevator_input = -elevator_input; } change_target_altitude(g.flybywire_climb_rate * elevator_input * delta_us_fast_loop * 0.0001f); if (is_zero(elevator_input) && !is_zero(last_elevator_input)) { // the user has just released the elevator, lock in // the current altitude set_target_altitude_current(); } // check for FBWB altitude limit check_minimum_altitude(); altitude_error_cm = calc_altitude_error_cm(); last_elevator_input = elevator_input; calc_throttle(); calc_nav_pitch(); }
/* main handling for AUTO mode */ void Plane::handle_auto_mode(void) { uint16_t nav_cmd_id; if (mission.state() != AP_Mission::MISSION_RUNNING) { // this could happen if AP_Landing::restart_landing_sequence() returns false which would only happen if: // restart_landing_sequence() is called when not executing a NAV_LAND or there is no previous nav point set_mode(RTL, MODE_REASON_MISSION_END); GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Aircraft in auto without a running mission"); return; } nav_cmd_id = mission.get_current_nav_cmd().id; if (quadplane.in_vtol_auto()) { quadplane.control_auto(next_WP_loc); } else if (nav_cmd_id == MAV_CMD_NAV_TAKEOFF || (nav_cmd_id == MAV_CMD_NAV_LAND && flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT)) { takeoff_calc_roll(); takeoff_calc_pitch(); calc_throttle(); } else if (nav_cmd_id == MAV_CMD_NAV_LAND) { calc_nav_roll(); calc_nav_pitch(); if (landing.is_complete()) { // during final approach constrain roll to the range // allowed for level flight nav_roll_cd = constrain_int32(nav_roll_cd, -g.level_roll_limit*100UL, g.level_roll_limit*100UL); // we are in the final stage of a landing - force // zero throttle channel_throttle->set_servo_out(0); } else { calc_throttle(); } } else { // we are doing normal AUTO flight, the special cases // are for takeoff and landing if (nav_cmd_id != MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT) { steer_state.hold_course_cd = -1; } calc_nav_roll(); calc_nav_pitch(); calc_throttle(); } }
/* main handling for AUTO mode */ void Plane::handle_auto_mode(void) { uint16_t nav_cmd_id; // we should be either running a mission or RTLing home if (mission.state() == AP_Mission::MISSION_RUNNING) { nav_cmd_id = mission.get_current_nav_cmd().id; }else{ nav_cmd_id = auto_rtl_command.id; } if (quadplane.in_vtol_auto()) { quadplane.control_auto(next_WP_loc); } else if (nav_cmd_id == MAV_CMD_NAV_TAKEOFF || (nav_cmd_id == MAV_CMD_NAV_LAND && flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT)) { takeoff_calc_roll(); takeoff_calc_pitch(); calc_throttle(); } else if (nav_cmd_id == MAV_CMD_NAV_LAND) { calc_nav_roll(); calc_nav_pitch(); if (auto_state.land_complete) { // during final approach constrain roll to the range // allowed for level flight nav_roll_cd = constrain_int32(nav_roll_cd, -g.level_roll_limit*100UL, g.level_roll_limit*100UL); } calc_throttle(); if (auto_state.land_complete) { // we are in the final stage of a landing - force // zero throttle channel_throttle->servo_out = 0; } } else { // we are doing normal AUTO flight, the special cases // are for takeoff and landing if (nav_cmd_id != MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT) { steer_state.hold_course_cd = -1; } auto_state.land_complete = false; auto_state.land_pre_flare = false; calc_nav_roll(); calc_nav_pitch(); calc_throttle(); } }
/* main handling for AUTO mode */ void Plane::handle_auto_mode(void) { uint16_t nav_cmd_id; if (mission.state() != AP_Mission::MISSION_RUNNING) { // this could happen if AP_Landing::restart_landing_sequence() returns false which would only happen if: // restart_landing_sequence() is called when not executing a NAV_LAND or there is no previous nav point set_mode(RTL, MODE_REASON_MISSION_END); gcs().send_text(MAV_SEVERITY_INFO, "Aircraft in auto without a running mission"); return; } nav_cmd_id = mission.get_current_nav_cmd().id; if (quadplane.in_vtol_auto()) { quadplane.control_auto(next_WP_loc); } else if (nav_cmd_id == MAV_CMD_NAV_TAKEOFF || (nav_cmd_id == MAV_CMD_NAV_LAND && flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND)) { takeoff_calc_roll(); takeoff_calc_pitch(); calc_throttle(); } else if (nav_cmd_id == MAV_CMD_NAV_LAND) { calc_nav_roll(); calc_nav_pitch(); // allow landing to restrict the roll limits nav_roll_cd = landing.constrain_roll(nav_roll_cd, g.level_roll_limit*100UL); if (landing.is_throttle_suppressed()) { // if landing is considered complete throttle is never allowed, regardless of landing type SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0); } else { calc_throttle(); } } else { // we are doing normal AUTO flight, the special cases // are for takeoff and landing if (nav_cmd_id != MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT) { steer_state.hold_course_cd = -1; } calc_nav_roll(); calc_nav_pitch(); calc_throttle(); } }
/* calculate desired pitch angle during takeoff, setting nav_pitch_cd */ void Plane::takeoff_calc_pitch(void) { if (auto_state.highest_airspeed < g.takeoff_rotate_speed) { // we have not reached rotate speed, use a target pitch of 5 // degrees. This should be enough to get the tail off the // ground, while making it unlikely that overshoot in the // pitch controller will cause a prop strike nav_pitch_cd = 500; return; } if (ahrs.airspeed_sensor_enabled()) { calc_nav_pitch(); if (nav_pitch_cd < auto_state.takeoff_pitch_cd) { nav_pitch_cd = auto_state.takeoff_pitch_cd; } } else { nav_pitch_cd = ((gps.ground_speed()*100) / (float)g.airspeed_cruise_cm) * auto_state.takeoff_pitch_cd; nav_pitch_cd = constrain_int32(nav_pitch_cd, 500, auto_state.takeoff_pitch_cd); } }
/* handle speed and height control in FBWB or CRUISE mode. In this mode the elevator is used to change target altitude. The throttle is used to change target airspeed or throttle */ void Plane::update_fbwb_speed_height(void) { uint32_t now = micros(); if (now - target_altitude.last_elev_check_us >= 100000) { // we don't run this on every loop as it would give too small granularity on quadplanes at 300Hz, and // give below 1cm altitude change, which would result in no climb or descent float dt = (now - target_altitude.last_elev_check_us) * 1.0e-6; dt = constrain_float(dt, 0.1, 0.15); target_altitude.last_elev_check_us = now; float elevator_input = channel_pitch->get_control_in() / 4500.0f; if (g.flybywire_elev_reverse) { elevator_input = -elevator_input; } int32_t alt_change_cm = g.flybywire_climb_rate * elevator_input * dt * 100; change_target_altitude(alt_change_cm); if (is_zero(elevator_input) && !is_zero(target_altitude.last_elevator_input)) { // the user has just released the elevator, lock in // the current altitude set_target_altitude_current(); } target_altitude.last_elevator_input = elevator_input; } check_fbwb_minimum_altitude(); altitude_error_cm = calc_altitude_error_cm(); calc_throttle(); calc_nav_pitch(); }
/* main flight mode dependent update code */ void Plane::update_flight_mode(void) { enum FlightMode effective_mode = control_mode; if (control_mode == AUTO && g.auto_fbw_steer == 42) { effective_mode = FLY_BY_WIRE_A; } if (effective_mode != AUTO) { // hold_course is only used in takeoff and landing steer_state.hold_course_cd = -1; } // ensure we are fly-forward if (quadplane.in_vtol_mode()) { ahrs.set_fly_forward(false); } else { ahrs.set_fly_forward(true); } switch (effective_mode) { case AUTO: handle_auto_mode(); break; case GUIDED: if (auto_state.vtol_loiter && quadplane.available()) { quadplane.guided_update(); break; } // fall through case RTL: case LOITER: calc_nav_roll(); calc_nav_pitch(); calc_throttle(); break; case TRAINING: { training_manual_roll = false; training_manual_pitch = false; update_load_factor(); // if the roll is past the set roll limit, then // we set target roll to the limit if (ahrs.roll_sensor >= roll_limit_cd) { nav_roll_cd = roll_limit_cd; } else if (ahrs.roll_sensor <= -roll_limit_cd) { nav_roll_cd = -roll_limit_cd; } else { training_manual_roll = true; nav_roll_cd = 0; } // if the pitch is past the set pitch limits, then // we set target pitch to the limit if (ahrs.pitch_sensor >= aparm.pitch_limit_max_cd) { nav_pitch_cd = aparm.pitch_limit_max_cd; } else if (ahrs.pitch_sensor <= pitch_limit_min_cd) { nav_pitch_cd = pitch_limit_min_cd; } else { training_manual_pitch = true; nav_pitch_cd = 0; } if (fly_inverted()) { nav_pitch_cd = -nav_pitch_cd; } break; } case ACRO: { // handle locked/unlocked control if (acro_state.locked_roll) { nav_roll_cd = acro_state.locked_roll_err; } else { nav_roll_cd = ahrs.roll_sensor; } if (acro_state.locked_pitch) { nav_pitch_cd = acro_state.locked_pitch_cd; } else { nav_pitch_cd = ahrs.pitch_sensor; } break; } case AUTOTUNE: case FLY_BY_WIRE_A: { // set nav_roll and nav_pitch using sticks nav_roll_cd = channel_roll->norm_input() * roll_limit_cd; nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd); update_load_factor(); float pitch_input = channel_pitch->norm_input(); if (pitch_input > 0) { nav_pitch_cd = pitch_input * aparm.pitch_limit_max_cd; } else { nav_pitch_cd = -(pitch_input * pitch_limit_min_cd); } adjust_nav_pitch_throttle(); nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get()); if (fly_inverted()) { nav_pitch_cd = -nav_pitch_cd; } if (failsafe.ch3_failsafe && g.short_fs_action == 2) { // FBWA failsafe glide nav_roll_cd = 0; nav_pitch_cd = 0; channel_throttle->set_servo_out(0); } if (g.fbwa_tdrag_chan > 0) { // check for the user enabling FBWA taildrag takeoff mode bool tdrag_mode = (hal.rcin->read(g.fbwa_tdrag_chan-1) > 1700); if (tdrag_mode && !auto_state.fbwa_tdrag_takeoff_mode) { if (auto_state.highest_airspeed < g.takeoff_tdrag_speed1) { auto_state.fbwa_tdrag_takeoff_mode = true; gcs_send_text(MAV_SEVERITY_WARNING, "FBWA tdrag mode"); } } } break; } case FLY_BY_WIRE_B: // Thanks to Yury MonZon for the altitude limit code! nav_roll_cd = channel_roll->norm_input() * roll_limit_cd; nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd); update_load_factor(); update_fbwb_speed_height(); break; case CRUISE: /* in CRUISE mode we use the navigation code to control roll when heading is locked. Heading becomes unlocked on any aileron or rudder input */ if ((channel_roll->get_control_in() != 0 || rudder_input != 0)) { cruise_state.locked_heading = false; cruise_state.lock_timer_ms = 0; } if (!cruise_state.locked_heading) { nav_roll_cd = channel_roll->norm_input() * roll_limit_cd; nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd); update_load_factor(); } else { calc_nav_roll(); } update_fbwb_speed_height(); break; case STABILIZE: nav_roll_cd = 0; nav_pitch_cd = 0; // throttle is passthrough break; case CIRCLE: // we have no GPS installed and have lost radio contact // or we just want to fly around in a gentle circle w/o GPS, // holding altitude at the altitude we set when we // switched into the mode nav_roll_cd = roll_limit_cd / 3; update_load_factor(); calc_nav_pitch(); calc_throttle(); break; case MANUAL: // servo_out is for Sim control only // --------------------------------- channel_roll->set_servo_out(channel_roll->pwm_to_angle()); channel_pitch->set_servo_out(channel_pitch->pwm_to_angle()); steering_control.steering = steering_control.rudder = channel_rudder->pwm_to_angle(); break; //roll: -13788.000, pitch: -13698.000, thr: 0.000, rud: -13742.000 case QSTABILIZE: case QHOVER: case QLOITER: case QLAND: case QRTL: { // set nav_roll and nav_pitch using sticks int16_t roll_limit = MIN(roll_limit_cd, quadplane.aparm.angle_max); nav_roll_cd = channel_roll->norm_input() * roll_limit; nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit, roll_limit); float pitch_input = channel_pitch->norm_input(); if (pitch_input > 0) { nav_pitch_cd = pitch_input * MIN(aparm.pitch_limit_max_cd, quadplane.aparm.angle_max); } else { nav_pitch_cd = pitch_input * MIN(-pitch_limit_min_cd, quadplane.aparm.angle_max); } nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get()); break; } case INITIALISING: // handled elsewhere break; } }
/* main flight mode dependent update code */ void Plane::update_flight_mode(void) { enum FlightMode effective_mode = control_mode; if (control_mode == AUTO && g.auto_fbw_steer == 42) { effective_mode = FLY_BY_WIRE_A; } if (effective_mode != AUTO) { // hold_course is only used in takeoff and landing steer_state.hold_course_cd = -1; } // ensure we are fly-forward when we are flying as a pure fixed // wing aircraft. This helps the EKF produce better state // estimates as it can make stronger assumptions if (quadplane.in_vtol_mode() || quadplane.in_assisted_flight()) { ahrs.set_fly_forward(false); } else if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) { ahrs.set_fly_forward(landing.is_flying_forward()); } else { ahrs.set_fly_forward(true); } switch (effective_mode) { case AUTO: handle_auto_mode(); break; case AVOID_ADSB: case GUIDED: if (auto_state.vtol_loiter && quadplane.available()) { quadplane.guided_update(); break; } FALLTHROUGH; case RTL: case LOITER: calc_nav_roll(); calc_nav_pitch(); calc_throttle(); break; case TRAINING: { training_manual_roll = false; training_manual_pitch = false; update_load_factor(); // if the roll is past the set roll limit, then // we set target roll to the limit if (ahrs.roll_sensor >= roll_limit_cd) { nav_roll_cd = roll_limit_cd; } else if (ahrs.roll_sensor <= -roll_limit_cd) { nav_roll_cd = -roll_limit_cd; } else { training_manual_roll = true; nav_roll_cd = 0; } // if the pitch is past the set pitch limits, then // we set target pitch to the limit if (ahrs.pitch_sensor >= aparm.pitch_limit_max_cd) { nav_pitch_cd = aparm.pitch_limit_max_cd; } else if (ahrs.pitch_sensor <= pitch_limit_min_cd) { nav_pitch_cd = pitch_limit_min_cd; } else { training_manual_pitch = true; nav_pitch_cd = 0; } if (fly_inverted()) { nav_pitch_cd = -nav_pitch_cd; } break; } case ACRO: { // handle locked/unlocked control if (acro_state.locked_roll) { nav_roll_cd = acro_state.locked_roll_err; } else { nav_roll_cd = ahrs.roll_sensor; } if (acro_state.locked_pitch) { nav_pitch_cd = acro_state.locked_pitch_cd; } else { nav_pitch_cd = ahrs.pitch_sensor; } break; } case AUTOTUNE: case FLY_BY_WIRE_A: { // set nav_roll and nav_pitch using sticks nav_roll_cd = channel_roll->norm_input() * roll_limit_cd; nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd); update_load_factor(); float pitch_input = channel_pitch->norm_input(); if (pitch_input > 0) { nav_pitch_cd = pitch_input * aparm.pitch_limit_max_cd; } else { nav_pitch_cd = -(pitch_input * pitch_limit_min_cd); } adjust_nav_pitch_throttle(); nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get()); if (fly_inverted()) { nav_pitch_cd = -nav_pitch_cd; } if (failsafe.rc_failsafe && g.fs_action_short == FS_ACTION_SHORT_FBWA) { // FBWA failsafe glide nav_roll_cd = 0; nav_pitch_cd = 0; SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_MIN); } if (g.fbwa_tdrag_chan > 0) { // check for the user enabling FBWA taildrag takeoff mode bool tdrag_mode = (RC_Channels::get_radio_in(g.fbwa_tdrag_chan-1) > 1700); if (tdrag_mode && !auto_state.fbwa_tdrag_takeoff_mode) { if (auto_state.highest_airspeed < g.takeoff_tdrag_speed1) { auto_state.fbwa_tdrag_takeoff_mode = true; gcs().send_text(MAV_SEVERITY_WARNING, "FBWA tdrag mode"); } } } break; } case FLY_BY_WIRE_B: // Thanks to Yury MonZon for the altitude limit code! nav_roll_cd = channel_roll->norm_input() * roll_limit_cd; nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd); update_load_factor(); update_fbwb_speed_height(); break; case CRUISE: /* in CRUISE mode we use the navigation code to control roll when heading is locked. Heading becomes unlocked on any aileron or rudder input */ if (channel_roll->get_control_in() != 0 || channel_rudder->get_control_in() != 0) { cruise_state.locked_heading = false; cruise_state.lock_timer_ms = 0; } if (!cruise_state.locked_heading) { nav_roll_cd = channel_roll->norm_input() * roll_limit_cd; nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd); update_load_factor(); } else { calc_nav_roll(); } update_fbwb_speed_height(); break; case STABILIZE: nav_roll_cd = 0; nav_pitch_cd = 0; // throttle is passthrough break; case CIRCLE: // we have no GPS installed and have lost radio contact // or we just want to fly around in a gentle circle w/o GPS, // holding altitude at the altitude we set when we // switched into the mode nav_roll_cd = roll_limit_cd / 3; update_load_factor(); calc_nav_pitch(); calc_throttle(); break; case MANUAL: SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, channel_roll->get_control_in_zero_dz()); SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, channel_pitch->get_control_in_zero_dz()); steering_control.steering = steering_control.rudder = channel_rudder->get_control_in_zero_dz(); break; case QSTABILIZE: case QHOVER: case QLOITER: case QLAND: case QRTL: { // set nav_roll and nav_pitch using sticks int16_t roll_limit = MIN(roll_limit_cd, quadplane.aparm.angle_max); float pitch_input = channel_pitch->norm_input(); // Scale from normalized input [-1,1] to centidegrees if (quadplane.tailsitter_active()) { // separate limit for tailsitter roll, if set if (quadplane.tailsitter.max_roll_angle > 0) { roll_limit = quadplane.tailsitter.max_roll_angle * 100.0f; } // angle max for tailsitter pitch nav_pitch_cd = pitch_input * quadplane.aparm.angle_max; } else { // pitch is further constrained by LIM_PITCH_MIN/MAX which may impose // tighter (possibly asymmetrical) limits than Q_ANGLE_MAX if (pitch_input > 0) { nav_pitch_cd = pitch_input * MIN(aparm.pitch_limit_max_cd, quadplane.aparm.angle_max); } else { nav_pitch_cd = pitch_input * MIN(-pitch_limit_min_cd, quadplane.aparm.angle_max); } nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get()); } nav_roll_cd = (channel_roll->get_control_in() / 4500.0) * roll_limit; nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit, roll_limit); break; } case INITIALISING: // handled elsewhere break; } }