Fixed-wing Position controller: add modes for auto altitude and auto descend

- bit of clean up
- add GPS failsafe mode auto_altitude, that will keep current altitude with a fixed-bank angle
for some time, then switches to auto_descend that will descend with constant sink rate
of 0.5m/s
- params controlling GPS failsafe are not FW params: NAV_GPSF_R --> FW_GPSF_R and
NAV_GPSF_LT --> FW_GPSF_LT

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2021-10-25 11:48:05 +02:00 committed by Roman Bapst
parent c73a1b4c68
commit b77487d69c
4 changed files with 197 additions and 43 deletions

View File

@ -195,6 +195,20 @@ bool param_modify_on_import(bson_node_t node)
}
}
// 2021-10-21: translate NAV_GPSF_LT to FW_GPSF_LT and NAV_GPSF_R to FW_GPSF_R
{
if (strcmp("NAV_GPSF_LT", node->name) == 0) {
strcpy(node->name, "FW_GPSF_LT");
PX4_INFO("copying %s -> %s", "NAV_GPSF_LT", "FW_GPSF_LT");
}
if (strcmp("NAV_GPSF_R", node->name) == 0) {
strcpy(node->name, "FW_GPSF_R");
PX4_INFO("copying and inverting sign %s -> %s", "NAV_GPSF_R", "FW_GPSF_R");
}
}
// translate (SPI) calibration ID parameters. This can be removed after the next release (current release=1.10)
if (node->type != BSON_INT32) {

View File

@ -236,7 +236,6 @@ FixedwingPositionControl::vehicle_command_poll()
abort_landing(true);
}
}
}
}
@ -493,7 +492,7 @@ FixedwingPositionControl::status_publish()
pos_ctrl_status.timestamp = hrt_absolute_time();
pos_ctrl_status.type = _type;
pos_ctrl_status.type = _position_sp_type;
_pos_ctrl_status_pub.publish(pos_ctrl_status);
}
@ -671,7 +670,7 @@ FixedwingPositionControl::do_takeoff_help(float *hold_altitude, float *pitch_lim
}
void
FixedwingPositionControl::set_control_mode_current(bool pos_sp_curr_valid)
FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now, bool pos_sp_curr_valid)
{
/* only run position controller in fixed-wing mode and during transitions for VTOL */
if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.in_transition_mode) {
@ -679,11 +678,41 @@ FixedwingPositionControl::set_control_mode_current(bool pos_sp_curr_valid)
return; // do not publish the setpoint
}
if ((_control_mode.flag_control_auto_enabled || _control_mode.flag_control_offboard_enabled) && pos_sp_curr_valid) {
if (((_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled) ||
_control_mode.flag_control_offboard_enabled) && pos_sp_curr_valid) {
_control_mode_current = FW_POSCTRL_MODE_AUTO;
} else if (_control_mode.flag_control_velocity_enabled && _control_mode.flag_control_altitude_enabled) {
if (_control_mode_current != FW_POSCTRL_MODE_POSITION) {
} else if (_control_mode.flag_control_auto_enabled && _control_mode.flag_control_climb_rate_enabled) {
// reset timer the first time we switch into this mode
if (_control_mode_current != FW_POSCTRL_MODE_AUTO_ALTITUDE && _control_mode_current != FW_POSCTRL_MODE_AUTO_CLIMBRATE) {
_time_in_fixed_bank_loiter = now;
}
if (hrt_elapsed_time(&_time_in_fixed_bank_loiter) < (_param_nav_gpsf_lt.get() * 1_s)
&& !_vehicle_status.in_transition_mode) {
if (_control_mode_current != FW_POSCTRL_MODE_AUTO_ALTITUDE) {
// Need to init because last loop iteration was in a different mode
_hold_alt = _current_altitude;
mavlink_log_critical(&_mavlink_log_pub, "Start loiter with fixed bank angle.\t");
events::send(events::ID("fixedwing_position_control_fb_loiter"), events::Log::Critical,
"Start loiter with fixed bank angle");
}
_control_mode_current = FW_POSCTRL_MODE_AUTO_ALTITUDE;
} else {
if (_control_mode_current != FW_POSCTRL_MODE_AUTO_CLIMBRATE && !_vehicle_status.in_transition_mode) {
mavlink_log_critical(&_mavlink_log_pub, "Start descending.\t");
events::send(events::ID("fixedwing_position_control_descend"), events::Log::Critical, "Start descending");
}
_control_mode_current = FW_POSCTRL_MODE_AUTO_CLIMBRATE;
}
} else if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_position_enabled) {
if (_control_mode_current != FW_POSCTRL_MODE_MANUAL_POSITION) {
/* Need to init because last loop iteration was in a different mode */
_hold_alt = _current_altitude;
_hdg_hold_yaw = _yaw; // yaw is not controlled, so set setpoint to current yaw
@ -696,15 +725,16 @@ FixedwingPositionControl::set_control_mode_current(bool pos_sp_curr_valid)
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
}
_control_mode_current = FW_POSCTRL_MODE_POSITION;
_control_mode_current = FW_POSCTRL_MODE_MANUAL_POSITION;
} else if (_control_mode.flag_control_altitude_enabled) {
if (_control_mode_current != FW_POSCTRL_MODE_POSITION && _control_mode_current != FW_POSCTRL_MODE_ALTITUDE) {
} else if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_altitude_enabled) {
if (_control_mode_current != FW_POSCTRL_MODE_MANUAL_POSITION
&& _control_mode_current != FW_POSCTRL_MODE_MANUAL_ALTITUDE) {
/* Need to init because last loop iteration was in a different mode */
_hold_alt = _current_altitude;
}
_control_mode_current = FW_POSCTRL_MODE_ALTITUDE;
_control_mode_current = FW_POSCTRL_MODE_MANUAL_ALTITUDE;
} else {
_control_mode_current = FW_POSCTRL_MODE_OTHER;
@ -724,9 +754,6 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
_att_sp.fw_control_yaw = false; // by default we don't want yaw to be contoller directly with rudder
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF; // by default we don't use flaps
/* no throttle limit as default */
float throttle_max = 1.0f;
/* save time when airplane is in air */
if (!_was_in_air && !_landed) {
_was_in_air = true;
@ -767,11 +794,9 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
publishOrbitStatus(pos_sp_curr);
}
const uint8_t position_sp_type = handle_setpoint_type(pos_sp_curr.type, pos_sp_curr);
_position_sp_type = handle_setpoint_type(pos_sp_curr.type, pos_sp_curr);
_type = position_sp_type; // for publication
switch (position_sp_type) {
switch (_position_sp_type) {
case position_setpoint_s::SETPOINT_TYPE_IDLE:
_att_sp.thrust_body[0] = 0.0f;
_att_sp.roll_body = 0.0f;
@ -796,12 +821,12 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
}
/* reset landing state */
if (position_sp_type != position_setpoint_s::SETPOINT_TYPE_LAND) {
if (_position_sp_type != position_setpoint_s::SETPOINT_TYPE_LAND) {
reset_landing_state();
}
/* reset takeoff/launch state */
if (position_sp_type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
if (_position_sp_type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
reset_takeoff_state();
}
@ -823,7 +848,7 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
_runway_takeoff.runwayTakeoffEnabled()) {
_att_sp.thrust_body[0] = _runway_takeoff.getThrottle(now, min(get_tecs_thrust(), throttle_max));
_att_sp.thrust_body[0] = _runway_takeoff.getThrottle(now, get_tecs_thrust());
} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
@ -833,10 +858,10 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
/* Copy thrust and pitch values from tecs */
if (_landed) {
// when we are landed state we want the motor to spin at idle speed
_att_sp.thrust_body[0] = min(_param_fw_thr_idle.get(), throttle_max);
_att_sp.thrust_body[0] = min(_param_fw_thr_idle.get(), 1.f);
} else {
_att_sp.thrust_body[0] = min(get_tecs_thrust(), throttle_max);
_att_sp.thrust_body[0] = get_tecs_thrust();
}
}
@ -854,8 +879,68 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
if (use_tecs_pitch) {
_att_sp.pitch_body = get_tecs_pitch();
}
}
_last_manual = !_control_mode.flag_control_position_enabled;
void
FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const hrt_abstime &now)
{
// only control altitude and airspeed ("fixed-bank loiter")
tecs_update_pitch_throttle(now, _hold_alt,
_param_fw_airspd_trim.get(),
radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()),
_param_fw_thr_min.get(),
_param_fw_thr_max.get(),
_param_fw_thr_cruise.get(),
false,
_param_fw_p_lim_min.get());
_att_sp.roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle
_att_sp.yaw_body = 0.f;
if (_landed) {
_att_sp.thrust_body[0] = _param_fw_thr_min.get();
} else {
_att_sp.thrust_body[0] = min(get_tecs_thrust(), _param_fw_thr_max.get());
}
_att_sp.pitch_body = get_tecs_pitch();
}
void
FixedwingPositionControl::control_auto_descend(const hrt_abstime &now)
{
// only control height rate
// Hard-code descend rate to 0.5m/s. This is a compromise to give the system to recover,
// but not letting it drift too far away.
const float descend_rate = -0.5f;
tecs_update_pitch_throttle(now, _hold_alt,
_param_fw_airspd_trim.get(),
radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()),
_param_fw_thr_min.get(),
_param_fw_thr_max.get(),
_param_fw_thr_cruise.get(),
false,
_param_fw_p_lim_min.get(),
tecs_status_s::TECS_MODE_NORMAL,
descend_rate);
_att_sp.roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle
_att_sp.yaw_body = 0.f;
if (_landed) {
_att_sp.thrust_body[0] = _param_fw_thr_min.get();
} else {
_att_sp.thrust_body[0] = min(get_tecs_thrust(), _param_fw_thr_max.get());
}
_att_sp.pitch_body = get_tecs_pitch();
}
uint8_t
@ -927,6 +1012,11 @@ FixedwingPositionControl::handle_setpoint_type(const uint8_t setpoint_type, cons
}
}
// set to type loiter during VTOL transitions in Land mode (to not start FW landing logic)
if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LAND && _vehicle_status.in_transition_mode) {
position_sp_type = position_setpoint_s::SETPOINT_TYPE_LOITER;
}
return position_sp_type;
}
@ -1641,7 +1731,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec
}
void
FixedwingPositionControl::control_altitude(const hrt_abstime &now, const Vector2d &curr_pos,
FixedwingPositionControl::control_manual_altitude(const hrt_abstime &now, const Vector2d &curr_pos,
const Vector2f &ground_speed)
{
/* ALTITUDE CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */
@ -1693,12 +1783,10 @@ FixedwingPositionControl::control_altitude(const hrt_abstime &now, const Vector2
}
_att_sp.pitch_body = get_tecs_pitch();
_last_manual = !_control_mode.flag_control_position_enabled;
}
void
FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2d &curr_pos,
FixedwingPositionControl::control_manual_position(const hrt_abstime &now, const Vector2d &curr_pos,
const Vector2f &ground_speed)
{
const float dt = math::constrain((now - _control_position_last_called) * 1e-6f, 0.01f, 0.05f);
@ -1813,8 +1901,6 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
}
_att_sp.pitch_body = get_tecs_pitch();
_last_manual = !_control_mode.flag_control_position_enabled;
}
float
@ -1832,7 +1918,7 @@ float
FixedwingPositionControl::get_tecs_thrust()
{
if (_is_tecs_running) {
return _tecs.get_throttle_setpoint();
return min(_tecs.get_throttle_setpoint(), 1.f);
}
// return 0 to prevent stale tecs state when it's not running
@ -1959,7 +2045,7 @@ FixedwingPositionControl::Run()
Vector2d curr_pos(_current_latitude, _current_longitude);
Vector2f ground_speed(_local_pos.vx, _local_pos.vy);
set_control_mode_current(_pos_sp_triplet.current.valid);
set_control_mode_current(_local_pos.timestamp, _pos_sp_triplet.current.valid);
switch (_control_mode_current) {
case FW_POSCTRL_MODE_AUTO: {
@ -1968,13 +2054,23 @@ FixedwingPositionControl::Run()
break;
}
case FW_POSCTRL_MODE_POSITION: {
control_position(_local_pos.timestamp, curr_pos, ground_speed);
case FW_POSCTRL_MODE_AUTO_ALTITUDE: {
control_auto_fixed_bank_alt_hold(_local_pos.timestamp);
break;
}
case FW_POSCTRL_MODE_ALTITUDE: {
control_altitude(_local_pos.timestamp, curr_pos, ground_speed);
case FW_POSCTRL_MODE_AUTO_CLIMBRATE: {
control_auto_descend(_local_pos.timestamp);
break;
}
case FW_POSCTRL_MODE_MANUAL_POSITION: {
control_manual_position(_local_pos.timestamp, curr_pos, ground_speed);
break;
}
case FW_POSCTRL_MODE_MANUAL_ALTITUDE: {
control_manual_altitude(_local_pos.timestamp, curr_pos, ground_speed);
break;
}
@ -2008,7 +2104,8 @@ FixedwingPositionControl::Run()
if (_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_velocity_enabled ||
_control_mode.flag_control_acceleration_enabled ||
_control_mode.flag_control_altitude_enabled) {
_control_mode.flag_control_altitude_enabled ||
_control_mode.flag_control_climb_rate_enabled) {
const Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
q.copyTo(_att_sp.q_d);
@ -2025,6 +2122,7 @@ FixedwingPositionControl::Run()
}
_l1_control.reset_has_guidance_updated();
_last_manual = !_control_mode.flag_control_position_enabled;
}
perf_end(_loop_perf);
@ -2168,7 +2266,8 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo
bool in_air_alt_control = (!_landed &&
(_control_mode.flag_control_auto_enabled ||
_control_mode.flag_control_velocity_enabled ||
_control_mode.flag_control_altitude_enabled));
_control_mode.flag_control_altitude_enabled ||
_control_mode.flag_control_climb_rate_enabled));
/* update TECS vehicle state estimates */
_tecs.update_vehicle_state_estimates(_airspeed, _body_acceleration(0), (_local_pos.timestamp > 0), in_air_alt_control,

View File

@ -257,14 +257,18 @@ private:
float _manual_control_setpoint_altitude{0.0f};
float _manual_control_setpoint_airspeed{0.0f};
hrt_abstime _time_in_fixed_bank_loiter{0};
ECL_L1_Pos_Controller _l1_control;
TECS _tecs;
uint8_t _type{0};
uint8_t _position_sp_type{0};
enum FW_POSCTRL_MODE {
FW_POSCTRL_MODE_AUTO,
FW_POSCTRL_MODE_POSITION,
FW_POSCTRL_MODE_ALTITUDE,
FW_POSCTRL_MODE_AUTO_ALTITUDE,
FW_POSCTRL_MODE_AUTO_CLIMBRATE,
FW_POSCTRL_MODE_MANUAL_POSITION,
FW_POSCTRL_MODE_MANUAL_ALTITUDE,
FW_POSCTRL_MODE_OTHER
} _control_mode_current{FW_POSCTRL_MODE_OTHER}; ///< used to check the mode in the last control loop iteration. Use to check if the last iteration was in the same mode.
@ -327,6 +331,10 @@ private:
void control_auto(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed,
const position_setpoint_s &pos_sp_prev,
const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next);
void control_auto_fixed_bank_alt_hold(const hrt_abstime &now);
void control_auto_descend(const hrt_abstime &now);
void control_auto_position(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed,
const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
void control_auto_loiter(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed,
@ -338,8 +346,8 @@ private:
void control_auto_landing(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed,
const position_setpoint_s &pos_sp_prev,
const position_setpoint_s &pos_sp_curr);
void control_altitude(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed);
void control_position(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed);
void control_manual_altitude(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed);
void control_manual_position(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed);
float get_tecs_pitch();
float get_tecs_thrust();
@ -350,7 +358,7 @@ private:
void reset_takeoff_state(bool force = false);
void reset_landing_state();
Vector2f get_nav_speed_2d(const Vector2f &ground_speed);
void set_control_mode_current(bool pos_sp_curr_valid);
void set_control_mode_current(const hrt_abstime &now, bool pos_sp_curr_valid);
void publishOrbitStatus(const position_setpoint_s pos_sp);
@ -424,6 +432,10 @@ private:
(ParamBool<px4::params::FW_POSCTL_INV_ST>) _param_fw_posctl_inv_st,
(ParamInt<px4::params::FW_GPSF_LT>) _param_nav_gpsf_lt,
(ParamFloat<px4::params::FW_GPSF_R>) _param_nav_gpsf_r,
// external parameters
(ParamInt<px4::params::FW_ARSP_MODE>) _param_fw_arsp_mode,

View File

@ -825,3 +825,32 @@ PARAM_DEFINE_FLOAT(FW_T_CLMB_R_SP, 3.0f);
* @group FW TECS
*/
PARAM_DEFINE_FLOAT(FW_T_SINK_R_SP, 2.0f);
/**
* GPS failure loiter time
*
* The time in seconds the system should do open loop loiter and wait for GPS recovery
* before it starts descending. Set to 0 to disable. Roll angle is set to FW_GPSF_R.
* Does only apply for fixed-wing vehicles or VTOLs with NAV_FORCE_VT set to 0.
*
* @unit s
* @min 0
* @max 3600
* @group Mission
*/
PARAM_DEFINE_INT32(FW_GPSF_LT, 30);
/**
* GPS failure fixed roll angle
*
* Roll in degrees during the loiter after the vehicle has lost GPS in an auto mode (e.g. mission or loiter).
* Does only apply for fixed-wing vehicles or VTOLs with NAV_FORCE_VT set to 0.
*
* @unit deg
* @min 0.0
* @max 30.0
* @decimal 1
* @increment 0.5
* @group Mission
*/
PARAM_DEFINE_FLOAT(FW_GPSF_R, 15.0f);