From e66e82228f766fab3551c0b6568cfb0a51e52657 Mon Sep 17 00:00:00 2001 From: Jaeyoung-Lim Date: Sun, 7 Nov 2021 15:08:34 +0100 Subject: [PATCH] Add support for offboard velocity setpoints for fixedwing vehicles This commit adds a velocity controller which the setpoint can be passed using offboard setpoints --- .../FixedwingPositionControl.cpp | 84 +++++++++++++++++++ .../FixedwingPositionControl.hpp | 2 + 2 files changed, 86 insertions(+) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index b85b64ead0..33382936fe 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -787,6 +787,10 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c control_auto_position(now, curr_pos, ground_speed, pos_sp_prev, current_sp); break; + case position_setpoint_s::SETPOINT_TYPE_VELOCITY: + control_auto_velocity(now, curr_pos, ground_speed, pos_sp_prev, pos_sp_curr); + break; + case position_setpoint_s::SETPOINT_TYPE_LOITER: control_auto_loiter(now, curr_pos, ground_speed, pos_sp_prev, current_sp, pos_sp_next); break; @@ -926,6 +930,10 @@ FixedwingPositionControl::control_auto_descend(const hrt_abstime &now) uint8_t FixedwingPositionControl::handle_setpoint_type(const uint8_t setpoint_type, const position_setpoint_s &pos_sp_curr) { + if (!_control_mode.flag_control_position_enabled && _control_mode.flag_control_velocity_enabled) { + return position_setpoint_s::SETPOINT_TYPE_VELOCITY; + } + Vector2d curr_wp{0, 0}; /* current waypoint (the one currently heading for) */ @@ -1096,6 +1104,67 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const Ve radians(_param_fw_p_lim_min.get())); } +void +FixedwingPositionControl::control_auto_velocity(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) +{ + float mission_airspeed = _param_fw_airspd_trim.get(); + + if (PX4_ISFINITE(pos_sp_curr.cruising_speed) && + pos_sp_curr.cruising_speed > 0.1f) { + + mission_airspeed = pos_sp_curr.cruising_speed; + } + + float tecs_fw_thr_min; + float tecs_fw_thr_max; + float tecs_fw_mission_throttle; + + float mission_throttle = _param_fw_thr_cruise.get(); + + if (PX4_ISFINITE(pos_sp_curr.cruising_throttle) && + pos_sp_curr.cruising_throttle >= 0.0f) { + mission_throttle = pos_sp_curr.cruising_throttle; + } + + if (mission_throttle < _param_fw_thr_min.get()) { + /* enable gliding with this waypoint */ + _tecs.set_speed_weight(2.0f); + tecs_fw_thr_min = 0.0; + tecs_fw_thr_max = 0.0; + tecs_fw_mission_throttle = 0.0; + + } else { + tecs_fw_thr_min = _param_fw_thr_min.get(); + tecs_fw_thr_max = _param_fw_thr_max.get(); + tecs_fw_mission_throttle = mission_throttle; + } + + // waypoint is a plain navigation waypoint + float position_sp_alt = pos_sp_curr.alt; + + + //Offboard velocity control + Vector2f target_velocity{pos_sp_curr.vx, pos_sp_curr.vy}; + _target_bearing = wrap_pi(atan2f(target_velocity(1), target_velocity(0))); + _l1_control.navigate_heading(_target_bearing, _yaw, get_nav_speed_2d(target_velocity)); + + _att_sp.roll_body = _l1_control.get_roll_setpoint(); + _att_sp.yaw_body = _yaw; + + tecs_update_pitch_throttle(now, position_sp_alt, + calculate_target_airspeed(mission_airspeed, ground_speed), + radians(_param_fw_p_lim_min.get()), + radians(_param_fw_p_lim_max.get()), + tecs_fw_thr_min, + tecs_fw_thr_max, + tecs_fw_mission_throttle, + false, + radians(_param_fw_p_lim_min.get()), + tecs_status_s::TECS_MODE_NORMAL, + pos_sp_curr.vz); +} + void FixedwingPositionControl::control_auto_loiter(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, @@ -1951,6 +2020,21 @@ FixedwingPositionControl::Run() _pos_sp_triplet.current.cruising_throttle = NAN; // ignored } + } else if (PX4_ISFINITE(trajectory_setpoint.vx) && PX4_ISFINITE(trajectory_setpoint.vx) + && PX4_ISFINITE(trajectory_setpoint.vz)) { + _pos_sp_triplet = {}; // clear any existing + + _pos_sp_triplet.timestamp = trajectory_setpoint.timestamp; + _pos_sp_triplet.current.timestamp = trajectory_setpoint.timestamp; + _pos_sp_triplet.current.valid = true; + _pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; + _pos_sp_triplet.current.vx = trajectory_setpoint.vx; + _pos_sp_triplet.current.vy = trajectory_setpoint.vy; + _pos_sp_triplet.current.vz = trajectory_setpoint.vz; + _pos_sp_triplet.current.alt = NAN; + _pos_sp_triplet.current.cruising_speed = NAN; // ignored + _pos_sp_triplet.current.cruising_throttle = NAN; // ignored + } else { mavlink_log_critical(&_mavlink_log_pub, "Invalid offboard setpoint\t"); events::send(events::ID("fixedwing_position_control_invalid_offboard_sp"), events::Log::Error, diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 58f82f7193..3f2db4eb42 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -329,6 +329,8 @@ private: 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, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next); + void control_auto_velocity(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_takeoff(const hrt_abstime &now, const float dt, const Vector2d &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev,