From de4dda2d17165b58ebc395f79e7f8c67d2579803 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 30 Sep 2022 09:10:41 +1000 Subject: [PATCH] ArduPlane: change namespace of MultiCopter and FixedWing params this stops the libraries knowing anything about AP_Vehicle --- ArduPlane/ArduPlane.cpp | 35 ++++++++++++++++++----------------- ArduPlane/Attitude.cpp | 16 ++++++++-------- ArduPlane/GCS_Mavlink.cpp | 4 ++-- ArduPlane/Plane.h | 10 +++++----- ArduPlane/altitude.cpp | 10 +++++----- ArduPlane/avoidance_adsb.cpp | 2 +- ArduPlane/commands_logic.cpp | 6 +++--- ArduPlane/events.cpp | 6 +++--- ArduPlane/is_flying.cpp | 20 ++++++++++---------- ArduPlane/mode_auto.cpp | 2 +- ArduPlane/mode_takeoff.cpp | 10 +++++----- ArduPlane/navigation.cpp | 2 +- ArduPlane/quadplane.h | 5 ++++- ArduPlane/sensors.cpp | 2 +- ArduPlane/servos.cpp | 24 ++++++++++++------------ ArduPlane/system.cpp | 4 ++-- ArduPlane/takeoff.cpp | 2 +- 17 files changed, 82 insertions(+), 78 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 0fd56ce33d..f4c5bb6d3d 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -479,7 +479,7 @@ void Plane::update_fly_forward(void) } #endif - if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) { + if (flight_stage == AP_FixedWing::FlightStage::LAND) { ahrs.set_fly_forward(landing.is_flying_forward()); return; } @@ -490,15 +490,15 @@ void Plane::update_fly_forward(void) /* set the flight stage */ -void Plane::set_flight_stage(AP_Vehicle::FixedWing::FlightStage fs) +void Plane::set_flight_stage(AP_FixedWing::FlightStage fs) { if (fs == flight_stage) { return; } - landing.handle_flight_stage_change(fs == AP_Vehicle::FixedWing::FLIGHT_LAND); + landing.handle_flight_stage_change(fs == AP_FixedWing::FlightStage::LAND); - if (fs == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) { + if (fs == AP_FixedWing::FlightStage::ABORT_LANDING) { gcs().send_text(MAV_SEVERITY_NOTICE, "Landing aborted, climbing to %dm", int(auto_state.takeoff_altitude_rel_cm/100)); } @@ -546,7 +546,8 @@ void Plane::update_alt() if (control_mode->does_auto_throttle() && !throttle_suppressed) { float distance_beyond_land_wp = 0; - if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND && current_loc.past_interval_finish_line(prev_WP_loc, next_WP_loc)) { + if (flight_stage == AP_FixedWing::FlightStage::LAND && + current_loc.past_interval_finish_line(prev_WP_loc, next_WP_loc)) { distance_beyond_land_wp = current_loc.get_distance(next_WP_loc); } @@ -580,49 +581,49 @@ void Plane::update_flight_stage(void) if (control_mode == &mode_auto) { #if HAL_QUADPLANE_ENABLED if (quadplane.in_vtol_auto()) { - set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_VTOL); + set_flight_stage(AP_FixedWing::FlightStage::VTOL); return; } #endif if (auto_state.takeoff_complete == false) { - set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_TAKEOFF); + set_flight_stage(AP_FixedWing::FlightStage::TAKEOFF); return; } else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) { - if (landing.is_commanded_go_around() || flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) { + if (landing.is_commanded_go_around() || flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) { // abort mode is sticky, it must complete while executing NAV_LAND - set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND); + set_flight_stage(AP_FixedWing::FlightStage::ABORT_LANDING); } else if (landing.get_abort_throttle_enable() && get_throttle_input() >= 90 && landing.request_go_around()) { gcs().send_text(MAV_SEVERITY_INFO,"Landing aborted via throttle"); - set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND); + set_flight_stage(AP_FixedWing::FlightStage::ABORT_LANDING); } else { - set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_LAND); + set_flight_stage(AP_FixedWing::FlightStage::LAND); } return; } #if HAL_QUADPLANE_ENABLED if (quadplane.in_assisted_flight()) { - set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_VTOL); + set_flight_stage(AP_FixedWing::FlightStage::VTOL); return; } #endif - set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL); + set_flight_stage(AP_FixedWing::FlightStage::NORMAL); } else if (control_mode != &mode_takeoff) { // If not in AUTO then assume normal operation for normal TECS operation. // This prevents TECS from being stuck in the wrong stage if you switch from // AUTO to, say, FBWB during a landing, an aborted landing or takeoff. - set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL); + set_flight_stage(AP_FixedWing::FlightStage::NORMAL); } return; } #if HAL_QUADPLANE_ENABLED if (quadplane.in_vtol_mode() || quadplane.in_assisted_flight()) { - set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_VTOL); + set_flight_stage(AP_FixedWing::FlightStage::VTOL); return; } #endif - set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL); + set_flight_stage(AP_FixedWing::FlightStage::NORMAL); } @@ -662,7 +663,7 @@ float Plane::tecs_hgt_afe(void) coming. */ float hgt_afe; - if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) { + if (flight_stage == AP_FixedWing::FlightStage::LAND) { hgt_afe = height_above_target(); hgt_afe -= rangefinder_correction(); } else { diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index ea88970c25..785977c7eb 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -51,7 +51,7 @@ float Plane::calc_speed_scaler(void) } if (!plane.ahrs.airspeed_sensor_enabled() && (plane.g2.flight_options & FlightOptions::SURPRESS_TKOFF_SCALING) && - (plane.flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF)) { //scaling is surpressed during climb phase of automatic takeoffs with no airspeed sensor being used due to problems with inaccurate airspeed estimates + (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF)) { //scaling is surpressed during climb phase of automatic takeoffs with no airspeed sensor being used due to problems with inaccurate airspeed estimates return MIN(speed_scaler, 1.0f) ; } return speed_scaler; @@ -767,8 +767,8 @@ void Plane::calc_nav_yaw_ground(void) { if (gps.ground_speed() < 1 && is_zero(get_throttle_input()) && - flight_stage != AP_Vehicle::FixedWing::FLIGHT_TAKEOFF && - flight_stage != AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) { + flight_stage != AP_FixedWing::FlightStage::TAKEOFF && + flight_stage != AP_FixedWing::FlightStage::ABORT_LANDING) { // manual rudder control while still steer_state.locked_course = false; steer_state.locked_course_err = 0; @@ -784,8 +784,8 @@ void Plane::calc_nav_yaw_ground(void) steer_state.last_steer_ms = now_ms; float steer_rate = (rudder_input()/4500.0f) * g.ground_steer_dps; - if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || - flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) { + if (flight_stage == AP_FixedWing::FlightStage::TAKEOFF || + flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) { steer_rate = 0; } if (!is_zero(steer_rate)) { @@ -794,8 +794,8 @@ void Plane::calc_nav_yaw_ground(void) } else if (!steer_state.locked_course) { // pilot has released the rudder stick or we are still - lock the course steer_state.locked_course = true; - if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_TAKEOFF && - flight_stage != AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) { + if (flight_stage != AP_FixedWing::FlightStage::TAKEOFF && + flight_stage != AP_FixedWing::FlightStage::ABORT_LANDING) { steer_state.locked_course_err = 0; } } @@ -889,7 +889,7 @@ void Plane::calc_nav_roll() void Plane::adjust_nav_pitch_throttle(void) { int8_t throttle = throttle_percentage(); - if (throttle >= 0 && throttle < aparm.throttle_cruise && flight_stage != AP_Vehicle::FixedWing::FLIGHT_VTOL) { + if (throttle >= 0 && throttle < aparm.throttle_cruise && flight_stage != AP_FixedWing::FlightStage::VTOL) { float p = (aparm.throttle_cruise - throttle) / (float)aparm.throttle_cruise; nav_pitch_cd -= g.stab_pitch_down * 100.0f * p; } diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 77d8e383d7..e4b0f68796 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -357,7 +357,7 @@ void GCS_MAVLINK_Plane::send_pid_tuning() pid_info = &plane.steerController.get_pid_info(); send_pid_info(pid_info, PID_TUNING_STEER, pid_info->actual); } - if ((g.gcs_pid_mask & TUNING_BITS_LAND) && (plane.flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND)) { + if ((g.gcs_pid_mask & TUNING_BITS_LAND) && (plane.flight_stage == AP_FixedWing::FlightStage::LAND)) { AP_AHRS &ahrs = AP::ahrs(); const Vector3f &gyro = ahrs.get_gyro(); send_pid_info(plane.landing.get_pid_info(), PID_TUNING_LANDING, degrees(gyro.z)); @@ -1022,7 +1022,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l case MAV_CMD_DO_GO_AROUND: { uint16_t mission_id = plane.mission.get_current_nav_cmd().id; - bool is_in_landing = (plane.flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) || + bool is_in_landing = (plane.flight_stage == AP_FixedWing::FlightStage::LAND) || (mission_id == MAV_CMD_NAV_LAND) || (mission_id == MAV_CMD_NAV_VTOL_LAND); if (is_in_landing) { diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 2eeede9577..005ef28af1 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -165,7 +165,7 @@ public: private: // key aircraft parameters passed to multiple libraries - AP_Vehicle::FixedWing aparm; + AP_FixedWing aparm; // Global parameters are all contained within the 'g' and 'g2' classes. Parameters g; @@ -191,7 +191,7 @@ private: // flight modes convenience array AP_Int8 *flight_modes = &g.flight_mode1; - AP_Vehicle::FixedWing::Rangefinder_State rangefinder_state; + AP_FixedWing::Rangefinder_State rangefinder_state; #if AP_RPM_ENABLED AP_RPM rpm_sensor; @@ -561,7 +561,7 @@ private: #if LANDING_GEAR_ENABLED == ENABLED // landing gear state struct { - AP_Vehicle::FixedWing::FlightStage last_flight_stage; + AP_FixedWing::FlightStage last_flight_stage; } gear; #endif @@ -594,7 +594,7 @@ private: int8_t throttle_watt_limit_min; // for reverse thrust uint32_t throttle_watt_limit_timer_ms; - AP_Vehicle::FixedWing::FlightStage flight_stage = AP_Vehicle::FixedWing::FLIGHT_NORMAL; + AP_FixedWing::FlightStage flight_stage = AP_FixedWing::FlightStage::NORMAL; // probability of aircraft is currently in flight. range from 0 to // 1 where 1 is 100% sure we're in flight @@ -1013,7 +1013,7 @@ private: void update_control_mode(void); void update_fly_forward(void); void update_flight_stage(); - void set_flight_stage(AP_Vehicle::FixedWing::FlightStage fs); + void set_flight_stage(AP_FixedWing::FlightStage fs); // navigation.cpp void loiter_angle_reset(void); diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 6ad97848e1..c80508b02d 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -448,7 +448,7 @@ void Plane::set_offset_altitude_location(const Location &start_loc, const Locati } #endif - if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND) { + if (flight_stage != AP_FixedWing::FlightStage::LAND) { // if we are within GLIDE_SLOPE_MIN meters of the target altitude // then reset the offset to not use a glide slope. This allows for // more accurate flight of missions where the aircraft may lose or @@ -539,7 +539,7 @@ float Plane::mission_alt_offset(void) { float ret = g.alt_offset; if (control_mode == &mode_auto && - (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND || auto_state.wp_is_land_approach)) { + (flight_stage == AP_FixedWing::FlightStage::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 ret += landing.alt_offset; @@ -641,7 +641,7 @@ float Plane::rangefinder_correction(void) } // for now we only support the rangefinder for landing - bool using_rangefinder = (g.rangefinder_landing && flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND); + bool using_rangefinder = (g.rangefinder_landing && flight_stage == AP_FixedWing::FlightStage::LAND); if (!using_rangefinder) { return 0; } @@ -657,7 +657,7 @@ void Plane::rangefinder_terrain_correction(float &height) { #if AP_TERRAIN_AVAILABLE if (!g.rangefinder_landing || - flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND || + flight_stage != AP_FixedWing::FlightStage::LAND || !terrain_enabled_in_current_mode()) { return; } @@ -707,7 +707,7 @@ void Plane::rangefinder_height_update(void) } else { rangefinder_state.in_range = true; bool flightstage_good_for_rangefinder_landing = false; - if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) { + if (flight_stage == AP_FixedWing::FlightStage::LAND) { flightstage_good_for_rangefinder_landing = true; } #if HAL_QUADPLANE_ENABLED diff --git a/ArduPlane/avoidance_adsb.cpp b/ArduPlane/avoidance_adsb.cpp index 0bdbaa6fde..e0dabe8af0 100644 --- a/ArduPlane/avoidance_adsb.cpp +++ b/ArduPlane/avoidance_adsb.cpp @@ -27,7 +27,7 @@ MAV_COLLISION_ACTION AP_Avoidance_Plane::handle_avoidance(const AP_Avoidance::Ob bool flightmode_prohibits_action = false; 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.flight_stage == AP_FixedWing::FlightStage::LAND) || // TODO: consider allowing action during approach plane.control_mode == &plane.mode_autotune) { flightmode_prohibits_action = true; } diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 7e365b932f..6f758539f6 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -246,7 +246,7 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Ret return quadplane.verify_vtol_land(); } #endif - if (flight_stage == AP_Vehicle::FixedWing::FlightStage::FLIGHT_ABORT_LAND) { + if (flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) { return landing.verify_abort_landing(prev_WP_loc, next_WP_loc, current_loc, auto_state.takeoff_altitude_rel_cm, throttle_suppressed); } else { @@ -413,9 +413,9 @@ void Plane::do_land(const AP_Mission::Mission_Command& cmd) landing.do_land(cmd, relative_altitude); - if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) { + if (flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) { // if we were in an abort we need to explicitly move out of the abort state, as it's sticky - set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_LAND); + set_flight_stage(AP_FixedWing::FlightStage::LAND); } #if AP_FENCE_ENABLED diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index 479b0670d3..17ba24ce50 100644 --- a/ArduPlane/events.cpp +++ b/ArduPlane/events.cpp @@ -4,7 +4,7 @@ // for use in failsafe code. bool Plane::failsafe_in_landing_sequence() const { - if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) { + if (flight_stage == AP_FixedWing::FlightStage::LAND) { return true; } #if HAL_QUADPLANE_ENABLED @@ -238,7 +238,7 @@ void Plane::handle_battery_failsafe(const char *type_str, const int8_t action) FALLTHROUGH; #endif // HAL_QUADPLANE_ENABLED case Failsafe_Action_Land: { - bool already_landing = flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND; + bool already_landing = flight_stage == AP_FixedWing::FlightStage::LAND; #if HAL_QUADPLANE_ENABLED if (control_mode == &mode_qland || control_mode == &mode_loiter_qland) { already_landing = true; @@ -259,7 +259,7 @@ void Plane::handle_battery_failsafe(const char *type_str, const int8_t action) FALLTHROUGH; } case Failsafe_Action_RTL: { - bool already_landing = flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND; + bool already_landing = flight_stage == AP_FixedWing::FlightStage::LAND; #if HAL_QUADPLANE_ENABLED if (control_mode == &mode_qland || control_mode == &mode_loiter_qland || quadplane.in_vtol_land_sequence()) { diff --git a/ArduPlane/is_flying.cpp b/ArduPlane/is_flying.cpp index 81733a1587..904a0c1855 100644 --- a/ArduPlane/is_flying.cpp +++ b/ArduPlane/is_flying.cpp @@ -83,10 +83,10 @@ void Plane::update_is_flying_5Hz(void) switch (flight_stage) { - case AP_Vehicle::FixedWing::FLIGHT_TAKEOFF: + case AP_FixedWing::FlightStage::TAKEOFF: break; - case AP_Vehicle::FixedWing::FLIGHT_NORMAL: + case AP_FixedWing::FlightStage::NORMAL: if (in_preLaunch_flight_stage()) { // while on the ground, an uncalibrated airspeed sensor can drift to 7m/s so // ensure we aren't showing a false positive. @@ -96,17 +96,17 @@ void Plane::update_is_flying_5Hz(void) } break; - case AP_Vehicle::FixedWing::FLIGHT_VTOL: + case AP_FixedWing::FlightStage::VTOL: // TODO: detect ground impacts break; - case AP_Vehicle::FixedWing::FLIGHT_LAND: + case AP_FixedWing::FlightStage::LAND: if (landing.is_on_approach() && auto_state.sink_rate > 0.2f) { is_flying_bool = true; } break; - case AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND: + case AP_FixedWing::FlightStage::ABORT_LANDING: if (auto_state.sink_rate < -0.5f) { // steep climb is_flying_bool = true; @@ -121,7 +121,7 @@ void Plane::update_is_flying_5Hz(void) // when disarmed assume not flying and need overwhelming evidence that we ARE flying is_flying_bool = airspeed_movement && gps_confirmed_movement; - if ((flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF) || landing.is_flaring()) { + if ((flight_stage == AP_FixedWing::FlightStage::TAKEOFF) || landing.is_flaring()) { is_flying_bool = false; } } @@ -257,7 +257,7 @@ void Plane::crash_detection_update(void) } else { switch (flight_stage) { - case AP_Vehicle::FixedWing::FLIGHT_TAKEOFF: + case AP_FixedWing::FlightStage::TAKEOFF: if (g.takeoff_throttle_min_accel > 0 && !throttle_suppressed) { // if you have an acceleration holding back throttle, but you met the @@ -269,14 +269,14 @@ void Plane::crash_detection_update(void) // TODO: handle auto missions without NAV_TAKEOFF mission cmd break; - case AP_Vehicle::FixedWing::FLIGHT_NORMAL: + case AP_FixedWing::FlightStage::NORMAL: if (!in_preLaunch_flight_stage() && been_auto_flying) { crashed = true; crash_state.debounce_time_total_ms = CRASH_DETECTION_DELAY_MS; } break; - case AP_Vehicle::FixedWing::FLIGHT_VTOL: + case AP_FixedWing::FlightStage::VTOL: // we need a totally new method for this crashed = false; break; @@ -337,6 +337,6 @@ bool Plane::in_preLaunch_flight_stage(void) #endif return (control_mode == &mode_auto && throttle_suppressed && - flight_stage == AP_Vehicle::FixedWing::FLIGHT_NORMAL && + flight_stage == AP_FixedWing::FlightStage::NORMAL && mission.get_current_nav_cmd().id == MAV_CMD_NAV_TAKEOFF); } diff --git a/ArduPlane/mode_auto.cpp b/ArduPlane/mode_auto.cpp index 17affcf0a2..fcf86b2c80 100644 --- a/ArduPlane/mode_auto.cpp +++ b/ArduPlane/mode_auto.cpp @@ -80,7 +80,7 @@ void ModeAuto::update() #endif if (nav_cmd_id == MAV_CMD_NAV_TAKEOFF || - (nav_cmd_id == MAV_CMD_NAV_LAND && plane.flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND)) { + (nav_cmd_id == MAV_CMD_NAV_LAND && plane.flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING)) { plane.takeoff_calc_roll(); plane.takeoff_calc_pitch(); plane.calc_throttle(); diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index 3eb8604c37..f65394c4e3 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/ArduPlane/mode_takeoff.cpp @@ -66,7 +66,7 @@ void ModeTakeoff::update() plane.prev_WP_loc = plane.current_loc; plane.next_WP_loc = plane.current_loc; takeoff_started = true; - plane.set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL); + plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL); } } @@ -87,7 +87,7 @@ void ModeTakeoff::update() plane.auto_state.takeoff_pitch_cd = level_pitch * 100; - plane.set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_TAKEOFF); + plane.set_flight_stage(AP_FixedWing::FlightStage::TAKEOFF); if (!plane.throttle_suppressed) { gcs().send_text(MAV_SEVERITY_INFO, "Takeoff to %.0fm at %.1fm to %.1f deg", @@ -99,7 +99,7 @@ void ModeTakeoff::update() // we finish the initial level takeoff if we climb past // TKOFF_LVL_ALT or we pass the target location. The check for // target location prevents us flying forever if we can't climb - if (plane.flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF && + if (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF && (plane.current_loc.alt - start_loc.alt >= level_alt*100 || start_loc.get_distance(plane.current_loc) >= target_dist)) { // reached level alt, re-calculate bearing to cope with systems with no compass @@ -112,14 +112,14 @@ void ModeTakeoff::update() plane.next_WP_loc.offset_bearing(direction, MAX(dist-dist_done, 0)); plane.next_WP_loc.alt = start_loc.alt + target_alt*100.0; - plane.set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL); + plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL); #if AP_FENCE_ENABLED plane.fence.auto_enable_fence_after_takeoff(); #endif } - if (plane.flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF) { + if (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF) { SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 100.0); plane.takeoff_calc_roll(); plane.takeoff_calc_pitch(); diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index 16c29f690d..ba05b64b60 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -224,7 +224,7 @@ void Plane::calc_airspeed_errors() } #endif - } else if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) { + } else if (flight_stage == AP_FixedWing::FlightStage::LAND) { // Landing airspeed target target_airspeed_cm = landing.get_target_airspeed_cm(); } else if (control_mode == &mode_guided && new_airspeed_cm > 0) { //DO_CHANGE_SPEED overrides onboard guided speed commands, user would have re-enter guided mode to revert diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 2ce137b022..e022f2316e 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -19,6 +19,7 @@ #include #include #include +#include #include #include "qautotune.h" #include "defines.h" @@ -173,7 +174,9 @@ public: private: AP_AHRS &ahrs; - AP_Vehicle::MultiCopter aparm; + + // key aircraft parameters passed to multiple libraries + AP_MultiCopter aparm; AP_InertialNav inertial_nav{ahrs}; diff --git a/ArduPlane/sensors.cpp b/ArduPlane/sensors.cpp index f89ddbeae0..91d3ed5b77 100644 --- a/ArduPlane/sensors.cpp +++ b/ArduPlane/sensors.cpp @@ -18,7 +18,7 @@ void Plane::read_rangefinder(void) #endif { // use the best available alt estimate via baro above home - if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) { + if (flight_stage == AP_FixedWing::FlightStage::LAND) { // ensure the rangefinder is powered-on when land alt is higher than home altitude. // This is done using the target alt which we know is below us and we are sinking to it height = height_above_target(); diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 141b58c451..4051981f9b 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -40,13 +40,13 @@ void Plane::throttle_slew_limit(SRV_Channel::Aux_servo_function_t func) 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) { + } else if (landing.get_throttle_slewrate() != 0 && flight_stage == AP_FixedWing::FlightStage::LAND) { slewrate = landing.get_throttle_slewrate(); } } if (g.takeoff_throttle_slewrate != 0 && - (flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || - flight_stage == AP_Vehicle::FixedWing::FLIGHT_VTOL)) { + (flight_stage == AP_FixedWing::FlightStage::TAKEOFF || + flight_stage == AP_FixedWing::FlightStage::VTOL)) { // for VTOL we use takeoff slewrate, which helps with transition slewrate = g.takeoff_throttle_slewrate; } @@ -492,7 +492,7 @@ void Plane::throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle) */ void Plane::set_servos_controlled(void) { - if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) { + if (flight_stage == AP_FixedWing::FlightStage::LAND) { // allow landing to override servos if it would like to landing.override_servos(); } @@ -512,8 +512,8 @@ void Plane::set_servos_controlled(void) } bool flight_stage_determines_max_throttle = false; - if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || - flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND + if (flight_stage == AP_FixedWing::FlightStage::TAKEOFF || + flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING ) { flight_stage_determines_max_throttle = true; } @@ -654,19 +654,19 @@ void Plane::set_servos_flaps(void) possibility of oscillation */ switch (flight_stage) { - case AP_Vehicle::FixedWing::FLIGHT_TAKEOFF: - case AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND: + case AP_FixedWing::FlightStage::TAKEOFF: + case AP_FixedWing::FlightStage::ABORT_LANDING: if (g.takeoff_flap_percent != 0) { auto_flap_percent = g.takeoff_flap_percent; } break; - case AP_Vehicle::FixedWing::FLIGHT_NORMAL: + case AP_FixedWing::FlightStage::NORMAL: if (g.takeoff_flap_percent != 0 && in_preLaunch_flight_stage()) { // TODO: move this to a new FLIGHT_PRE_TAKEOFF stage auto_flap_percent = g.takeoff_flap_percent; } break; - case AP_Vehicle::FixedWing::FLIGHT_LAND: + case AP_FixedWing::FlightStage::LAND: if (landing.get_flap_percent() != 0) { auto_flap_percent = landing.get_flap_percent(); } @@ -699,10 +699,10 @@ void Plane::set_landing_gear(void) { if (control_mode == &mode_auto && hal.util->get_soft_armed() && is_flying() && gear.last_flight_stage != flight_stage) { switch (flight_stage) { - case AP_Vehicle::FixedWing::FLIGHT_LAND: + case AP_FixedWing::FlightStage::LAND: g2.landing_gear.deploy_for_landing(); break; - case AP_Vehicle::FixedWing::FLIGHT_NORMAL: + case AP_FixedWing::FlightStage::NORMAL: g2.landing_gear.retract_after_takeoff(); break; default: diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 6f16700083..98a0762012 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -353,7 +353,7 @@ void Plane::check_long_failsafe() const uint32_t tnow = millis(); // only act on changes // ------------------- - if (failsafe.state != FAILSAFE_LONG && failsafe.state != FAILSAFE_GCS && flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND) { + if (failsafe.state != FAILSAFE_LONG && failsafe.state != FAILSAFE_GCS && flight_stage != AP_FixedWing::FlightStage::LAND) { uint32_t radio_timeout_ms = failsafe.last_valid_rc_ms; if (failsafe.state == FAILSAFE_SHORT) { // time is relative to when short failsafe enabled @@ -400,7 +400,7 @@ void Plane::check_short_failsafe() // ------------------- if (g.fs_action_short != FS_ACTION_SHORT_DISABLED && failsafe.state == FAILSAFE_NONE && - flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND) { + flight_stage != AP_FixedWing::FlightStage::LAND) { // The condition is checked and the flag rc_failsafe is set in radio.cpp if(failsafe.rc_failsafe) { failsafe_short_on_event(FAILSAFE_SHORT, ModeReason::RADIO_FAILSAFE); diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp index b0cf80c9c0..ad2c544134 100644 --- a/ArduPlane/takeoff.cpp +++ b/ArduPlane/takeoff.cpp @@ -201,7 +201,7 @@ void Plane::takeoff_calc_pitch(void) */ int16_t Plane::get_takeoff_pitch_min_cd(void) { - if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_TAKEOFF) { + if (flight_stage != AP_FixedWing::FlightStage::TAKEOFF) { return auto_state.takeoff_pitch_cd; }