diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 7da86efbb0..4e65aec2c1 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -132,6 +132,7 @@ VtolAttitudeControl::VtolAttitudeControl() : _params_handles.vtol_type = param_find("VT_TYPE"); _params_handles.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK"); _params_handles.fw_min_alt = param_find("VT_FW_MIN_ALT"); + _params_handles.fw_alt_err = param_find("VT_FW_ALT_ERR"); _params_handles.fw_qc_max_pitch = param_find("VT_FW_QC_P"); _params_handles.fw_qc_max_roll = param_find("VT_FW_QC_R"); _params_handles.front_trans_time_openloop = param_find("VT_F_TR_OL_TM"); @@ -364,6 +365,22 @@ VtolAttitudeControl::vehicle_local_pos_poll() } +/** +* Check for setpoint updates. +*/ +void +VtolAttitudeControl::vehicle_local_pos_sp_poll() +{ + bool updated; + /* Check if parameters have changed */ + orb_check(_local_pos_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_sub, &_local_pos_sp); + } + +} + /** * Check for position setpoint updates. */ @@ -571,6 +588,10 @@ VtolAttitudeControl::parameters_update() param_get(_params_handles.fw_min_alt, &v); _params.fw_min_alt = v; + /* maximum negative altitude error for FW mode (Adaptive QuadChute) */ + param_get(_params_handles.fw_alt_err, &v); + _params.fw_alt_err = v; + /* maximum pitch angle (QuadChute) */ param_get(_params_handles.fw_qc_max_pitch, &l); _params.fw_qc_max_pitch = l; @@ -666,6 +687,7 @@ void VtolAttitudeControl::task_main() _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + _local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _battery_status_sub = orb_subscribe(ORB_ID(battery_status)); @@ -754,6 +776,7 @@ void VtolAttitudeControl::task_main() vehicle_rates_sp_fw_poll(); parameters_update_poll(); vehicle_local_pos_poll(); // Check for new sensor values + vehicle_local_pos_sp_poll(); pos_sp_triplet_poll(); vehicle_airspeed_poll(); vehicle_battery_poll(); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 9e009dcbde..8a34aca282 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -75,6 +75,7 @@ #include #include #include +#include #include #include #include @@ -114,6 +115,7 @@ public: struct actuator_controls_s *get_actuators_mc_in() {return &_actuators_mc_in;} struct actuator_controls_s *get_actuators_fw_in() {return &_actuators_fw_in;} struct vehicle_local_position_s *get_local_pos() {return &_local_pos;} + struct vehicle_local_position_setpoint_s *get_local_pos_sp() {return &_local_pos_sp;} struct position_setpoint_triplet_s *get_pos_sp_triplet() {return &_pos_sp_triplet;} struct airspeed_s *get_airspeed() {return &_airspeed;} struct battery_status_s *get_batt_status() {return &_batt_status;} @@ -140,6 +142,7 @@ private: int _params_sub; //parameter updates subscription int _manual_control_sp_sub; //manual control setpoint subscription int _local_pos_sub; // sensor subscription + int _local_pos_sp_sub; // setpoint subscription int _pos_sp_triplet_sub; // local position setpoint subscription int _airspeed_sub; // airspeed subscription int _battery_status_sub; // battery status subscription @@ -173,6 +176,7 @@ private: struct actuator_controls_s _actuators_mc_in; //actuator controls from mc_att_control struct actuator_controls_s _actuators_fw_in; //actuator controls from fw_att_control struct vehicle_local_position_s _local_pos; + struct vehicle_local_position_setpoint_s _local_pos_sp; struct position_setpoint_triplet_s _pos_sp_triplet; struct airspeed_s _airspeed; // airspeed struct battery_status_s _batt_status; // battery status @@ -196,6 +200,7 @@ private: param_t vtol_type; param_t elevons_mc_lock; param_t fw_min_alt; + param_t fw_alt_err; param_t fw_qc_max_pitch; param_t fw_qc_max_roll; param_t front_trans_time_openloop; @@ -227,6 +232,7 @@ private: void vehicle_rates_sp_mc_poll(); void vehicle_rates_sp_fw_poll(); void vehicle_local_pos_poll(); // Check for changes in sensor values + void vehicle_local_pos_sp_poll(); // Check for changes in setpoint values void pos_sp_triplet_poll(); // Check for changes in position setpoint values void vehicle_airspeed_poll(); // Check for changes in airspeed void vehicle_attitude_setpoint_poll(); //Check for attitude setpoint updates. diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index 6f8329a200..554c9d2f83 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -308,6 +308,16 @@ PARAM_DEFINE_FLOAT(VT_TRANS_MIN_TM, 2.0f); */ PARAM_DEFINE_FLOAT(VT_FW_MIN_ALT, 0.0f); +/** + * Adaptive QuadChute + * + * Maximum negative altitude error, when in fixed wing the altitude drops below this copared to the altitude setpoint + * the vehicle will transition back to MC mode and enter failsafe RTL + * @min 0.0 + * @max 200.0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_FW_ALT_ERR, 0.0f); /** * QuadChute Max Pitch diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 298c62355e..a8bd5c7699 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -65,6 +65,7 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) : _actuators_mc_in = _attc->get_actuators_mc_in(); _actuators_fw_in = _attc->get_actuators_fw_in(); _local_pos = _attc->get_local_pos(); + _local_pos_sp = _attc->get_local_pos_sp(); _airspeed = _attc->get_airspeed(); _batt_status = _attc->get_batt_status(); _tecs_status = _attc->get_tecs_status(); @@ -220,7 +221,28 @@ void VtolType::check_quadchute_condition() if (_params->fw_min_alt > FLT_EPSILON) { if (-(_local_pos->z) < _params->fw_min_alt) { - _attc->abort_front_transition("Minimum altitude breached"); + _attc->abort_front_transition("QuadChute: Minimum altitude breached"); + } + } + + // adaptive quadchute + // We use tecs for tracking in FW and local_pos_sp during transitions + if (_params->fw_alt_err > FLT_EPSILON) { + float altErr = 0.0f; + + if (_tecs_running) { + altErr = _tecs_status->altitudeSp - _tecs_status->altitude_filtered; + + } else { + altErr = -_local_pos_sp->z - -_local_pos->z; + + if (!_local_pos->z_valid) { + altErr = 0.0f; + } + } + + if (altErr > _params->fw_alt_err) { + _attc->abort_front_transition("QuadChute: Altitude error too large"); } } diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 7a05971eb8..b7f82c9c48 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -60,6 +60,7 @@ struct Params { int32_t vtol_type; int32_t elevons_mc_lock; // lock elevons in multicopter mode float fw_min_alt; // minimum relative altitude for FW mode (QuadChute) + float fw_alt_err; // maximum negative altitude error for FW mode (Adaptive QuadChute) float fw_qc_max_pitch; // maximum pitch angle FW mode (QuadChute) float fw_qc_max_roll; // maximum roll angle FW mode (QuadChute) float front_trans_time_openloop; @@ -168,6 +169,7 @@ protected: struct actuator_controls_s *_actuators_mc_in; //actuator controls from mc_att_control struct actuator_controls_s *_actuators_fw_in; //actuator controls from fw_att_control struct vehicle_local_position_s *_local_pos; + struct vehicle_local_position_setpoint_s *_local_pos_sp; struct airspeed_s *_airspeed; // airspeed struct battery_status_s *_batt_status; // battery status struct tecs_status_s *_tecs_status;