diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 1109b82e6a74..d47de8b9e875 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -668,8 +668,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2 float mission_throttle = _param_fw_thr_cruise.get(); if (PX4_ISFINITE(pos_sp_curr.cruising_throttle) && - pos_sp_curr.cruising_throttle > 0.01f) { - + pos_sp_curr.cruising_throttle >= 0.0f) { mission_throttle = pos_sp_curr.cruising_throttle; } @@ -684,16 +683,34 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2 _att_sp.roll_body = _l1_control.get_roll_setpoint(); _att_sp.yaw_body = _l1_control.nav_bearing(); + float tecs_fw_thr_min; + float tecs_fw_thr_max; + float tecs_fw_mission_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; + } + tecs_update_pitch_throttle(now, pos_sp_curr.alt, calculate_target_airspeed(mission_airspeed, ground_speed), radians(_param_fw_p_lim_min.get()) - radians(_param_fw_psp_off.get()), radians(_param_fw_p_lim_max.get()) - radians(_param_fw_psp_off.get()), - _param_fw_thr_min.get(), - _param_fw_thr_max.get(), - mission_throttle, + tecs_fw_thr_min, + tecs_fw_thr_max, + tecs_fw_mission_throttle, false, radians(_param_fw_p_lim_min.get())); + } else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { /* waypoint is a loiter waypoint */ @@ -741,13 +758,31 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2 _tecs.set_time_const_throt(_param_fw_thrtc_sc.get() * _param_fw_t_thro_const.get()); } + + float tecs_fw_thr_min; + float tecs_fw_thr_max; + float tecs_fw_mission_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 = _param_fw_thr_cruise.get(); + } + tecs_update_pitch_throttle(now, alt_sp, calculate_target_airspeed(mission_airspeed, ground_speed), radians(_param_fw_p_lim_min.get()) - radians(_param_fw_psp_off.get()), radians(_param_fw_p_lim_max.get()) - radians(_param_fw_psp_off.get()), - _param_fw_thr_min.get(), - _param_fw_thr_max.get(), - _param_fw_thr_cruise.get(), + tecs_fw_thr_min, + tecs_fw_thr_max, + tecs_fw_mission_throttle, false, radians(_param_fw_p_lim_min.get())); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index f2d0f56475c1..c6f6ca3c15c2 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -811,6 +811,10 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t bool is_land_sp = (bool)(set_position_target_local_ned.type_mask & 0x2000); bool is_loiter_sp = (bool)(set_position_target_local_ned.type_mask & 0x3000); bool is_idle_sp = (bool)(set_position_target_local_ned.type_mask & 0x4000); + bool is_gliding_sp = (bool)(set_position_target_local_ned.type_mask & + (POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_Z_IGNORE + | POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_VZ_IGNORE + | POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_AZ_IGNORE)); offboard_control_mode.timestamp = hrt_absolute_time(); _offboard_control_mode_pub.publish(offboard_control_mode); @@ -851,6 +855,9 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t } else if (is_idle_sp) { pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_IDLE; + } else if (is_gliding_sp) { + pos_sp_triplet.current.cruising_throttle = 0.0f; + } else { pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; } diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index fd3aad8a2053..7db1125ec2e3 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -1912,6 +1912,7 @@ bool Mission::position_setpoint_equal(const position_setpoint_s *p1, const posit (p1->acceleration_is_force == p2->acceleration_is_force) && (fabsf(p1->acceptance_radius - p2->acceptance_radius) < FLT_EPSILON) && (fabsf(p1->cruising_speed - p2->cruising_speed) < FLT_EPSILON) && - (fabsf(p1->cruising_throttle - p2->cruising_throttle) < FLT_EPSILON)); + ((fabsf(p1->cruising_throttle - p2->cruising_throttle) < FLT_EPSILON) || (!PX4_ISFINITE(p1->cruising_throttle) + && !PX4_ISFINITE(p2->cruising_throttle)))); } diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 5aa9a092143e..a92e131b0474 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -240,7 +240,7 @@ class Navigator : public ModuleBase, public ModuleParams /** * Set the target throttle */ - void set_cruising_throttle(float throttle = -1.0f) { _mission_throttle = throttle; } + void set_cruising_throttle(float throttle = NAN) { _mission_throttle = throttle; } /** * Get the acceptance radius given the mission item preset radius @@ -393,7 +393,7 @@ class Navigator : public ModuleBase, public ModuleParams float _mission_cruising_speed_mc{-1.0f}; float _mission_cruising_speed_fw{-1.0f}; - float _mission_throttle{-1.0f}; + float _mission_throttle{NAN}; // update subscriptions void params_update(); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 13b2cb1a8366..142f732c804d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -907,7 +907,7 @@ Navigator::get_cruising_throttle() return _mission_throttle; } else { - return -1.0f; + return NAN; } }