fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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-09-19 21:48:10 +02:00 committed by Lorenz Meier
parent 2c78e9de5d
commit e0fc0a847c
10 changed files with 263 additions and 104 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -73,7 +73,7 @@ Mission::Mission(Navigator *navigator, const char *name) :
_param_dist_1wp(this, "MIS_DIST_1WP", false),
_param_altmode(this, "MIS_ALTMODE", false),
_param_yawmode(this, "MIS_YAWMODE", false),
_param_force_vtol(this, "VT_NAV_FORCE_VT", false),
_param_force_vtol(this, "NAV_FORCE_VT", false),
_param_fw_climbout_diff(this, "FW_CLMBOUT_DIFF", false),
_onboard_mission{},
_offboard_mission{},
@ -130,6 +130,7 @@ Mission::on_inactive()
if (need_to_reset_mission(false)) {
reset_offboard_mission(_offboard_mission);
update_offboard_mission();
_navigator->reset_cruising_speed();
}
} else {
@ -159,13 +160,10 @@ Mission::on_inactive()
/* require takeoff after non-loiter or landing */
if (!_navigator->get_can_loiter_at_sp() || _navigator->get_land_detected()->landed) {
_need_takeoff = true;
/* Reset work item type to default if auto take-off has been paused or aborted,
and we landed in manual mode. */
if (_work_item_type == WORK_ITEM_TYPE_TAKEOFF) {
_work_item_type = WORK_ITEM_TYPE_DEFAULT;
}
}
/* reset so current mission item gets restarted if mission was paused */
_work_item_type = WORK_ITEM_TYPE_DEFAULT;
}
void
@ -198,6 +196,7 @@ Mission::on_active()
if (need_to_reset_mission(true)) {
reset_offboard_mission(_offboard_mission);
update_offboard_mission();
_navigator->reset_cruising_speed();
offboard_updated = true;
}
@ -330,6 +329,8 @@ Mission::update_onboard_mission()
_navigator->get_mission_result()->seq_reached = 0;
_navigator->get_mission_result()->seq_total = _onboard_mission.count;
/* reset work item if new mission has been accepted */
_work_item_type = WORK_ITEM_TYPE_DEFAULT;
_navigator->increment_mission_instance_count();
_navigator->set_mission_result_updated();
@ -378,6 +379,9 @@ Mission::update_offboard_mission()
_navigator->get_mission_result()->reached = false;
_navigator->get_mission_result()->seq_reached = 0;
_navigator->get_mission_result()->seq_total = _offboard_mission.count;
/* reset work item if new mission has been accepted */
_work_item_type = WORK_ITEM_TYPE_DEFAULT;
}
} else {
@ -533,9 +537,9 @@ Mission::set_mission_items()
if (item_contains_position(&_mission_item)) {
/* force vtol land */
if (_mission_item.nav_cmd == NAV_CMD_LAND && _navigator->get_vstatus()->is_vtol
&& _param_force_vtol.get() && !_navigator->get_vstatus()->is_rotary_wing) {
if (_mission_item.nav_cmd == NAV_CMD_LAND && _param_force_vtol.get() == 1
&& !_navigator->get_vstatus()->is_rotary_wing
&& _navigator->get_vstatus()->is_vtol) {
_mission_item.nav_cmd = NAV_CMD_VTOL_LAND;
}
@ -543,7 +547,9 @@ Mission::set_mission_items()
set_previous_pos_setpoint();
/* do takeoff before going to setpoint if needed and not already in takeoff */
if (do_need_takeoff() && _work_item_type != WORK_ITEM_TYPE_TAKEOFF) {
if (do_need_takeoff() &&
_work_item_type == WORK_ITEM_TYPE_DEFAULT &&
new_work_item_type == WORK_ITEM_TYPE_DEFAULT) {
new_work_item_type = WORK_ITEM_TYPE_TAKEOFF;
@ -566,40 +572,63 @@ Mission::set_mission_items()
_mission_item.altitude_is_relative = false;
_mission_item.autocontinue = true;
_mission_item.time_inside = 0.0f;
}
/* if we just did a takeoff navigate to the actual waypoint now */
if (_work_item_type == WORK_ITEM_TYPE_TAKEOFF) {
} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
/* if there is no need to do a takeoff but we have a takeoff item, treat is as waypoint */
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
/* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */
_mission_item.yaw = NAN;
if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
&& _navigator->get_vstatus()->is_rotary_wing
&& !_navigator->get_land_detected()->landed
&& has_next_position_item) {
} else if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) {
/* check if the vtol_takeoff command is on top of us */
if (do_need_move_to_takeoff()) {
new_work_item_type = WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF;
} else {
new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
}
_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
_mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW;
_mission_item.yaw = _navigator->get_global_position()->yaw;
if (_navigator->get_vstatus()->is_rotary_wing) {
/* haven't transitioned yet, trigger vtol takeoff logic below */
_work_item_type = WORK_ITEM_TYPE_TAKEOFF;
} else {
new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
/* already in fixed-wing, go to waypoint */
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
/* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */
_mission_item.yaw = NAN;
_mission_item.time_inside = 0.0f;
}
/* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */
_mission_item.yaw = NAN;
}
/* if we just did a normal takeoff navigate to the actual waypoint now */
if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF &&
_work_item_type == WORK_ITEM_TYPE_TAKEOFF &&
new_work_item_type == WORK_ITEM_TYPE_DEFAULT) {
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
/* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */
_mission_item.yaw = NAN;
}
/* if we just did a VTOL takeoff, prepare transition */
if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF &&
_work_item_type == WORK_ITEM_TYPE_TAKEOFF &&
new_work_item_type == WORK_ITEM_TYPE_DEFAULT &&
_navigator->get_vstatus()->is_rotary_wing &&
!_navigator->get_land_detected()->landed) {
/* check if the vtol_takeoff waypoint is on top of us */
if (do_need_move_to_takeoff()) {
new_work_item_type = WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF;
}
_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
_mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW;
_mission_item.yaw = _navigator->get_global_position()->yaw;
/* set position setpoint to target during the transition */
// TODO: if has_next_position_item and use_next set next, or if use_heading set generated
generate_waypoint_from_heading(&pos_sp_triplet->current, _mission_item.yaw);
}
/* takeoff completed and transitioned, move to takeoff wp as fixed wing */
if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
&& _work_item_type == WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF) {
&& _work_item_type == WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF
&& new_work_item_type == WORK_ITEM_TYPE_DEFAULT) {
new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
@ -610,15 +639,19 @@ Mission::set_mission_items()
/* move to land wp as fixed wing */
if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND
&& _work_item_type == WORK_ITEM_TYPE_DEFAULT
&& new_work_item_type == WORK_ITEM_TYPE_DEFAULT
&& !_navigator->get_land_detected()->landed) {
new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;
/* use current mission item as next position item */
memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s));
has_next_position_item = true;
float altitude = _navigator->get_global_position()->alt;
if (pos_sp_triplet->current.valid) {
if (pos_sp_triplet->current.valid
&& pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
altitude = pos_sp_triplet->current.alt;
}
@ -632,6 +665,7 @@ Mission::set_mission_items()
/* transition to MC */
if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND
&& _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND
&& new_work_item_type == WORK_ITEM_TYPE_DEFAULT
&& !_navigator->get_vstatus()->is_rotary_wing
&& !_navigator->get_land_detected()->landed) {
@ -644,7 +678,8 @@ Mission::set_mission_items()
/* move to landing waypoint before descent if necessary */
if (do_need_move_to_land() &&
(_work_item_type == WORK_ITEM_TYPE_DEFAULT ||
_work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) {
_work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION) &&
new_work_item_type == WORK_ITEM_TYPE_DEFAULT) {
new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;
@ -661,6 +696,11 @@ Mission::set_mission_items()
*/
float altitude = _navigator->get_global_position()->alt;
if (pos_sp_triplet->current.valid
&& pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
altitude = pos_sp_triplet->current.alt;
}
_mission_item.altitude = altitude;
_mission_item.altitude_is_relative = false;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
@ -670,10 +710,11 @@ Mission::set_mission_items()
}
/* we just moved to the landing waypoint, now descend */
if (_work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND
&& _navigator->get_vstatus()->is_rotary_wing) {
if (_work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND &&
new_work_item_type == WORK_ITEM_TYPE_DEFAULT) {
new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
/* XXX: noop */
}
/* ignore yaw for landing items */
@ -688,19 +729,53 @@ Mission::set_mission_items()
/* turn towards next waypoint before MC to FW transition */
if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
&& _work_item_type != WORK_ITEM_TYPE_ALIGN
&& _work_item_type == WORK_ITEM_TYPE_DEFAULT
&& new_work_item_type == WORK_ITEM_TYPE_DEFAULT
&& _navigator->get_vstatus()->is_rotary_wing
&& !_navigator->get_land_detected()->landed
&& has_next_position_item) {
new_work_item_type = WORK_ITEM_TYPE_ALIGN;
set_align_mission_item(&_mission_item, &mission_item_next_position);
/* set position setpoint to target during the transition */
mission_item_to_position_setpoint(&mission_item_next_position, &pos_sp_triplet->current);
}
/* yaw is aligned now */
if (_work_item_type == WORK_ITEM_TYPE_ALIGN) {
if (_work_item_type == WORK_ITEM_TYPE_ALIGN &&
new_work_item_type == WORK_ITEM_TYPE_DEFAULT) {
new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
/* set position setpoint to target during the transition */
set_previous_pos_setpoint();
generate_waypoint_from_heading(&pos_sp_triplet->current, pos_sp_triplet->current.yaw);
}
/* don't advance mission after FW to MC command */
if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
&& _work_item_type == WORK_ITEM_TYPE_DEFAULT
&& new_work_item_type == WORK_ITEM_TYPE_DEFAULT
&& !_navigator->get_vstatus()->is_rotary_wing
&& !_navigator->get_land_detected()->landed
&& pos_sp_triplet->current.valid) {
new_work_item_type = WORK_ITEM_TYPE_CMD_BEFORE_MOVE;
}
/* after FW to MC transition finish moving to the waypoint */
if (_work_item_type == WORK_ITEM_TYPE_CMD_BEFORE_MOVE &&
new_work_item_type == WORK_ITEM_TYPE_DEFAULT
&& pos_sp_triplet->current.valid) {
new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
copy_positon_if_valid(&_mission_item, &pos_sp_triplet->current);
_mission_item.autocontinue = true;
_mission_item.time_inside = 0;
}
}
@ -766,24 +841,29 @@ Mission::do_need_takeoff()
float takeoff_alt = calculate_takeoff_altitude(&_mission_item);
/* force takeoff if landed (additional protection) */
if (_navigator->get_land_detected()->landed) {
/* force takeoff if landed (additional protection) */
_need_takeoff = true;
} else if (_navigator->get_global_position()->alt > takeoff_alt - _navigator->get_acceptance_radius()) {
/* if in-air and already above takeoff height, don't do takeoff */
} else if (_navigator->get_global_position()->alt > takeoff_alt) {
_need_takeoff = false;
} else if (_navigator->get_global_position()->alt <= takeoff_alt - _navigator->get_acceptance_radius()
&& (_mission_item.nav_cmd == NAV_CMD_TAKEOFF
|| _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF)) {
/* if in-air but below takeoff height and we have a takeoff item */
_need_takeoff = true;
}
/* check if current mission item is one that requires takeoff before */
if (_need_takeoff && (
_mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
_mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT ||
_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF)) {
_mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH)) {
_need_takeoff = false;
return true;
@ -825,7 +905,7 @@ Mission::do_need_move_to_takeoff()
void
Mission::copy_positon_if_valid(struct mission_item_s *mission_item, struct position_setpoint_s *setpoint)
{
if (setpoint->valid) {
if (setpoint->valid && setpoint->type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
mission_item->lat = setpoint->lat;
mission_item->lon = setpoint->lon;
mission_item->altitude = setpoint->alt;
@ -1261,6 +1341,7 @@ Mission::set_mission_item_reached()
void
Mission::set_current_offboard_mission_item()
{
_navigator->get_mission_result()->reached = false;
_navigator->get_mission_result()->finished = false;
_navigator->get_mission_result()->seq_current = _current_offboard_mission_index;
_navigator->set_mission_result_updated();
@ -1278,7 +1359,6 @@ Mission::set_mission_finished()
void
Mission::check_mission_valid(bool force)
{
if ((!_home_inited && _navigator->home_position_valid()) || force) {
dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
@ -1368,3 +1448,18 @@ Mission::need_to_reset_mission(bool active)
return false;
}
void
Mission::generate_waypoint_from_heading(struct position_setpoint_s *setpoint, float yaw)
{
waypoint_from_heading_and_distance(
_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
yaw,
1000000.0f,
&(setpoint->lat),
&(setpoint->lon));
setpoint->alt = _navigator->get_global_position()->alt;
setpoint->type = position_setpoint_s::SETPOINT_TYPE_POSITION;
setpoint->yaw = yaw;
}

5
src/modules/navigator/mission.h
View File

@ -227,6 +227,11 @@ private:
*/
bool need_to_reset_mission(bool active);
/**
* Project current location with heading to far away location and fill setpoint.
*/
void generate_waypoint_from_heading(struct position_setpoint_s *setpoint, float yaw);
control::BlockParamInt _param_onboard_enabled;
control::BlockParamFloat _param_takeoff_alt;
control::BlockParamFloat _param_dist_1wp;

39
src/modules/navigator/mission_block.cpp
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;

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

@ -148,7 +148,9 @@ protected:
control::BlockParamFloat _param_yaw_timeout;
control::BlockParamFloat _param_yaw_err;
control::BlockParamInt _param_vtol_wv_land;
control::BlockParamInt _param_vtol_wv_takeoff;
control::BlockParamInt _param_vtol_wv_loiter;
control::BlockParamInt _param_force_vtol;
};
#endif

24
src/modules/navigator/mission_params.c
View File

@ -160,15 +160,33 @@ PARAM_DEFINE_FLOAT(MIS_YAW_ERR, 12.0f);
/**
* Weather-vane mode landings for missions
*
* @boolean
* @unit bool
* @min 0
* @max 1
* @increment 1
* @decimal 0
* @group Mission
*/
PARAM_DEFINE_INT32(VT_WV_LND_EN, 0);
/**
* Weather-vane mode for loiter mode
* Enable weather-vane mode takeoff for missions
*
* @boolean
* @min 0
* @max 1
* @decimal 0
* @group Mission
*/
PARAM_DEFINE_INT32(VT_WV_TKO_EN, 0);
/**
* Weather-vane mode for loiter
*
* @unit bool
* @min 0
* @max 1
* @increment 1
* @decimal 0
* @group Mission
*/
PARAM_DEFINE_INT32(VT_WV_LTR_EN, 0);

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

@ -59,6 +59,7 @@ enum NAV_CMD {
NAV_CMD_WAYPOINT = 16,
NAV_CMD_LOITER_UNLIMITED = 17,
NAV_CMD_LOITER_TIME_LIMIT = 19,
NAV_CMD_RETURN_TO_LAUNCH = 20,
NAV_CMD_LAND = 21,
NAV_CMD_TAKEOFF = 22,
NAV_CMD_LOITER_TO_ALT = 31,

20
src/modules/navigator/navigator.h
View File

@ -95,7 +95,7 @@ public:
*
* @return OK on success.
*/
int start();
int start();
/**
* Display the navigator status.
@ -182,8 +182,21 @@ public:
/**
* Set the cruising speed
*
* Passing a negative value or leaving the parameter away will reset the cruising speed
* to its default value.
*
* For VTOL: sets cuising speed for current mode only (multirotor or fixed-wing).
*
*/
void set_cruising_speed(float speed=-1.0f) { _mission_cruising_speed = speed; }
void set_cruising_speed(float speed=-1.0f);
/**
* Reset cruising speed to default values
*
* For VTOL: resets both cruising speeds.
*/
void reset_cruising_speed();
/**
* Get the target throttle
@ -302,7 +315,8 @@ private:
control::BlockParamFloat _param_cruising_speed_plane;
control::BlockParamFloat _param_cruising_throttle_plane;
float _mission_cruising_speed;
float _mission_cruising_speed_mc;
float _mission_cruising_speed_fw;
float _mission_throttle;
/**

63
src/modules/navigator/navigator_main.cpp
View File

@ -161,7 +161,8 @@ Navigator::Navigator() :
_param_cruising_speed_hover(this, "MPC_XY_CRUISE", false),
_param_cruising_speed_plane(this, "FW_AIRSPD_TRIM", false),
_param_cruising_throttle_plane(this, "FW_THR_CRUISE", false),
_mission_cruising_speed(-1.0f),
_mission_cruising_speed_mc(-1.0f),
_mission_cruising_speed_fw(-1.0f),
_mission_throttle(-1.0f)
{
/* Create a list of our possible navigation types */
@ -517,7 +518,24 @@ Navigator::task_main()
}
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_PAUSE_CONTINUE) {
PX4_INFO("got pause/continue command");
warnx("navigator: got pause/continue command");
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED) {
if (cmd.param2 > FLT_EPSILON) {
// XXX not differentiating ground and airspeed yet
set_cruising_speed(cmd.param2);
} else {
set_cruising_speed();
/* if no speed target was given try to set throttle */
if (cmd.param3 > FLT_EPSILON) {
set_cruising_throttle(cmd.param3 / 100);
} else {
set_cruising_throttle();
}
}
}
}
@ -738,20 +756,48 @@ float
Navigator::get_cruising_speed()
{
/* there are three options: The mission-requested cruise speed, or the current hover / plane speed */
if (_mission_cruising_speed > 0.0f) {
return _mission_cruising_speed;
} else if (_vstatus.is_rotary_wing) {
return _param_cruising_speed_hover.get();
if (_vstatus.is_rotary_wing) {
if (_mission_cruising_speed_mc > FLT_EPSILON
&& _vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) {
return _mission_cruising_speed_mc;
} else {
return _param_cruising_speed_hover.get();
}
} else {
return _param_cruising_speed_plane.get();
if (_mission_cruising_speed_fw > FLT_EPSILON
&& _vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) {
return _mission_cruising_speed_fw;
} else {
return _param_cruising_speed_plane.get();
}
}
}
void
Navigator::set_cruising_speed(float speed) {
if (_vstatus.is_rotary_wing) {
_mission_cruising_speed_mc = speed;
} else {
_mission_cruising_speed_fw = speed;
}
}
void
Navigator::reset_cruising_speed()
{
_mission_cruising_speed_mc = -1.0f;
_mission_cruising_speed_fw = -1.0f;
}
float
Navigator::get_cruising_throttle()
{
/* Return the mission-requested cruise speed, or default FW_THR_CRUISE value */
if (_mission_throttle > 0.0f) {
if (_mission_throttle > FLT_EPSILON) {
return _mission_throttle;
} else {
return _param_cruising_throttle_plane.get();
@ -882,6 +928,7 @@ Navigator::publish_mission_result()
}
/* reset some of the flags */
_mission_result.seq_reached = false;
_mission_result.seq_current = 0;
_mission_result.item_do_jump_changed = false;
_mission_result.item_changed_index = 0;

9
src/modules/navigator/navigator_params.c
View File

@ -175,3 +175,12 @@ PARAM_DEFINE_INT32(NAV_AH_LON, 1518423250);
* @group Data Link Loss
*/
PARAM_DEFINE_FLOAT(NAV_AH_ALT, 600.0f);
/**
* Force VTOL mode takeoff and land
*
* @min 0
* @max 1
* @group Mission
*/
PARAM_DEFINE_INT32(NAV_FORCE_VT, 1);

9
src/modules/vtol_att_control/vtol_att_control_params.c
View File

@ -296,15 +296,6 @@ PARAM_DEFINE_FLOAT(VT_TRANS_TIMEOUT, 15.0f);
*/
PARAM_DEFINE_FLOAT(VT_TRANS_MIN_TM, 2.0f);
/**
* Force VTOL mode takeoff and land
*
* @min 0
* @max 1
* @group VTOL Attitude Control
*/
PARAM_DEFINE_INT32(VT_NAV_FORCE_VT, 1);
/**
* QuadChute
*