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

Navigator: Do not interpret DO_SET_SPEED as 3D location, reset speed on negative value.

Browse Source
This commit is contained in:
Lorenz Meier 2016-02-17 16:51:13 +01:00
parent c97dca0cdb
commit 2a2dca84bf
5 changed files with 10 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -125,6 +125,8 @@ MissionBlock::is_mission_item_reached()
// 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();
}
return true;
@ -319,7 +321,8 @@ MissionBlock::item_contains_position(const struct mission_item_s *item)
if (item->nav_cmd == NAV_CMD_DO_DIGICAM_CONTROL ||
item->nav_cmd == NAV_CMD_DO_SET_CAM_TRIGG_DIST ||
item->nav_cmd == NAV_CMD_DO_VTOL_TRANSITION ||
item->nav_cmd == NAV_CMD_DO_SET_SERVO) {
item->nav_cmd == NAV_CMD_DO_SET_SERVO ||
item->nav_cmd == NAV_CMD_DO_CHANGE_SPEED) {
return false;
}

4
src/modules/navigator/mission_feasibility_checker.cpp
View File

@ -242,12 +242,12 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s
missionitem.nav_cmd != NAV_CMD_PATHPLANNING &&
missionitem.nav_cmd != NAV_CMD_DO_JUMP &&
missionitem.nav_cmd != NAV_CMD_DO_SET_SERVO &&
missionitem.nav_cmd != vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED &&
missionitem.nav_cmd != NAV_CMD_DO_CHANGE_SPEED &&
missionitem.nav_cmd != NAV_CMD_DO_DIGICAM_CONTROL &&
missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_DIST &&
missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION) {
mavlink_log_critical(_mavlink_fd, "Rejecting mission item %i: unsupported action.", (int)(i+1));
mavlink_log_critical(_mavlink_fd, "Rejecting mission item %i: unsupported cmd: %d", (int)(i+1), (int)missionitem.nav_cmd);
return false;
}

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

@ -62,6 +62,7 @@ enum NAV_CMD {
NAV_CMD_ROI = 80,
NAV_CMD_PATHPLANNING = 81,
NAV_CMD_DO_JUMP = 177,
NAV_CMD_DO_CHANGE_SPEED = 178,
NAV_CMD_DO_SET_SERVO=183,
NAV_CMD_DO_REPEAT_SERVO=184,
NAV_CMD_DO_DIGICAM_CONTROL=203,

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

@ -166,7 +166,7 @@ public:
/**
* Set the cruising speed
*/
void set_cruising_speed(float speed) { _mission_cruising_speed = speed; }
void set_cruising_speed(float speed=-1.0f) { _mission_cruising_speed = speed; }
/**
* Get the acceptance radius given the mission item preset radius

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

@ -154,7 +154,7 @@ Navigator::Navigator() :
_param_rcloss_obc(this, "RCL_OBC"),
_param_cruising_speed_hover(this, "MPC_XY_VEL_MAX", false),
_param_cruising_speed_plane(this, "FW_AIRSPD_TRIM", false),
_mission_cruising_speed(0.0f)
_mission_cruising_speed(-1.0f)
{
/* Create a list of our possible navigation types */
_navigation_mode_array[0] = &_mission;
@ -622,7 +622,7 @@ 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.001f) {
if (_mission_cruising_speed > 0.0f) {
return _mission_cruising_speed;
} else if (_vstatus.is_rotary_wing) {
return _param_cruising_speed_hover.get();