From caa8e066f07e0e07fe4901b317cddb6bd37ea771 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 30 Aug 2024 13:30:48 +1000 Subject: [PATCH] Plane: added glider pullup support --- ArduPlane/ArduPlane.cpp | 23 ++++ ArduPlane/Parameters.cpp | 6 + ArduPlane/Parameters.h | 2 + ArduPlane/Plane.h | 14 ++ ArduPlane/commands_logic.cpp | 52 +++++++- ArduPlane/mode.cpp | 3 + ArduPlane/mode_auto.cpp | 13 ++ ArduPlane/navigation.cpp | 2 +- ArduPlane/pullup.cpp | 244 +++++++++++++++++++++++++++++++++++ ArduPlane/pullup.h | 50 +++++++ 10 files changed, 402 insertions(+), 7 deletions(-) create mode 100644 ArduPlane/pullup.cpp create mode 100644 ArduPlane/pullup.h diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index e523b3ea04b804..afdcb1c53da31d 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -216,6 +216,13 @@ void Plane::update_speed_height(void) should_run_tecs = false; } #endif + +#if AP_PLANE_GLIDER_PULLUP_ENABLED + if (pullup.in_pullup()) { + should_run_tecs = false; + } +#endif + if (should_run_tecs) { // Call TECS 50Hz update. Note that we call this regardless of // throttle suppressed, as this needs to be running for @@ -512,6 +519,12 @@ void Plane::update_fly_forward(void) } #endif + if (auto_state.idle_mode) { + // don't fuse airspeed when in balloon lift + ahrs.set_fly_forward(false); + return; + } + if (flight_stage == AP_FixedWing::FlightStage::LAND) { ahrs.set_fly_forward(landing.is_flying_forward()); return; @@ -578,6 +591,16 @@ void Plane::update_alt() should_run_tecs = false; } #endif + + if (auto_state.idle_mode) { + should_run_tecs = false; + } + +#if AP_PLANE_GLIDER_PULLUP_ENABLED + if (pullup.in_pullup()) { + should_run_tecs = false; + } +#endif if (should_run_tecs && !throttle_suppressed) { diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index bbb8d234486eaf..99d73a70170279 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1019,6 +1019,12 @@ const AP_Param::Info Plane::var_info[] = { // @Path: mode_takeoff.cpp GOBJECT(mode_takeoff, "TKOFF_", ModeTakeoff), +#if AP_PLANE_GLIDER_PULLUP_ENABLED + // @Group: PUP_ + // @Path: pullup.cpp + GOBJECT(pullup, "PUP_", GliderPullup), +#endif + // @Group: // @Path: ../libraries/AP_Vehicle/AP_Vehicle.cpp PARAM_VEHICLE_INFO, diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 38801d99631cc2..612de354fc8c12 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -359,6 +359,8 @@ class Parameters { k_param_autotune_options, k_param_takeoff_throttle_min, k_param_takeoff_options, + + k_param_pullup = 270, }; AP_Int16 format_version; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index c86b9b92cbbbe5..dfe12ba0982e62 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -121,6 +121,7 @@ #include "avoidance_adsb.h" #endif #include "AP_Arming.h" +#include "pullup.h" /* main APM:Plane class @@ -175,6 +176,9 @@ class Plane : public AP_Vehicle { #if AP_EXTERNAL_CONTROL_ENABLED friend class AP_ExternalControl_Plane; #endif +#if AP_PLANE_GLIDER_PULLUP_ENABLED + friend class GliderPullup; +#endif Plane(void); @@ -515,6 +519,11 @@ class Plane : public AP_Vehicle { // have we checked for an auto-land? bool checked_for_autoland; + // Altitude threshold to complete a takeoff command in autonomous modes. Centimeters + // are we in idle mode? used for balloon launch to stop servo + // movement until altitude is reached + bool idle_mode; + // are we in VTOL mode in AUTO? bool vtol_mode; @@ -959,6 +968,7 @@ class Plane : public AP_Vehicle { void do_loiter_turns(const AP_Mission::Mission_Command& cmd); void do_loiter_time(const AP_Mission::Mission_Command& cmd); void do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd); + void do_altitude_wait(const AP_Mission::Mission_Command& cmd); void do_loiter_to_alt(const AP_Mission::Mission_Command& cmd); void do_vtol_takeoff(const AP_Mission::Mission_Command& cmd); void do_vtol_land(const AP_Mission::Mission_Command& cmd); @@ -1272,6 +1282,10 @@ class Plane : public AP_Vehicle { // last target alt we passed to tecs int32_t tecs_target_alt_cm; +#if AP_PLANE_GLIDER_PULLUP_ENABLED + GliderPullup pullup; +#endif // AP_PLANE_GLIDER_PULLUP_ENABLED + public: void failsafe_check(void); bool is_landing() const override; diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 9fff6923ef9c1c..843f42cb5f7a21 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -27,6 +27,9 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) nav_controller->set_data_is_stale(); + // start non-idle + auto_state.idle_mode = false; + // reset loiter start time. New command is a new loiter loiter.start_time_ms = 0; @@ -92,6 +95,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) break; case MAV_CMD_NAV_ALTITUDE_WAIT: + do_altitude_wait(cmd); break; #if HAL_QUADPLANE_ENABLED @@ -501,6 +505,15 @@ void Plane::do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd) reset_offset_altitude(); } +void Plane::do_altitude_wait(const AP_Mission::Mission_Command& cmd) +{ + // set all servos to trim until we reach altitude or descent speed + auto_state.idle_mode = true; +#if AP_PLANE_GLIDER_PULLUP_ENABLED + pullup.reset(); +#endif +} + void Plane::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd) { //set target alt @@ -766,6 +779,11 @@ bool Plane::verify_loiter_to_alt(const AP_Mission::Mission_Command &cmd) result = verify_loiter_heading(false); } + // additional check for altitude target, even if blown off course + if (current_loc.alt < cmd.content.location.alt) { + result = true; + } + if (result) { gcs().send_text(MAV_SEVERITY_INFO,"Loiter to alt complete"); } @@ -831,17 +849,38 @@ bool Plane::verify_continue_and_change_alt() */ bool ModeAuto::verify_altitude_wait(const AP_Mission::Mission_Command &cmd) { - if (plane.current_loc.alt > cmd.content.altitude_wait.altitude*100.0f) { - gcs().send_text(MAV_SEVERITY_INFO,"Reached altitude"); - return true; +#if AP_PLANE_GLIDER_PULLUP_ENABLED + if (plane.pullup.in_pullup()) { + return plane.pullup.verify_pullup(); } - if (plane.auto_state.sink_rate > cmd.content.altitude_wait.descent_rate) { +#endif + + const float alt_diff = plane.current_loc.alt - cmd.content.altitude_wait.altitude*100.0f; + bool completed = false; + if (alt_diff > 0) { + gcs().send_text(MAV_SEVERITY_INFO,"Reached altitude"); + completed = true; + } else if (cmd.content.altitude_wait.descent_rate > 0 && + plane.auto_state.sink_rate > cmd.content.altitude_wait.descent_rate) { gcs().send_text(MAV_SEVERITY_INFO, "Reached descent rate %.1f m/s", (double)plane.auto_state.sink_rate); - return true; + completed = true; } + if (completed) { +#if AP_PLANE_GLIDER_PULLUP_ENABLED + if (plane.pullup.pullup_start()) { + // we are doing a pullup, ALTITUDE_WAIT not complete until pullup is done + return false; + } +#endif + return true; + } + + const float time_to_alt = alt_diff / MIN(plane.auto_state.sink_rate, -0.01); + // if requested, wiggle servos - if (cmd.content.altitude_wait.wiggle_time != 0) { + if (cmd.content.altitude_wait.wiggle_time != 0 && + (plane.auto_state.sink_rate > 0 || time_to_alt > cmd.content.altitude_wait.wiggle_time*5)) { if (wiggle.stage == 0 && AP_HAL::millis() - wiggle.last_ms > cmd.content.altitude_wait.wiggle_time*1000) { wiggle.stage = 1; @@ -1291,3 +1330,4 @@ bool Plane::in_auto_mission_id(uint16_t command) const { return control_mode == &mode_auto && mission.get_current_nav_id() == command; } + diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index a74d7c1a9367a0..f9a811c8eb2b3c 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -101,6 +101,9 @@ bool Mode::enter() plane.target_altitude.terrain_following_pending = false; #endif + // start with normal servo control + plane.auto_state.idle_mode = false; + bool enter_result = _enter(); if (enter_result) { diff --git a/ArduPlane/mode_auto.cpp b/ArduPlane/mode_auto.cpp index 3153d7de31e069..c2111796605714 100644 --- a/ArduPlane/mode_auto.cpp +++ b/ArduPlane/mode_auto.cpp @@ -79,6 +79,12 @@ void ModeAuto::update() } #endif +#if AP_PLANE_GLIDER_PULLUP_ENABLED + if (plane.pullup.in_pullup()) { + return; + } +#endif + if (nav_cmd_id == MAV_CMD_NAV_TAKEOFF || (nav_cmd_id == MAV_CMD_NAV_LAND && plane.flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING)) { plane.takeoff_calc_roll(); @@ -166,6 +172,13 @@ bool ModeAuto::is_landing() const void ModeAuto::run() { +#if AP_PLANE_GLIDER_PULLUP_ENABLED + if (plane.pullup.in_pullup()) { + plane.pullup.stabilize_pullup(); + return; + } +#endif + if (plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_ALTITUDE_WAIT) { wiggle_servos(); diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index e3297f894334e8..6e98b1d6fb3c3d 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -321,7 +321,7 @@ void Plane::update_loiter_update_nav(uint16_t radius) if ((loiter.start_time_ms == 0 && (control_mode == &mode_auto || control_mode == &mode_guided) && auto_state.crosstrack && - current_loc.get_distance(next_WP_loc) > radius*3) || + current_loc.get_distance(next_WP_loc) > 3 * nav_controller->loiter_radius(fabsf(radius))) || quadplane_qrtl_switch) { /* if never reached loiter point and using crosstrack and somewhat far away from loiter point diff --git a/ArduPlane/pullup.cpp b/ArduPlane/pullup.cpp new file mode 100644 index 00000000000000..aef2e49e538793 --- /dev/null +++ b/ArduPlane/pullup.cpp @@ -0,0 +1,244 @@ +#include "Plane.h" +/* + support for pullup after an alitude wait. Used for high altitude gliders + */ + +#if AP_PLANE_GLIDER_PULLUP_ENABLED + +// Pullup control parameters +const AP_Param::GroupInfo GliderPullup::var_info[] = { + + // @Param: ENABLE + // @DisplayName: Enable pullup after altitude wait + // @Description: Enable pullup after altitude wait + // @Values: 0:Disabled, 1:Enabled + // @User: Advanced + AP_GROUPINFO_FLAGS("ENABLE", 1, GliderPullup, enable, 0, AP_PARAM_FLAG_ENABLE), + + // @Param: ELEV_OFS + // @DisplayName: Elevator deflection used before starting pullup + // @Description: Elevator deflection offset from -1 to 1 while waiting for airspeed to rise before starting close loop control of the pullup. + // @Range: -1.0 1.0 + // @User: Advanced + AP_GROUPINFO("ELEV_OFS", 2, GliderPullup, elev_offset, 0.1f), + + // @Param: NG_LIM + // @DisplayName: Maximum normal load factor during pullup + // @Description: This is the nominal maximum value of normal load factor used during the closed loop pitch rate control of the pullup. + // @Range: 1.0 4.0 + // @User: Advanced + AP_GROUPINFO("NG_LIM", 3, GliderPullup, ng_limit, 2.0f), + + // @Param: NG_JERK_LIM + // @DisplayName: Maximum normal load factor rate of change during pullup + // @Description: The normal load factor used for closed loop pitch rate control of the pullup will be ramped up to the value set by PUP_NG_LIM at the rate of change set by this parameter. The parameter value specified will be scaled internally by 1/EAS2TAS. + // @Units: 1/s + // @Range: 0.1 10.0 + // @User: Advanced + AP_GROUPINFO("NG_JERK_LIM", 4, GliderPullup, ng_jerk_limit, 4.0f), + + // @Param: PITCH + // @DisplayName: Target pitch angle during pullup + // @Description: The vehicle will attempt achieve this pitch angle during the pull-up maneouvre. + // @Units: deg + // @Range: -5 15 + // @User: Advanced + AP_GROUPINFO("PITCH", 5, GliderPullup, pitch_dem, 3), + + // @Param: ARSPD_START + // @DisplayName: Pullup target airspeed + // @Description: Target airspeed for initial airspeed wait + // @Units: m/s + // @Range: 0 100 + // @User: Advanced + AP_GROUPINFO("ARSPD_START", 6, GliderPullup, airspeed_start, 30), + + // @Param: PITCH_START + // @DisplayName: Pullup target pitch + // @Description: Target pitch for initial pullup + // @Units: deg + // @Range: -90 0 + // @User: Advanced + AP_GROUPINFO("PITCH_START", 7, GliderPullup, pitch_start, -60), + + AP_GROUPEND +}; + +// constructor +GliderPullup::GliderPullup(void) +{ + AP_Param::setup_object_defaults(this, var_info); +} + +/* + return true if in a pullup manoeuvre at the end of NAV_ALTITUDE_WAIT +*/ +bool GliderPullup::in_pullup(void) const +{ + return plane.control_mode == &plane.mode_auto && + plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_ALTITUDE_WAIT && + stage != Stage::NONE; +} + +/* + start a pullup maneouvre, called when NAV_ALTITUDE_WAIT has reached + altitude or exceeded descent rate +*/ +bool GliderPullup::pullup_start(void) +{ + if (enable != 1) { + return false; + } + + // release balloon + SRV_Channels::set_output_scaled(SRV_Channel::k_lift_release, 100); + + stage = Stage::WAIT_AIRSPEED; + plane.auto_state.idle_mode = false; + float aspeed; + if (!plane.ahrs.airspeed_estimate(aspeed)) { + aspeed = -1; + } + gcs().send_text(MAV_SEVERITY_INFO, "Start pullup airspeed %.1fm/s at %.1fm AMSL", aspeed, plane.current_loc.alt*0.01); + return true; +} + +/* + first stage pullup from balloon release, verify completion + */ +bool GliderPullup::verify_pullup(void) +{ + const auto &ahrs = plane.ahrs; + const auto ¤t_loc = plane.current_loc; + const auto &aparm = plane.aparm; + + switch (stage) { + case Stage::WAIT_AIRSPEED: { + float aspeed; + if (ahrs.airspeed_estimate(aspeed) && aspeed > airspeed_start) { + gcs().send_text(MAV_SEVERITY_INFO, "Pullup airspeed %.1fm/s alt %.1fm AMSL", aspeed, current_loc.alt*0.01); + stage = Stage::WAIT_PITCH; + } + return false; + } + + case Stage::WAIT_PITCH: { + if (ahrs.pitch_sensor*0.01 > pitch_start && fabsf(ahrs.roll_sensor*0.01) < 90) { + gcs().send_text(MAV_SEVERITY_INFO, "Pullup pitch p=%.1f r=%.1f alt %.1fm AMSL", + ahrs.pitch_sensor*0.01, + ahrs.roll_sensor*0.01, + current_loc.alt*0.01); + stage = Stage::WAIT_LEVEL; + } + return false; + } + + case Stage::PUSH_NOSE_DOWN: { + if (fabsf(ahrs.roll_sensor*0.01) < aparm.roll_limit) { + stage = Stage::WAIT_LEVEL; + } + return false; + } + + case Stage::WAIT_LEVEL: { + // When pitch has raised past lower limit used by speed controller, wait for airspeed to approach + // target value before handing over control of pitch demand to speed controller + bool pitchup_complete = ahrs.pitch_sensor*0.01 > MIN(0, aparm.pitch_limit_min); + const float pitch_lag_time = 1.0f * sqrtf(ahrs.get_EAS2TAS()); + float aspeed; + const float aspeed_derivative = (ahrs.get_accel().x + GRAVITY_MSS * ahrs.get_DCM_rotation_body_to_ned().c.x) / ahrs.get_EAS2TAS(); + bool airspeed_low = ahrs.airspeed_estimate(aspeed) ? (aspeed + aspeed_derivative * pitch_lag_time) < 0.01f * (float)plane.target_airspeed_cm : true; + bool roll_control_lost = fabsf(ahrs.roll_sensor*0.01) > aparm.roll_limit; + if (pitchup_complete && airspeed_low && !roll_control_lost) { + gcs().send_text(MAV_SEVERITY_INFO, "Pullup level r=%.1f p=%.1f alt %.1fm AMSL", + ahrs.roll_sensor*0.01, ahrs.pitch_sensor*0.01, current_loc.alt*0.01); + break; + } else if (pitchup_complete && roll_control_lost) { + // push nose down and wait to get roll control back + gcs().send_text(MAV_SEVERITY_ALERT, "Pullup level roll bad r=%.1f p=%.1f", + ahrs.roll_sensor*0.01, + ahrs.pitch_sensor*0.01); + stage = Stage::PUSH_NOSE_DOWN; + } + return false; + } + case Stage::NONE: + break; + } + + // all done + stage = Stage::NONE; + return true; +} + +/* + stabilize during pullup from balloon drop + */ +void GliderPullup::stabilize_pullup(void) +{ + const float speed_scaler = plane.get_speed_scaler(); + switch (stage) { + case Stage::WAIT_AIRSPEED: { + SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elev_offset*4500); + SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, 0); + plane.nav_pitch_cd = 0; + plane.nav_roll_cd = 0; + uint32_t now = AP_HAL::millis(); + if (now - plane.last_stabilize_ms > 1000) { + plane.pitchController.reset_I(); + plane.yawController.reset_I(); + } + plane.last_stabilize_ms = now; + SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.rollController.get_rate_out(0, speed_scaler)); + plane.logger.Write_PID(LOG_PIDR_MSG, plane.rollController.get_pid_info()); + ng_demand = 0.0; + break; + } + case Stage::WAIT_PITCH: { + plane.yawController.reset_I(); + plane.nav_roll_cd = 0; + plane.nav_pitch_cd = 0; + plane.last_stabilize_ms = AP_HAL::millis(); + SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, 0); + SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.rollController.get_rate_out(0, speed_scaler)); + float aspeed; + const auto &ahrs = plane.ahrs; + if (ahrs.airspeed_estimate(aspeed)) { + // apply a rate of change limit to the ng pullup demand + ng_demand += MAX(ng_jerk_limit / ahrs.get_EAS2TAS(), 0.1f) * plane.scheduler.get_loop_period_s(); + ng_demand = MIN(ng_demand, ng_limit); + const float VTAS_ref = ahrs.get_EAS2TAS() * aspeed; + const float pullup_accel = ng_demand * GRAVITY_MSS; + const float demanded_rate_dps = degrees(pullup_accel / VTAS_ref); + const uint32_t elev_trim_offset_cd = 4500.0f * elev_offset * (1.0f - ng_demand / ng_limit); + SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elev_trim_offset_cd + plane.pitchController.get_rate_out(demanded_rate_dps, speed_scaler)); + plane.logger.Write_PID(LOG_PIDP_MSG, plane.pitchController.get_pid_info()); + plane.logger.Write_PID(LOG_PIDR_MSG, plane.rollController.get_pid_info()); + } + break; + } + case Stage::PUSH_NOSE_DOWN: { + plane.nav_pitch_cd = plane.aparm.pitch_limit_min*100; + plane.last_stabilize_ms = AP_HAL::millis(); + plane.stabilize_pitch(); + plane.nav_roll_cd = 0; + plane.stabilize_roll(); + plane.stabilize_yaw(); + ng_demand = 0.0f; + break; + } + case Stage::WAIT_LEVEL: + plane.nav_pitch_cd = MAX((plane.aparm.pitch_limit_min + 5), pitch_dem)*100; + plane.last_stabilize_ms = AP_HAL::millis(); + plane.stabilize_pitch(); + plane.nav_roll_cd = 0; + plane.stabilize_roll(); + plane.stabilize_yaw(); + ng_demand = 0.0f; + break; + case Stage::NONE: + break; + } +} + +#endif // AP_PLANE_GLIDER_PULLUP_ENABLED diff --git a/ArduPlane/pullup.h b/ArduPlane/pullup.h new file mode 100644 index 00000000000000..d085e143343f1e --- /dev/null +++ b/ArduPlane/pullup.h @@ -0,0 +1,50 @@ +#include "Plane.h" + +/* + support for pullup after NAV_ALTITUDE_WAIT for gliders + */ + +#ifndef AP_PLANE_GLIDER_PULLUP_ENABLED +#define AP_PLANE_GLIDER_PULLUP_ENABLED CONFIG_HAL_BOARD == HAL_BOARD_SITL +#endif + +#if AP_PLANE_GLIDER_PULLUP_ENABLED + +class GliderPullup +{ +public: + GliderPullup(void); + + void reset(void) { + stage = Stage::NONE; + } + bool in_pullup() const; + bool verify_pullup(void); + void stabilize_pullup(void); + bool pullup_complete(void); + bool pullup_start(void); + + enum class Stage : uint8_t { + NONE=0, + WAIT_AIRSPEED, + WAIT_PITCH, + WAIT_LEVEL, + PUSH_NOSE_DOWN, + }; + + static const struct AP_Param::GroupInfo var_info[]; + +private: + Stage stage; + AP_Int8 enable; + AP_Float elev_offset; // fraction of full elevator applied during WAIT_AIRSPEED and released during WAIT_PITCH + AP_Float ng_limit; + AP_Float airspeed_start; + AP_Float pitch_start; + AP_Float ng_jerk_limit; + AP_Float pitch_dem; + float ng_demand; +}; + +#endif // AP_PLANE_GLIDER_PULLUP_ENABLED +