fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 11:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Mission:

Browse Source 
- weathervane on takeoff
- separate cruising speeds for VTOL in MC and FW
- cruising speed resets
- mission work item logic is more clear
- fixed double execution of certain work item states
- enable cruise speed change on the fly by command
- moved VTOL transition target position generation to mission code and set always
This commit is contained in:
Andreas Antener
2016-12-28 11:53:19 +01:00
committed by Lorenz Meier
parent 2c78e9de5d
commit e0fc0a847c
10 changed files with 263 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_block.cpp
+8 -31
View File
@@ -79,7 +79,9 @@ MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
_param_yaw_timeout(this, "MIS_YAW_TMT", false),
_param_yaw_err(this, "MIS_YAW_ERR", false),
_param_vtol_wv_land(this, "VT_WV_LND_EN", false),
_param_vtol_wv_loiter(this, "VT_WV_LTR_EN", false)
_param_vtol_wv_takeoff(this, "VT_WV_TKO_EN", false),
_param_vtol_wv_loiter(this, "VT_WV_LTR_EN", false),
_param_force_vtol(this, "NAV_FORCE_VT", false)
{
}
@@ -131,19 +133,6 @@ MissionBlock::is_mission_item_reached()
}
case NAV_CMD_DO_CHANGE_SPEED:
// XXX not differentiating ground and airspeed yet
if (_mission_item.params[1] > 0.0f) {
_navigator->set_cruising_speed(_mission_item.params[1]);
} else {
_navigator->set_cruising_speed();
/* if no speed target was given try to set throttle */
if (_mission_item.params[2] > 0.0f) {
_navigator->set_cruising_throttle(_mission_item.params[2] / 100);
} else {
_navigator->set_cruising_throttle();
}
}
return true;
default:
@@ -498,22 +487,6 @@ MissionBlock::item_contains_position(const struct mission_item_s *item)
void
MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp)
{
/* set the correct setpoint for vtol transition */
if (item->nav_cmd == NAV_CMD_DO_VTOL_TRANSITION && PX4_ISFINITE(item->yaw)
&& item->params[0] >= vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW - 0.5f) {
sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION;
waypoint_from_heading_and_distance(_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
item->yaw,
1000000.0f,
&sp->lat,
&sp->lon);
sp->alt = _navigator->get_global_position()->alt;
}
/* don't change the setpoint for non-position items */
if (!item_contains_position(item)) {
return;
@@ -542,6 +515,9 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
sp->pitch_min = item->pitch_min;
case NAV_CMD_VTOL_TAKEOFF:
sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
if(_navigator->get_vstatus()->is_vtol && _param_vtol_wv_takeoff.get()){
sp->disable_mc_yaw_control = true;
}
break;
case NAV_CMD_LAND:
@@ -679,7 +655,8 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio
{
/* VTOL transition to RW before landing */
if (_navigator->get_vstatus()->is_vtol && !_navigator->get_vstatus()->is_rotary_wing) {
if (_navigator->get_vstatus()->is_vtol && !_navigator->get_vstatus()->is_rotary_wing &&
_param_force_vtol.get() == 1) {
struct vehicle_command_s cmd = {};
cmd.command = NAV_CMD_DO_VTOL_TRANSITION;
cmd.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;