dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

FW move altitude first order hold (FOH) and loiter to position special cases from navigator to position controller 

Co-authored-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
Daniel Agar 2020-10-06 16:57:12 -04:00 committed by GitHub
parent 9838e340e0
commit c01fabaf11
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
7 changed files with 125 additions and 218 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/position_controller_status.msg
View File

@ -14,3 +14,5 @@ float32 acceptance_radius # the optimal distance to a waypoint to switch to the
float32 yaw_acceptance # NaN if not set
float32 altitude_acceptance # the optimal vertical distance to a waypoint to switch to the next
uint8 type

112
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View File

@ -365,6 +365,8 @@ FixedwingPositionControl::status_publish()
pos_ctrl_status.timestamp = hrt_absolute_time();
pos_ctrl_status.type = _type;
_pos_ctrl_status_pub.publish(pos_ctrl_status);
}
@ -672,13 +674,97 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
mission_throttle = pos_sp_curr.cruising_throttle;
}
if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
const float acc_rad = _l1_control.switch_distance(500.0f);
uint8_t position_sp_type = pos_sp_curr.type;
if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
// TAKEOFF: handle like a regular POSITION setpoint if already flying
if (!in_takeoff_situation() && (_airspeed >= _param_fw_airspd_min.get() || !_airspeed_valid)) {
// SETPOINT_TYPE_TAKEOFF -> SETPOINT_TYPE_POSITION
position_sp_type = position_setpoint_s::SETPOINT_TYPE_POSITION;
}
} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION
|| pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
float dist_xy = -1.f;
float dist_z = -1.f;
const float dist = get_distance_to_point_global_wgs84(
(double)curr_wp(0), (double)curr_wp(1), pos_sp_curr.alt,
_current_latitude, _current_longitude, _current_altitude,
&dist_xy, &dist_z);
if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
// POSITION: achieve position setpoint altitude via loiter
// close to waypoint, but altitude error greater than twice acceptance
if ((dist >= 0.f)
&& (dist_z > 2.f * _param_fw_clmbout_diff.get())
&& (dist_xy < 2.f * math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius)))) {
// SETPOINT_TYPE_POSITION -> SETPOINT_TYPE_LOITER
position_sp_type = position_setpoint_s::SETPOINT_TYPE_LOITER;
} else if ((dist_xy < 2.f * math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius))) && !pos_sp_next.valid) {
position_sp_type = position_setpoint_s::SETPOINT_TYPE_LOITER;
}
} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
// LOITER: use SETPOINT_TYPE_POSITION to get to SETPOINT_TYPE_LOITER
if ((dist >= 0.f)
&& (dist_z > 2.f * _param_fw_clmbout_diff.get())
&& (dist_xy > 2.f * math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius)))) {
// SETPOINT_TYPE_LOITER -> SETPOINT_TYPE_POSITION
position_sp_type = position_setpoint_s::SETPOINT_TYPE_POSITION;
}
}
}
_type = position_sp_type;
if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
_att_sp.thrust_body[0] = 0.0f;
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
/* waypoint is a plain navigation waypoint */
} else if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
// waypoint is a plain navigation waypoint
float position_sp_alt = pos_sp_curr.alt;
// Altitude first order hold (FOH)
if (pos_sp_prev.valid && PX4_ISFINITE(pos_sp_prev.alt) &&
((pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_POSITION) ||
(pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_LOITER) ||
(pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF))
) {
const float d_curr_prev = get_distance_to_next_waypoint((double)curr_wp(0), (double)curr_wp(1),
pos_sp_prev.lat, pos_sp_prev.lon);
// Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one
if (d_curr_prev > math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius))) {
// Calculate distance to current waypoint
const float d_curr = get_distance_to_next_waypoint((double)curr_wp(0), (double)curr_wp(1),
_current_latitude, _current_longitude);
// Save distance to waypoint if it is the smallest ever achieved, however make sure that
// _min_current_sp_distance_xy is never larger than the distance between the current and the previous wp
_min_current_sp_distance_xy = math::min(math::min(d_curr, _min_current_sp_distance_xy), d_curr_prev);
// if the minimal distance is smaller than the acceptance radius, we should be at waypoint alt
// navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude
if (_min_current_sp_distance_xy > math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius))) {
// The setpoint is set linearly and such that the system reaches the current altitude at the acceptance
// radius around the current waypoint
const float delta_alt = (pos_sp_curr.alt - pos_sp_prev.alt);
const float grad = -delta_alt / (d_curr_prev - math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius)));
const float a = pos_sp_prev.alt - grad * d_curr_prev;
position_sp_alt = a + grad * _min_current_sp_distance_xy;
}
}
}
_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, nav_speed_2d);
_att_sp.roll_body = _l1_control.get_roll_setpoint();
_att_sp.yaw_body = _l1_control.nav_bearing();
@ -700,7 +786,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
tecs_fw_mission_throttle = mission_throttle;
}
tecs_update_pitch_throttle(now, pos_sp_curr.alt,
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_psp_off.get()),
radians(_param_fw_p_lim_max.get()) - radians(_param_fw_psp_off.get()),
@ -711,8 +797,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
radians(_param_fw_p_lim_min.get()));
} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
} else if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
/* waypoint is a loiter waypoint */
float loiter_radius = pos_sp_curr.loiter_radius;
uint8_t loiter_direction = pos_sp_curr.loiter_direction;
@ -786,20 +871,20 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
false,
radians(_param_fw_p_lim_min.get()));
} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
} else if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_LAND) {
control_landing(now, curr_pos, ground_speed, pos_sp_prev, pos_sp_curr);
} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
} else if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
control_takeoff(now, curr_pos, ground_speed, pos_sp_prev, pos_sp_curr);
}
/* reset landing state */
if (pos_sp_curr.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 (pos_sp_curr.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
if (position_sp_type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
reset_takeoff_state();
}
@ -1576,7 +1661,12 @@ FixedwingPositionControl::Run()
airspeed_poll();
_manual_control_setpoint_sub.update(&_manual_control_setpoint);
_pos_sp_triplet_sub.update(&_pos_sp_triplet);
if (_pos_sp_triplet_sub.update(&_pos_sp_triplet)) {
// reset the altitude foh (first order hold) logic
_min_current_sp_distance_xy = FLT_MAX;
}
vehicle_attitude_poll();
vehicle_command_poll();
vehicle_control_mode_poll();

3
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
View File

@ -194,6 +194,8 @@ private:
float _althold_epv{0.0f}; ///< the position estimate accuracy when engaging alt hold
bool _was_in_deadband{false}; ///< wether the last stick input was in althold deadband
float _min_current_sp_distance_xy{FLT_MAX};
position_setpoint_s _hdg_hold_prev_wp {}; ///< position where heading hold started
position_setpoint_s _hdg_hold_curr_wp {}; ///< position to which heading hold flies
@ -262,6 +264,7 @@ private:
ECL_L1_Pos_Controller _l1_control;
TECS _tecs;
uint8_t _type{0};
enum FW_POSCTRL_MODE {
FW_POSCTRL_MODE_AUTO,
FW_POSCTRL_MODE_POSITION,

90
src/modules/navigator/mission.cpp
View File

@ -222,10 +222,6 @@ Mission::on_active()
set_mission_items();
}
} else if (_mission_type != MISSION_TYPE_NONE && _param_mis_altmode.get() == MISSION_ALTMODE_FOH) {
altitude_sp_foh_update();
} else {
/* if waypoint position reached allow loiter on the setpoint */
if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) {
@ -584,9 +580,6 @@ Mission::advance_mission()
void
Mission::set_mission_items()
{
/* reset the altitude foh (first order hold) logic, if altitude foh is enabled (param) a new foh element starts now */
_min_current_sp_distance_xy = FLT_MAX;
/* the home dist check provides user feedback, so we initialize it to this */
bool user_feedback_done = false;
@ -1152,14 +1145,6 @@ Mission::set_mission_items()
}
}
/* Save the distance between the current sp and the previous one */
if (pos_sp_triplet->current.valid && pos_sp_triplet->previous.valid) {
_distance_current_previous = get_distance_to_next_waypoint(
pos_sp_triplet->current.lat, pos_sp_triplet->current.lon,
pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon);
}
_navigator->set_position_setpoint_triplet_updated();
}
@ -1354,81 +1339,6 @@ Mission::heading_sp_update()
}
}
void
Mission::altitude_sp_foh_update()
{
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
/* Don't change setpoint if last and current waypoint are not valid
* or if the previous altitude isn't from a position or loiter setpoint or
* if rotary wing since that is handled in the mc_pos_control
*/
if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid || !PX4_ISFINITE(pos_sp_triplet->previous.alt)
|| !(pos_sp_triplet->previous.type == position_setpoint_s::SETPOINT_TYPE_POSITION ||
pos_sp_triplet->previous.type == position_setpoint_s::SETPOINT_TYPE_LOITER) ||
_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
return;
}
// Calculate acceptance radius, i.e. the radius within which we do not perform a first order hold anymore
float acc_rad = _navigator->get_acceptance_radius(_mission_item.acceptance_radius);
if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
acc_rad = _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f);
}
/* Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one */
if (_distance_current_previous - acc_rad < FLT_EPSILON) {
return;
}
/* Don't do FOH for non-missions, landing and takeoff waypoints, the ground may be near
* and the FW controller has a custom landing logic
*
* NAV_CMD_LOITER_TO_ALT doesn't change altitude until reaching desired lat/lon
* */
if (_mission_item.nav_cmd == NAV_CMD_LAND
|| _mission_item.nav_cmd == NAV_CMD_VTOL_LAND
|| _mission_item.nav_cmd == NAV_CMD_TAKEOFF
|| _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT
|| !_navigator->is_planned_mission()) {
return;
}
/* Calculate distance to current waypoint */
float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
/* Save distance to waypoint if it is the smallest ever achieved, however make sure that
* _min_current_sp_distance_xy is never larger than the distance between the current and the previous wp */
_min_current_sp_distance_xy = math::min(math::min(d_current, _min_current_sp_distance_xy),
_distance_current_previous);
/* if the minimal distance is smaller then the acceptance radius, we should be at waypoint alt
* navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude */
if (_min_current_sp_distance_xy < acc_rad) {
pos_sp_triplet->current.alt = get_absolute_altitude_for_item(_mission_item);
} else {
/* update the altitude sp of the 'current' item in the sp triplet, but do not update the altitude sp
* of the mission item as it is used to check if the mission item is reached
* The setpoint is set linearly and such that the system reaches the current altitude at the acceptance
* radius around the current waypoint
**/
float delta_alt = (get_absolute_altitude_for_item(_mission_item) - pos_sp_triplet->previous.alt);
float grad = -delta_alt / (_distance_current_previous - acc_rad);
float a = pos_sp_triplet->previous.alt - grad * _distance_current_previous;
pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy;
}
// we set altitude directly so we can run this in parallel to the heading update
_navigator->set_position_setpoint_triplet_updated();
}
void
Mission::cruising_speed_sp_update()
{

16
src/modules/navigator/mission.h
View File

@ -76,11 +76,6 @@ public:
void on_activation() override;
void on_active() override;
enum mission_altitude_mode {
MISSION_ALTMODE_ZOH = 0,
MISSION_ALTMODE_FOH = 1
};
bool set_current_mission_index(uint16_t index);
bool land_start();
@ -155,11 +150,6 @@ private:
*/
void heading_sp_update();
/**
* Updates the altitude sp to follow a foh
*/
void altitude_sp_foh_update();
/**
* Update the cruising speed setpoint.
*/
@ -243,7 +233,6 @@ private:
DEFINE_PARAMETERS(
(ParamFloat<px4::params::MIS_DIST_1WP>) _param_mis_dist_1wp,
(ParamFloat<px4::params::MIS_DIST_WPS>) _param_mis_dist_wps,
(ParamInt<px4::params::MIS_ALTMODE>) _param_mis_altmode,
(ParamInt<px4::params::MIS_MNT_YAW_CTL>) _param_mis_mnt_yaw_ctl
)
@ -272,11 +261,6 @@ private:
bool _mission_waypoints_changed{false};
bool _mission_changed{false}; /** < true if the mission changed since the mission mode was active */
float _min_current_sp_distance_xy{FLT_MAX}; /**< minimum distance which was achieved to the current waypoint */
float _distance_current_previous{0.0f}; /**< distance from previous to current sp in pos_sp_triplet,
only use if current and previous are valid */
enum work_item_type {
WORK_ITEM_TYPE_DEFAULT, /**< default mission item */
WORK_ITEM_TYPE_TAKEOFF, /**< takeoff before moving to waypoint */

105
src/modules/navigator/mission_block.cpp
View File

@ -150,40 +150,6 @@ MissionBlock::is_mission_item_reached()
_navigator->get_global_position()->alt,
&dist_xy, &dist_z);
/* FW special case for NAV_CMD_WAYPOINT to achieve altitude via loiter */
if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING &&
(_mission_item.nav_cmd == NAV_CMD_WAYPOINT)) {
struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current;
/* close to waypoint, but altitude error greater than twice acceptance */
if ((dist >= 0.0f)
&& (dist_z > 2 * _navigator->get_altitude_acceptance_radius())
&& (dist_xy < 2 * _navigator->get_loiter_radius())) {
/* SETPOINT_TYPE_POSITION -> SETPOINT_TYPE_LOITER */
if (curr_sp->type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
curr_sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER;
curr_sp->loiter_radius = _navigator->get_loiter_radius();
curr_sp->loiter_direction = 1;
_navigator->set_position_setpoint_triplet_updated();
}
} else {
/* restore SETPOINT_TYPE_POSITION */
if (curr_sp->type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
/* loiter acceptance criteria required to revert back to SETPOINT_TYPE_POSITION */
if ((dist >= 0.0f)
&& (dist_z < _navigator->get_loiter_radius())
&& (dist_xy <= _navigator->get_loiter_radius() * 1.2f)) {
curr_sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION;
_navigator->set_position_setpoint_triplet_updated();
}
}
}
}
if ((_mission_item.nav_cmd == NAV_CMD_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF)
&& _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
@ -218,6 +184,7 @@ MissionBlock::is_mission_item_reached()
} else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING &&
(_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT)) {
/* Loiter mission item on a non rotary wing: the aircraft is going to circle the
* coordinates with a radius equal to the loiter_radius field. It is not flying
* through the waypoint center.
@ -239,50 +206,24 @@ MissionBlock::is_mission_item_reached()
} else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING &&
(_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT)) {
if (dist >= 0.f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f)
&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
// NAV_CMD_LOITER_TO_ALT only uses mission item altitude once it's in the loiter
// first check if the altitude setpoint is the mission setpoint
struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current;
_waypoint_position_reached = true;
if (fabsf(curr_sp->alt - altitude_amsl) >= FLT_EPSILON) {
// check if the initial loiter has been accepted
dist_xy = -1.0f;
dist_z = -1.0f;
// set required yaw from bearing to the next mission item
if (_mission_item.force_heading) {
const position_setpoint_s &next_sp = _navigator->get_position_setpoint_triplet()->next;
dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, curr_sp->alt,
_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
_navigator->get_global_position()->alt,
&dist_xy, &dist_z);
if (next_sp.valid) {
_mission_item.yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
next_sp.lat, next_sp.lon);
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f)
&& dist_z <= _navigator->get_default_altitude_acceptance_radius()) {
_waypoint_yaw_reached = false;
// now set the loiter to the final altitude in the NAV_CMD_LOITER_TO_ALT mission item
curr_sp->alt = altitude_amsl;
_navigator->set_position_setpoint_triplet_updated();
}
} else {
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f)
&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
_waypoint_position_reached = true;
// set required yaw from bearing to the next mission item
if (_mission_item.force_heading) {
const position_setpoint_s &next_sp = _navigator->get_position_setpoint_triplet()->next;
if (next_sp.valid) {
_mission_item.yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
next_sp.lat, next_sp.lon);
_waypoint_yaw_reached = false;
} else {
_waypoint_yaw_reached = true;
}
} else {
_waypoint_yaw_reached = true;
}
}
}
@ -365,7 +306,6 @@ MissionBlock::is_mission_item_reached()
}
/* Check if the waypoint and the requested yaw setpoint. */
if (_waypoint_position_reached && !_waypoint_yaw_reached) {
if ((_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
@ -615,20 +555,13 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
sp->type = position_setpoint_s::SETPOINT_TYPE_LAND;
break;
case NAV_CMD_LOITER_TO_ALT:
// initially use current altitude, and switch to mission item altitude once in loiter position
if (_navigator->get_loiter_min_alt() > 0.0f) { // ignore _param_loiter_min_alt if smaller than 0 (-1)
sp->alt = math::max(_navigator->get_global_position()->alt,
_navigator->get_home_position()->alt + _navigator->get_loiter_min_alt());
} else {
sp->alt = _navigator->get_global_position()->alt;
}
// fall through
case NAV_CMD_LOITER_TIME_LIMIT:
// FALLTHROUGH
case NAV_CMD_LOITER_UNLIMITED:
// FALLTHROUGH
case NAV_CMD_LOITER_TO_ALT:
sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER;
break;

15
src/modules/navigator/mission_params.c
View File

@ -114,21 +114,6 @@ PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 900);
*/
PARAM_DEFINE_FLOAT(MIS_DIST_WPS, 900);
/**
* Altitude setpoint mode
*
* 0: the system will follow a zero order hold altitude setpoint
* 1: the system will follow a first order hold altitude setpoint
* values follow the definition in enum mission_altitude_mode
*
* @min 0
* @max 1
* @value 0 Zero Order Hold
* @value 1 First Order Hold
* @group Mission
*/
PARAM_DEFINE_INT32(MIS_ALTMODE, 1);
/**
* Enable yaw control of the mount. (Only affects multicopters and ROI mission items)
*