fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 07:28:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

Support gliding setpoints for autonomous soaring (#14643)

Browse Source 
* Enable offboard gliding position setpoints

Adds a capability to make the vehicle glide when given position setpoints.

Gliding setpoints are enabled by setting POSITION_TARGET_TYPEMASK_Z_IGNORE, POSITION_TARGET_TYPEMASK_VZ_IGNORE,  POSITION_TARGET_TYPEMASK_AZ_IGNORE on the typemask of position setpoints.
Include zero cruising throttle
Invalid setpoints with nan

* Handle nans for curise_throttle
This commit is contained in:
JaeyoungLim
2020-09-14 10:56:29 +02:00
committed by GitHub
parent 8bc512dc93
commit 0592f6ba6e
5 changed files with 55 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+43 -8
View File
@@ -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()));
src/modules/mavlink/mavlink_receiver.cpp
+7
View File
@@ -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;
}
src/modules/navigator/mission.cpp
+2 -1
View File
@@ -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))));
}
src/modules/navigator/navigator.h
+2 -2
View File
@@ -240,7 +240,7 @@ public:
/**
* 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 @@ private:
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();
src/modules/navigator/navigator_main.cpp
+1 -1
View File
@@ -907,7 +907,7 @@ Navigator::get_cruising_throttle()
return _mission_throttle;
} else {
return -1.0f;
return NAN;
}
}