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

fw_pos_control_l1: add takeoff minimum pitch parameter 

- remove mavlink mechanism for setting minimum pitch
This commit is contained in:
Daniel Agar 2018-04-04 18:47:24 -04:00
parent 89f19fd657
commit f61d8539cb
15 changed files with 39 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
integrationtests/python_src/px4_it/mavros/missions/FW_mission_1.plan
View File

@ -22,7 +22,7 @@
"doJumpId": 1,
"frame": 3,
"params": [
15,
0,
0,
0,
null,

2
integrationtests/python_src/px4_it/mavros/missions/MC_mission_box.plan
View File

@ -22,7 +22,7 @@
"doJumpId": 1,
"frame": 3,
"params": [
15,
0,
0,
0,
null,

2
integrationtests/python_src/px4_it/mavros/missions/VTOL_mission_1.plan
View File

@ -22,7 +22,7 @@
"doJumpId": 1,
"frame": 3,
"params": [
15,
0,
0,
0,
null,

2
integrationtests/python_src/px4_it/mavros/missions/avoidance.plan
View File

@ -22,7 +22,7 @@
"doJumpId": 1,
"frame": 3,
"params": [
15,
0,
0,
0,
null,

1
msg/position_setpoint.msg
View File

@ -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

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

@ -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
@ -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 */
@ -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());
}
}
}

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

@ -417,7 +417,9 @@ private:
(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
)

12
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
View File

@ -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);
/**
*
*

7
src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp
View File

@ -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;
}
}

2
src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h
View File

@ -86,7 +86,7 @@ public:
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();

22
src/modules/mavlink/mavlink_mission.cpp
View File

@ -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;
@ -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:
@ -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;

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

@ -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
@ -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 */
@ -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) &&

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

@ -582,9 +582,6 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
} else {
sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
// set pitch and ensure that the hold time is zero
sp->pitch_min = item.pitch_min;
}
break;
@ -667,7 +664,7 @@ MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance)
}
void
MissionBlock::set_takeoff_item(struct mission_item_s *item, float abs_altitude, float min_pitch)
MissionBlock::set_takeoff_item(struct mission_item_s *item, float abs_altitude)
{
item->nav_cmd = NAV_CMD_TAKEOFF;
@ -680,7 +677,6 @@ MissionBlock::set_takeoff_item(struct mission_item_s *item, float abs_altitude,
item->altitude_is_relative = false;
item->loiter_radius = _navigator->get_loiter_radius();
item->pitch_min = min_pitch;
item->autocontinue = false;
item->origin = ORIGIN_ONBOARD;
}

2
src/modules/navigator/mission_block.h
View File

@ -116,7 +116,7 @@ protected:
/**
* Set a takeoff mission item
*/
void set_takeoff_item(struct mission_item_s *item, float abs_altitude, float min_pitch = 0.0f);
void set_takeoff_item(struct mission_item_s *item, float abs_altitude);
/**
* Set a land mission item

1
src/modules/navigator/navigation.h
View File

@ -153,7 +153,6 @@ struct mission_item_s {
struct {
union {
float time_inside; /**< time that the MAV should stay inside the radius before advancing in seconds */
float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */
float circle_radius; /**< geofence circle radius in meters (only used for NAV_CMD_NAV_FENCE_CIRCLE*) */
};
float acceptance_radius; /**< default radius in which the mission is accepted as reached in meters */