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

Fixed-wing pitch and roll offset clean up #16792

Merged
merged 3 commits into from
Feb 12, 2021
Merged

Fixed-wing pitch and roll offset clean up #16792

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
0419ed6
fw_pos_control_l1: add takeoff minimum pitch parameter
dagar Apr 4, 2018
99c6b90
fw_pos_control_l1: apply pitch setpoint offset centrally
sfuhrer Feb 8, 2021
43cba8e
Fixed-wing: remove roll offset param (FW_RSP_OFF)
sfuhrer Feb 11, 2021
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
2 changes: 1 addition & 1 deletion integrationtests/python_src/px4_it/mavros/missions/FW_mission_1.plan
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@
"doJumpId": 1,
"frame": 3,
"params": [
15,
0,
0,
0,
null,
Expand Down
2 changes: 1 addition & 1 deletion integrationtests/python_src/px4_it/mavros/missions/MC_mission_box.plan
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@
"doJumpId": 1,
"frame": 3,
"params": [
15,
0,
0,
0,
null,
Expand Down
2 changes: 1 addition & 1 deletion integrationtests/python_src/px4_it/mavros/missions/VTOL_mission_1.plan
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@
"doJumpId": 1,
"frame": 3,
"params": [
15,
0,
0,
0,
null,
Expand Down
2 changes: 1 addition & 1 deletion integrationtests/python_src/px4_it/mavros/missions/avoidance.plan
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@
"doJumpId": 1,
"frame": 3,
"params": [
15,
0,
0,
0,
null,
Expand Down
1 change: 0 additions & 1 deletion msg/position_setpoint.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,6 @@ int8 landing_gear # landing gear: see definition of the states in landing_gear.

float32 loiter_radius # loiter radius (only for fixed wing), in m
int8 loiter_direction # loiter direction: 1 = CW, -1 = CCW
float32 pitch_min # minimal pitch angle for fixed wing takeoff waypoints

float32 a_x # acceleration x setpoint
float32 a_y # acceleration y setpoint
Expand Down
4 changes: 1 addition & 3 deletions src/modules/fw_att_control/FixedwingAttitudeControl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -157,9 +157,7 @@ FixedwingAttitudeControl::vehicle_manual_poll()
if (_vcontrol_mode.flag_control_attitude_enabled) {
// STABILIZED mode generate the attitude setpoint from manual user inputs

_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get()) + radians(_param_fw_rsp_off.get());
_att_sp.roll_body = constrain(_att_sp.roll_body,
-radians(_param_fw_man_r_max.get()), radians(_param_fw_man_r_max.get()));
_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get());

_att_sp.pitch_body = -_manual_control_setpoint.x * radians(_param_fw_man_p_max.get())
+ radians(_param_fw_psp_off.get());
Expand Down
1 change: 0 additions & 1 deletion src/modules/fw_att_control/FixedwingAttitudeControl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -193,7 +193,6 @@ class FixedwingAttitudeControl final : public ModuleBase<FixedwingAttitudeContro
(ParamFloat<px4::params::FW_RR_I>) _param_fw_rr_i,
(ParamFloat<px4::params::FW_RR_IMAX>) _param_fw_rr_imax,
(ParamFloat<px4::params::FW_RR_P>) _param_fw_rr_p,
(ParamFloat<px4::params::FW_RSP_OFF>) _param_fw_rsp_off,

(ParamBool<px4::params::FW_W_EN>) _param_fw_w_en,
(ParamFloat<px4::params::FW_W_RMAX>) _param_fw_w_rmax,
Expand Down
22 changes: 3 additions & 19 deletions src/modules/fw_att_control/fw_att_control_params.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -414,27 +414,11 @@ PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f);
PARAM_DEFINE_FLOAT(FW_WR_FF, 0.2f);

/**
* Roll setpoint offset
*
* An airframe specific offset of the roll setpoint in degrees, the value is
* added to the roll setpoint and should correspond to the typical cruise speed
* of the airframe.
*
* @unit deg
* @min -90.0
* @max 90.0
* @decimal 1
* @increment 0.5
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_RSP_OFF, 0.0f);

/**
* Pitch setpoint offset
* Pitch setpoint offset (pitch at level flight)
*
* An airframe specific offset of the pitch setpoint in degrees, the value is
* added to the pitch setpoint and should correspond to the typical cruise
* speed of the airframe.
* added to the pitch setpoint and should correspond to the pitch at
* typical cruise speed of the airframe.
*
* @unit deg
* @min -90.0
Expand Down
48 changes: 21 additions & 27 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 @@ -789,7 +789,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
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;
_att_sp.pitch_body = radians(_param_fw_psp_off.get());

} else if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
// waypoint is a plain navigation waypoint
Expand Down Expand Up @@ -834,8 +834,8 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2

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()),
radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()),
tecs_fw_thr_min,
tecs_fw_thr_max,
tecs_fw_mission_throttle,
Expand Down Expand Up @@ -891,8 +891,8 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2

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()),
radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()),
tecs_fw_thr_min,
tecs_fw_thr_max,
tecs_fw_mission_throttle,
Expand Down Expand Up @@ -1222,7 +1222,7 @@ FixedwingPositionControl::control_takeoff(const hrt_abstime &now, const Vector2f
_param_fw_thr_max.get(), // XXX should we also set runway_takeoff_throttle here?
_param_fw_thr_cruise.get(),
_runway_takeoff.climbout(),
radians(_runway_takeoff.getMinPitch(pos_sp_curr.pitch_min, 10.0f, _param_fw_p_lim_min.get())),
radians(_runway_takeoff.getMinPitch(_takeoff_pitch_min.get(), _param_fw_p_lim_min.get())),
tecs_status_s::TECS_MODE_TAKEOFF);

// assign values
Expand Down Expand Up @@ -1293,7 +1293,7 @@ FixedwingPositionControl::control_takeoff(const hrt_abstime &now, const Vector2f
takeoff_throttle,
_param_fw_thr_cruise.get(),
true,
max(radians(pos_sp_curr.pitch_min), radians(10.0f)),
radians(_takeoff_pitch_min.get()),
tecs_status_s::TECS_MODE_TAKEOFF);

/* limit roll motion to ensure enough lift */
Expand All @@ -1320,7 +1320,7 @@ FixedwingPositionControl::control_takeoff(const hrt_abstime &now, const Vector2f

/* Set default roll and pitch setpoints during detection phase */
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = max(radians(pos_sp_curr.pitch_min), radians(10.0f));
_att_sp.pitch_body = radians(_takeoff_pitch_min.get());
}
}
}
Expand Down Expand Up @@ -1528,7 +1528,7 @@ FixedwingPositionControl::control_landing(const hrt_abstime &now, const Vector2f

if (!_land_noreturn_vertical) {
// just started with the flaring phase
_flare_pitch_sp = 0.0f;
_flare_pitch_sp = radians(_param_fw_psp_off.get());
_flare_height = _current_altitude - terrain_alt;
mavlink_log_info(&_mavlink_log_pub, "Landing, flaring");
_land_noreturn_vertical = true;
Expand Down Expand Up @@ -1599,11 +1599,11 @@ float
FixedwingPositionControl::get_tecs_pitch()
{
if (_is_tecs_running) {
return _tecs.get_pitch_setpoint();
return _tecs.get_pitch_setpoint() + radians(_param_fw_psp_off.get());
}

// return 0 to prevent stale tecs state when it's not running
return 0.0f;
// return level flight pitch offset to prevent stale tecs state when it's not running
return radians(_param_fw_psp_off.get());
}

float
Expand Down Expand Up @@ -1715,11 +1715,6 @@ FixedwingPositionControl::Run()
if (control_position(_local_pos.timestamp, curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current,
_pos_sp_triplet.next)) {


// add attitude setpoint offsets
_att_sp.roll_body += radians(_param_fw_rsp_off.get());
_att_sp.pitch_body += radians(_param_fw_psp_off.get());

if (_control_mode.flag_control_manual_enabled) {
_att_sp.roll_body = constrain(_att_sp.roll_body, -radians(_param_fw_man_r_max.get()),
radians(_param_fw_man_r_max.get()));
Expand Down Expand Up @@ -1827,14 +1822,14 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo

} else if (_was_in_transition) {
// after transition we ramp up desired airspeed from the speed we had coming out of the transition
_asp_after_transition += dt * 2; // increase 2m/s
_asp_after_transition += dt * 2.0f; // increase 2m/s

if (_asp_after_transition < airspeed_sp && _airspeed < airspeed_sp) {
airspeed_sp = max(_asp_after_transition, _airspeed);

} else {
_was_in_transition = false;
_asp_after_transition = 0;
_asp_after_transition = 0.0f;
}
}
}
Expand All @@ -1854,17 +1849,14 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo

if (_vehicle_status.engine_failure) {
/* Force the slow downwards spiral */
pitch_min_rad = M_DEG_TO_RAD_F * -1.0f;
pitch_max_rad = M_DEG_TO_RAD_F * 5.0f;
pitch_min_rad = radians(-1.0f);
Copy link
Member

@dagar dagar Feb 8, 2021 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Note, we should revisit engine failure.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Do you know of anybody that uses it actually?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I did a few years ago. I'd bet people don't even know it exists.

pitch_max_rad = radians(5.0f);
}

/* No underspeed protection in landing mode */
_tecs.set_detect_underspeed_enabled(!(mode == tecs_status_s::TECS_MODE_LAND
|| mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM));

/* Using tecs library */
float pitch_for_tecs = _pitch - radians(_param_fw_psp_off.get());

/* tell TECS to update its state, but let it know when it cannot actually control the plane */
bool in_air_alt_control = (!_landed &&
(_control_mode.flag_control_auto_enabled ||
Expand Down Expand Up @@ -1892,12 +1884,14 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo
}
}

_tecs.update_pitch_throttle(pitch_for_tecs,
_tecs.update_pitch_throttle(_pitch - radians(_param_fw_psp_off.get()),
_current_altitude, alt_sp,
airspeed_sp, _airspeed, _eas2tas,
climbout_mode, climbout_pitch_min_rad,
climbout_mode,
climbout_pitch_min_rad - radians(_param_fw_psp_off.get()),
throttle_min, throttle_max, throttle_cruise,
pitch_min_rad, pitch_max_rad);
pitch_min_rad - radians(_param_fw_psp_off.get()),
pitch_max_rad - radians(_param_fw_psp_off.get()));

tecs_status_publish();
}
Expand Down
5 changes: 3 additions & 2 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 @@ -413,11 +413,12 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
(ParamInt<px4::params::FW_ARSP_MODE>) _param_fw_arsp_mode,

(ParamFloat<px4::params::FW_PSP_OFF>) _param_fw_psp_off,
(ParamFloat<px4::params::FW_RSP_OFF>) _param_fw_rsp_off,
(ParamFloat<px4::params::FW_MAN_P_MAX>) _param_fw_man_p_max,
(ParamFloat<px4::params::FW_MAN_R_MAX>) _param_fw_man_r_max,

(ParamFloat<px4::params::NAV_LOITER_RAD>) _param_nav_loiter_rad
(ParamFloat<px4::params::NAV_LOITER_RAD>) _param_nav_loiter_rad,

(ParamFloat<px4::params::FW_TKO_PITCH_MIN>) _takeoff_pitch_min

)

Expand Down
12 changes: 12 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 @@ -268,6 +268,18 @@ PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 10.0f);
*/
PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f);

/**
* Minimum pitch during takeoff.
*
* @unit deg
* @min -5.0
* @max 30.0
* @decimal 1
* @increment 0.5
* @group FW L1 Control
*/
PARAM_DEFINE_FLOAT(FW_TKO_PITCH_MIN, 10.0f);

/**
*
*
Expand Down
7 changes: 3 additions & 4 deletions src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -221,13 +221,12 @@ bool RunwayTakeoff::resetIntegrators()
* the climbtout minimum pitch (parameter).
* Otherwise use the minimum that is enforced generally (parameter).
*/
float RunwayTakeoff::getMinPitch(float sp_min, float climbout_min, float min)
float RunwayTakeoff::getMinPitch(float climbout_min, float min)
{
if (_state < RunwayTakeoffState::FLY) {
return math::max(sp_min, climbout_min);
}
return climbout_min;

else {
} else {
return min;
}
}
Expand Down
2 changes: 1 addition & 1 deletion src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -86,7 +86,7 @@ class __EXPORT RunwayTakeoff : public ModuleParams
float getYaw(float navigatorYaw);
float getThrottle(const hrt_abstime &now, float tecsThrottle);
bool resetIntegrators();
float getMinPitch(float sp_min, float climbout_min, float min);
float getMinPitch(float climbout_min, float min);
float getMaxPitch(float max);
matrix::Vector2f getStartWP();

Expand Down
22 changes: 11 additions & 11 deletions src/modules/mavlink/mavlink_mission.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1305,13 +1305,6 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
mission_item->altitude_is_relative = true;
}

/* this field is shared with pitch_min (and circle_radius for geofence) in memory and
* exclusive in the MAVLink spec. Set it to 0 first
* and then set minimum pitch later only for the
* corresponding item
*/
mission_item->time_inside = 0.0f;

switch (mavlink_mission_item->command) {
case MAV_CMD_NAV_WAYPOINT:
mission_item->nav_cmd = NAV_CMD_WAYPOINT;
Expand Down Expand Up @@ -1345,9 +1338,17 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
break;

case MAV_CMD_NAV_TAKEOFF:
mission_item->nav_cmd = NAV_CMD_TAKEOFF;
mission_item->pitch_min = mavlink_mission_item->param1;
mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4));

// reject takeoff item if minimum pitch (parameter 1) is set
if (PX4_ISFINITE(mavlink_mission_item->param1) && (fabsf(mavlink_mission_item->param1) > FLT_EPSILON)) {
_mavlink->send_statustext_critical("Takeoff rejected, remove deprecated minimum pitch");
return MAV_MISSION_INVALID_PARAM1;

} else {
mission_item->nav_cmd = NAV_CMD_TAKEOFF;
mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4));
}

break;

case MAV_CMD_NAV_LOITER_TO_ALT:
Expand Down Expand Up @@ -1652,7 +1653,6 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
break;

case NAV_CMD_TAKEOFF:
mavlink_mission_item->param1 = mission_item->pitch_min;
mavlink_mission_item->param4 = math::degrees(mission_item->yaw);
break;

Expand Down
10 changes: 0 additions & 10 deletions src/modules/navigator/mission.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -752,11 +752,6 @@ Mission::set_mission_items()
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
/* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */
_mission_item.yaw = NAN;
/* since _mission_item.time_inside and and _mission_item.pitch_min build a union, we need to set time_inside to zero
* since in NAV_CMD_TAKEOFF mode there is currently no time_inside.
* Note also that resetting time_inside to zero will cause pitch_min to be zero as well.
*/
_mission_item.time_inside = 0.0f;

} else if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
&& _work_item_type == WORK_ITEM_TYPE_DEFAULT
Expand All @@ -783,10 +778,6 @@ Mission::set_mission_items()
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
/* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */
_mission_item.yaw = NAN;
/* since _mission_item.time_inside and and _mission_item.pitch_min build a union, we need to set time_inside to zero
* since in NAV_CMD_TAKEOFF mode there is currently no time_inside.
*/
_mission_item.time_inside = 0.0f;
}

/* if we just did a VTOL takeoff, prepare transition */
Expand Down Expand Up @@ -1864,7 +1855,6 @@ bool Mission::position_setpoint_equal(const position_setpoint_s *p1, const posit
(p1->yawspeed_valid == p2->yawspeed_valid) &&
(fabsf(p1->loiter_radius - p2->loiter_radius) < FLT_EPSILON) &&
(p1->loiter_direction == p2->loiter_direction) &&
(fabsf(p1->pitch_min - p2->pitch_min) < FLT_EPSILON) &&
(fabsf(p1->a_x - p2->a_x) < FLT_EPSILON) &&
(fabsf(p1->a_y - p2->a_y) < FLT_EPSILON) &&
(fabsf(p1->a_z - p2->a_z) < FLT_EPSILON) &&
Expand Down
Loading