/* 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; } }
/* 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(); } }