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

Vtol takeoff/land handling in work items

Browse Source
This commit is contained in:
sander
2016-03-24 10:47:21 +01:00
committed by Lorenz Meier
parent 8d8c3f9683
commit 903a0cd689
2 changed files with 81 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission.cpp
+68 -61
View File
@@ -62,7 +62,6 @@
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/vehicle_command.h>
#include "mission.h"
#include "navigator.h"
@@ -80,7 +79,6 @@ Mission::Mission(Navigator *navigator, const char *name) :
_current_onboard_mission_index(-1),
_current_offboard_mission_index(-1),
_need_takeoff(true),
_takeoff_vtol_transition(false),
_mission_type(MISSION_TYPE_NONE),
_inited(false),
_home_inited(false),
@@ -464,27 +462,46 @@ Mission::set_mission_items()
}
/* if we just did a takeoff navigate to the actual waypoint now */
if (_work_item_type == WORK_ITEM_TYPE_TAKEOFF) {
if (_work_item_type == WORK_ITEM_TYPE_TAKEOFF) {
/* handle VTOL TAKEOFF command */
if(_takeoff_vtol_transition){
struct vehicle_command_s cmd = {};
cmd.command = NAV_CMD_DO_VTOL_TRANSITION;
cmd.param1 = vehicle_status_s::VEHICLE_VTOL_STATE_FW;
if (_cmd_pub != nullptr) {
orb_publish(ORB_ID(vehicle_command), _cmd_pub, &cmd);
} else {
_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &cmd);
}
_takeoff_vtol_transition = false;
if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) {
new_work_item_type = WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF;
} else {
new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
}
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 (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
&& _work_item_type == WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF
&& _navigator->get_vstatus()->is_rotary_wing
&& !_navigator->get_vstatus()->condition_landed
&& has_next_position_item) {
new_work_item_type = WORK_ITEM_TYPE_ALIGN;
set_align_mission_item(&_mission_item, &mission_item_next_position);
}
if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
&& _work_item_type == WORK_ITEM_TYPE_ALIGN) {
_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
_mission_item.params[0] = vehicle_status_s::VEHICLE_VTOL_STATE_FW;
new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
}
if(_mission_item.nav_cmd == NAV_CMD_VTOL_LAND ||
(_mission_item.nav_cmd == NAV_CMD_LAND && _param_force_vtol.get())){
_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
_mission_item.params[0] = vehicle_status_s::VEHICLE_VTOL_STATE_MC;
new_work_item_type = WORK_ITEM_TYPE_TRANSITON_BEFORE_LAND;
}
/* move to landing waypoint before descent if necessary */
if (do_need_move_to_land() && _work_item_type != WORK_ITEM_TYPE_MOVE_TO_LAND) {
new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;
@@ -535,17 +552,7 @@ Mission::set_mission_items()
&& has_next_position_item) {
new_work_item_type = WORK_ITEM_TYPE_ALIGN;
_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;
_mission_item.yaw = get_bearing_to_next_waypoint(
_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
mission_item_next_position.lat,
mission_item_next_position.lon);
_mission_item.force_heading = true;
set_align_mission_item(&_mission_item, &mission_item_next_position);
}
/* yaw is aligned now */
@@ -570,7 +577,7 @@ Mission::set_mission_items()
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);
copy_positon_if_valid(&_mission_item, &(pos_sp_triplet->current));
_mission_item.autocontinue = true;
_mission_item.time_inside = 0;
}
@@ -629,23 +636,6 @@ Mission::set_mission_items()
_navigator->set_position_setpoint_triplet_updated();
}
void
Mission::copy_positon_if_valid(struct mission_item_s &mission_item, struct position_setpoint_s &setpoint)
{
if (setpoint.valid) {
_mission_item.lat = setpoint.lat;
_mission_item.lon = setpoint.lon;
_mission_item.altitude = setpoint.alt;
} else {
_mission_item.lat = _navigator->get_global_position()->lat;
_mission_item.lon = _navigator->get_global_position()->lon;
_mission_item.altitude = _navigator->get_global_position()->alt;
}
_mission_item.altitude_is_relative = false;
}
bool
Mission::do_need_takeoff()
{
@@ -671,10 +661,6 @@ Mission::do_need_takeoff()
_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH)) {
if(_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF){
_takeoff_vtol_transition = true;
}
_need_takeoff = false;
return true;
}
@@ -686,18 +672,6 @@ Mission::do_need_takeoff()
bool
Mission::do_need_move_to_land()
{
if(_mission_item.nav_cmd == NAV_CMD_VTOL_LAND || (_mission_item.nav_cmd == NAV_CMD_LAND && _param_force_vtol.get())){
struct vehicle_command_s cmd = {};
cmd.command = NAV_CMD_DO_VTOL_TRANSITION;
cmd.param1 = vehicle_status_s::VEHICLE_VTOL_STATE_MC;
if (_cmd_pub != nullptr) {
orb_publish(ORB_ID(vehicle_command), _cmd_pub, &cmd);
} else {
_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &cmd);
}
_mission_item.nav_cmd = NAV_CMD_LAND;
}
if (_navigator->get_vstatus()->is_rotary_wing && _mission_item.nav_cmd == NAV_CMD_LAND) {
float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
@@ -709,6 +683,39 @@ Mission::do_need_move_to_land()
return false;
}
void
Mission::copy_positon_if_valid(struct mission_item_s *mission_item, struct position_setpoint_s *setpoint)
{
if (setpoint->valid) {
mission_item->lat = setpoint->lat;
mission_item->lon = setpoint->lon;
mission_item->altitude = setpoint->alt;
} else {
mission_item->lat = _navigator->get_global_position()->lat;
mission_item->lon = _navigator->get_global_position()->lon;
mission_item->altitude = _navigator->get_global_position()->alt;
}
mission_item->altitude_is_relative = false;
}
void
Mission::set_align_mission_item(struct mission_item_s *mission_item, struct mission_item_s *mission_item_next)
{
mission_item->nav_cmd = NAV_CMD_WAYPOINT;
copy_positon_if_valid(mission_item, &(_navigator->get_position_setpoint_triplet()->current));
mission_item->altitude_is_relative = false;
mission_item->autocontinue = true;
mission_item->time_inside = 0;
mission_item->yaw = get_bearing_to_next_waypoint(
_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
mission_item_next->lat,
mission_item_next->lon);
mission_item->force_heading = true;
}
float
Mission::calculate_takeoff_altitude(struct mission_item_s *mission_item)
{
src/modules/navigator/mission.h
+13 -7
View File
@@ -119,11 +119,6 @@ private:
*/
void set_mission_items();
/**
* Copies position from setpoint if valid, otherwise copies current position
*/
void copy_positon_if_valid(struct mission_item_s &mission_item, struct position_setpoint_s &setpoint);
/**
* Returns true if we need to do a takeoff at the current state
*/
@@ -134,6 +129,16 @@ private:
*/
bool do_need_move_to_land();
/**
* Copies position from setpoint if valid, otherwise copies current position
*/
void copy_positon_if_valid(struct mission_item_s *mission_item, struct position_setpoint_s *setpoint);
/**
* Create mission item to align towards next waypoint
*/
void set_align_mission_item(struct mission_item_s *mission_item, struct mission_item_s *mission_item_next);
/**
* Calculate takeoff height for mission item considering ground clearance
*/
@@ -226,7 +231,6 @@ private:
int _current_onboard_mission_index;
int _current_offboard_mission_index;
bool _need_takeoff; /**< if true, then takeoff must be performed before going to the first waypoint (if needed) */
bool _takeoff_vtol_transition; /**< if true, a vtol transition will be performed after takeoff */
enum {
MISSION_TYPE_NONE,
@@ -252,7 +256,9 @@ private:
WORK_ITEM_TYPE_TAKEOFF, /**< takeoff before moving to waypoint */
WORK_ITEM_TYPE_MOVE_TO_LAND, /**< move to land waypoint before descent */
WORK_ITEM_TYPE_ALIGN, /**< align for next waypoint */
WORK_ITEM_TYPE_CMD_BEFORE_MOVE /**< */
WORK_ITEM_TYPE_CMD_BEFORE_MOVE, /**< */
WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF, /**< */
WORK_ITEM_TYPE_TRANSITON_BEFORE_LAND /**< */
} _work_item_type; /**< current type of work to do (sub mission item) */
};