Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fw_pos_control_l1 add new simple min groundspeed #11381

Merged
merged 2 commits into from
Aug 7, 2019
Merged

fw_pos_control_l1 add new simple min groundspeed #11381

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
9ee3655
fw_pos_control_l1 add new simple min groundspeed
dagar Jan 3, 2019
4f0575b
Merge branch 'master' into pr-fw_min_gndspeed
LorenzMeier Jun 25, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
65 changes: 20 additions & 45 deletions src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -421,7 +421,7 @@ FixedwingPositionControl::get_demanded_airspeed()
}

float
FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand)
FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand, const Vector2f &ground_speed)
{
/*
* Calculate accelerated stall airspeed factor from commanded bank angle and use it to increase minimum airspeed.
Expand All @@ -442,55 +442,32 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand)

if (_airspeed_valid && PX4_ISFINITE(_att_sp.roll_body)) {

adjusted_min_airspeed = constrain(_parameters.airspeed_min / sqrtf(cosf(_att_sp.roll_body)), _parameters.airspeed_min,
_parameters.airspeed_max);
adjusted_min_airspeed = constrain(_parameters.airspeed_min / sqrtf(cosf(_att_sp.roll_body)),
_parameters.airspeed_min, _parameters.airspeed_max);
}

// add minimum ground speed undershoot (only non-zero in presence of sufficient wind)
// sanity check: limit to range
return constrain(airspeed_demand + _groundspeed_undershoot, adjusted_min_airspeed, _parameters.airspeed_max);
}
// groundspeed undershoot
if (!_l1_control.circle_mode()) {

void
FixedwingPositionControl::calculate_gndspeed_undershoot(const Vector2f &curr_pos,
const Vector2f &ground_speed,
const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
{
if (pos_sp_curr.valid && !_l1_control.circle_mode()) {
/* rotate ground speed vector with current attitude */
// rotate ground speed vector with current attitude
Vector2f yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
yaw_vector.normalize();
float ground_speed_body = yaw_vector * ground_speed;

/* The minimum desired ground speed is the minimum airspeed projected on to the ground using the altitude and horizontal difference between the waypoints if available*/
float distance = 0.0f;
float delta_altitude = 0.0f;

if (pos_sp_prev.valid) {
distance = get_distance_to_next_waypoint(pos_sp_prev.lat, pos_sp_prev.lon, pos_sp_curr.lat, pos_sp_curr.lon);
delta_altitude = pos_sp_curr.alt - pos_sp_prev.alt;

} else {
distance = get_distance_to_next_waypoint((double)curr_pos(0), (double)curr_pos(1), pos_sp_curr.lat, pos_sp_curr.lon);
delta_altitude = pos_sp_curr.alt - _global_pos.alt;
}

float ground_speed_desired = _parameters.airspeed_min * cosf(atan2f(delta_altitude, distance));
const float ground_speed_body = yaw_vector * ground_speed;

/*
* Ground speed undershoot is the amount of ground velocity not reached
* by the plane. Consequently it is zero if airspeed is >= min ground speed
* and positive if airspeed < min ground speed.
*
* This error value ensures that a plane (as long as its throttle capability is
* not exceeded) travels towards a waypoint (and is not pushed more and more away
* by wind). Not countering this would lead to a fly-away.
*/
_groundspeed_undershoot = max(ground_speed_desired - ground_speed_body, 0.0f);

} else {
_groundspeed_undershoot = 0.0f;
if (ground_speed_body < _groundspeed_min.get()) {
airspeed_demand += max(_groundspeed_min.get() - ground_speed_body, 0.0f);
}
}

// add minimum ground speed undershoot (only non-zero in presence of sufficient wind)
// sanity check: limit to range
return constrain(airspeed_demand, adjusted_min_airspeed, _parameters.airspeed_max);
}

void
Expand Down Expand Up @@ -774,8 +751,6 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
_att_sp.fw_control_yaw = false; // by default we don't want yaw to be contoller directly with rudder
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF; // by default we don't use flaps

calculate_gndspeed_undershoot(curr_pos, ground_speed, pos_sp_prev, pos_sp_curr);

Vector2f nav_speed_2d{ground_speed};

if (_airspeed_valid) {
Expand Down Expand Up @@ -883,7 +858,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
_att_sp.yaw_body = _l1_control.nav_bearing();

tecs_update_pitch_throttle(pos_sp_curr.alt,
calculate_target_airspeed(mission_airspeed),
calculate_target_airspeed(mission_airspeed, ground_speed),
radians(_parameters.pitch_limit_min) - _parameters.pitchsp_offset_rad,
radians(_parameters.pitch_limit_max) - _parameters.pitchsp_offset_rad,
_parameters.throttle_min,
Expand Down Expand Up @@ -931,7 +906,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
}

tecs_update_pitch_throttle(alt_sp,
calculate_target_airspeed(mission_airspeed),
calculate_target_airspeed(mission_airspeed, ground_speed),
radians(_parameters.pitch_limit_min) - _parameters.pitchsp_offset_rad,
radians(_parameters.pitch_limit_max) - _parameters.pitchsp_offset_rad,
_parameters.throttle_min,
Expand Down Expand Up @@ -1257,7 +1232,7 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector
const float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_parameters.pitch_limit_max);

tecs_update_pitch_throttle(pos_sp_curr.alt,
calculate_target_airspeed(_runway_takeoff.getMinAirspeedScaling() * _parameters.airspeed_min),
calculate_target_airspeed(_runway_takeoff.getMinAirspeedScaling() * _parameters.airspeed_min, ground_speed),
radians(_parameters.pitch_limit_min),
radians(takeoff_pitch_max_deg),
_parameters.throttle_min,
Expand Down Expand Up @@ -1343,7 +1318,7 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector

} else {
tecs_update_pitch_throttle(pos_sp_curr.alt,
calculate_target_airspeed(_parameters.airspeed_trim),
calculate_target_airspeed(_parameters.airspeed_trim, ground_speed),
radians(_parameters.pitch_limit_min),
radians(_parameters.pitch_limit_max),
_parameters.throttle_min,
Expand Down Expand Up @@ -1557,7 +1532,7 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector
const float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f;

tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel,
calculate_target_airspeed(airspeed_land),
calculate_target_airspeed(airspeed_land, ground_speed),
radians(_parameters.land_flare_pitch_min_deg),
radians(_parameters.land_flare_pitch_max_deg),
0.0f,
Expand Down Expand Up @@ -1625,7 +1600,7 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector
const float airspeed_approach = _parameters.land_airspeed_scale * _parameters.airspeed_min;

tecs_update_pitch_throttle(altitude_desired,
calculate_target_airspeed(airspeed_approach),
calculate_target_airspeed(airspeed_approach, ground_speed),
radians(_parameters.pitch_limit_min),
radians(_parameters.pitch_limit_max),
_parameters.throttle_min,
Expand Down
7 changes: 4 additions & 3 deletions src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -371,6 +371,9 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
param_t vtol_type;
} _parameter_handles {}; ///< handles for interesting parameters */

DEFINE_PARAMETERS(
(ParamFloat<px4::params::FW_GND_SPD_MIN>) _groundspeed_min
)

// Update our local parameter cache.
int parameters_update();
Expand Down Expand Up @@ -436,9 +439,7 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
float get_tecs_thrust();

float get_demanded_airspeed();
float calculate_target_airspeed(float airspeed_demand);
void calculate_gndspeed_undershoot(const Vector2f &curr_pos, const Vector2f &ground_speed,
const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
float calculate_target_airspeed(float airspeed_demand, const Vector2f &ground_speed);

/**
* Handle incoming vehicle commands
Expand Down
15 changes: 15 additions & 0 deletions src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -727,3 +727,18 @@ PARAM_DEFINE_FLOAT(FW_T_HRATE_FF, 0.8f);
* @group FW TECS
*/
PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.02f);

/**
* Minimum groundspeed
*
* The controller will increase the commanded airspeed to maintain
* this minimum groundspeed to the next waypoint.
*
* @unit m/s
* @min 0.0
* @max 40
* @decimal 1
* @increment 0.5
* @group FW TECS
*/
PARAM_DEFINE_FLOAT(FW_GND_SPD_MIN, 5.0f);