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

FW Position Control: control_backtransition(): always track line from start (#22853) 

Remove option to track from previous wp to reduce complexity and fix
case where prev=current point and the line following broke down.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2024-03-08 19:11:08 +01:00 committed by GitHub
parent 04099ed483
commit 85a882e1ce
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
2 changed files with 8 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
src/modules/fw_pos_control/FixedwingPositionControl.cpp
View File

@ -2201,7 +2201,6 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
} }
void FixedwingPositionControl::control_backtransition(const float control_interval, const Vector2f &ground_speed, void FixedwingPositionControl::control_backtransition(const float control_interval, const Vector2f &ground_speed,
const position_setpoint_s &pos_sp_prev,
const position_setpoint_s &pos_sp_curr) const position_setpoint_s &pos_sp_curr)
{ {
float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed, float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed,
@ -2213,22 +2212,14 @@ void FixedwingPositionControl::control_backtransition(const float control_interv
_npfg.setAirspeedNom(target_airspeed * _eas2tas); _npfg.setAirspeedNom(target_airspeed * _eas2tas);
_npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas);
// Set the position where the backtransition started the first ime we pass through here.
if (_position_setpoint_previous_valid) { // Will get reset if not in transition anymore.
Vector2f prev_wp_local = _global_local_proj_ref.project(pos_sp_prev.lat, pos_sp_prev.lon); if (!_lpos_where_backtrans_started.isAllFinite()) {
navigateLine(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); _lpos_where_backtrans_started = curr_pos_local;
} else {
// if we don't have a previous waypoint for line following, then create one using the current position at the
// start of the transition
if (!_lpos_where_backtrans_started.isAllFinite()) {
_lpos_where_backtrans_started = curr_pos_local;
}
navigateLine(_lpos_where_backtrans_started, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
} }
navigateLine(_lpos_where_backtrans_started, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
_att_sp.roll_body = getCorrectedNpfgRollSetpoint(); _att_sp.roll_body = getCorrectedNpfgRollSetpoint();
target_airspeed = _npfg.getAirspeedRef() / _eas2tas; target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
@ -2452,6 +2443,7 @@ FixedwingPositionControl::Run()
if (_vehicle_status_sub.update(&_vehicle_status)) { if (_vehicle_status_sub.update(&_vehicle_status)) {
if (!_vehicle_status.in_transition_mode) { if (!_vehicle_status.in_transition_mode) {
// reset position of backtransition start if not in transition
_lpos_where_backtrans_started = Vector2f(NAN, NAN); _lpos_where_backtrans_started = Vector2f(NAN, NAN);
} }
} }
@ -2551,8 +2543,7 @@ FixedwingPositionControl::Run()
} }
case FW_POSCTRL_MODE_TRANSITON: { case FW_POSCTRL_MODE_TRANSITON: {
control_backtransition(control_interval, ground_speed, _pos_sp_triplet.previous, control_backtransition(control_interval, ground_speed, _pos_sp_triplet.current);
_pos_sp_triplet.current);
break; break;
} }
} }

2
src/modules/fw_pos_control/FixedwingPositionControl.hpp
View File

@ -710,11 +710,9 @@ private:
* *
* @param control_interval Time since last position control call [s] * @param control_interval Time since last position control call [s]
* @param ground_speed Local 2D ground speed of vehicle [m/s] * @param ground_speed Local 2D ground speed of vehicle [m/s]
* @param pos_sp_prev previous position setpoint
* @param pos_sp_curr current position setpoint * @param pos_sp_curr current position setpoint
*/ */
void control_backtransition(const float control_interval, const Vector2f &ground_speed, void control_backtransition(const float control_interval, const Vector2f &ground_speed,
const position_setpoint_s &pos_sp_prev,
const position_setpoint_s &pos_sp_curr); const position_setpoint_s &pos_sp_curr);
float get_tecs_pitch(); float get_tecs_pitch();