diff --git a/ArduPlane/APM_Config.h.reference b/ArduPlane/APM_Config.h.reference index de61f6000c..929651a315 100644 --- a/ArduPlane/APM_Config.h.reference +++ b/ArduPlane/APM_Config.h.reference @@ -291,12 +291,12 @@ // //#define FLIGHT_MODE_CHANNEL 8 // -//#define FLIGHT_MODE_1 RTL -//#define FLIGHT_MODE_2 RTL -//#define FLIGHT_MODE_3 STABILIZE -//#define FLIGHT_MODE_4 STABILIZE -//#define FLIGHT_MODE_5 MANUAL -//#define FLIGHT_MODE_6 MANUAL +//#define FLIGHT_MODE_1 Mode::Number::RTL +//#define FLIGHT_MODE_2 Mode::Number::RTL +//#define FLIGHT_MODE_3 Mode::Number::STABILIZE +//#define FLIGHT_MODE_4 Mode::Number::STABILIZE +//#define FLIGHT_MODE_5 Mode::Number::MANUAL +//#define FLIGHT_MODE_6 Mode::Number::MANUAL // ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index 73494ccf63..78c9a72346 100644 --- a/ArduPlane/AP_Arming.cpp +++ b/ArduPlane/AP_Arming.cpp @@ -76,7 +76,7 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure) } } - if (plane.control_mode == AUTO && plane.mission.num_commands() <= 1) { + if (plane.control_mode == &plane.mode_auto && plane.mission.num_commands() <= 1) { check_failed(ARMING_CHECK_NONE, display_failure, "No mission loaded"); ret = false; } diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 5374ca3f35..8dd0168ad8 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -36,7 +36,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = { SCHED_TASK(read_radio, 50, 100), SCHED_TASK(check_short_failsafe, 50, 100), SCHED_TASK(update_speed_height, 50, 200), - SCHED_TASK(update_flight_mode, 400, 100), + SCHED_TASK(update_control_mode, 400, 100), SCHED_TASK(stabilize, 400, 100), SCHED_TASK(set_servos, 400, 100), SCHED_TASK(update_throttle_hover, 100, 90), @@ -415,65 +415,16 @@ void Plane::update_GPS_10Hz(void) } /* - main handling for AUTO mode + main control mode dependent update code */ -void Plane::handle_auto_mode(void) +void Plane::update_control_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; + Mode *effective_mode = control_mode; + if (control_mode == &mode_auto && g.auto_fbw_steer == 42) { + effective_mode = &mode_fbwa; } - 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(); - } -} - -/* - 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) { + if (effective_mode != &mode_auto) { // hold_course is only used in takeoff and landing steer_state.hold_course_cd = -1; } @@ -490,199 +441,7 @@ void Plane::update_flight_mode(void) 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; - 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; - 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; - 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: - case QAUTOTUNE: { - // 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 QACRO: - case INITIALISING: - // handled elsewhere - break; - } + effective_mode->update(); } void Plane::update_navigation() @@ -696,14 +455,14 @@ void Plane::update_navigation() qrtl_radius = abs(aparm.loiter_radius); } - switch(control_mode) { - case AUTO: + switch (control_mode->mode_number()) { + case Mode::Number::AUTO: if (ahrs.home_is_set()) { mission.update(); } break; - case RTL: + case Mode::Number::RTL: if (quadplane.available() && quadplane.rtl_mode == 1 && (nav_controller->reached_loiter_target() || location_passed_point(current_loc, prev_WP_loc, next_WP_loc) || @@ -714,7 +473,7 @@ void Plane::update_navigation() are within the maximum of the stopping distance and the RTL_RADIUS */ - set_mode(QRTL, MODE_REASON_UNKNOWN); + set_mode(mode_qrtl, MODE_REASON_UNKNOWN); break; } else if (g.rtl_autoland == 1 && !auto_state.checked_for_autoland && @@ -723,7 +482,7 @@ void Plane::update_navigation() // we've reached the RTL point, see if we have a landing sequence if (mission.jump_to_landing_sequence()) { // switch from RTL -> AUTO - set_mode(AUTO, MODE_REASON_UNKNOWN); + set_mode(mode_auto, MODE_REASON_UNKNOWN); } // prevent running the expensive jump_to_landing_sequence @@ -735,7 +494,7 @@ void Plane::update_navigation() // Go directly to the landing sequence if (mission.jump_to_landing_sequence()) { // switch from RTL -> AUTO - set_mode(AUTO, MODE_REASON_UNKNOWN); + set_mode(mode_auto, MODE_REASON_UNKNOWN); } // prevent running the expensive jump_to_landing_sequence @@ -749,32 +508,32 @@ void Plane::update_navigation() // fall through to LOITER FALLTHROUGH; - case LOITER: - case AVOID_ADSB: - case GUIDED: + case Mode::Number::LOITER: + case Mode::Number::AVOID_ADSB: + case Mode::Number::GUIDED: update_loiter(radius); break; - case CRUISE: + case Mode::Number::CRUISE: update_cruise(); break; - case MANUAL: - case STABILIZE: - case TRAINING: - case INITIALISING: - case ACRO: - case FLY_BY_WIRE_A: - case AUTOTUNE: - case FLY_BY_WIRE_B: - case CIRCLE: - case QSTABILIZE: - case QHOVER: - case QLOITER: - case QLAND: - case QRTL: - case QAUTOTUNE: - case QACRO: + case Mode::Number::MANUAL: + case Mode::Number::STABILIZE: + case Mode::Number::TRAINING: + case Mode::Number::INITIALISING: + case Mode::Number::ACRO: + case Mode::Number::FLY_BY_WIRE_A: + case Mode::Number::AUTOTUNE: + case Mode::Number::FLY_BY_WIRE_B: + case Mode::Number::CIRCLE: + case Mode::Number::QSTABILIZE: + case Mode::Number::QHOVER: + case Mode::Number::QLOITER: + case Mode::Number::QLAND: + case Mode::Number::QRTL: + case Mode::Number::QAUTOTUNE: + case Mode::Number::QACRO: // nothing to do break; } @@ -855,7 +614,7 @@ void Plane::update_flight_stage(void) { // Update the speed & height controller states if (auto_throttle_mode && !throttle_suppressed) { - if (control_mode==AUTO) { + if (control_mode == &mode_auto) { if (quadplane.in_vtol_auto()) { set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_VTOL); } else if (auto_state.takeoff_complete == false) { diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index f3771ab68f..aefac45cc7 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -89,7 +89,7 @@ void Plane::stabilize_roll(float speed_scaler) } bool disable_integrator = false; - if (control_mode == STABILIZE && channel_roll->get_control_in() != 0) { + if (control_mode == &mode_stabilize && channel_roll->get_control_in() != 0) { disable_integrator = true; } SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rollController.get_servo_out(nav_roll_cd - ahrs.roll_sensor, @@ -113,7 +113,7 @@ void Plane::stabilize_pitch(float speed_scaler) } int32_t demanded_pitch = nav_pitch_cd + g.pitch_trim_cd + SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * g.kff_throttle_to_pitch; bool disable_integrator = false; - if (control_mode == STABILIZE && channel_pitch->get_control_in() != 0) { + if (control_mode == &mode_stabilize && channel_pitch->get_control_in() != 0) { disable_integrator = true; } SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor, @@ -145,19 +145,19 @@ void Plane::stick_mix_channel(RC_Channel *channel, int16_t &servo_out) void Plane::stabilize_stick_mixing_direct() { if (!stick_mixing_enabled() || - control_mode == ACRO || - control_mode == FLY_BY_WIRE_A || - control_mode == AUTOTUNE || - control_mode == FLY_BY_WIRE_B || - control_mode == CRUISE || - control_mode == QSTABILIZE || - control_mode == QHOVER || - control_mode == QLOITER || - control_mode == QLAND || - control_mode == QRTL || - control_mode == QACRO || - control_mode == TRAINING || - control_mode == QAUTOTUNE) { + control_mode == &mode_acro || + control_mode == &mode_fbwa || + control_mode == &mode_autotune || + control_mode == &mode_fbwb || + control_mode == &mode_cruise || + control_mode == &mode_qstabilize || + control_mode == &mode_qhover || + control_mode == &mode_qloiter || + control_mode == &mode_qland || + control_mode == &mode_qrtl || + control_mode == &mode_qacro || + control_mode == &mode_training || + control_mode == &mode_qautotune) { return; } int16_t aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron); @@ -176,20 +176,20 @@ void Plane::stabilize_stick_mixing_direct() void Plane::stabilize_stick_mixing_fbw() { if (!stick_mixing_enabled() || - control_mode == ACRO || - control_mode == FLY_BY_WIRE_A || - control_mode == AUTOTUNE || - control_mode == FLY_BY_WIRE_B || - control_mode == CRUISE || - control_mode == QSTABILIZE || - control_mode == QHOVER || - control_mode == QLOITER || - control_mode == QLAND || - control_mode == QRTL || - control_mode == QACRO || - control_mode == TRAINING || - control_mode == QAUTOTUNE || - (control_mode == AUTO && g.auto_fbw_steer == 42)) { + control_mode == &mode_acro || + control_mode == &mode_fbwa || + control_mode == &mode_autotune || + control_mode == &mode_fbwb || + control_mode == &mode_cruise || + control_mode == &mode_qstabilize || + control_mode == &mode_qhover || + control_mode == &mode_qloiter || + control_mode == &mode_qland || + control_mode == &mode_qrtl || + control_mode == &mode_qacro || + control_mode == &mode_training || + control_mode == &mode_qautotune || + (control_mode == &mode_auto && g.auto_fbw_steer == 42)) { return; } // do FBW style stick mixing. We don't treat it linearly @@ -374,7 +374,7 @@ void Plane::stabilize_acro(float speed_scaler) */ void Plane::stabilize() { - if (control_mode == MANUAL) { + if (control_mode == &mode_manual) { // nothing to do return; } @@ -389,26 +389,26 @@ void Plane::stabilize() nav_roll_cd = 0; } - if (control_mode == TRAINING) { + if (control_mode == &mode_training) { stabilize_training(speed_scaler); - } else if (control_mode == ACRO) { + } else if (control_mode == &mode_acro) { stabilize_acro(speed_scaler); - } else if ((control_mode == QSTABILIZE || - control_mode == QHOVER || - control_mode == QLOITER || - control_mode == QLAND || - control_mode == QRTL || - control_mode == QACRO || - control_mode == QAUTOTUNE) && + } else if ((control_mode == &mode_qstabilize || + control_mode == &mode_qhover || + control_mode == &mode_qloiter || + control_mode == &mode_qland || + control_mode == &mode_qrtl || + control_mode == &mode_qacro || + control_mode == &mode_qautotune) && !quadplane.in_tailsitter_vtol_transition()) { quadplane.control_run(); } else { - if (g.stick_mixing == STICK_MIXING_FBW && control_mode != STABILIZE) { + if (g.stick_mixing == STICK_MIXING_FBW && control_mode != &mode_stabilize) { stabilize_stick_mixing_fbw(); } stabilize_roll(speed_scaler); stabilize_pitch(speed_scaler); - if (g.stick_mixing == STICK_MIXING_DIRECT || control_mode == STABILIZE) { + if (g.stick_mixing == STICK_MIXING_DIRECT || control_mode == &mode_stabilize) { stabilize_stick_mixing_direct(); } stabilize_yaw(speed_scaler); @@ -449,7 +449,7 @@ void Plane::calc_throttle() int32_t commanded_throttle = SpdHgt_Controller->get_throttle_demand(); // Received an external msg that guides throttle in the last 3 seconds? - if ((control_mode == GUIDED || control_mode == AVOID_ADSB) && + if ((control_mode == &mode_guided || control_mode == &mode_avoidADSB) && plane.guided_state.last_forced_throttle_ms > 0 && millis() - plane.guided_state.last_forced_throttle_ms < 3000) { commanded_throttle = plane.guided_state.forced_throttle; @@ -473,12 +473,12 @@ void Plane::calc_nav_yaw_coordinated(float speed_scaler) int16_t commanded_rudder; // Received an external msg that guides yaw in the last 3 seconds? - if ((control_mode == GUIDED || control_mode == AVOID_ADSB) && + if ((control_mode == &mode_guided || control_mode == &mode_avoidADSB) && plane.guided_state.last_forced_rpy_ms.z > 0 && millis() - plane.guided_state.last_forced_rpy_ms.z < 3000) { commanded_rudder = plane.guided_state.forced_rpy_cd.z; } else { - if (control_mode == STABILIZE && rudder_in != 0) { + if (control_mode == &mode_stabilize && rudder_in != 0) { disable_integrator = true; } @@ -561,7 +561,7 @@ void Plane::calc_nav_pitch() int32_t commanded_pitch = SpdHgt_Controller->get_pitch_demand(); // Received an external msg that guides roll in the last 3 seconds? - if ((control_mode == GUIDED || control_mode == AVOID_ADSB) && + if ((control_mode == &mode_guided || control_mode == &mode_avoidADSB) && plane.guided_state.last_forced_rpy_ms.y > 0 && millis() - plane.guided_state.last_forced_rpy_ms.y < 3000) { commanded_pitch = plane.guided_state.forced_rpy_cd.y; @@ -579,7 +579,7 @@ void Plane::calc_nav_roll() int32_t commanded_roll = nav_controller->nav_roll_cd(); // Received an external msg that guides roll in the last 3 seconds? - if ((control_mode == GUIDED || control_mode == AVOID_ADSB) && + if ((control_mode == &mode_guided || control_mode == &mode_avoidADSB) && plane.guided_state.last_forced_rpy_ms.x > 0 && millis() - plane.guided_state.last_forced_rpy_ms.x < 3000) { commanded_roll = plane.guided_state.forced_rpy_cd.x; diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 262dcd86b4..52366b020a 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -19,39 +19,39 @@ MAV_MODE GCS_MAVLINK_Plane::base_mode() const // only get useful information from the custom_mode, which maps to // the APM flight mode and has a well defined meaning in the // ArduPlane documentation - switch (plane.control_mode) { - case MANUAL: - case TRAINING: - case ACRO: - case QACRO: + switch (plane.control_mode->mode_number()) { + case Mode::Number::MANUAL: + case Mode::Number::TRAINING: + case Mode::Number::ACRO: + case Mode::Number::QACRO: _base_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; break; - case STABILIZE: - case FLY_BY_WIRE_A: - case AUTOTUNE: - case FLY_BY_WIRE_B: - case QSTABILIZE: - case QHOVER: - case QLOITER: - case QLAND: - case CRUISE: - case QAUTOTUNE: + case Mode::Number::STABILIZE: + case Mode::Number::FLY_BY_WIRE_A: + case Mode::Number::AUTOTUNE: + case Mode::Number::FLY_BY_WIRE_B: + case Mode::Number::QSTABILIZE: + case Mode::Number::QHOVER: + case Mode::Number::QLOITER: + case Mode::Number::QLAND: + case Mode::Number::CRUISE: + case Mode::Number::QAUTOTUNE: _base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED; break; - case AUTO: - case RTL: - case LOITER: - case AVOID_ADSB: - case GUIDED: - case CIRCLE: - case QRTL: + case Mode::Number::AUTO: + case Mode::Number::RTL: + case Mode::Number::LOITER: + case Mode::Number::AVOID_ADSB: + case Mode::Number::GUIDED: + case Mode::Number::CIRCLE: + case Mode::Number::QRTL: _base_mode = MAV_MODE_FLAG_GUIDED_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; // note that MAV_MODE_FLAG_AUTO_ENABLED does not match what // APM does in any mode, as that is defined as "system finds its own goal // positions", which APM does not currently do break; - case INITIALISING: + case Mode::Number::INITIALISING: break; } @@ -59,12 +59,12 @@ MAV_MODE GCS_MAVLINK_Plane::base_mode() const _base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; } - if (plane.control_mode != MANUAL && plane.control_mode != INITIALISING) { + if (plane.control_mode != &plane.mode_manual && plane.control_mode != &plane.mode_initializing) { // stabiliser of some form is enabled _base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; } - if (plane.g.stick_mixing != STICK_MIXING_DISABLED && plane.control_mode != INITIALISING) { + if (plane.g.stick_mixing != STICK_MIXING_DISABLED && plane.control_mode != &plane.mode_initializing) { // all modes except INITIALISING have some form of manual // override if stick mixing is enabled _base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; @@ -77,7 +77,7 @@ MAV_MODE GCS_MAVLINK_Plane::base_mode() const #endif // we are armed if we are not initialising - if (plane.control_mode != INITIALISING && plane.arming.is_armed()) { + if (plane.control_mode != &plane.mode_initializing && plane.arming.is_armed()) { _base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } @@ -89,12 +89,12 @@ MAV_MODE GCS_MAVLINK_Plane::base_mode() const uint32_t GCS_Plane::custom_mode() const { - return plane.control_mode; + return plane.control_mode->mode_number(); } MAV_STATE GCS_MAVLINK_Plane::system_status() const { - if (plane.control_mode == INITIALISING) { + if (plane.control_mode == &plane.mode_initializing) { return MAV_STATE_CALIBRATING; } if (plane.any_failsafe_triggered()) { @@ -156,7 +156,7 @@ void Plane::send_fence_status(mavlink_channel_t chan) void GCS_MAVLINK_Plane::send_nav_controller_output() const { - if (plane.control_mode == MANUAL) { + if (plane.control_mode == &plane.mode_manual) { return; } const QuadPlane &quadplane = plane.quadplane; @@ -171,7 +171,7 @@ void GCS_MAVLINK_Plane::send_nav_controller_output() const targets.z * 1.0e-2f, wp_nav_valid ? quadplane.wp_nav->get_wp_bearing_to_destination() : 0, wp_nav_valid ? MIN(quadplane.wp_nav->get_wp_distance_to_destination(), UINT16_MAX) : 0, - (plane.control_mode != QSTABILIZE) ? quadplane.pos_control->get_alt_error() * 1.0e-2f : 0, + (plane.control_mode != &plane.mode_qstabilize) ? quadplane.pos_control->get_alt_error() * 1.0e-2f : 0, plane.airspeed_error * 100, wp_nav_valid ? quadplane.wp_nav->crosstrack_error() : 0); } else { @@ -191,7 +191,7 @@ void GCS_MAVLINK_Plane::send_nav_controller_output() const void GCS_MAVLINK_Plane::send_position_target_global_int() { - if (plane.control_mode == MANUAL) { + if (plane.control_mode == &plane.mode_manual) { return; } Location &next_WP_loc = plane.next_WP_loc; @@ -334,7 +334,7 @@ void GCS_MAVLINK_Plane::send_pid_info(const AP_Logger::PID_Info *pid_info, */ void GCS_MAVLINK_Plane::send_pid_tuning() { - if (plane.control_mode == MANUAL) { + if (plane.control_mode == &plane.mode_manual) { // no PIDs should be used in manual return; } @@ -655,7 +655,7 @@ bool GCS_MAVLINK_Plane::in_hil_mode() const */ bool GCS_MAVLINK_Plane::handle_guided_request(AP_Mission::Mission_Command &cmd) { - if (plane.control_mode != GUIDED) { + if (plane.control_mode != &plane.mode_guided) { // only accept position updates when in GUIDED mode return false; } @@ -783,8 +783,8 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_packet(const mavlink_command_in // location is valid load and set if (((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) || - (plane.control_mode == GUIDED)) { - plane.set_mode(GUIDED, MODE_REASON_GCS_COMMAND); + (plane.control_mode == &plane.mode_guided)) { + plane.set_mode(plane.mode_guided, MODE_REASON_GCS_COMMAND); plane.guided_WP_loc = requested_position; // add home alt if needed @@ -814,7 +814,9 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l // controlled modes (e.g., MANUAL, TRAINING) // this command should be ignored since it comes in from GCS // or a companion computer: - if (plane.control_mode != GUIDED && plane.control_mode != AUTO && plane.control_mode != AVOID_ADSB) { + if ((plane.control_mode != &plane.mode_guided) && + (plane.control_mode != &plane.mode_auto) && + (plane.control_mode != &plane.mode_avoidADSB)) { // failed return MAV_RESULT_FAILED; } @@ -828,11 +830,11 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l } case MAV_CMD_NAV_LOITER_UNLIM: - plane.set_mode(LOITER, MODE_REASON_GCS_COMMAND); + plane.set_mode(plane.mode_loiter, MODE_REASON_GCS_COMMAND); return MAV_RESULT_ACCEPTED; case MAV_CMD_NAV_RETURN_TO_LAUNCH: - plane.set_mode(RTL, MODE_REASON_GCS_COMMAND); + plane.set_mode(plane.mode_rtl, MODE_REASON_GCS_COMMAND); return MAV_RESULT_ACCEPTED; case MAV_CMD_NAV_TAKEOFF: { @@ -846,7 +848,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l } case MAV_CMD_MISSION_START: - plane.set_mode(AUTO, MODE_REASON_GCS_COMMAND); + plane.set_mode(plane.mode_auto, MODE_REASON_GCS_COMMAND); return MAV_RESULT_ACCEPTED; case MAV_CMD_COMPONENT_ARM_DISARM: @@ -868,7 +870,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l case MAV_CMD_DO_LAND_START: // attempt to switch to next DO_LAND_START command in the mission if (plane.mission.jump_to_landing_sequence()) { - plane.set_mode(AUTO, MODE_REASON_UNKNOWN); + plane.set_mode(plane.mode_auto, MODE_REASON_UNKNOWN); return MAV_RESULT_ACCEPTED; } return MAV_RESULT_FAILED; @@ -1185,7 +1187,8 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) // in e.g., RTL, CICLE. Specifying a single mode for companion // computer control is more safe (even more so when using // FENCE_ACTION = 4 for geofence failures). - if (plane.control_mode != GUIDED && plane.control_mode != AVOID_ADSB) { // don't screw up failsafes + if ((plane.control_mode != &plane.mode_guided) && + (plane.control_mode != &plane.mode_avoidADSB)) { // don't screw up failsafes break; } @@ -1270,7 +1273,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) mavlink_msg_set_position_target_local_ned_decode(msg, &packet); // exit if vehicle is not in Guided mode - if (plane.control_mode != GUIDED) { + if (plane.control_mode != &plane.mode_guided) { break; } @@ -1295,7 +1298,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) // in modes such as RTL, CIRCLE, etc. Specifying ONLY one mode // for companion computer control is more safe (provided // one uses the FENCE_ACTION = 4 (RTL) for geofence failures). - if (plane.control_mode != GUIDED && plane.control_mode != AVOID_ADSB) { + if (plane.control_mode != &plane.mode_guided && plane.control_mode != &plane.mode_avoidADSB) { //don't screw up failsafes break; } @@ -1397,7 +1400,7 @@ void GCS_MAVLINK_Plane::handle_mission_set_current(AP_Mission &mission, mavlink_ { plane.auto_state.next_wp_crosstrack = false; GCS_MAVLINK::handle_mission_set_current(mission, msg); - if (plane.control_mode == AUTO && plane.mission.state() == AP_Mission::MISSION_STOPPED) { + if (plane.control_mode == &plane.mode_auto && plane.mission.state() == AP_Mission::MISSION_STOPPED) { plane.mission.resume(); } } @@ -1412,31 +1415,11 @@ AP_AdvancedFailsafe *GCS_MAVLINK_Plane::get_advanced_failsafe() const */ bool GCS_MAVLINK_Plane::set_mode(const uint8_t mode) { - switch (mode) { - case MANUAL: - case CIRCLE: - case STABILIZE: - case TRAINING: - case ACRO: - case FLY_BY_WIRE_A: - case AUTOTUNE: - case FLY_BY_WIRE_B: - case CRUISE: - case AVOID_ADSB: - case GUIDED: - case AUTO: - case RTL: - case LOITER: - case QSTABILIZE: - case QHOVER: - case QLOITER: - case QLAND: - case QRTL: - case QAUTOTUNE: - plane.set_mode((enum FlightMode)mode, MODE_REASON_GCS_COMMAND); - return true; + Mode *new_mode = plane.mode_from_mode_num((enum Mode::Number)mode); + if (new_mode == nullptr) { + return false; } - return false; + return plane.set_mode(*new_mode, MODE_REASON_GCS_COMMAND); } uint64_t GCS_MAVLINK_Plane::capabilities() const diff --git a/ArduPlane/GCS_Plane.cpp b/ArduPlane/GCS_Plane.cpp index 7aafcc9388..faa8cd313d 100644 --- a/ArduPlane/GCS_Plane.cpp +++ b/ArduPlane/GCS_Plane.cpp @@ -50,43 +50,43 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void) bool rate_controlled = false; bool attitude_stabilized = false; - switch (plane.control_mode) { - case MANUAL: + switch (plane.control_mode->mode_number()) { + case Mode::Number::MANUAL: break; - case ACRO: - case QACRO: + case Mode::Number::ACRO: + case Mode::Number::QACRO: rate_controlled = true; break; - case STABILIZE: - case FLY_BY_WIRE_A: - case AUTOTUNE: - case QSTABILIZE: - case QHOVER: - case QLAND: - case QLOITER: - case QAUTOTUNE: - case FLY_BY_WIRE_B: - case CRUISE: + case Mode::Number::STABILIZE: + case Mode::Number::FLY_BY_WIRE_A: + case Mode::Number::AUTOTUNE: + case Mode::Number::QSTABILIZE: + case Mode::Number::QHOVER: + case Mode::Number::QLAND: + case Mode::Number::QLOITER: + case Mode::Number::QAUTOTUNE: + case Mode::Number::FLY_BY_WIRE_B: + case Mode::Number::CRUISE: rate_controlled = true; attitude_stabilized = true; break; - case TRAINING: + case Mode::Number::TRAINING: if (!plane.training_manual_roll || !plane.training_manual_pitch) { rate_controlled = true; attitude_stabilized = true; } break; - case AUTO: - case RTL: - case LOITER: - case AVOID_ADSB: - case GUIDED: - case CIRCLE: - case QRTL: + case Mode::Number::AUTO: + case Mode::Number::RTL: + case Mode::Number::LOITER: + case Mode::Number::AVOID_ADSB: + case Mode::Number::GUIDED: + case Mode::Number::CIRCLE: + case Mode::Number::QRTL: rate_controlled = true; attitude_stabilized = true; control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_YAW_POSITION; @@ -97,7 +97,7 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void) control_sensors_health |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; break; - case INITIALISING: + case Mode::Number::INITIALISING: break; } diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index d56c1f3f3b..104772a053 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -299,7 +299,7 @@ void Plane::Log_Write_Vehicle_Startup_Messages() { // only 200(?) bytes are guaranteed by AP_Logger Log_Write_Startup(TYPE_GROUNDSTART_MSG); - logger.Write_Mode(control_mode, control_mode_reason); + logger.Write_Mode(control_mode->mode_number(), control_mode_reason); ahrs.Log_Write_Home_And_Origin(); gps.Write_AP_Logger_Log_Startup_messages(); } diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index b4b9f68e1b..3766fd8f84 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -582,7 +582,7 @@ const AP_Param::Info Plane::var_info[] = { // @Description: This selects the mode to start in on boot. This is useful for when you want to start in AUTO mode on boot without a receiver. // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL,22:QAUTOTUNE // @User: Advanced - GSCALAR(initial_mode, "INITIAL_MODE", MANUAL), + GSCALAR(initial_mode, "INITIAL_MODE", Mode::Number::MANUAL), // @Param: LIM_ROLL_CD // @DisplayName: Maximum Bank Angle diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index fecb08493f..f9a8ec8769 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -106,6 +106,7 @@ // Local modules #include "defines.h" +#include "mode.h" #ifdef ENABLE_SCRIPTING #include @@ -136,7 +137,7 @@ protected: void setup_IO_failsafe(void); // return the AFS mapped control mode - enum control_mode afs_mode(void); + enum AP_AdvancedFailsafe::control_mode afs_mode(void); }; /* @@ -157,6 +158,30 @@ public: friend class RC_Channel_Plane; friend class RC_Channels_Plane; + friend class Mode; + friend class ModeCircle; + friend class ModeStabilize; + friend class ModeTraining; + friend class ModeAcro; + friend class ModeFBWA; + friend class ModeFBWB; + friend class ModeCruise; + friend class ModeAutoTune; + friend class ModeAuto; + friend class ModeRTL; + friend class ModeLoiter; + friend class ModeAvoidADSB; + friend class ModeGuided; + friend class ModeInitializing; + friend class ModeManual; + friend class ModeQStabilize; + friend class ModeQHover; + friend class ModeQLoiter; + friend class ModeQLand; + friend class ModeQRTL; + friend class ModeQAcro; + friend class ModeQAutotune; + Plane(void); // HAL::Callbacks implementation. @@ -299,11 +324,34 @@ private: AP_OSD osd; #endif + ModeCircle mode_circle; + ModeStabilize mode_stabilize; + ModeTraining mode_training; + ModeAcro mode_acro; + ModeFBWA mode_fbwa; + ModeFBWB mode_fbwb; + ModeCruise mode_cruise; + ModeAutoTune mode_autotune; + ModeAuto mode_auto; + ModeRTL mode_rtl; + ModeLoiter mode_loiter; + ModeAvoidADSB mode_avoidADSB; + ModeGuided mode_guided; + ModeInitializing mode_initializing; + ModeManual mode_manual; + ModeQStabilize mode_qstabilize; + ModeQHover mode_qhover; + ModeQLoiter mode_qloiter; + ModeQLand mode_qland; + ModeQRTL mode_qrtl; + ModeQAcro mode_qacro; + ModeQAutotune mode_qautotune; + // This is the state of the flight control system // There are multiple states defined such as MANUAL, FBW-A, AUTO - enum FlightMode control_mode = INITIALISING; + Mode *control_mode = &mode_initializing; mode_reason_t control_mode_reason = MODE_REASON_UNKNOWN; - enum FlightMode previous_mode = INITIALISING; + Mode *previous_mode = &mode_initializing; mode_reason_t previous_mode_reason = MODE_REASON_UNKNOWN; // time of last mode change @@ -329,7 +377,7 @@ private: bool adsb:1; // saved flight mode - enum FlightMode saved_mode; + enum Mode::Number saved_mode_number; // A tracking variable for type of failsafe active // Used for failsafe based on loss of RC signal or GCS signal @@ -904,9 +952,9 @@ private: void rpm_update(void); void init_ardupilot(); void startup_ground(void); - enum FlightMode get_previous_mode(); - void set_mode(enum FlightMode mode, mode_reason_t reason); - void exit_mode(enum FlightMode mode); + bool set_mode(Mode& new_mode, const mode_reason_t reason); + bool set_mode_by_number(const Mode::Number new_mode_number, const mode_reason_t reason); + Mode *mode_from_mode_num(const enum Mode::Number num); void check_long_failsafe(); void check_short_failsafe(); void startup_INS_ground(void); @@ -938,7 +986,7 @@ private: void update_logging1(void); void update_logging2(void); void avoidance_adsb_update(void); - void update_flight_mode(void); + void update_control_mode(void); void stabilize(); void set_servos_idle(void); void set_servos(); @@ -957,7 +1005,6 @@ private: void update_is_flying_5Hz(void); void crash_detection_update(void); bool in_preLaunch_flight_stage(void); - void handle_auto_mode(void); void calc_throttle(); void calc_nav_roll(); void calc_nav_pitch(); @@ -1007,7 +1054,7 @@ private: void do_set_home(const AP_Mission::Mission_Command& cmd); bool start_command_callback(const AP_Mission::Mission_Command &cmd); bool verify_command_callback(const AP_Mission::Mission_Command& cmd); - void notify_flight_mode(enum FlightMode mode); + void notify_mode(const Mode& mode); void log_init(); void parachute_check(); #if PARACHUTE == ENABLED diff --git a/ArduPlane/afs_plane.cpp b/ArduPlane/afs_plane.cpp index 773cba53ca..9e1378d5bb 100644 --- a/ArduPlane/afs_plane.cpp +++ b/ArduPlane/afs_plane.cpp @@ -86,7 +86,7 @@ AP_AdvancedFailsafe::control_mode AP_AdvancedFailsafe_Plane::afs_mode(void) if (plane.auto_throttle_mode) { return AP_AdvancedFailsafe::AFS_AUTO; } - if (plane.control_mode == MANUAL) { + if (plane.control_mode == &plane.mode_manual) { return AP_AdvancedFailsafe::AFS_MANUAL; } return AP_AdvancedFailsafe::AFS_STABILIZED; diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 12b26421d6..c9d0013433 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -27,8 +27,8 @@ void Plane::adjust_altitude_target() { Location target_location; - if (control_mode == FLY_BY_WIRE_B || - control_mode == CRUISE) { + if (control_mode == &mode_fbwb || + control_mode == &mode_cruise) { return; } if (landing.is_flaring()) { @@ -75,10 +75,10 @@ void Plane::setup_glide_slope(void) work out if we will gradually change altitude, or try to get to the new altitude as quickly as possible. */ - switch (control_mode) { - case RTL: - case AVOID_ADSB: - case GUIDED: + switch (control_mode->mode_number()) { + case Mode::Number::RTL: + case Mode::Number::AVOID_ADSB: + case Mode::Number::GUIDED: /* glide down slowly if above target altitude, but ascend more rapidly if below it. See https://github.com/ArduPilot/ardupilot/issues/39 @@ -90,7 +90,7 @@ void Plane::setup_glide_slope(void) } break; - case AUTO: + case Mode::Number::AUTO: // we only do glide slide handling in AUTO when above 20m or // when descending. The 20 meter threshold is arbitrary, and // is basically to prevent situations where we try to slowly @@ -466,7 +466,7 @@ int32_t Plane::adjusted_relative_altitude_cm(void) float Plane::mission_alt_offset(void) { float ret = g.alt_offset; - if (control_mode == AUTO && + if (control_mode == &mode_auto && (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND || auto_state.wp_is_land_approach)) { // when landing after an aborted landing due to too high glide // slope we use an offset from the last landing attempt @@ -506,7 +506,7 @@ float Plane::lookahead_adjustment(void) int32_t bearing_cd; int16_t distance; // work out distance and bearing to target - if (control_mode == FLY_BY_WIRE_B) { + if (control_mode == &mode_fbwb) { // there is no target waypoint in FBWB, so use yaw as an approximation bearing_cd = ahrs.yaw_sensor; distance = g.terrain_lookahead; @@ -611,9 +611,9 @@ void Plane::rangefinder_height_update(void) rangefinder_state.in_range = true; if (!rangefinder_state.in_use && (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND || - control_mode == QLAND || - control_mode == QRTL || - (control_mode == AUTO && quadplane.is_vtol_land(plane.mission.get_current_nav_cmd().id))) && + control_mode == &mode_qland || + control_mode == &mode_qrtl || + (control_mode == &mode_auto && quadplane.is_vtol_land(plane.mission.get_current_nav_cmd().id))) && g.rangefinder_landing) { rangefinder_state.in_use = true; gcs().send_text(MAV_SEVERITY_INFO, "Rangefinder engaged at %.2fm", (double)rangefinder_state.height_estimate); diff --git a/ArduPlane/avoidance_adsb.cpp b/ArduPlane/avoidance_adsb.cpp index da92dc73ce..7e163614da 100644 --- a/ArduPlane/avoidance_adsb.cpp +++ b/ArduPlane/avoidance_adsb.cpp @@ -19,15 +19,15 @@ MAV_COLLISION_ACTION AP_Avoidance_Plane::handle_avoidance(const AP_Avoidance::Ob plane.failsafe.adsb = true; failsafe_state_change = true; // record flight mode in case it's required for the recovery - prev_control_mode = plane.control_mode; + prev_control_mode_number = plane.control_mode->mode_number(); } // take no action in some flight modes - if (plane.control_mode == MANUAL || - (plane.control_mode == AUTO && !plane.auto_state.takeoff_complete) || + if (plane.control_mode == &plane.mode_manual || + (plane.control_mode == &plane.mode_auto && !plane.auto_state.takeoff_complete) || (plane.flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) || // TODO: consider allowing action during approach - plane.control_mode == AUTOTUNE || - plane.control_mode == QLAND) { + plane.control_mode == &plane.mode_autotune || + plane.control_mode == &plane.mode_qland) { actual_action = MAV_COLLISION_ACTION_NONE; } @@ -36,16 +36,16 @@ MAV_COLLISION_ACTION AP_Avoidance_Plane::handle_avoidance(const AP_Avoidance::Ob case MAV_COLLISION_ACTION_RTL: if (failsafe_state_change) { - plane.set_mode(RTL, MODE_REASON_AVOIDANCE); + plane.set_mode(plane.mode_rtl, MODE_REASON_AVOIDANCE); } break; case MAV_COLLISION_ACTION_HOVER: if (failsafe_state_change) { if (plane.quadplane.is_flying()) { - plane.set_mode(QLOITER, MODE_REASON_AVOIDANCE); + plane.set_mode(plane.mode_qloiter, MODE_REASON_AVOIDANCE); } else { - plane.set_mode(LOITER, MODE_REASON_AVOIDANCE); + plane.set_mode(plane.mode_loiter, MODE_REASON_AVOIDANCE); } } break; @@ -113,16 +113,16 @@ void AP_Avoidance_Plane::handle_recovery(uint8_t recovery_action) break; case AP_AVOIDANCE_RECOVERY_RESUME_PREVIOUS_FLIGHTMODE: - plane.set_mode(prev_control_mode, MODE_REASON_AVOIDANCE_RECOVERY); + plane.set_mode_by_number(prev_control_mode_number, MODE_REASON_AVOIDANCE_RECOVERY); break; case AP_AVOIDANCE_RECOVERY_RTL: - plane.set_mode(RTL, MODE_REASON_AVOIDANCE_RECOVERY); + plane.set_mode(plane.mode_rtl, MODE_REASON_AVOIDANCE_RECOVERY); break; case AP_AVOIDANCE_RECOVERY_RESUME_IF_AUTO_ELSE_LOITER: - if (prev_control_mode == AUTO) { - plane.set_mode(AUTO, MODE_REASON_AVOIDANCE_RECOVERY); + if (prev_control_mode_number == Mode::Number::AUTO) { + plane.set_mode(plane.mode_auto, MODE_REASON_AVOIDANCE_RECOVERY); } // else do nothing, same as AP_AVOIDANCE_RECOVERY_LOITER break; @@ -138,12 +138,12 @@ void AP_Avoidance_Plane::handle_recovery(uint8_t recovery_action) bool AP_Avoidance_Plane::check_flightmode(bool allow_mode_change) { // ensure plane is in avoid_adsb mode - if (allow_mode_change && plane.control_mode != AVOID_ADSB) { - plane.set_mode(AVOID_ADSB, MODE_REASON_AVOIDANCE); + if (allow_mode_change && plane.control_mode != &plane.mode_avoidADSB) { + plane.set_mode(plane.mode_avoidADSB, MODE_REASON_AVOIDANCE); } // check flight mode - return (plane.control_mode == AVOID_ADSB); + return (plane.control_mode == &plane.mode_avoidADSB); } bool AP_Avoidance_Plane::handle_avoidance_vertical(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change) diff --git a/ArduPlane/avoidance_adsb.h b/ArduPlane/avoidance_adsb.h index 809aa8214d..8261aed82a 100644 --- a/ArduPlane/avoidance_adsb.h +++ b/ArduPlane/avoidance_adsb.h @@ -34,5 +34,5 @@ protected: bool handle_avoidance_horizontal(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change); // control mode before avoidance began - FlightMode prev_control_mode = RTL; + enum Mode::Number prev_control_mode_number = Mode::Number::RTL; }; diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index a354123297..638442e93c 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -75,7 +75,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) break; case MAV_CMD_NAV_RETURN_TO_LAUNCH: - set_mode(RTL, MODE_REASON_UNKNOWN); + set_mode(mode_rtl, MODE_REASON_UNKNOWN); break; case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT: @@ -321,7 +321,7 @@ void Plane::do_RTL(int32_t rtl_altitude) setup_glide_slope(); setup_turn_angle(); - logger.Write_Mode(control_mode, control_mode_reason); + logger.Write_Mode(control_mode->mode_number(), control_mode_reason); } /* @@ -930,7 +930,7 @@ void Plane::do_set_home(const AP_Mission::Mission_Command& cmd) // we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode bool Plane::start_command_callback(const AP_Mission::Mission_Command &cmd) { - if (control_mode == AUTO) { + if (control_mode == &mode_auto) { return start_command(cmd); } return true; @@ -940,7 +940,7 @@ bool Plane::start_command_callback(const AP_Mission::Mission_Command &cmd) // we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode bool Plane::verify_command_callback(const AP_Mission::Mission_Command& cmd) { - if (control_mode == AUTO) { + if (control_mode == &mode_auto) { bool cmd_complete = verify_command(cmd); // send message to GCS @@ -957,8 +957,8 @@ bool Plane::verify_command_callback(const AP_Mission::Mission_Command& cmd) // we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode void Plane::exit_mission_callback() { - if (control_mode == AUTO) { - set_mode(RTL, MODE_REASON_MISSION_END); + if (control_mode == &mode_auto) { + set_mode(mode_rtl, MODE_REASON_MISSION_END); gcs().send_text(MAV_SEVERITY_INFO, "Mission complete, changing mode to RTL"); } } diff --git a/ArduPlane/config.h b/ArduPlane/config.h index e2b569e7c9..e156e895cd 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -104,22 +104,22 @@ #endif #if !defined(FLIGHT_MODE_1) - # define FLIGHT_MODE_1 RTL + # define FLIGHT_MODE_1 Mode::Number::RTL #endif #if !defined(FLIGHT_MODE_2) - # define FLIGHT_MODE_2 RTL + # define FLIGHT_MODE_2 Mode::Number::RTL #endif #if !defined(FLIGHT_MODE_3) - # define FLIGHT_MODE_3 FLY_BY_WIRE_A + # define FLIGHT_MODE_3 Mode::Number::FLY_BY_WIRE_A #endif #if !defined(FLIGHT_MODE_4) - # define FLIGHT_MODE_4 FLY_BY_WIRE_A + # define FLIGHT_MODE_4 Mode::Number::FLY_BY_WIRE_A #endif #if !defined(FLIGHT_MODE_5) - # define FLIGHT_MODE_5 MANUAL + # define FLIGHT_MODE_5 Mode::Number::MANUAL #endif #if !defined(FLIGHT_MODE_6) - # define FLIGHT_MODE_6 MANUAL + # define FLIGHT_MODE_6 Mode::Number::MANUAL #endif diff --git a/ArduPlane/control_modes.cpp b/ArduPlane/control_modes.cpp index b6f2e8b3e6..dec778e1cf 100644 --- a/ArduPlane/control_modes.cpp +++ b/ArduPlane/control_modes.cpp @@ -1,5 +1,79 @@ #include "Plane.h" +Mode *Plane::mode_from_mode_num(const enum Mode::Number num) +{ + Mode *ret = nullptr; + switch (num) { + case Mode::Number::MANUAL: + ret = &mode_manual; + break; + case Mode::Number::CIRCLE: + ret = &mode_circle; + break; + case Mode::Number::STABILIZE: + ret = &mode_stabilize; + break; + case Mode::Number::TRAINING: + ret = &mode_training; + break; + case Mode::Number::ACRO: + ret = &mode_acro; + break; + case Mode::Number::FLY_BY_WIRE_A: + ret = &mode_fbwa; + break; + case Mode::Number::FLY_BY_WIRE_B: + ret = &mode_fbwb; + break; + case Mode::Number::CRUISE: + ret = &mode_cruise; + break; + case Mode::Number::AUTOTUNE: + ret = &mode_autotune; + break; + case Mode::Number::AUTO: + ret = &mode_auto; + break; + case Mode::Number::RTL: + ret = &mode_rtl; + break; + case Mode::Number::LOITER: + ret = &mode_loiter; + break; + case Mode::Number::AVOID_ADSB: + ret = &mode_avoidADSB; + break; + case Mode::Number::GUIDED: + ret = &mode_guided; + break; + case Mode::Number::INITIALISING: + ret = &mode_initializing; + break; + case Mode::Number::QSTABILIZE: + ret = &mode_qstabilize; + break; + case Mode::Number::QHOVER: + ret = &mode_qhover; + break; + case Mode::Number::QLOITER: + ret = &mode_qloiter; + break; + case Mode::Number::QLAND: + ret = &mode_qland; + break; + case Mode::Number::QRTL: + ret = &mode_qrtl; + break; + case Mode::Number::QACRO: + ret = &mode_qacro; + break; + case Mode::Number::QAUTOTUNE: + ret = &mode_qautotune; + break; + } + return ret; +} + void Plane::read_control_switch() { static bool switch_debouncer; @@ -39,7 +113,7 @@ void Plane::read_control_switch() return; } - set_mode((enum FlightMode)(flight_modes[switchPosition].get()), MODE_REASON_TX_COMMAND); + set_mode_by_number((enum Mode::Number)flight_modes[switchPosition].get(), MODE_REASON_TX_COMMAND); oldSwitchPosition = switchPosition; } @@ -114,14 +188,14 @@ void Plane::autotune_enable(bool enable) */ bool Plane::fly_inverted(void) { - if (control_mode == MANUAL) { + if (control_mode == &plane.mode_manual) { return false; } if (inverted_flight) { // controlled with aux switch return true; } - if (control_mode == AUTO && auto_state.inverted_flight) { + if (control_mode == &mode_auto && auto_state.inverted_flight) { return true; } return false; diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 546d63331c..0250828df8 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -45,31 +45,6 @@ enum failsafe_action_long { FS_ACTION_LONG_PARACHUTE = 3, }; -enum FlightMode { - MANUAL = 0, - CIRCLE = 1, - STABILIZE = 2, - TRAINING = 3, - ACRO = 4, - FLY_BY_WIRE_A = 5, - FLY_BY_WIRE_B = 6, - CRUISE = 7, - AUTOTUNE = 8, - AUTO = 10, - RTL = 11, - LOITER = 12, - AVOID_ADSB = 14, - GUIDED = 15, - INITIALISING = 16, - QSTABILIZE = 17, - QHOVER = 18, - QLOITER = 19, - QLAND = 20, - QRTL = 21, - QAUTOTUNE = 22, - QACRO = 23, -}; - enum mode_reason_t { MODE_REASON_UNKNOWN=0, MODE_REASON_TX_COMMAND, diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index ea19e1201f..6ad6e49efe 100644 --- a/ArduPlane/events.cpp +++ b/ArduPlane/events.cpp @@ -6,57 +6,58 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, mode_reason_t re failsafe.state = fstype; failsafe.short_timer_ms = millis(); gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe. Short event on: type=%u/reason=%u", fstype, reason); - switch(control_mode) + switch (control_mode->mode_number()) { - case MANUAL: - case STABILIZE: - case ACRO: - case FLY_BY_WIRE_A: - case AUTOTUNE: - case FLY_BY_WIRE_B: - case CRUISE: - case TRAINING: - failsafe.saved_mode = control_mode; + case Mode::Number::MANUAL: + case Mode::Number::STABILIZE: + case Mode::Number::ACRO: + case Mode::Number::FLY_BY_WIRE_A: + case Mode::Number::AUTOTUNE: + case Mode::Number::FLY_BY_WIRE_B: + case Mode::Number::CRUISE: + case Mode::Number::TRAINING: + failsafe.saved_mode_number = control_mode->mode_number(); failsafe.saved_mode_set = true; if(g.fs_action_short == FS_ACTION_SHORT_FBWA) { - set_mode(FLY_BY_WIRE_A, reason); + set_mode(mode_fbwa, reason); } else { - set_mode(CIRCLE, reason); + set_mode(mode_circle, reason); } break; - case QSTABILIZE: - case QLOITER: - case QHOVER: - case QAUTOTUNE: - failsafe.saved_mode = control_mode; + case Mode::Number::QSTABILIZE: + case Mode::Number::QLOITER: + case Mode::Number::QHOVER: + case Mode::Number::QAUTOTUNE: + case Mode::Number::QACRO: + failsafe.saved_mode_number = control_mode->mode_number(); failsafe.saved_mode_set = true; - set_mode(QLAND, reason); + set_mode(mode_qland, reason); break; - case AUTO: - case AVOID_ADSB: - case GUIDED: - case LOITER: + case Mode::Number::AUTO: + case Mode::Number::AVOID_ADSB: + case Mode::Number::GUIDED: + case Mode::Number::LOITER: if(g.fs_action_short != FS_ACTION_SHORT_BESTGUESS) { - failsafe.saved_mode = control_mode; + failsafe.saved_mode_number = control_mode->mode_number(); failsafe.saved_mode_set = true; if(g.fs_action_short == FS_ACTION_SHORT_FBWA) { - set_mode(FLY_BY_WIRE_A, reason); + set_mode(mode_fbwa, reason); } else { - set_mode(CIRCLE, reason); + set_mode(mode_circle, reason); } } break; - case CIRCLE: - case RTL: - case QLAND: - case QRTL: - default: + case Mode::Number::CIRCLE: + case Mode::Number::RTL: + case Mode::Number::QLAND: + case Mode::Number::QRTL: + case Mode::Number::INITIALISING: break; } - gcs().send_text(MAV_SEVERITY_INFO, "Flight mode = %u", (unsigned)control_mode); + gcs().send_text(MAV_SEVERITY_INFO, "Flight mode = %u", (unsigned)control_mode->mode_number()); } void Plane::failsafe_long_on_event(enum failsafe_state fstype, mode_reason_t reason) @@ -66,57 +67,58 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, mode_reason_t rea // If the GCS is locked up we allow control to revert to RC RC_Channels::clear_overrides(); failsafe.state = fstype; - switch(control_mode) + switch (control_mode->mode_number()) { - case MANUAL: - case STABILIZE: - case ACRO: - case FLY_BY_WIRE_A: - case AUTOTUNE: - case FLY_BY_WIRE_B: - case CRUISE: - case TRAINING: - case CIRCLE: + case Mode::Number::MANUAL: + case Mode::Number::STABILIZE: + case Mode::Number::ACRO: + case Mode::Number::FLY_BY_WIRE_A: + case Mode::Number::AUTOTUNE: + case Mode::Number::FLY_BY_WIRE_B: + case Mode::Number::CRUISE: + case Mode::Number::TRAINING: + case Mode::Number::CIRCLE: if(g.fs_action_long == FS_ACTION_LONG_PARACHUTE) { #if PARACHUTE == ENABLED parachute_release(); #endif } else if (g.fs_action_long == FS_ACTION_LONG_GLIDE) { - set_mode(FLY_BY_WIRE_A, reason); + set_mode(mode_fbwa, reason); } else { - set_mode(RTL, reason); + set_mode(mode_rtl, reason); } break; - case QSTABILIZE: - case QHOVER: - case QLOITER: - case QAUTOTUNE: - set_mode(QLAND, reason); + case Mode::Number::QSTABILIZE: + case Mode::Number::QHOVER: + case Mode::Number::QLOITER: + case Mode::Number::QACRO: + case Mode::Number::QAUTOTUNE: + set_mode(mode_qland, reason); break; - case AUTO: - case AVOID_ADSB: - case GUIDED: - case LOITER: + case Mode::Number::AUTO: + case Mode::Number::AVOID_ADSB: + case Mode::Number::GUIDED: + case Mode::Number::LOITER: if(g.fs_action_long == FS_ACTION_LONG_PARACHUTE) { #if PARACHUTE == ENABLED parachute_release(); #endif } else if (g.fs_action_long == FS_ACTION_LONG_GLIDE) { - set_mode(FLY_BY_WIRE_A, reason); + set_mode(mode_fbwa, reason); } else if (g.fs_action_long == FS_ACTION_LONG_RTL) { - set_mode(RTL, reason); + set_mode(mode_rtl, reason); } break; - case RTL: - case QLAND: - case QRTL: - default: + case Mode::Number::RTL: + case Mode::Number::QLAND: + case Mode::Number::QRTL: + case Mode::Number::INITIALISING: break; } - gcs().send_text(MAV_SEVERITY_INFO, "Flight mode = %u", (unsigned)control_mode); + gcs().send_text(MAV_SEVERITY_INFO, "Flight mode = %u", (unsigned)control_mode->mode_number()); } void Plane::failsafe_short_off_event(mode_reason_t reason) @@ -127,9 +129,9 @@ void Plane::failsafe_short_off_event(mode_reason_t reason) // re-read the switch so we can return to our preferred mode // -------------------------------------------------------- - if (control_mode == CIRCLE && failsafe.saved_mode_set) { + if (control_mode == &mode_circle && failsafe.saved_mode_set) { failsafe.saved_mode_set = false; - set_mode(failsafe.saved_mode, reason); + set_mode_by_number(failsafe.saved_mode_number, reason); } } @@ -145,23 +147,23 @@ void Plane::handle_battery_failsafe(const char *type_str, const int8_t action) switch ((Failsafe_Action)action) { case Failsafe_Action_QLand: if (quadplane.available()) { - plane.set_mode(QLAND, MODE_REASON_BATTERY_FAILSAFE); + plane.set_mode(mode_qland, MODE_REASON_BATTERY_FAILSAFE); break; } FALLTHROUGH; case Failsafe_Action_Land: - if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND && control_mode != QLAND) { + if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND && control_mode != &mode_qland) { // never stop a landing if we were already committed if (plane.mission.jump_to_landing_sequence()) { - plane.set_mode(AUTO, MODE_REASON_BATTERY_FAILSAFE); + plane.set_mode(mode_auto, MODE_REASON_BATTERY_FAILSAFE); break; } } FALLTHROUGH; case Failsafe_Action_RTL: - if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND && control_mode != QLAND ) { + if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND && control_mode != &mode_qland ) { // never stop a landing if we were already committed - set_mode(RTL, MODE_REASON_BATTERY_FAILSAFE); + set_mode(mode_rtl, MODE_REASON_BATTERY_FAILSAFE); aparm.throttle_cruise.load(); } break; diff --git a/ArduPlane/geofence.cpp b/ArduPlane/geofence.cpp index f2a33f2635..71a07b10f2 100644 --- a/ArduPlane/geofence.cpp +++ b/ArduPlane/geofence.cpp @@ -291,14 +291,14 @@ void Plane::geofence_check(bool altitude_check_only) // GUIDED to the return point if (geofence_state != nullptr && (g.fence_action == FENCE_ACTION_GUIDED || g.fence_action == FENCE_ACTION_GUIDED_THR_PASS || g.fence_action == FENCE_ACTION_RTL) && - (control_mode == GUIDED || control_mode == AVOID_ADSB) && + (control_mode == &mode_guided || control_mode == &mode_avoidADSB) && geofence_present() && geofence_state->boundary_uptodate && geofence_state->old_switch_position == oldSwitchPosition && guided_WP_loc.lat == geofence_state->guided_lat && guided_WP_loc.lng == geofence_state->guided_lng) { geofence_state->old_switch_position = 254; - set_mode(get_previous_mode(), MODE_REASON_GCS_COMMAND); + set_mode(*previous_mode, MODE_REASON_GCS_COMMAND); } return; } @@ -354,7 +354,7 @@ void Plane::geofence_check(bool altitude_check_only) // we are outside the fence if (geofence_state->fence_triggered && - (control_mode == GUIDED || control_mode == AVOID_ADSB || control_mode == RTL || g.fence_action == FENCE_ACTION_REPORT)) { + (control_mode == &mode_guided || control_mode == &mode_avoidADSB || control_mode == &mode_rtl || g.fence_action == FENCE_ACTION_REPORT)) { // we have already triggered, don't trigger again until the // user disables/re-enables using the fence channel switch return; @@ -386,9 +386,9 @@ void Plane::geofence_check(bool altitude_check_only) int8_t saved_auto_trim = g.auto_trim; g.auto_trim.set(0); if (g.fence_action == FENCE_ACTION_RTL) { - set_mode(RTL, MODE_REASON_FENCE_BREACH); + set_mode(mode_rtl, MODE_REASON_FENCE_BREACH); } else { - set_mode(GUIDED, MODE_REASON_FENCE_BREACH); + set_mode(mode_guided, MODE_REASON_FENCE_BREACH); } g.auto_trim.set(saved_auto_trim); @@ -438,7 +438,7 @@ bool Plane::geofence_stickmixing(void) { if (geofence_enabled() && geofence_state != nullptr && geofence_state->fence_triggered && - (control_mode == GUIDED || control_mode == AVOID_ADSB)) { + (control_mode == &mode_guided || control_mode == &mode_avoidADSB)) { // don't mix in user input return false; } diff --git a/ArduPlane/is_flying.cpp b/ArduPlane/is_flying.cpp index f16561e004..16133df8fe 100644 --- a/ArduPlane/is_flying.cpp +++ b/ArduPlane/is_flying.cpp @@ -53,7 +53,7 @@ void Plane::update_is_flying_5Hz(void) gps_confirmed_movement; // locked and we're moving } - if (control_mode == AUTO) { + if (control_mode == &mode_auto) { /* make is_flying() more accurate during various auto modes */ @@ -146,7 +146,7 @@ void Plane::update_is_flying_5Hz(void) started_flying_ms = now_ms; } - if ((control_mode == AUTO) && + if ((control_mode == &mode_auto) && ((auto_state.started_flying_in_auto_ms == 0) || !previous_is_flying) ) { // We just started flying, note that time also @@ -192,7 +192,7 @@ bool Plane::is_flying(void) */ void Plane::crash_detection_update(void) { - if (control_mode != AUTO || !aparm.crash_detection_enable) + if (control_mode != &mode_auto || !aparm.crash_detection_enable) { // crash detection is only available in AUTO mode crash_state.debounce_timer_ms = 0; @@ -317,7 +317,7 @@ void Plane::crash_detection_update(void) * return true if we are in a pre-launch phase of an auto-launch, typically used in bungee launches */ bool Plane::in_preLaunch_flight_stage(void) { - return (control_mode == AUTO && + return (control_mode == &mode_auto && throttle_suppressed && flight_stage == AP_Vehicle::FixedWing::FLIGHT_NORMAL && mission.get_current_nav_cmd().id == MAV_CMD_NAV_TAKEOFF && diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp new file mode 100644 index 0000000000..0fd6148c5a --- /dev/null +++ b/ArduPlane/mode.cpp @@ -0,0 +1,76 @@ +#include "Plane.h" + +Mode::Mode() +{ +} + +void Mode::exit() +{ + // call sub-classes exit + _exit(); +} + +bool Mode::enter() +{ + // cancel inverted flight + plane.auto_state.inverted_flight = false; + + // don't cross-track when starting a mission + plane.auto_state.next_wp_crosstrack = false; + + // reset landing check + plane.auto_state.checked_for_autoland = false; + + // zero locked course + plane.steer_state.locked_course_err = 0; + + // reset crash detection + plane.crash_state.is_crashed = false; + plane.crash_state.impact_detected = false; + + // reset external attitude guidance + plane.guided_state.last_forced_rpy_ms.zero(); + plane.guided_state.last_forced_throttle_ms = 0; + +#if CAMERA == ENABLED + plane.camera.set_is_auto_mode(this == &plane.mode_auto); +#endif + + // zero initial pitch and highest airspeed on mode change + plane.auto_state.highest_airspeed = 0; + plane.auto_state.initial_pitch_cd = plane.ahrs.pitch_sensor; + + // disable taildrag takeoff on mode change + plane.auto_state.fbwa_tdrag_takeoff_mode = false; + + // start with previous WP at current location + plane.prev_WP_loc = plane.current_loc; + + // new mode means new loiter + plane.loiter.start_time_ms = 0; + + // record time of mode change + plane.last_mode_change_ms = AP_HAL::millis(); + + // assume non-VTOL mode + plane.auto_state.vtol_mode = false; + plane.auto_state.vtol_loiter = false; + + // reset steering integrator on mode change + plane.steerController.reset_I(); + + bool enter_result = _enter(); + + if (enter_result) { + // ------------------- + // these must be done AFTER _enter() because they use the results to set more flags + + // start with throttle suppressed in auto_throttle modes + plane.throttle_suppressed = plane.auto_throttle_mode; + + plane.adsb.set_is_auto_mode(plane.auto_navigation_mode); + } + + return enter_result; +} + diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h new file mode 100644 index 0000000000..0495a30193 --- /dev/null +++ b/ArduPlane/mode.h @@ -0,0 +1,433 @@ +#pragma once + +#include + +class Mode +{ +public: + + /* Do not allow copies */ + Mode(const Mode &other) = delete; + Mode &operator=(const Mode&) = delete; + + // Auto Pilot modes + // ---------------- + enum Number { + MANUAL = 0, + CIRCLE = 1, + STABILIZE = 2, + TRAINING = 3, + ACRO = 4, + FLY_BY_WIRE_A = 5, + FLY_BY_WIRE_B = 6, + CRUISE = 7, + AUTOTUNE = 8, + AUTO = 10, + RTL = 11, + LOITER = 12, + AVOID_ADSB = 14, + GUIDED = 15, + INITIALISING = 16, + QSTABILIZE = 17, + QHOVER = 18, + QLOITER = 19, + QLAND = 20, + QRTL = 21, + QAUTOTUNE = 22, + QACRO = 23, + }; + + // Constructor + Mode(); + + // enter this mode, always returns true/success + bool enter(); + + // perform any cleanups required: + void exit(); + + // returns a unique number specific to this mode + virtual Number mode_number() const = 0; + + // returns full text name + virtual const char *name() const = 0; + + // returns a string for this flightmode, exactly 4 bytes + virtual const char *name4() const = 0; + + // + // methods that sub classes should override to affect movement of the vehicle in this mode + // + + // convert user input to targets, implement high level control for this mode + virtual void update() = 0; + +protected: + + // subclasses override this to perform checks before entering the mode + virtual bool _enter() { return true; } + + // subclasses override this to perform any required cleanup when exiting the mode + virtual void _exit() { return; } +}; + + +class ModeAcro : public Mode +{ +public: + + Mode::Number mode_number() const override { return Mode::Number::ACRO; } + const char *name() const override { return "ACRO"; } + const char *name4() const override { return "ACRO"; } + + // methods that affect movement of the vehicle in this mode + void update() override; + +protected: + + bool _enter() override; +}; + +class ModeAuto : public Mode +{ +public: + + Number mode_number() const override { return Number::AUTO; } + const char *name() const override { return "AUTO"; } + const char *name4() const override { return "AUTO"; } + + // methods that affect movement of the vehicle in this mode + void update() override; + +protected: + + bool _enter() override; + void _exit() override; +}; + + +class ModeAutoTune : public Mode +{ +public: + + Number mode_number() const override { return Number::AUTOTUNE; } + const char *name() const override { return "AUTOTUNE"; } + const char *name4() const override { return "ATUN"; } + + // methods that affect movement of the vehicle in this mode + void update() override; + +protected: + + bool _enter() override; + void _exit() override; +}; + +class ModeGuided : public Mode +{ +public: + + Number mode_number() const override { return Number::GUIDED; } + const char *name() const override { return "GUIDED"; } + const char *name4() const override { return "GUID"; } + + // methods that affect movement of the vehicle in this mode + void update() override; + +protected: + + bool _enter() override; +}; + +class ModeCircle: public Mode +{ +public: + + Number mode_number() const override { return Number::CIRCLE; } + const char *name() const override { return "CIRCLE"; } + const char *name4() const override { return "CIRC"; } + + // methods that affect movement of the vehicle in this mode + void update() override; + +protected: + + bool _enter() override; +}; + +class ModeLoiter : public Mode +{ +public: + + Number mode_number() const override { return Number::LOITER; } + const char *name() const override { return "LOITER"; } + const char *name4() const override { return "LOIT"; } + + // methods that affect movement of the vehicle in this mode + void update() override; + +protected: + + bool _enter() override; +}; + +class ModeManual : public Mode +{ +public: + + Number mode_number() const override { return Number::MANUAL; } + const char *name() const override { return "MANUAL"; } + const char *name4() const override { return "MANU"; } + + // methods that affect movement of the vehicle in this mode + void update() override; + +protected: + + bool _enter() override; + void _exit() override; +}; + + +class ModeRTL : public Mode +{ +public: + + Number mode_number() const override { return Number::RTL; } + const char *name() const override { return "RTL"; } + const char *name4() const override { return "RTL "; } + + // methods that affect movement of the vehicle in this mode + void update() override; + +protected: + + bool _enter() override; +}; + +class ModeStabilize : public Mode +{ +public: + + Number mode_number() const override { return Number::STABILIZE; } + const char *name() const override { return "STABILIZE"; } + const char *name4() const override { return "STAB"; } + + // methods that affect movement of the vehicle in this mode + void update() override; + +protected: + + bool _enter() override; +}; + +class ModeTraining : public Mode +{ +public: + + Number mode_number() const override { return Number::TRAINING; } + const char *name() const override { return "TRAINING"; } + const char *name4() const override { return "TRAN"; } + + // methods that affect movement of the vehicle in this mode + void update() override; + +protected: + + bool _enter() override; +}; + +class ModeInitializing : public Mode +{ +public: + + Number mode_number() const override { return Number::INITIALISING; } + const char *name() const override { return "INITIALISING"; } + const char *name4() const override { return "INIT"; } + + // methods that affect movement of the vehicle in this mode + void update() override { } + +protected: + + bool _enter() override; +}; + +class ModeFBWA : public Mode +{ +public: + + Number mode_number() const override { return Number::FLY_BY_WIRE_A; } + const char *name() const override { return "FLY_BY_WIRE_A"; } + const char *name4() const override { return "FBWA"; } + + // methods that affect movement of the vehicle in this mode + void update() override; + + bool _enter() override; + +protected: + +}; + +class ModeFBWB : public Mode +{ +public: + + Number mode_number() const override { return Number::FLY_BY_WIRE_B; } + const char *name() const override { return "FLY_BY_WIRE_B"; } + const char *name4() const override { return "FBWB"; } + + // methods that affect movement of the vehicle in this mode + void update() override; + +protected: + + bool _enter() override; +}; + +class ModeCruise : public Mode +{ +public: + + Number mode_number() const override { return Number::CRUISE; } + const char *name() const override { return "CRUISE"; } + const char *name4() const override { return "CRUS"; } + + // methods that affect movement of the vehicle in this mode + void update() override; + +protected: + + bool _enter() override; +}; + +class ModeAvoidADSB : public Mode +{ +public: + + Number mode_number() const override { return Number::AVOID_ADSB; } + const char *name() const override { return "AVOID_ADSB"; } + const char *name4() const override { return "AVOI"; } + + // methods that affect movement of the vehicle in this mode + void update() override; + +protected: + + bool _enter() override; +}; + +class ModeQStabilize : public Mode +{ +public: + + Number mode_number() const override { return Number::QSTABILIZE; } + const char *name() const override { return "QSTABILIZE"; } + const char *name4() const override { return "QSTB"; } + + // methods that affect movement of the vehicle in this mode + void update() override; + + // used as a base class for all Q modes + bool _enter() override; + +protected: + +}; + +class ModeQHover : public Mode +{ +public: + + Number mode_number() const override { return Number::QHOVER; } + const char *name() const override { return "QHOVER"; } + const char *name4() const override { return "QHOV"; } + + // methods that affect movement of the vehicle in this mode + void update() override; + +protected: + + bool _enter() override; +}; + +class ModeQLoiter : public Mode +{ +public: + + Number mode_number() const override { return Number::QLOITER; } + const char *name() const override { return "QLOITER"; } + const char *name4() const override { return "QLOT"; } + + // methods that affect movement of the vehicle in this mode + void update() override; + +protected: + + bool _enter() override; +}; + +class ModeQLand : public Mode +{ +public: + + Number mode_number() const override { return Number::QLAND; } + const char *name() const override { return "QLAND"; } + const char *name4() const override { return "QLND"; } + + // methods that affect movement of the vehicle in this mode + void update() override; + +protected: + + bool _enter() override; +}; + +class ModeQRTL : public Mode +{ +public: + + Number mode_number() const override { return Number::QRTL; } + const char *name() const override { return "QRTL"; } + const char *name4() const override { return "QRTL"; } + + // methods that affect movement of the vehicle in this mode + void update() override; + +protected: + + bool _enter() override; +}; + +class ModeQAcro : public Mode +{ +public: + + Number mode_number() const override { return Number::QACRO; } + const char *name() const override { return "QACO"; } + const char *name4() const override { return "QACRO"; } + + // methods that affect movement of the vehicle in this mode + void update() override; + +protected: + + bool _enter() override; +}; + +class ModeQAutotune : public Mode +{ +public: + + Number mode_number() const override { return Number::QAUTOTUNE; } + const char *name() const override { return "QAUTOTUNE"; } + const char *name4() const override { return "QATN"; } + + // methods that affect movement of the vehicle in this mode + void update() override; + +protected: + + bool _enter() override; +}; diff --git a/ArduPlane/mode_acro.cpp b/ArduPlane/mode_acro.cpp new file mode 100644 index 0000000000..8b13df6589 --- /dev/null +++ b/ArduPlane/mode_acro.cpp @@ -0,0 +1,29 @@ +#include "mode.h" +#include "Plane.h" + +bool ModeAcro::_enter() +{ + plane.throttle_allows_nudging = false; + plane.auto_throttle_mode = false; + plane.auto_navigation_mode = false; + plane.acro_state.locked_roll = false; + plane.acro_state.locked_pitch = false; + + return true; +} + +void ModeAcro::update() +{ + // handle locked/unlocked control + if (plane.acro_state.locked_roll) { + plane.nav_roll_cd = plane.acro_state.locked_roll_err; + } else { + plane.nav_roll_cd = plane.ahrs.roll_sensor; + } + if (plane.acro_state.locked_pitch) { + plane.nav_pitch_cd = plane.acro_state.locked_pitch_cd; + } else { + plane.nav_pitch_cd = plane.ahrs.pitch_sensor; + } +} + diff --git a/ArduPlane/mode_auto.cpp b/ArduPlane/mode_auto.cpp new file mode 100644 index 0000000000..340ab0c103 --- /dev/null +++ b/ArduPlane/mode_auto.cpp @@ -0,0 +1,80 @@ +#include "mode.h" +#include "Plane.h" + +bool ModeAuto::_enter() +{ + plane.throttle_allows_nudging = true; + plane.auto_throttle_mode = true; + plane.auto_navigation_mode = true; + if (plane.quadplane.available() && plane.quadplane.enable == 2) { + plane.auto_state.vtol_mode = true; + } else { + plane.auto_state.vtol_mode = false; + } + plane.next_WP_loc = plane.prev_WP_loc = plane.current_loc; + // start or resume the mission, based on MIS_AUTORESET + plane.mission.start_or_resume(); + + plane.g2.soaring_controller.init_cruising(); + + return true; +} + +void ModeAuto::_exit() +{ + if (plane.mission.state() == AP_Mission::MISSION_RUNNING) { + plane.mission.stop(); + + if (plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND && + !plane.quadplane.is_vtol_land(plane.mission.get_current_nav_cmd().id)) + { + plane.landing.restart_landing_sequence(); + } + } + plane.auto_state.started_flying_in_auto_ms = 0; +} + +void ModeAuto::update() +{ + if (plane.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 + plane.set_mode(plane.mode_rtl, MODE_REASON_MISSION_END); + gcs().send_text(MAV_SEVERITY_INFO, "Aircraft in auto without a running mission"); + return; + } + + uint16_t nav_cmd_id = plane.mission.get_current_nav_cmd().id; + + if (plane.quadplane.in_vtol_auto()) { + plane.quadplane.control_auto(plane.next_WP_loc); + } else if (nav_cmd_id == MAV_CMD_NAV_TAKEOFF || + (nav_cmd_id == MAV_CMD_NAV_LAND && plane.flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND)) { + plane.takeoff_calc_roll(); + plane.takeoff_calc_pitch(); + plane.calc_throttle(); + } else if (nav_cmd_id == MAV_CMD_NAV_LAND) { + plane.calc_nav_roll(); + plane.calc_nav_pitch(); + + // allow landing to restrict the roll limits + plane.nav_roll_cd = plane.landing.constrain_roll(plane.nav_roll_cd, plane.g.level_roll_limit*100UL); + + if (plane.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 { + plane.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) { + plane.steer_state.hold_course_cd = -1; + } + plane.calc_nav_roll(); + plane.calc_nav_pitch(); + plane.calc_throttle(); + } +} + diff --git a/ArduPlane/mode_autotune.cpp b/ArduPlane/mode_autotune.cpp new file mode 100644 index 0000000000..35efa4bf02 --- /dev/null +++ b/ArduPlane/mode_autotune.cpp @@ -0,0 +1,24 @@ +#include "mode.h" +#include "Plane.h" + +bool ModeAutoTune::_enter() +{ + plane.throttle_allows_nudging = false; + plane.auto_throttle_mode = false; + plane.auto_navigation_mode = false; + plane.autotune_start(); + + return true; +} + +void ModeAutoTune::_exit() +{ + // restore last gains + plane.autotune_restore(); +} + +void ModeAutoTune::update() +{ + plane.mode_fbwa.update(); +} + diff --git a/ArduPlane/mode_avoidADSB.cpp b/ArduPlane/mode_avoidADSB.cpp new file mode 100644 index 0000000000..5336a323e6 --- /dev/null +++ b/ArduPlane/mode_avoidADSB.cpp @@ -0,0 +1,13 @@ +#include "mode.h" +#include "Plane.h" + +bool ModeAvoidADSB::_enter() +{ + return plane.mode_guided.enter(); +} + +void ModeAvoidADSB::update() +{ + plane.mode_guided.update(); +} + diff --git a/ArduPlane/mode_circle.cpp b/ArduPlane/mode_circle.cpp new file mode 100644 index 0000000000..413efea01a --- /dev/null +++ b/ArduPlane/mode_circle.cpp @@ -0,0 +1,26 @@ +#include "mode.h" +#include "Plane.h" + +bool ModeCircle::_enter() +{ + // the altitude to circle at is taken from the current altitude + plane.throttle_allows_nudging = false; + plane.auto_throttle_mode = true; + plane.auto_navigation_mode = true; + plane.next_WP_loc.alt = plane.current_loc.alt; + + return true; +} + +void ModeCircle::update() +{ + // 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 + plane.nav_roll_cd = plane.roll_limit_cd / 3; + plane.update_load_factor(); + plane.calc_nav_pitch(); + plane.calc_throttle(); +} + diff --git a/ArduPlane/mode_cruise.cpp b/ArduPlane/mode_cruise.cpp new file mode 100644 index 0000000000..6c6dc4fd72 --- /dev/null +++ b/ArduPlane/mode_cruise.cpp @@ -0,0 +1,40 @@ +#include "mode.h" +#include "Plane.h" + +bool ModeCruise::_enter() +{ + plane.throttle_allows_nudging = false; + plane.auto_throttle_mode = true; + plane.auto_navigation_mode = false; + plane.cruise_state.locked_heading = false; + plane.cruise_state.lock_timer_ms = 0; + + // for ArduSoar soaring_controller + plane.g2.soaring_controller.init_cruising(); + + plane.set_target_altitude_current(); + + return true; +} + +void ModeCruise::update() +{ + /* + 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 (plane.channel_roll->get_control_in() != 0 || plane.channel_rudder->get_control_in() != 0) { + plane.cruise_state.locked_heading = false; + plane.cruise_state.lock_timer_ms = 0; + } + + if (!plane.cruise_state.locked_heading) { + plane.nav_roll_cd = plane.channel_roll->norm_input() * plane.roll_limit_cd; + plane.update_load_factor(); + } else { + plane.calc_nav_roll(); + } + plane.update_fbwb_speed_height(); +} + diff --git a/ArduPlane/mode_fbwa.cpp b/ArduPlane/mode_fbwa.cpp new file mode 100644 index 0000000000..121d801ec0 --- /dev/null +++ b/ArduPlane/mode_fbwa.cpp @@ -0,0 +1,45 @@ +#include "mode.h" +#include "Plane.h" + +bool ModeFBWA::_enter() +{ + plane.throttle_allows_nudging = false; + plane.auto_throttle_mode = false; + plane.auto_navigation_mode = false; + + return true; +} + +void ModeFBWA::update() +{ + // set nav_roll and nav_pitch using sticks + plane.nav_roll_cd = plane.channel_roll->norm_input() * plane.roll_limit_cd; + plane.update_load_factor(); + float pitch_input = plane.channel_pitch->norm_input(); + if (pitch_input > 0) { + plane.nav_pitch_cd = pitch_input * plane.aparm.pitch_limit_max_cd; + } else { + plane.nav_pitch_cd = -(pitch_input * plane.pitch_limit_min_cd); + } + plane.adjust_nav_pitch_throttle(); + plane.nav_pitch_cd = constrain_int32(plane.nav_pitch_cd, plane.pitch_limit_min_cd, plane.aparm.pitch_limit_max_cd.get()); + if (plane.fly_inverted()) { + plane.nav_pitch_cd = -plane.nav_pitch_cd; + } + if (plane.failsafe.rc_failsafe && plane.g.fs_action_short == FS_ACTION_SHORT_FBWA) { + // FBWA failsafe glide + plane.nav_roll_cd = 0; + plane.nav_pitch_cd = 0; + SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_MIN); + } + if (plane.g.fbwa_tdrag_chan > 0) { + // check for the user enabling FBWA taildrag takeoff mode + bool tdrag_mode = (RC_Channels::get_radio_in(plane.g.fbwa_tdrag_chan-1) > 1700); + if (tdrag_mode && !plane.auto_state.fbwa_tdrag_takeoff_mode) { + if (plane.auto_state.highest_airspeed < plane.g.takeoff_tdrag_speed1) { + plane.auto_state.fbwa_tdrag_takeoff_mode = true; + plane.gcs().send_text(MAV_SEVERITY_WARNING, "FBWA tdrag mode"); + } + } + } +} diff --git a/ArduPlane/mode_fbwb.cpp b/ArduPlane/mode_fbwb.cpp new file mode 100644 index 0000000000..e7f5843877 --- /dev/null +++ b/ArduPlane/mode_fbwb.cpp @@ -0,0 +1,26 @@ +#include "mode.h" +#include "Plane.h" + +bool ModeFBWB::_enter() +{ + plane.throttle_allows_nudging = false; + plane.auto_throttle_mode = true; + plane.auto_navigation_mode = false; + + // for ArduSoar soaring_controller + plane.g2.soaring_controller.init_cruising(); + + plane.set_target_altitude_current(); + + return true; +} + +void ModeFBWB::update() +{ + // Thanks to Yury MonZon for the altitude limit code! + plane.nav_roll_cd = plane.channel_roll->norm_input() * plane.roll_limit_cd; + plane.update_load_factor(); + plane.update_fbwb_speed_height(); + +} + diff --git a/ArduPlane/mode_guided.cpp b/ArduPlane/mode_guided.cpp new file mode 100644 index 0000000000..78cc2508c8 --- /dev/null +++ b/ArduPlane/mode_guided.cpp @@ -0,0 +1,30 @@ +#include "mode.h" +#include "Plane.h" + +bool ModeGuided::_enter() +{ + plane.throttle_allows_nudging = true; + plane.auto_throttle_mode = true; + plane.auto_navigation_mode = true; + plane.guided_throttle_passthru = false; + /* + when entering guided mode we set the target as the current + location. This matches the behaviour of the copter code + */ + plane.guided_WP_loc = plane.current_loc; + plane.set_guided_WP(); + + return true; +} + +void ModeGuided::update() +{ + if (plane.auto_state.vtol_loiter && plane.quadplane.available()) { + plane.quadplane.guided_update(); + } else { + plane.calc_nav_roll(); + plane.calc_nav_pitch(); + plane.calc_throttle(); + } +} + diff --git a/ArduPlane/mode_initializing.cpp b/ArduPlane/mode_initializing.cpp new file mode 100644 index 0000000000..7aa10b8dbe --- /dev/null +++ b/ArduPlane/mode_initializing.cpp @@ -0,0 +1,12 @@ +#include "mode.h" +#include "Plane.h" + +bool ModeInitializing::_enter() +{ + plane.throttle_allows_nudging = true; + plane.auto_throttle_mode = true; + plane.auto_navigation_mode = false; + + return true; +} + diff --git a/ArduPlane/mode_loiter.cpp b/ArduPlane/mode_loiter.cpp new file mode 100644 index 0000000000..a107b53a88 --- /dev/null +++ b/ArduPlane/mode_loiter.cpp @@ -0,0 +1,25 @@ +#include "mode.h" +#include "Plane.h" + +bool ModeLoiter::_enter() +{ + plane.throttle_allows_nudging = true; + plane.auto_throttle_mode = true; + plane.auto_navigation_mode = true; + plane.do_loiter_at_location(); + + if (plane.g2.soaring_controller.is_active() && plane.g2.soaring_controller.suppress_throttle()) { + plane.g2.soaring_controller.init_thermalling(); + plane.g2.soaring_controller.get_target(plane.next_WP_loc); // ahead on flight path + } + + return true; +} + +void ModeLoiter::update() +{ + plane.calc_nav_roll(); + plane.calc_nav_pitch(); + plane.calc_throttle(); +} + diff --git a/ArduPlane/mode_manual.cpp b/ArduPlane/mode_manual.cpp new file mode 100644 index 0000000000..1b27a79e78 --- /dev/null +++ b/ArduPlane/mode_manual.cpp @@ -0,0 +1,26 @@ +#include "mode.h" +#include "Plane.h" + +bool ModeManual::_enter() +{ + plane.throttle_allows_nudging = false; + plane.auto_throttle_mode = false; + plane.auto_navigation_mode = false; + + return true; +} + +void ModeManual::_exit() +{ + if (plane.g.auto_trim > 0) { + plane.trim_radio(); + } +} + +void ModeManual::update() +{ + SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.channel_roll->get_control_in_zero_dz()); + SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.channel_pitch->get_control_in_zero_dz()); + plane.steering_control.steering = plane.steering_control.rudder = plane.channel_rudder->get_control_in_zero_dz(); +} + diff --git a/ArduPlane/mode_qacro.cpp b/ArduPlane/mode_qacro.cpp new file mode 100644 index 0000000000..9962462ea8 --- /dev/null +++ b/ArduPlane/mode_qacro.cpp @@ -0,0 +1,14 @@ +#include "mode.h" +#include "Plane.h" + +bool ModeQAcro::_enter() +{ + //return false; + return plane.mode_qstabilize._enter(); +} + +void ModeQAcro::update() +{ + plane.mode_qstabilize.update(); +} + diff --git a/ArduPlane/mode_qautotune.cpp b/ArduPlane/mode_qautotune.cpp new file mode 100644 index 0000000000..e4df660909 --- /dev/null +++ b/ArduPlane/mode_qautotune.cpp @@ -0,0 +1,13 @@ +#include "mode.h" +#include "Plane.h" + +bool ModeQAutotune::_enter() +{ + return plane.mode_qstabilize._enter(); +} + +void ModeQAutotune::update() +{ + plane.mode_qstabilize.update(); +} + diff --git a/ArduPlane/mode_qhover.cpp b/ArduPlane/mode_qhover.cpp new file mode 100644 index 0000000000..390a2de0ea --- /dev/null +++ b/ArduPlane/mode_qhover.cpp @@ -0,0 +1,14 @@ +#include "mode.h" +#include "Plane.h" + +bool ModeQHover::_enter() +{ + return plane.mode_qstabilize._enter(); +} + +void ModeQHover::update() +{ + plane.mode_qstabilize.update(); +} + + diff --git a/ArduPlane/mode_qland.cpp b/ArduPlane/mode_qland.cpp new file mode 100644 index 0000000000..0d2a272d7a --- /dev/null +++ b/ArduPlane/mode_qland.cpp @@ -0,0 +1,13 @@ +#include "mode.h" +#include "Plane.h" + +bool ModeQLand::_enter() +{ + return plane.mode_qstabilize._enter(); +} + +void ModeQLand::update() +{ + plane.mode_qstabilize.update(); +} + diff --git a/ArduPlane/mode_qloiter.cpp b/ArduPlane/mode_qloiter.cpp new file mode 100644 index 0000000000..eecd918248 --- /dev/null +++ b/ArduPlane/mode_qloiter.cpp @@ -0,0 +1,14 @@ +#include "mode.h" +#include "Plane.h" + +bool ModeQLoiter::_enter() +{ + return plane.mode_qstabilize._enter(); +} + +void ModeQLoiter::update() +{ + plane.mode_qstabilize.update(); +} + + diff --git a/ArduPlane/mode_qrtl.cpp b/ArduPlane/mode_qrtl.cpp new file mode 100644 index 0000000000..3909d7b4e3 --- /dev/null +++ b/ArduPlane/mode_qrtl.cpp @@ -0,0 +1,13 @@ +#include "mode.h" +#include "Plane.h" + +bool ModeQRTL::_enter() +{ + return plane.mode_qstabilize._enter(); +} + +void ModeQRTL::update() +{ + plane.mode_qstabilize.update(); +} + diff --git a/ArduPlane/mode_qstabilize.cpp b/ArduPlane/mode_qstabilize.cpp new file mode 100644 index 0000000000..a87c7b0c7a --- /dev/null +++ b/ArduPlane/mode_qstabilize.cpp @@ -0,0 +1,40 @@ +#include "mode.h" +#include "Plane.h" + +bool ModeQStabilize::_enter() +{ + plane.throttle_allows_nudging = true; + plane.auto_navigation_mode = false; + if (!plane.quadplane.init_mode() && plane.previous_mode != nullptr) { + plane.control_mode = plane.previous_mode; + } else { + plane.auto_throttle_mode = false; + plane.auto_state.vtol_mode = true; + } + + return true; +} + +void ModeQStabilize::update() +{ + // set nav_roll and nav_pitch using sticks + int16_t roll_limit = MIN(plane.roll_limit_cd, plane.quadplane.aparm.angle_max); + plane.nav_roll_cd = (plane.channel_roll->get_control_in() / 4500.0) * roll_limit; + plane.nav_roll_cd = constrain_int32(plane.nav_roll_cd, -roll_limit, roll_limit); + float pitch_input = plane.channel_pitch->norm_input(); + // Scale from normalized input [-1,1] to centidegrees + if (plane.quadplane.tailsitter_active()) { + // For tailsitters, the pitch range is symmetrical: [-Q_ANGLE_MAX,Q_ANGLE_MAX] + plane.nav_pitch_cd = pitch_input * plane.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) { + plane.nav_pitch_cd = pitch_input * MIN(plane.aparm.pitch_limit_max_cd, plane.quadplane.aparm.angle_max); + } else { + plane.nav_pitch_cd = pitch_input * MIN(-plane.pitch_limit_min_cd, plane.quadplane.aparm.angle_max); + } + plane.nav_pitch_cd = constrain_int32(plane.nav_pitch_cd, plane.pitch_limit_min_cd, plane.aparm.pitch_limit_max_cd.get()); + } +} + diff --git a/ArduPlane/mode_rtl.cpp b/ArduPlane/mode_rtl.cpp new file mode 100644 index 0000000000..9ad2370222 --- /dev/null +++ b/ArduPlane/mode_rtl.cpp @@ -0,0 +1,21 @@ +#include "mode.h" +#include "Plane.h" + +bool ModeRTL::_enter() +{ + plane.throttle_allows_nudging = true; + plane.auto_throttle_mode = true; + plane.auto_navigation_mode = true; + plane.prev_WP_loc = plane.current_loc; + plane.do_RTL(plane.get_RTL_altitude()); + + return true; +} + +void ModeRTL::update() +{ + plane.calc_nav_roll(); + plane.calc_nav_pitch(); + plane.calc_throttle(); +} + diff --git a/ArduPlane/mode_stabilize.cpp b/ArduPlane/mode_stabilize.cpp new file mode 100644 index 0000000000..27ecff0d2e --- /dev/null +++ b/ArduPlane/mode_stabilize.cpp @@ -0,0 +1,18 @@ +#include "mode.h" +#include "Plane.h" + +bool ModeStabilize::_enter() +{ + plane.throttle_allows_nudging = false; + plane.auto_throttle_mode = false; + plane.auto_navigation_mode = false; + + return true; +} + +void ModeStabilize::update() +{ + plane.nav_roll_cd = 0; + plane.nav_pitch_cd = 0; +} + diff --git a/ArduPlane/mode_training.cpp b/ArduPlane/mode_training.cpp new file mode 100644 index 0000000000..f39eb3611a --- /dev/null +++ b/ArduPlane/mode_training.cpp @@ -0,0 +1,44 @@ +#include "mode.h" +#include "Plane.h" + +bool ModeTraining::_enter() +{ + plane.throttle_allows_nudging = false; + plane.auto_throttle_mode = false; + plane.auto_navigation_mode = false; + + return true; +} + +void ModeTraining::update() +{ + plane.training_manual_roll = false; + plane.training_manual_pitch = false; + plane.update_load_factor(); + + // if the roll is past the set roll limit, then + // we set target roll to the limit + if (plane.ahrs.roll_sensor >= plane.roll_limit_cd) { + plane.nav_roll_cd = plane.roll_limit_cd; + } else if (plane.ahrs.roll_sensor <= -plane.roll_limit_cd) { + plane.nav_roll_cd = -plane.roll_limit_cd; + } else { + plane.training_manual_roll = true; + plane.nav_roll_cd = 0; + } + + // if the pitch is past the set pitch limits, then + // we set target pitch to the limit + if (plane.ahrs.pitch_sensor >= plane.aparm.pitch_limit_max_cd) { + plane.nav_pitch_cd = plane.aparm.pitch_limit_max_cd; + } else if (plane.ahrs.pitch_sensor <= plane.pitch_limit_min_cd) { + plane.nav_pitch_cd = plane.pitch_limit_min_cd; + } else { + plane.training_manual_pitch = true; + plane.nav_pitch_cd = 0; + } + if (plane.fly_inverted()) { + plane.nav_pitch_cd = -plane.nav_pitch_cd; + } +} + diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index a75ca8cfa1..361be7e253 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -108,7 +108,7 @@ void Plane::calc_airspeed_errors() ahrs.airspeed_estimate(&airspeed_measured); // FBW_B/cruise airspeed target - if (!failsafe.rc_failsafe && (control_mode == FLY_BY_WIRE_B || control_mode == CRUISE)) { + if (!failsafe.rc_failsafe && (control_mode == &mode_fbwb || control_mode == &mode_cruise)) { if (g2.flight_options & FlightOptions::CRUISE_TRIM_AIRSPEED) { target_airspeed_cm = aparm.airspeed_cruise_cm; } else if (g2.flight_options & FlightOptions::CRUISE_TRIM_THROTTLE) { @@ -141,7 +141,7 @@ void Plane::calc_airspeed_errors() } else if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) { // Landing airspeed target target_airspeed_cm = landing.get_target_airspeed_cm(); - } else if ((control_mode == AUTO) && + } else if ((control_mode == &mode_auto) && (quadplane.options & QuadPlane::OPTION_MISSION_LAND_FW_APPROACH) && ((vtol_approach_s.approach_stage == Landing_ApproachStage::APPROACH_LINE) || (vtol_approach_s.approach_stage == Landing_ApproachStage::VTOL_LANDING))) { @@ -162,7 +162,7 @@ void Plane::calc_airspeed_errors() // above. if (auto_throttle_mode && aparm.min_gndspeed_cm > 0 && - control_mode != CIRCLE) { + control_mode != &mode_circle) { int32_t min_gnd_target_airspeed = airspeed_measured*100 + groundspeed_undershoot; if (min_gnd_target_airspeed > target_airspeed_cm) { target_airspeed_cm = min_gnd_target_airspeed; @@ -223,10 +223,10 @@ void Plane::update_loiter(uint16_t radius) quadplane.guided_start(); } } else if ((loiter.start_time_ms == 0 && - (control_mode == AUTO || control_mode == GUIDED) && + (control_mode == &mode_auto || control_mode == &mode_guided) && auto_state.crosstrack && current_loc.get_distance(next_WP_loc) > radius*3) || - (control_mode == RTL && quadplane.available() && quadplane.rtl_mode == 1)) { + (control_mode == &mode_rtl && quadplane.available() && quadplane.rtl_mode == 1)) { /* if never reached loiter point and using crosstrack and somewhat far away from loiter point navigate to it like in auto-mode for normal crosstrack behavior @@ -245,7 +245,7 @@ void Plane::update_loiter(uint16_t radius) auto_state.wp_proportion > 1) { // we've reached the target, start the timer loiter.start_time_ms = millis(); - if (control_mode == GUIDED || control_mode == AVOID_ADSB) { + if (control_mode == &mode_guided || control_mode == &mode_avoidADSB) { // starting a loiter in GUIDED means we just reached the target point gcs().send_mission_item_reached_message(0); } diff --git a/ArduPlane/qautotune.cpp b/ArduPlane/qautotune.cpp index 41c10093a4..cbdb14973c 100644 --- a/ArduPlane/qautotune.cpp +++ b/ArduPlane/qautotune.cpp @@ -13,7 +13,7 @@ bool QAutoTune::init() } // use position hold while tuning if we were in QLOITER - bool position_hold = (plane.previous_mode == QLOITER); + bool position_hold = (plane.previous_mode == &plane.mode_qloiter); return init_internals(position_hold, plane.quadplane.attitude_control, diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index adb25bc56b..ab02b516db 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1022,7 +1022,7 @@ bool QuadPlane::is_flying(void) if (!available()) { return false; } - if (plane.control_mode == GUIDED && guided_takeoff) { + if (plane.control_mode == &plane.mode_guided && guided_takeoff) { return true; } if (motors->get_throttle() > 0.01f && !motors->limit.throttle_lower) { @@ -1069,13 +1069,13 @@ bool QuadPlane::is_flying_vtol(void) const // if we are demanding more than 1% throttle then don't consider aircraft landed return true; } - if (plane.control_mode == QACRO) { + if (plane.control_mode == &plane.mode_qacro) { return true; } - if (plane.control_mode == GUIDED && guided_takeoff) { + if (plane.control_mode == &plane.mode_guided && guided_takeoff) { return true; } - if (plane.control_mode == QSTABILIZE || plane.control_mode == QHOVER || plane.control_mode == QLOITER || plane.control_mode == QAUTOTUNE) { + if (plane.control_mode == &plane.mode_qstabilize || plane.control_mode == &plane.mode_qhover || plane.control_mode == &plane.mode_qloiter || plane.control_mode == &plane.mode_qautotune) { // in manual flight modes only consider aircraft landed when pilot demanded throttle is zero return plane.get_throttle_input() > 0; } @@ -1160,7 +1160,7 @@ void QuadPlane::control_loiter() plane.nav_pitch_cd, get_desired_yaw_rate_cds()); - if (plane.control_mode == QLAND) { + if (plane.control_mode == &plane.mode_qland) { if (poscontrol.state < QPOS_LAND_FINAL && check_land_final()) { poscontrol.state = QPOS_LAND_FINAL; // cut IC engine if enabled @@ -1172,7 +1172,7 @@ void QuadPlane::control_loiter() float descent_rate = (poscontrol.state == QPOS_LAND_FINAL)? land_speed_cms:landing_descent_rate_cms(height_above_ground); pos_control->set_alt_target_from_climb_rate(-descent_rate, plane.G_Dt, true); check_land_complete(); - } else if (plane.control_mode == GUIDED && guided_takeoff) { + } else if (plane.control_mode == &plane.mode_guided && guided_takeoff) { pos_control->set_alt_target_from_climb_rate_ff(0, plane.G_Dt, false); } else { // update altitude target and call position controller @@ -1362,9 +1362,9 @@ bool QuadPlane::assistance_needed(float aspeed) */ void QuadPlane::update_transition(void) { - if (plane.control_mode == MANUAL || - plane.control_mode == ACRO || - plane.control_mode == TRAINING) { + if (plane.control_mode == &plane.mode_manual || + plane.control_mode == &plane.mode_acro || + plane.control_mode == &plane.mode_training) { // in manual modes quad motors are always off if (!tilt.motors_active && !is_tailsitter()) { motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); @@ -1387,7 +1387,7 @@ void QuadPlane::update_transition(void) (transition_failure > 0) && ((now - transition_start_ms) > ((uint32_t)transition_failure * 1000))) { gcs().send_text(MAV_SEVERITY_CRITICAL, "Transition failed, exceeded time limit"); - plane.set_mode(QLAND, MODE_REASON_VTOL_FAILED_TRANSITION); + plane.set_mode(plane.mode_qland, MODE_REASON_VTOL_FAILED_TRANSITION); } float aspeed; @@ -1724,7 +1724,7 @@ void QuadPlane::update_throttle_suppression(void) } // allow for takeoff - if (plane.control_mode == AUTO && is_vtol_takeoff(plane.mission.get_current_nav_cmd().id)) { + if (plane.control_mode == &plane.mode_auto && is_vtol_takeoff(plane.mission.get_current_nav_cmd().id)) { return; } @@ -1776,7 +1776,7 @@ void QuadPlane::motors_output(bool run_rate_controller) motors->output(); return; } - if (esc_calibration && AP_Notify::flags.esc_calibration && plane.control_mode == QSTABILIZE) { + if (esc_calibration && AP_Notify::flags.esc_calibration && plane.control_mode == &plane.mode_qstabilize) { // output is direct from run_esc_calibration() return; } @@ -1820,27 +1820,27 @@ void QuadPlane::control_run(void) return; } - switch (plane.control_mode) { - case QACRO: + switch (plane.control_mode->mode_number()) { + case Mode::Number::QACRO: control_qacro(); // QACRO uses only the multicopter controller // so skip the Plane attitude control calls below return; - case QSTABILIZE: + case Mode::Number::QSTABILIZE: control_stabilize(); break; - case QHOVER: + case Mode::Number::QHOVER: control_hover(); break; - case QLOITER: - case QLAND: + case Mode::Number::QLOITER: + case Mode::Number::QLAND: control_loiter(); break; - case QRTL: + case Mode::Number::QRTL: control_qrtl(); break; #if QAUTOTUNE_ENABLED - case QAUTOTUNE: + case Mode::Number::QAUTOTUNE: qautotune.run(); break; #endif @@ -1868,30 +1868,30 @@ bool QuadPlane::init_mode(void) AP_Notify::flags.esc_calibration = false; - switch (plane.control_mode) { - case QSTABILIZE: + switch (plane.control_mode->mode_number()) { + case Mode::Number::QSTABILIZE: init_stabilize(); break; - case QHOVER: + case Mode::Number::QHOVER: init_hover(); break; - case QLOITER: + case Mode::Number::QLOITER: init_loiter(); break; - case QLAND: + case Mode::Number::QLAND: init_qland(); break; - case QRTL: + case Mode::Number::QRTL: init_qrtl(); break; - case GUIDED: + case Mode::Number::GUIDED: guided_takeoff = false; break; #if QAUTOTUNE_ENABLED - case QAUTOTUNE: + case Mode::Number::QAUTOTUNE: return qautotune.init(); #endif - case QACRO: + case Mode::Number::QACRO: init_qacro(); break; default: @@ -1909,7 +1909,7 @@ bool QuadPlane::handle_do_vtol_transition(enum MAV_VTOL_STATE state) gcs().send_text(MAV_SEVERITY_NOTICE, "VTOL not available"); return false; } - if (plane.control_mode != AUTO) { + if (plane.control_mode != &plane.mode_auto) { gcs().send_text(MAV_SEVERITY_NOTICE, "VTOL transition only in AUTO"); return false; } @@ -1945,7 +1945,7 @@ bool QuadPlane::in_vtol_auto(void) const if (!available()) { return false; } - if (plane.control_mode != AUTO) { + if (plane.control_mode != &plane.mode_auto) { return false; } if (plane.auto_state.vtol_mode) { @@ -1978,14 +1978,14 @@ bool QuadPlane::in_vtol_mode(void) const if (!available()) { return false; } - return (plane.control_mode == QSTABILIZE || - plane.control_mode == QHOVER || - plane.control_mode == QLOITER || - plane.control_mode == QLAND || - plane.control_mode == QRTL || - plane.control_mode == QACRO || - plane.control_mode == QAUTOTUNE || - ((plane.control_mode == GUIDED || plane.control_mode == AVOID_ADSB) && plane.auto_state.vtol_loiter) || + return (plane.control_mode == &plane.mode_qstabilize || + plane.control_mode == &plane.mode_qhover || + plane.control_mode == &plane.mode_qloiter || + plane.control_mode == &plane.mode_qland || + plane.control_mode == &plane.mode_qrtl || + plane.control_mode == &plane.mode_qacro || + plane.control_mode == &plane.mode_qautotune || + ((plane.control_mode == &plane.mode_guided || plane.control_mode == &plane.mode_avoidADSB) && plane.auto_state.vtol_loiter) || in_vtol_auto()); } @@ -2146,7 +2146,7 @@ void QuadPlane::vtol_position_controller(void) case QPOS_POSITION1: case QPOS_POSITION2: { bool vtol_loiter_auto = false; - if (plane.control_mode == AUTO) { + if (plane.control_mode == &plane.mode_auto) { switch (plane.mission.get_current_nav_cmd().id) { case MAV_CMD_NAV_LOITER_UNLIM: case MAV_CMD_NAV_LOITER_TIME: @@ -2156,7 +2156,7 @@ void QuadPlane::vtol_position_controller(void) break; } } - if (plane.control_mode == QRTL || plane.control_mode == GUIDED || vtol_loiter_auto) { + if (plane.control_mode == &plane.mode_qrtl || plane.control_mode == &plane.mode_guided || vtol_loiter_auto) { plane.ahrs.get_position(plane.current_loc); float target_altitude = plane.next_WP_loc.alt; if (poscontrol.slow_descent) { @@ -2531,7 +2531,7 @@ bool QuadPlane::verify_vtol_land(void) plane.auto_state.wp_distance < 2) { poscontrol.state = QPOS_LAND_DESCEND; gcs().send_text(MAV_SEVERITY_INFO,"Land descend started"); - if (plane.control_mode == AUTO) { + if (plane.control_mode == &plane.mode_auto) { // set height to mission height, so we can use the mission // WP height for triggering land final if no rangefinder // available @@ -2562,7 +2562,7 @@ void QuadPlane::Log_Write_QControl_Tuning() { float des_alt_m = 0.0f; int16_t target_climb_rate_cms = 0; - if (plane.control_mode != QSTABILIZE) { + if (plane.control_mode != &plane.mode_qstabilize) { des_alt_m = pos_control->get_alt_target() / 100.0f; target_climb_rate_cms = pos_control->get_vel_target_z(); } @@ -2605,9 +2605,9 @@ int8_t QuadPlane::forward_throttle_pct(void) if (!in_vtol_mode() || !motors->armed() || vel_forward.gain <= 0 || - plane.control_mode == QSTABILIZE || - plane.control_mode == QHOVER || - plane.control_mode == QAUTOTUNE || + plane.control_mode == &plane.mode_qstabilize || + plane.control_mode == &plane.mode_qhover || + plane.control_mode == &plane.mode_qautotune || motors->get_desired_spool_state() < AP_Motors::DESIRED_GROUND_IDLE) { return 0; } @@ -2694,9 +2694,9 @@ float QuadPlane::get_weathervane_yaw_rate_cds(void) if (!in_vtol_mode() || !motors->armed() || weathervane.gain <= 0 || - plane.control_mode == QSTABILIZE || - plane.control_mode == QHOVER || - plane.control_mode == QAUTOTUNE) { + plane.control_mode == &plane.mode_qstabilize || + plane.control_mode == &plane.mode_qhover || + plane.control_mode == &plane.mode_qautotune) { weathervane.last_output = 0; return 0; } @@ -2750,7 +2750,7 @@ void QuadPlane::guided_start(void) */ void QuadPlane::guided_update(void) { - if (plane.control_mode == GUIDED && guided_takeoff && plane.current_loc.alt < plane.next_WP_loc.alt) { + if (plane.control_mode == &plane.mode_guided && guided_takeoff && plane.current_loc.alt < plane.next_WP_loc.alt) { throttle_wait = false; motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); takeoff_controller(); @@ -2778,7 +2778,7 @@ bool QuadPlane::guided_mode_enabled(void) return false; } // only use quadplane guided when in AUTO or GUIDED mode - if (plane.control_mode != GUIDED && plane.control_mode != AUTO) { + if (plane.control_mode != &plane.mode_guided && plane.control_mode != &plane.mode_auto) { return false; } return guided_mode != 0; @@ -2806,7 +2806,7 @@ void QuadPlane::adjust_alt_target(float altitude_cm) // user initiated takeoff for guided mode bool QuadPlane::do_user_takeoff(float takeoff_altitude) { - if (plane.control_mode != GUIDED) { + if (plane.control_mode != &plane.mode_guided) { gcs().send_text(MAV_SEVERITY_INFO, "User Takeoff only in GUIDED mode"); return false; } @@ -2831,7 +2831,7 @@ bool QuadPlane::do_user_takeoff(float takeoff_altitude) // return true if the wp_nav controller is being updated bool QuadPlane::using_wp_nav(void) const { - return plane.control_mode == QLOITER || plane.control_mode == QLAND || plane.control_mode == QRTL; + return plane.control_mode == &plane.mode_qloiter || plane.control_mode == &plane.mode_qland || plane.control_mode == &plane.mode_qrtl; } /* @@ -2917,7 +2917,7 @@ void QuadPlane::update_throttle_thr_mix(void) return; } - if (plane.control_mode == QSTABILIZE) { + if (plane.control_mode == &plane.mode_qstabilize) { // manual throttle if (plane.get_throttle_input() <= 0) { attitude_control->set_throttle_mix_min(); diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 9d5d3d7300..659639e960 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -25,6 +25,17 @@ public: friend class QAutoTune; friend class AP_Arming_Plane; + friend class Mode; + friend class ModeAuto; + friend class ModeAvoidADSB; + friend class ModeGuided; + friend class ModeQHover; + friend class ModeQLand; + friend class ModeQLoiter; + friend class ModeQRTL; + friend class ModeQStabilize; + friend class ModeQAutotune; + QuadPlane(AP_AHRS_NavEKF &_ahrs); // var_info for holding Parameter information diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index 16e1ca0f68..21f89d1e49 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -128,7 +128,7 @@ void Plane::rudder_arm_disarm_check() // if not in a manual throttle mode and not in CRUISE or FBWB // modes then disallow rudder arming/disarming if (auto_throttle_mode && - (control_mode != CRUISE && control_mode != FLY_BY_WIRE_B)) { + (control_mode != &mode_cruise && control_mode != &mode_fbwb)) { rudder_arm_timer = 0; return; } @@ -189,7 +189,7 @@ void Plane::read_radio() failsafe.last_valid_rc_ms = millis(); - if (control_mode == TRAINING) { + if (control_mode == &mode_training) { // in training mode we don't want to use a deadzone, as we // want manual pass through when not exceeding attitude limits channel_roll->recompute_pwm_no_deadzone(); @@ -218,7 +218,7 @@ void Plane::read_radio() quadplane.tailsitter_check_input(); // check for transmitter tuning changes - tuning.check_input(control_mode); + tuning.check_input(control_mode->mode_number()); } int16_t Plane::rudder_input(void) @@ -230,7 +230,7 @@ int16_t Plane::rudder_input(void) } if ((g2.flight_options & FlightOptions::DIRECT_RUDDER_ONLY) && - !(control_mode == MANUAL || control_mode == STABILIZE || control_mode == ACRO)) { + !(control_mode == &mode_manual || control_mode == &mode_stabilize || control_mode == &mode_acro)) { // the user does not want any input except in these modes return 0; } @@ -258,13 +258,14 @@ void Plane::control_failsafe() channel_pitch->set_control_in(0); channel_rudder->set_control_in(0); - switch (control_mode) { - case QSTABILIZE: - case QHOVER: - case QLOITER: - case QLAND: // throttle is ignored, but reset anyways - case QRTL: // throttle is ignored, but reset anyways - case QAUTOTUNE: + switch (control_mode->mode_number()) { + case Mode::Number::QSTABILIZE: + case Mode::Number::QHOVER: + case Mode::Number::QLOITER: + case Mode::Number::QLAND: // throttle is ignored, but reset anyways + case Mode::Number::QRTL: // throttle is ignored, but reset anyways + case Mode::Number::QACRO: + case Mode::Number::QAUTOTUNE: if (quadplane.available() && quadplane.motors->get_desired_spool_state() > AP_Motors::DESIRED_GROUND_IDLE) { // set half throttle to avoid descending at maximum rate, still has a slight descent due to throttle deadzone channel_throttle->set_control_in(channel_throttle->get_range() / 2); diff --git a/ArduPlane/reverse_thrust.cpp b/ArduPlane/reverse_thrust.cpp index f364f360e5..0414456886 100644 --- a/ArduPlane/reverse_thrust.cpp +++ b/ArduPlane/reverse_thrust.cpp @@ -29,8 +29,8 @@ bool Plane::allow_reverse_thrust(void) const return false; } - switch (control_mode) { - case AUTO: + switch (control_mode->mode_number()) { + case Mode::Number::AUTO: { uint16_t nav_cmd = mission.get_current_nav_cmd().id; @@ -64,23 +64,23 @@ bool Plane::allow_reverse_thrust(void) const } break; - case LOITER: + case Mode::Number::LOITER: allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_LOITER); break; - case RTL: + case Mode::Number::RTL: allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_RTL); break; - case CIRCLE: + case Mode::Number::CIRCLE: allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_CIRCLE); break; - case CRUISE: + case Mode::Number::CRUISE: allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_CRUISE); break; - case FLY_BY_WIRE_B: + case Mode::Number::FLY_BY_WIRE_B: allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_FBWB); break; - case AVOID_ADSB: - case GUIDED: + case Mode::Number::AVOID_ADSB: + case Mode::Number::GUIDED: allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_GUIDED); break; default: diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 740af2caf2..7125a84685 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -25,7 +25,7 @@ void Plane::throttle_slew_limit(SRV_Channel::Aux_servo_function_t func) { uint8_t slewrate = aparm.throttle_slewrate; - if (control_mode==AUTO) { + if (control_mode == &mode_auto) { if (auto_state.takeoff_complete == false && g.takeoff_throttle_slewrate != 0) { slewrate = g.takeoff_throttle_slewrate; } else if (landing.get_throttle_slewrate() != 0 && flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) { @@ -75,14 +75,14 @@ bool Plane::suppress_throttle(void) return false; } - if (control_mode==AUTO && g.auto_fbw_steer == 42) { + if (control_mode == &mode_auto && g.auto_fbw_steer == 42) { // user has throttle control return false; } bool gps_movement = (gps.status() >= AP_GPS::GPS_OK_FIX_2D && gps.ground_speed() >= 5); - if (control_mode==AUTO && + if (control_mode == &mode_auto && auto_state.takeoff_complete == false) { uint32_t launch_duration_ms = ((int32_t)g.takeoff_throttle_delay)*100 + 2000; @@ -391,11 +391,11 @@ void Plane::set_servos_controlled(void) // manual pass through of throttle while throttle is suppressed SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input(true)); } - } else if (control_mode == STABILIZE || - control_mode == TRAINING || - control_mode == ACRO || - control_mode == FLY_BY_WIRE_A || - control_mode == AUTOTUNE) { + } else if (control_mode == &mode_stabilize || + control_mode == &mode_training || + control_mode == &mode_acro || + control_mode == &mode_fbwa || + control_mode == &mode_autotune) { // a manual throttle mode if (failsafe.throttle_counter) { SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0); @@ -406,7 +406,7 @@ void Plane::set_servos_controlled(void) } else { SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in()); } - } else if ((control_mode == GUIDED || control_mode == AVOID_ADSB) && + } else if ((control_mode == &mode_guided || control_mode == &mode_avoidADSB) && guided_throttle_passthru) { // manual pass through of throttle while in GUIDED SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input(true)); @@ -418,8 +418,8 @@ void Plane::set_servos_controlled(void) #if SOARING_ENABLED == ENABLED // suppress throttle when soaring is active - if ((control_mode == FLY_BY_WIRE_B || control_mode == CRUISE || - control_mode == AUTO || control_mode == LOITER) && + if ((control_mode == &mode_fbwb || control_mode == &mode_cruise || + control_mode == &mode_auto || control_mode == &mode_loiter) && g2.soaring_controller.is_active() && g2.soaring_controller.get_throttle_suppressed()) { SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0); @@ -464,7 +464,7 @@ void Plane::set_servos_flaps(void) better than speed based flaps as it leads to less possibility of oscillation */ - if (control_mode == AUTO) { + if (control_mode == &mode_auto) { switch (flight_stage) { case AP_Vehicle::FixedWing::FLIGHT_TAKEOFF: case AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND: @@ -521,7 +521,7 @@ void Plane::change_landing_gear(AP_LandingGear::LandingGearCommand cmd) */ void Plane::set_landing_gear(void) { - if (control_mode == AUTO && hal.util->get_soft_armed() && is_flying()) { + if (control_mode == &mode_auto && hal.util->get_soft_armed() && is_flying()) { AP_LandingGear::LandingGearCommand cmd = (AP_LandingGear::LandingGearCommand)gear.last_auto_cmd; switch (flight_stage) { case AP_Vehicle::FixedWing::FLIGHT_LAND: @@ -634,7 +634,7 @@ void Plane::set_servos(void) // do any transition updates for quadplane quadplane.update(); - if (control_mode == AUTO && auto_state.idle_mode) { + if (control_mode == &mode_auto && auto_state.idle_mode) { // special handling for balloon launch set_servos_idle(); servos_output(); @@ -659,14 +659,14 @@ void Plane::set_servos(void) // clear ground_steering to ensure manual control if the yaw stabilizer doesn't run steering_control.ground_steering = false; - if (control_mode == TRAINING) { + if (control_mode == &mode_training) { steering_control.rudder = channel_rudder->get_control_in(); } SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, steering_control.rudder); SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering_control.steering); - if (control_mode == MANUAL) { + if (control_mode == &mode_manual) { set_servos_manual_passthrough(); } else { set_servos_controlled(); @@ -726,7 +726,7 @@ void Plane::set_servos(void) #endif if (landing.get_then_servos_neutral() > 0 && - control_mode == AUTO && + control_mode == &mode_auto && landing.get_disarm_delay() > 0 && landing.is_complete() && !arming.is_armed()) { @@ -776,7 +776,7 @@ void Plane::servos_output(void) servo_output_mixers(); // support MANUAL_RCMASK - if (g2.manual_rc_mask.get() != 0 && control_mode == MANUAL) { + if (g2.manual_rc_mask.get() != 0 && control_mode == &mode_manual) { SRV_Channels::copy_radio_in_out_mask(uint16_t(g2.manual_rc_mask.get())); } @@ -804,7 +804,7 @@ void Plane::update_throttle_hover() { void Plane::servos_auto_trim(void) { // only in auto modes and FBWA - if (!auto_throttle_mode && control_mode != FLY_BY_WIRE_A) { + if (!auto_throttle_mode && control_mode != &mode_fbwa) { return; } if (!hal.util->get_soft_armed()) { diff --git a/ArduPlane/soaring.cpp b/ArduPlane/soaring.cpp index b398a116cc..b025d98334 100644 --- a/ArduPlane/soaring.cpp +++ b/ArduPlane/soaring.cpp @@ -16,18 +16,18 @@ void Plane::update_soaring() { g2.soaring_controller.update_vario(); // Check for throttle suppression change. - switch (control_mode){ - case AUTO: + switch (control_mode->mode_number()) { + case Mode::Number::AUTO: g2.soaring_controller.suppress_throttle(); break; - case FLY_BY_WIRE_B: - case CRUISE: + case Mode::Number::FLY_BY_WIRE_B: + case Mode::Number::CRUISE: if (!g2.soaring_controller.suppress_throttle()) { gcs().send_text(MAV_SEVERITY_INFO, "Soaring: forcing RTL"); - set_mode(RTL, MODE_REASON_SOARING_FBW_B_WITH_MOTOR_RUNNING); + set_mode(mode_rtl, MODE_REASON_SOARING_FBW_B_WITH_MOTOR_RUNNING); } break; - case LOITER: + case Mode::Number::LOITER: // Do nothing. We will switch back to auto/rtl before enabling throttle. break; default: @@ -42,44 +42,44 @@ void Plane::update_soaring() { return; } - switch (control_mode){ - case AUTO: - case FLY_BY_WIRE_B: - case CRUISE: + switch (control_mode->mode_number()) { + case Mode::Number::AUTO: + case Mode::Number::FLY_BY_WIRE_B: + case Mode::Number::CRUISE: // Test for switch into thermalling mode g2.soaring_controller.update_cruising(); if (g2.soaring_controller.check_thermal_criteria()) { gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal detected, entering loiter"); - set_mode(LOITER, MODE_REASON_SOARING_THERMAL_DETECTED); + set_mode(mode_loiter, MODE_REASON_SOARING_THERMAL_DETECTED); } break; - case LOITER: + case Mode::Number::LOITER: // Update thermal estimate and check for switch back to AUTO g2.soaring_controller.update_thermalling(); // Update estimate if (g2.soaring_controller.check_cruise_criteria()) { // Exit as soon as thermal state estimate deteriorates - switch (previous_mode) { - case FLY_BY_WIRE_B: + switch (previous_mode->mode_number()) { + case Mode::Number::FLY_BY_WIRE_B: gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal ended, entering RTL"); - set_mode(RTL, MODE_REASON_SOARING_THERMAL_ESTIMATE_DETERIORATED); + set_mode(mode_rtl, MODE_REASON_SOARING_THERMAL_ESTIMATE_DETERIORATED); break; - case CRUISE: { + case Mode::Number::CRUISE: { // return to cruise with old ground course CruiseState cruise = cruise_state; gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal ended, restoring CRUISE"); - set_mode(CRUISE, MODE_REASON_SOARING_THERMAL_ESTIMATE_DETERIORATED); + set_mode(mode_cruise, MODE_REASON_SOARING_THERMAL_ESTIMATE_DETERIORATED); cruise_state = cruise; set_target_altitude_current(); break; } - case AUTO: + case Mode::Number::AUTO: gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal ended, restoring AUTO"); - set_mode(AUTO, MODE_REASON_SOARING_THERMAL_ESTIMATE_DETERIORATED); + set_mode(mode_auto, MODE_REASON_SOARING_THERMAL_ESTIMATE_DETERIORATED); break; default: diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 35a55833c3..304ba7ff68 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -74,7 +74,7 @@ void Plane::init_ardupilot() // initialise notify system notify.init(); - notify_flight_mode(control_mode); + notify_mode(*control_mode); init_rc_out_main(); @@ -161,7 +161,7 @@ void Plane::init_ardupilot() // choose the nav controller set_nav_controller(); - set_mode((FlightMode)g.initial_mode.get(), MODE_REASON_UNKNOWN); + set_mode_by_number((enum Mode::Number)g.initial_mode.get(), MODE_REASON_UNKNOWN); // set the correct flight mode // --------------------------- @@ -188,7 +188,7 @@ void Plane::init_ardupilot() //******************************************************************************** void Plane::startup_ground(void) { - set_mode(INITIALISING, MODE_REASON_UNKNOWN); + set_mode(mode_initializing, MODE_REASON_UNKNOWN); #if (GROUND_START_DELAY > 0) gcs().send_text(MAV_SEVERITY_NOTICE,"Ground start with delay"); @@ -234,62 +234,37 @@ void Plane::startup_ground(void) gcs().send_text(MAV_SEVERITY_INFO,"Ground start complete"); } -enum FlightMode Plane::get_previous_mode() { - return previous_mode; -} -void Plane::set_mode(enum FlightMode mode, mode_reason_t reason) +bool Plane::set_mode(Mode &new_mode, const mode_reason_t reason) { #if !QAUTOTUNE_ENABLED - if (mode == QAUTOTUNE) { + if (&new_mode == plane.mode_qautotune) { gcs().send_text(MAV_SEVERITY_INFO,"QAUTOTUNE disabled"); - mode = QHOVER; + new_mode = plane.mode_qhover; } #endif - if(control_mode == mode) { + if (control_mode == &new_mode) { // don't switch modes if we are already in the correct mode. - return; + return true; } - if(g.auto_trim > 0 && control_mode == MANUAL) { - trim_radio(); - } + // backup current control_mode and previous_mode + Mode &old_previous_mode = *previous_mode; + Mode &old_mode = *control_mode; - // perform any cleanup required for prev flight mode - exit_mode(control_mode); - - // cancel inverted flight - auto_state.inverted_flight = false; - - // don't cross-track when starting a mission - auto_state.next_wp_crosstrack = false; - - // reset landing check - auto_state.checked_for_autoland = false; - - // zero locked course - steer_state.locked_course_err = 0; - - // reset crash detection - crash_state.is_crashed = false; - crash_state.impact_detected = false; - - // reset external attitude guidance - guided_state.last_forced_rpy_ms.zero(); - guided_state.last_forced_throttle_ms = 0; - - // set mode + // update control_mode assuming success + // TODO: move these to be after enter() once start_command_callback() no longer checks control_mode previous_mode = control_mode; - control_mode = mode; + control_mode = &new_mode; previous_mode_reason = control_mode_reason; control_mode_reason = reason; #if CAMERA == ENABLED - camera.set_is_auto_mode(control_mode == AUTO); + camera.set_is_auto_mode(control_mode == &mode_auto); #endif - if (previous_mode == AUTOTUNE && control_mode != AUTOTUNE) { + if (previous_mode == &mode_autotune && control_mode != &mode_autotune) { // restore last gains autotune_restore(); } @@ -304,200 +279,45 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason) // start with previous WP at current location prev_WP_loc = current_loc; - // new mode means new loiter - loiter.start_time_ms = 0; + // attempt to enter new mode + if (!new_mode.enter()) { + // Log error that we failed to enter desired flight mode + gcs().send_text(MAV_SEVERITY_WARNING, "Flight mode change failed"); - // record time of mode change - last_mode_change_ms = AP_HAL::millis(); - - // assume non-VTOL mode - auto_state.vtol_mode = false; - auto_state.vtol_loiter = false; - - switch(control_mode) - { - case INITIALISING: - throttle_allows_nudging = true; - auto_throttle_mode = true; - auto_navigation_mode = false; - break; - - case MANUAL: - case STABILIZE: - case TRAINING: - case FLY_BY_WIRE_A: - throttle_allows_nudging = false; - auto_throttle_mode = false; - auto_navigation_mode = false; - break; - - case AUTOTUNE: - throttle_allows_nudging = false; - auto_throttle_mode = false; - auto_navigation_mode = false; - autotune_start(); - break; - - case ACRO: - throttle_allows_nudging = false; - auto_throttle_mode = false; - auto_navigation_mode = false; - acro_state.locked_roll = false; - acro_state.locked_pitch = false; - break; - - case CRUISE: - throttle_allows_nudging = false; - auto_throttle_mode = true; - auto_navigation_mode = false; - cruise_state.locked_heading = false; - cruise_state.lock_timer_ms = 0; - -#if SOARING_ENABLED == ENABLED - // for ArduSoar soaring_controller - g2.soaring_controller.init_cruising(); -#endif - - set_target_altitude_current(); - break; - - case FLY_BY_WIRE_B: - throttle_allows_nudging = false; - auto_throttle_mode = true; - auto_navigation_mode = false; - -#if SOARING_ENABLED == ENABLED - // for ArduSoar soaring_controller - g2.soaring_controller.init_cruising(); -#endif - - set_target_altitude_current(); - break; - - case CIRCLE: - // the altitude to circle at is taken from the current altitude - throttle_allows_nudging = false; - auto_throttle_mode = true; - auto_navigation_mode = true; - next_WP_loc.alt = current_loc.alt; - break; - - case AUTO: - throttle_allows_nudging = true; - auto_throttle_mode = true; - auto_navigation_mode = true; - if (quadplane.available() && quadplane.enable == 2) { - auto_state.vtol_mode = true; - } else { - auto_state.vtol_mode = false; - } - next_WP_loc = prev_WP_loc = current_loc; - // start or resume the mission, based on MIS_AUTORESET - mission.start_or_resume(); - -#if SOARING_ENABLED == ENABLED - g2.soaring_controller.init_cruising(); -#endif - break; - - case RTL: - throttle_allows_nudging = true; - auto_throttle_mode = true; - auto_navigation_mode = true; - prev_WP_loc = current_loc; - do_RTL(get_RTL_altitude()); - break; - - case LOITER: - throttle_allows_nudging = true; - auto_throttle_mode = true; - auto_navigation_mode = true; - do_loiter_at_location(); - -#if SOARING_ENABLED == ENABLED - if (g2.soaring_controller.is_active() && - g2.soaring_controller.suppress_throttle()) { - g2.soaring_controller.init_thermalling(); - g2.soaring_controller.get_target(next_WP_loc); // ahead on flight path - } -#endif - - break; - - case AVOID_ADSB: - case GUIDED: - throttle_allows_nudging = true; - auto_throttle_mode = true; - auto_navigation_mode = true; - guided_throttle_passthru = false; - /* - when entering guided mode we set the target as the current - location. This matches the behaviour of the copter code - */ - guided_WP_loc = current_loc; - set_guided_WP(); - break; - - case QSTABILIZE: - case QHOVER: - case QLOITER: - case QLAND: - case QRTL: - case QAUTOTUNE: - case QACRO: - throttle_allows_nudging = true; - auto_navigation_mode = false; - if (!quadplane.init_mode()) { - control_mode = previous_mode; - } else { - auto_throttle_mode = false; - auto_state.vtol_mode = true; - } - break; + // we failed entering new mode, roll back to old + previous_mode = &old_previous_mode; + control_mode = &old_mode; + return false; } - // start with throttle suppressed in auto_throttle modes - throttle_suppressed = auto_throttle_mode; + // exit previous mode + old_mode.exit(); - adsb.set_is_auto_mode(auto_navigation_mode); + // record reasons + previous_mode_reason = control_mode_reason; + control_mode_reason = reason; - logger.Write_Mode(control_mode, control_mode_reason); - - // update notify with flight mode change - notify_flight_mode(control_mode); + // log and notify mode change + logger.Write_Mode(control_mode->mode_number(), control_mode_reason); + notify_mode(*control_mode); // reset steering integrator on mode change steerController.reset_I(); // update RC failsafe, as mode change may have necessitated changing the failsafe throttle control_failsafe(); + + return true; } -// exit_mode - perform any cleanup required when leaving a flight mode -void Plane::exit_mode(enum FlightMode mode) +bool Plane::set_mode_by_number(const Mode::Number new_mode_number, const mode_reason_t reason) { - // stop mission when we leave auto - switch (mode) { - case AUTO: - if (mission.state() == AP_Mission::MISSION_RUNNING) { - mission.stop(); - - if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND && - !quadplane.is_vtol_land(mission.get_current_nav_cmd().id)) - { - landing.restart_landing_sequence(); - } - } - auto_state.started_flying_in_auto_ms = 0; - break; -#if QAUTOTUNE_ENABLED - case QAUTOTUNE: - quadplane.qautotune.stop(); - break; -#endif - default: - break; + Mode *new_mode = plane.mode_from_mode_num(new_mode_number); + if (new_mode == nullptr) { + gcs().send_text(MAV_SEVERITY_INFO, "Error: invalid mode number: %d", new_mode_number); + return false; } + return set_mode(*new_mode, reason); } void Plane::check_long_failsafe() @@ -514,7 +334,7 @@ void Plane::check_long_failsafe() if (failsafe.rc_failsafe && (tnow - radio_timeout_ms) > g.fs_timeout_long*1000) { failsafe_long_on_event(FAILSAFE_LONG, MODE_REASON_RADIO_FAILSAFE); - } else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HB_AUTO && control_mode == AUTO && + } else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HB_AUTO && control_mode == &mode_auto && failsafe.last_heartbeat_ms != 0 && (tnow - failsafe.last_heartbeat_ms) > g.fs_timeout_long*1000) { failsafe_long_on_event(FAILSAFE_GCS, MODE_REASON_GCS_FAILSAFE); @@ -613,79 +433,10 @@ void Plane::update_notify() } // sets notify object flight mode information -void Plane::notify_flight_mode(enum FlightMode mode) +void Plane::notify_mode(const Mode& mode) { - AP_Notify::flags.flight_mode = mode; - - // set flight mode string - switch (mode) { - case MANUAL: - notify.set_flight_mode_str("MANU"); - break; - case CIRCLE: - notify.set_flight_mode_str("CIRC"); - break; - case STABILIZE: - notify.set_flight_mode_str("STAB"); - break; - case TRAINING: - notify.set_flight_mode_str("TRAN"); - break; - case ACRO: - notify.set_flight_mode_str("ACRO"); - break; - case FLY_BY_WIRE_A: - notify.set_flight_mode_str("FBWA"); - break; - case FLY_BY_WIRE_B: - notify.set_flight_mode_str("FBWB"); - break; - case CRUISE: - notify.set_flight_mode_str("CRUS"); - break; - case AUTOTUNE: - notify.set_flight_mode_str("ATUN"); - break; - case AUTO: - notify.set_flight_mode_str("AUTO"); - break; - case RTL: - notify.set_flight_mode_str("RTL "); - break; - case LOITER: - notify.set_flight_mode_str("LOITER"); - break; - case AVOID_ADSB: - notify.set_flight_mode_str("AVOI"); - break; - case GUIDED: - notify.set_flight_mode_str("GUID"); - break; - case INITIALISING: - notify.set_flight_mode_str("INIT"); - break; - case QSTABILIZE: - notify.set_flight_mode_str("QSTB"); - break; - case QHOVER: - notify.set_flight_mode_str("QHOV"); - break; - case QLOITER: - notify.set_flight_mode_str("QLOT"); - break; - case QLAND: - notify.set_flight_mode_str("QLND"); - break; - case QRTL: - notify.set_flight_mode_str("QRTL"); - break; - case QAUTOTUNE: - notify.set_flight_mode_str("QAUTOTUNE"); - break; - default: - notify.set_flight_mode_str("----"); - break; - } + notify.flags.flight_mode = mode.mode_number(); + notify.set_flight_mode_str(mode.name4()); } /* @@ -746,7 +497,7 @@ bool Plane::disarm_motors(void) if (!arming.disarm()) { return false; } - if (control_mode != AUTO) { + if (control_mode != &mode_auto) { // reset the mission on disarm if we are not in auto mission.reset(); } diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp index 356e624395..6f28034d69 100644 --- a/ArduPlane/takeoff.cpp +++ b/ArduPlane/takeoff.cpp @@ -227,8 +227,8 @@ int16_t Plane::get_takeoff_pitch_min_cd(void) */ int8_t Plane::takeoff_tail_hold(void) { - bool in_takeoff = ((control_mode == AUTO && !auto_state.takeoff_complete) || - (control_mode == FLY_BY_WIRE_A && auto_state.fbwa_tdrag_takeoff_mode)); + bool in_takeoff = ((control_mode == &mode_auto && !auto_state.takeoff_complete) || + (control_mode == &mode_fbwa && auto_state.fbwa_tdrag_takeoff_mode)); if (!in_takeoff) { // not in takeoff return 0; diff --git a/ArduPlane/tiltrotor.cpp b/ArduPlane/tiltrotor.cpp index e14b63f5a3..90b7879e98 100644 --- a/ArduPlane/tiltrotor.cpp +++ b/ArduPlane/tiltrotor.cpp @@ -19,7 +19,7 @@ float QuadPlane::tilt_max_change(bool up) } if (tilt.tilt_type != TILT_TYPE_BINARY && !up) { bool fast_tilt = false; - if (plane.control_mode == MANUAL) { + if (plane.control_mode == &plane.mode_manual) { fast_tilt = true; } if (hal.util->get_soft_armed() && !in_vtol_mode() && !assisted_flight) { @@ -109,9 +109,9 @@ void QuadPlane::tiltrotor_continuous_update(void) 3) if we are in TRANSITION_TIMER mode then we are transitioning to forward flight and should put the rotors all the way forward */ - if (plane.control_mode == QSTABILIZE || - plane.control_mode == QHOVER || - plane.control_mode == QAUTOTUNE) { + if (plane.control_mode == &plane.mode_qstabilize || + plane.control_mode == &plane.mode_qhover || + plane.control_mode == &plane.mode_qautotune) { tiltrotor_slew(0); return; }