mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
Navigator move get_time_inside and cleanup (#7062)
This commit is contained in:
Daniel Agar
GitHub
parent
1913b970d7
commit
56b028148b
@ -44,28 +44,20 @@
|
||||
* @author Sander Smeets <sander@droneslab.com>
|
||||
*/
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include <unistd.h>
|
||||
#include <float.h>
|
||||
#include "mission.h"
|
||||
#include "navigator.h"
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include <dataman/dataman.h>
|
||||
#include <systemlib/mavlink_log.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <geo/geo.h>
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
#include <navigator/navigation.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/mission.h>
|
||||
#include <uORB/topics/mission_result.h>
|
||||
|
||||
#include "mission.h"
|
||||
#include "navigator.h"
|
||||
|
||||
Mission::Mission(Navigator *navigator, const char *name) :
|
||||
MissionBlock(navigator, name),
|
||||
_param_onboard_enabled(this, "MIS_ONBOARD_EN", false),
|
||||
@ -75,28 +67,11 @@ Mission::Mission(Navigator *navigator, const char *name) :
|
||||
_param_yawmode(this, "MIS_YAWMODE", false),
|
||||
_param_force_vtol(this, "NAV_FORCE_VT", false),
|
||||
_param_fw_climbout_diff(this, "FW_CLMBOUT_DIFF", false),
|
||||
_onboard_mission{},
|
||||
_offboard_mission{},
|
||||
_current_onboard_mission_index(-1),
|
||||
_current_offboard_mission_index(-1),
|
||||
_need_takeoff(true),
|
||||
_mission_type(MISSION_TYPE_NONE),
|
||||
_inited(false),
|
||||
_home_inited(false),
|
||||
_need_mission_reset(false),
|
||||
_missionFeasibilityChecker(),
|
||||
_min_current_sp_distance_xy(FLT_MAX),
|
||||
_distance_current_previous(0.0f),
|
||||
_work_item_type(WORK_ITEM_TYPE_DEFAULT)
|
||||
_missionFeasibilityChecker()
|
||||
{
|
||||
/* load initial params */
|
||||
updateParams();
|
||||
}
|
||||
|
||||
Mission::~Mission()
|
||||
{
|
||||
}
|
||||
|
||||
void
|
||||
Mission::on_inactive()
|
||||
{
|
||||
@ -136,7 +111,7 @@ Mission::on_inactive()
|
||||
} else {
|
||||
|
||||
/* load missions from storage */
|
||||
mission_s mission_state;
|
||||
mission_s mission_state = {};
|
||||
|
||||
dm_lock(DM_KEY_MISSION_STATE);
|
||||
|
||||
@ -221,6 +196,7 @@ Mission::on_active()
|
||||
}
|
||||
|
||||
} else if (_mission_type != MISSION_TYPE_NONE && _param_altmode.get() == MISSION_ALTMODE_FOH) {
|
||||
|
||||
altitude_sp_foh_update();
|
||||
|
||||
} else {
|
||||
@ -230,7 +206,6 @@ Mission::on_active()
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/* check if a cruise speed change has been commanded */
|
||||
if (_mission_type != MISSION_TYPE_NONE) {
|
||||
cruising_speed_sp_update();
|
||||
@ -251,10 +226,10 @@ Mission::on_active()
|
||||
|
||||
do_abort_landing();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
bool Mission::set_current_offboard_mission_index(unsigned index)
|
||||
bool
|
||||
Mission::set_current_offboard_mission_index(unsigned index)
|
||||
{
|
||||
if (index < _offboard_mission.count) {
|
||||
|
||||
@ -275,7 +250,7 @@ bool Mission::set_current_offboard_mission_index(unsigned index)
|
||||
return false;
|
||||
}
|
||||
|
||||
unsigned
|
||||
int
|
||||
Mission::find_offboard_land_start()
|
||||
{
|
||||
/* find the first MAV_CMD_DO_LAND_START and return the index
|
||||
@ -433,11 +408,11 @@ Mission::advance_mission()
|
||||
float
|
||||
Mission::get_absolute_altitude_for_item(struct mission_item_s &mission_item)
|
||||
{
|
||||
if (_mission_item.altitude_is_relative) {
|
||||
return _mission_item.altitude + _navigator->get_home_position()->alt;
|
||||
if (mission_item.altitude_is_relative) {
|
||||
return mission_item.altitude + _navigator->get_home_position()->alt;
|
||||
|
||||
} else {
|
||||
return _mission_item.altitude;
|
||||
return mission_item.altitude;
|
||||
}
|
||||
}
|
||||
|
||||
@ -544,6 +519,7 @@ Mission::set_mission_items()
|
||||
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;
|
||||
}
|
||||
|
||||
@ -825,7 +801,7 @@ Mission::set_mission_items()
|
||||
set_current_offboard_mission_item();
|
||||
}
|
||||
|
||||
if (_mission_item.autocontinue && Navigator::get_time_inside(_mission_item) < FLT_EPSILON) {
|
||||
if (_mission_item.autocontinue && get_time_inside(_mission_item) < FLT_EPSILON) {
|
||||
/* try to process next mission item */
|
||||
if (has_next_position_item) {
|
||||
/* got next mission item, update setpoint triplet */
|
||||
@ -994,7 +970,7 @@ Mission::heading_sp_update()
|
||||
}
|
||||
|
||||
/* set yaw angle for the waypoint if a loiter time has been specified */
|
||||
if (_waypoint_position_reached && Navigator::get_time_inside(_mission_item) > FLT_EPSILON) {
|
||||
if (_waypoint_position_reached && get_time_inside(_mission_item) > FLT_EPSILON) {
|
||||
// XXX: should actually be param4 from mission item
|
||||
// at the moment it will just keep the heading it has
|
||||
//_mission_item.yaw = _on_arrival_yaw;
|
||||
@ -1054,7 +1030,6 @@ Mission::heading_sp_update()
|
||||
_navigator->set_position_setpoint_triplet_updated();
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
Mission::altitude_sp_foh_update()
|
||||
{
|
||||
@ -1313,7 +1288,7 @@ Mission::read_mission_item(bool onboard, int offset, struct mission_item_s *miss
|
||||
void
|
||||
Mission::save_offboard_mission_state()
|
||||
{
|
||||
mission_s mission_state;
|
||||
mission_s mission_state = {};
|
||||
|
||||
/* lock MISSION_STATE item */
|
||||
dm_lock(DM_KEY_MISSION_STATE);
|
||||
|
||||
@ -45,24 +45,23 @@
|
||||
#ifndef NAVIGATOR_MISSION_H
|
||||
#define NAVIGATOR_MISSION_H
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include <controllib/blocks.hpp>
|
||||
#include <controllib/block/BlockParam.hpp>
|
||||
|
||||
#include <dataman/dataman.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/position_setpoint_triplet.h>
|
||||
#include <uORB/topics/home_position.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/mission.h>
|
||||
#include <uORB/topics/mission_result.h>
|
||||
|
||||
#include "navigator_mode.h"
|
||||
#include "mission_block.h"
|
||||
#include "mission_feasibility_checker.h"
|
||||
#include "navigator_mode.h"
|
||||
|
||||
#include <cfloat>
|
||||
|
||||
#include <controllib/block/BlockParam.hpp>
|
||||
#include <controllib/blocks.hpp>
|
||||
#include <dataman/dataman.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <uORB/topics/home_position.h>
|
||||
#include <uORB/topics/mission.h>
|
||||
#include <uORB/topics/mission_result.h>
|
||||
#include <uORB/topics/position_setpoint_triplet.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/uORB.h>
|
||||
|
||||
class Navigator;
|
||||
|
||||
@ -70,14 +69,11 @@ class Mission : public MissionBlock
|
||||
{
|
||||
public:
|
||||
Mission(Navigator *navigator, const char *name);
|
||||
~Mission() override = default;
|
||||
|
||||
virtual ~Mission();
|
||||
|
||||
virtual void on_inactive();
|
||||
|
||||
virtual void on_activation();
|
||||
|
||||
virtual void on_active();
|
||||
void on_inactive() override;
|
||||
void on_activation() override;
|
||||
void on_active() override;
|
||||
|
||||
enum mission_altitude_mode {
|
||||
MISSION_ALTMODE_ZOH = 0,
|
||||
@ -94,7 +90,7 @@ public:
|
||||
|
||||
bool set_current_offboard_mission_index(unsigned index);
|
||||
|
||||
unsigned find_offboard_land_start();
|
||||
int find_offboard_land_start();
|
||||
|
||||
private:
|
||||
/**
|
||||
@ -245,28 +241,28 @@ private:
|
||||
control::BlockParamInt _param_force_vtol;
|
||||
control::BlockParamFloat _param_fw_climbout_diff;
|
||||
|
||||
struct mission_s _onboard_mission;
|
||||
struct mission_s _offboard_mission;
|
||||
struct mission_s _onboard_mission {};
|
||||
struct mission_s _offboard_mission {};
|
||||
|
||||
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) */
|
||||
int _current_onboard_mission_index{-1};
|
||||
int _current_offboard_mission_index{-1};
|
||||
bool _need_takeoff{true}; /**< if true, then takeoff must be performed before going to the first waypoint (if needed) */
|
||||
|
||||
enum {
|
||||
MISSION_TYPE_NONE,
|
||||
MISSION_TYPE_ONBOARD,
|
||||
MISSION_TYPE_OFFBOARD
|
||||
} _mission_type;
|
||||
} _mission_type{MISSION_TYPE_NONE};
|
||||
|
||||
bool _inited;
|
||||
bool _home_inited;
|
||||
bool _need_mission_reset;
|
||||
bool _inited{false};
|
||||
bool _home_inited{false};
|
||||
bool _need_mission_reset{false};
|
||||
|
||||
MissionFeasibilityChecker _missionFeasibilityChecker; /**< class that checks if a mission is feasible */
|
||||
|
||||
float _min_current_sp_distance_xy; /**< minimum distance which was achieved to the current waypoint */
|
||||
float _min_current_sp_distance_xy{FLT_MAX}; /**< minimum distance which was achieved to the current waypoint */
|
||||
|
||||
float _distance_current_previous; /**< distance from previous to current sp in pos_sp_triplet,
|
||||
float _distance_current_previous{0.0f}; /**< distance from previous to current sp in pos_sp_triplet,
|
||||
only use if current and previous are valid */
|
||||
|
||||
enum work_item_type {
|
||||
@ -274,11 +270,10 @@ 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_TRANSITON_AFTER_TAKEOFF, /**< */
|
||||
WORK_ITEM_TYPE_TRANSITON_BEFORE_LAND, /**< */
|
||||
WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION /**< */
|
||||
} _work_item_type; /**< current type of work to do (sub mission item) */
|
||||
WORK_ITEM_TYPE_CMD_BEFORE_MOVE,
|
||||
WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF,
|
||||
WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION
|
||||
} _work_item_type{WORK_ITEM_TYPE_DEFAULT}; /**< current type of work to do (sub mission item) */
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@ -40,41 +40,24 @@
|
||||
* @author Andreas Antener <andreas@uaventure.com>
|
||||
*/
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include <unistd.h>
|
||||
#include "mission_block.h"
|
||||
#include "navigator.h"
|
||||
|
||||
#include <math.h>
|
||||
#include <float.h>
|
||||
|
||||
#include <systemlib/err.h>
|
||||
#include <geo/geo.h>
|
||||
#include <systemlib/mavlink_log.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/vtol_vehicle_status.h>
|
||||
|
||||
#include "navigator.h"
|
||||
#include "mission_block.h"
|
||||
|
||||
actuator_controls_s actuators;
|
||||
orb_advert_t actuator_pub_fd;
|
||||
|
||||
|
||||
MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
|
||||
NavigatorMode(navigator, name),
|
||||
_mission_item{},
|
||||
_waypoint_position_reached(false),
|
||||
_waypoint_yaw_reached(false),
|
||||
_time_first_inside_orbit(0),
|
||||
_action_start(0),
|
||||
_time_wp_reached(0),
|
||||
_actuators{},
|
||||
_actuator_pub(nullptr),
|
||||
_cmd_pub(nullptr),
|
||||
_param_loiter_min_alt(this, "MIS_LTRMIN_ALT", false),
|
||||
_param_yaw_timeout(this, "MIS_YAW_TMT", false),
|
||||
_param_yaw_err(this, "MIS_YAW_ERR", false),
|
||||
@ -86,10 +69,6 @@ MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
|
||||
{
|
||||
}
|
||||
|
||||
MissionBlock::~MissionBlock()
|
||||
{
|
||||
}
|
||||
|
||||
bool
|
||||
MissionBlock::is_mission_item_reached()
|
||||
{
|
||||
@ -125,6 +104,7 @@ MissionBlock::is_mission_item_reached()
|
||||
* We wait half a second to give the transition command time to propagate.
|
||||
* Then monitor the transition status for completion.
|
||||
*/
|
||||
// TODO: check desired transition state achieved and drop _action_start
|
||||
if (hrt_absolute_time() - _action_start > 500000 &&
|
||||
!_navigator->get_vstatus()->in_transition_mode) {
|
||||
|
||||
@ -145,8 +125,7 @@ MissionBlock::is_mission_item_reached()
|
||||
|
||||
hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
if ((_navigator->get_land_detected()->landed == false)
|
||||
&& !_waypoint_position_reached) {
|
||||
if (!_navigator->get_land_detected()->landed && !_waypoint_position_reached) {
|
||||
|
||||
float dist = -1.0f;
|
||||
float dist_xy = -1.0f;
|
||||
@ -375,8 +354,8 @@ MissionBlock::is_mission_item_reached()
|
||||
}
|
||||
|
||||
/* check if the MAV was long enough inside the waypoint orbit */
|
||||
if ((Navigator::get_time_inside(_mission_item) < FLT_EPSILON) ||
|
||||
(now - _time_first_inside_orbit >= (hrt_abstime)(Navigator::get_time_inside(_mission_item) * 1e6f))) {
|
||||
if ((get_time_inside(_mission_item) < FLT_EPSILON) ||
|
||||
(now - _time_first_inside_orbit >= (hrt_abstime)(get_time_inside(_mission_item) * 1e6f))) {
|
||||
|
||||
// exit xtrack location
|
||||
if (_mission_item.loiter_exit_xtrack &&
|
||||
@ -459,17 +438,17 @@ MissionBlock::issue_command(const struct mission_item_s *item)
|
||||
if (item->nav_cmd == NAV_CMD_DO_SET_SERVO) {
|
||||
PX4_INFO("do_set_servo command");
|
||||
// XXX: we should issue a vehicle command and handle this somewhere else
|
||||
memset(&actuators, 0, sizeof(actuators));
|
||||
_actuators = {};
|
||||
// params[0] actuator number to be set 0..5 (corresponds to AUX outputs 1..6)
|
||||
// params[1] new value for selected actuator in ms 900...2000
|
||||
actuators.control[(int)item->params[0]] = 1.0f / 2000 * -item->params[1];
|
||||
actuators.timestamp = hrt_absolute_time();
|
||||
_actuators.control[(int)item->params[0]] = 1.0f / 2000 * -item->params[1];
|
||||
_actuators.timestamp = hrt_absolute_time();
|
||||
|
||||
if (_actuator_pub != nullptr) {
|
||||
orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &actuators);
|
||||
orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &_actuators);
|
||||
|
||||
} else {
|
||||
_actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &actuators);
|
||||
_actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators);
|
||||
}
|
||||
|
||||
} else {
|
||||
@ -478,15 +457,20 @@ MissionBlock::issue_command(const struct mission_item_s *item)
|
||||
mission_item_to_vehicle_command(item, &cmd);
|
||||
_action_start = hrt_absolute_time();
|
||||
|
||||
if (_cmd_pub != nullptr) {
|
||||
orb_publish(ORB_ID(vehicle_command), _cmd_pub, &cmd);
|
||||
|
||||
} else {
|
||||
_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
|
||||
}
|
||||
_navigator->publish_vehicle_cmd(cmd);
|
||||
}
|
||||
}
|
||||
|
||||
float
|
||||
MissionBlock::get_time_inside(const struct mission_item_s &item)
|
||||
{
|
||||
if (item.nav_cmd == NAV_CMD_TAKEOFF) {
|
||||
return item.time_inside;
|
||||
}
|
||||
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
bool
|
||||
MissionBlock::item_contains_position(const struct mission_item_s *item)
|
||||
{
|
||||
@ -663,7 +647,6 @@ MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clea
|
||||
item->nav_cmd = NAV_CMD_DO_FOLLOW_REPOSITION;
|
||||
|
||||
/* use current target position */
|
||||
|
||||
item->lat = target.lat;
|
||||
item->lon = target.lon;
|
||||
item->altitude = _navigator->get_home_position()->alt;
|
||||
@ -709,18 +692,15 @@ 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;
|
||||
|
||||
if (_cmd_pub != nullptr) {
|
||||
orb_publish(ORB_ID(vehicle_command), _cmd_pub, &cmd);
|
||||
|
||||
} else {
|
||||
_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
|
||||
}
|
||||
_navigator->publish_vehicle_cmd(cmd);
|
||||
}
|
||||
|
||||
/* set the land item */
|
||||
@ -732,9 +712,8 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio
|
||||
item->lon = _navigator->get_global_position()->lon;
|
||||
item->yaw = _navigator->get_local_position()->yaw;
|
||||
|
||||
/* use home position */
|
||||
|
||||
} else {
|
||||
/* use home position */
|
||||
item->lat = _navigator->get_home_position()->lat;
|
||||
item->lon = _navigator->get_home_position()->lon;
|
||||
item->yaw = _navigator->get_home_position()->yaw;
|
||||
|
||||
@ -41,18 +41,16 @@
|
||||
#ifndef NAVIGATOR_MISSION_BLOCK_H
|
||||
#define NAVIGATOR_MISSION_BLOCK_H
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include "navigator_mode.h"
|
||||
|
||||
#include <navigator/navigation.h>
|
||||
|
||||
#include <uORB/topics/mission.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/position_setpoint_triplet.h>
|
||||
#include <uORB/topics/vtol_vehicle_status.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/follow_target.h>
|
||||
|
||||
#include "navigator_mode.h"
|
||||
#include <uORB/topics/mission.h>
|
||||
#include <uORB/topics/position_setpoint_triplet.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vtol_vehicle_status.h>
|
||||
|
||||
class Navigator;
|
||||
|
||||
@ -63,15 +61,11 @@ public:
|
||||
* Constructor
|
||||
*/
|
||||
MissionBlock(Navigator *navigator, const char *name);
|
||||
~MissionBlock() = default;
|
||||
|
||||
MissionBlock(const MissionBlock &) = delete;
|
||||
MissionBlock &operator=(const MissionBlock &) = delete;
|
||||
|
||||
/**
|
||||
* Destructor
|
||||
*/
|
||||
virtual ~MissionBlock();
|
||||
|
||||
/* TODO: move this to a helper class in navigator */
|
||||
static bool item_contains_position(const struct mission_item_s *item);
|
||||
|
||||
@ -81,6 +75,7 @@ protected:
|
||||
* @return true if successfully reached
|
||||
*/
|
||||
bool is_mission_item_reached();
|
||||
|
||||
/**
|
||||
* Reset all reached flags
|
||||
*/
|
||||
@ -121,28 +116,31 @@ protected:
|
||||
*/
|
||||
void set_idle_item(struct mission_item_s *item);
|
||||
|
||||
/**
|
||||
* Convert a mission item to a command
|
||||
*/
|
||||
void mission_item_to_vehicle_command(const struct mission_item_s *item, struct vehicle_command_s *cmd);
|
||||
|
||||
/**
|
||||
* Set follow_target item
|
||||
*/
|
||||
void set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s &target, float yaw);
|
||||
|
||||
/**
|
||||
* Convert a mission item to a command
|
||||
*/
|
||||
void mission_item_to_vehicle_command(const struct mission_item_s *item, struct vehicle_command_s *cmd);
|
||||
|
||||
void issue_command(const struct mission_item_s *item);
|
||||
|
||||
mission_item_s _mission_item;
|
||||
bool _waypoint_position_reached;
|
||||
bool _waypoint_yaw_reached;
|
||||
hrt_abstime _time_first_inside_orbit;
|
||||
hrt_abstime _action_start;
|
||||
hrt_abstime _time_wp_reached;
|
||||
float get_time_inside(const struct mission_item_s &item);
|
||||
|
||||
actuator_controls_s _actuators;
|
||||
orb_advert_t _actuator_pub;
|
||||
orb_advert_t _cmd_pub;
|
||||
mission_item_s _mission_item{};
|
||||
|
||||
bool _waypoint_position_reached{false};
|
||||
bool _waypoint_yaw_reached{false};
|
||||
|
||||
hrt_abstime _time_first_inside_orbit{0};
|
||||
hrt_abstime _action_start{0};
|
||||
hrt_abstime _time_wp_reached{0};
|
||||
|
||||
actuator_controls_s _actuators{};
|
||||
orb_advert_t _actuator_pub{nullptr};
|
||||
|
||||
control::BlockParamFloat _param_loiter_min_alt;
|
||||
control::BlockParamFloat _param_yaw_timeout;
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
/***************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2013-2017 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@ -41,37 +41,34 @@
|
||||
#ifndef NAVIGATOR_H
|
||||
#define NAVIGATOR_H
|
||||
|
||||
#include <systemlib/perf_counter.h>
|
||||
|
||||
#include <controllib/blocks.hpp>
|
||||
#include <controllib/block/BlockParam.hpp>
|
||||
#include <navigator/navigation.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/mission.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/position_setpoint_triplet.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/mission_result.h>
|
||||
#include <uORB/topics/geofence_result.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/vehicle_land_detected.h>
|
||||
|
||||
#include "navigator_mode.h"
|
||||
#include "mission.h"
|
||||
#include "loiter.h"
|
||||
#include "takeoff.h"
|
||||
#include "land.h"
|
||||
#include "rtl.h"
|
||||
#include "datalinkloss.h"
|
||||
#include "enginefailure.h"
|
||||
#include "follow_target.h"
|
||||
#include "gpsfailure.h"
|
||||
#include "rcloss.h"
|
||||
#include "geofence.h"
|
||||
#include "gpsfailure.h"
|
||||
#include "land.h"
|
||||
#include "loiter.h"
|
||||
#include "mission.h"
|
||||
#include "navigator_mode.h"
|
||||
#include "rcloss.h"
|
||||
#include "rtl.h"
|
||||
#include "takeoff.h"
|
||||
|
||||
#include <controllib/block/BlockParam.hpp>
|
||||
#include <controllib/blocks.hpp>
|
||||
#include <navigator/navigation.h>
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <uORB/topics/fw_pos_ctrl_status.h>
|
||||
#include <uORB/topics/geofence_result.h>
|
||||
#include <uORB/topics/mission.h>
|
||||
#include <uORB/topics/mission_result.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/position_setpoint_triplet.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/vehicle_land_detected.h>
|
||||
#include <uORB/uORB.h>
|
||||
|
||||
/**
|
||||
* Number of navigation modes that need on_active/on_inactive calls
|
||||
@ -81,15 +78,10 @@
|
||||
class Navigator : public control::SuperBlock
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* Constructor
|
||||
*/
|
||||
Navigator();
|
||||
|
||||
/**
|
||||
* Destructor, also kills the navigators task.
|
||||
*/
|
||||
~Navigator();
|
||||
Navigator(const Navigator &) = delete;
|
||||
Navigator operator=(const Navigator &) = delete;
|
||||
|
||||
/**
|
||||
* Start the navigator task.
|
||||
@ -124,6 +116,8 @@ public:
|
||||
*/
|
||||
void publish_att_sp();
|
||||
|
||||
void publish_vehicle_cmd(const struct vehicle_command_s &vcmd);
|
||||
|
||||
/**
|
||||
* Setters
|
||||
*/
|
||||
@ -134,21 +128,20 @@ public:
|
||||
/**
|
||||
* Getters
|
||||
*/
|
||||
struct vehicle_status_s *get_vstatus() { return &_vstatus; }
|
||||
struct vehicle_land_detected_s *get_land_detected() { return &_land_detected; }
|
||||
struct vehicle_control_mode_s *get_control_mode() { return &_control_mode; }
|
||||
struct vehicle_global_position_s *get_global_position() { return &_global_pos; }
|
||||
struct vehicle_local_position_s *get_local_position() { return &_local_pos; }
|
||||
struct vehicle_gps_position_s *get_gps_position() { return &_gps_pos; }
|
||||
struct sensor_combined_s *get_sensor_combined() { return &_sensor_combined; }
|
||||
struct fw_pos_ctrl_status_s *get_fw_pos_ctrl_status() { return &_fw_pos_ctrl_status; }
|
||||
struct home_position_s *get_home_position() { return &_home_pos; }
|
||||
bool home_position_valid() { return (_home_pos.timestamp > 0); }
|
||||
struct mission_result_s *get_mission_result() { return &_mission_result; }
|
||||
struct position_setpoint_triplet_s *get_position_setpoint_triplet() { return &_pos_sp_triplet; }
|
||||
struct position_setpoint_triplet_s *get_reposition_triplet() { return &_reposition_triplet; }
|
||||
struct position_setpoint_triplet_s *get_takeoff_triplet() { return &_takeoff_triplet; }
|
||||
struct mission_result_s *get_mission_result() { return &_mission_result; }
|
||||
struct geofence_result_s *get_geofence_result() { return &_geofence_result; }
|
||||
struct vehicle_attitude_setpoint_s *get_att_sp() { return &_att_sp; }
|
||||
struct vehicle_global_position_s *get_global_position() { return &_global_pos; }
|
||||
struct vehicle_gps_position_s *get_gps_position() { return &_gps_pos; }
|
||||
struct vehicle_land_detected_s *get_land_detected() { return &_land_detected; }
|
||||
struct vehicle_local_position_s *get_local_position() { return &_local_pos; }
|
||||
struct vehicle_status_s *get_vstatus() { return &_vstatus; }
|
||||
|
||||
bool home_position_valid() { return (_home_pos.timestamp > 0); }
|
||||
|
||||
int get_onboard_mission_sub() { return _onboard_mission_sub; }
|
||||
int get_offboard_mission_sub() { return _offboard_mission_sub; }
|
||||
@ -188,7 +181,7 @@ public:
|
||||
* 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).
|
||||
* For VTOL: sets cruising speed for current mode only (multirotor or fixed-wing).
|
||||
*
|
||||
*/
|
||||
void set_cruising_speed(float speed = -1.0f);
|
||||
@ -230,83 +223,70 @@ public:
|
||||
|
||||
bool abort_landing();
|
||||
|
||||
static float get_time_inside(struct mission_item_s &item) { return (item.nav_cmd == NAV_CMD_TAKEOFF) ? 0.0f : item.time_inside; }
|
||||
|
||||
private:
|
||||
|
||||
bool _task_should_exit; /**< if true, sensor task should exit */
|
||||
int _navigator_task; /**< task handle for sensor task */
|
||||
bool _task_should_exit{false}; /**< if true, sensor task should exit */
|
||||
int _navigator_task{-1}; /**< task handle for sensor task */
|
||||
|
||||
orb_advert_t _mavlink_log_pub; /**< the uORB advert to send messages over mavlink */
|
||||
orb_advert_t _mavlink_log_pub{nullptr}; /**< the uORB advert to send messages over mavlink */
|
||||
|
||||
int _global_pos_sub; /**< global position subscription */
|
||||
int _local_pos_sub; /**< local position subscription */
|
||||
int _gps_pos_sub; /**< gps position subscription */
|
||||
int _sensor_combined_sub; /**< sensor combined subscription */
|
||||
int _home_pos_sub; /**< home position subscription */
|
||||
int _vstatus_sub; /**< vehicle status subscription */
|
||||
int _land_detected_sub; /**< vehicle land detected subscription */
|
||||
int _fw_pos_ctrl_status_sub; /**< notification of vehicle capabilities updates */
|
||||
int _control_mode_sub; /**< vehicle control mode subscription */
|
||||
int _onboard_mission_sub; /**< onboard mission subscription */
|
||||
int _offboard_mission_sub; /**< offboard mission subscription */
|
||||
int _param_update_sub; /**< param update subscription */
|
||||
int _vehicle_command_sub; /**< vehicle commands (onboard and offboard) */
|
||||
int _fw_pos_ctrl_status_sub{-1}; /**< notification of vehicle capabilities updates */
|
||||
int _global_pos_sub{-1}; /**< global position subscription */
|
||||
int _gps_pos_sub{-1}; /**< gps position subscription */
|
||||
int _home_pos_sub{-1}; /**< home position subscription */
|
||||
int _land_detected_sub{-1}; /**< vehicle land detected subscription */
|
||||
int _local_pos_sub{-1}; /**< local position subscription */
|
||||
int _offboard_mission_sub{-1}; /**< offboard mission subscription */
|
||||
int _onboard_mission_sub{-1}; /**< onboard mission subscription */
|
||||
int _param_update_sub{-1}; /**< param update subscription */
|
||||
int _sensor_combined_sub{-1}; /**< sensor combined subscription */
|
||||
int _vehicle_command_sub{-1}; /**< vehicle commands (onboard and offboard) */
|
||||
int _vstatus_sub{-1}; /**< vehicle status subscription */
|
||||
|
||||
orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */
|
||||
orb_advert_t _mission_result_pub;
|
||||
orb_advert_t _geofence_result_pub;
|
||||
orb_advert_t _att_sp_pub; /**< publish att sp
|
||||
used only in very special failsafe modes
|
||||
when pos control is deactivated */
|
||||
orb_advert_t _att_sp_pub{nullptr};
|
||||
orb_advert_t _geofence_result_pub{nullptr};
|
||||
orb_advert_t _mission_result_pub{nullptr};
|
||||
orb_advert_t _pos_sp_triplet_pub{nullptr};
|
||||
orb_advert_t _vehicle_cmd_pub{nullptr};
|
||||
|
||||
vehicle_status_s _vstatus; /**< vehicle status */
|
||||
vehicle_land_detected_s _land_detected; /**< vehicle land_detected */
|
||||
vehicle_control_mode_s _control_mode; /**< vehicle control mode */
|
||||
vehicle_global_position_s _global_pos; /**< global vehicle position */
|
||||
fw_pos_ctrl_status_s _fw_pos_ctrl_status{}; /**< fixed wing navigation capabilities */
|
||||
geofence_result_s _geofence_result{};
|
||||
home_position_s _home_pos{}; /**< home position for RTL */
|
||||
mission_result_s _mission_result{};
|
||||
position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of position setpoints */
|
||||
position_setpoint_triplet_s _reposition_triplet{}; /**< triplet for non-mission direct position command */
|
||||
position_setpoint_triplet_s _takeoff_triplet{}; /**< triplet for non-mission direct takeoff command */
|
||||
sensor_combined_s _sensor_combined{}; /**< sensor values */
|
||||
vehicle_attitude_setpoint_s _att_sp{};
|
||||
vehicle_global_position_s _global_pos{}; /**< global vehicle position */
|
||||
vehicle_gps_position_s _gps_pos{}; /**< gps position */
|
||||
vehicle_land_detected_s _land_detected{}; /**< vehicle land_detected */
|
||||
vehicle_local_position_s _local_pos; /**< local vehicle position */
|
||||
vehicle_gps_position_s _gps_pos; /**< gps position */
|
||||
sensor_combined_s _sensor_combined; /**< sensor values */
|
||||
home_position_s _home_pos; /**< home position for RTL */
|
||||
mission_item_s _mission_item; /**< current mission item */
|
||||
fw_pos_ctrl_status_s _fw_pos_ctrl_status; /**< fixed wing navigation capabilities */
|
||||
position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */
|
||||
position_setpoint_triplet_s _reposition_triplet; /**< triplet for non-mission direct position command */
|
||||
position_setpoint_triplet_s _takeoff_triplet; /**< triplet for non-mission direct takeoff command */
|
||||
vehicle_status_s _vstatus{}; /**< vehicle status */
|
||||
|
||||
mission_result_s _mission_result;
|
||||
geofence_result_s _geofence_result;
|
||||
vehicle_attitude_setpoint_s _att_sp;
|
||||
|
||||
bool _mission_item_valid; /**< flags if the current mission item is valid */
|
||||
int _mission_instance_count; /**< instance count for the current mission */
|
||||
int _mission_instance_count{-1}; /**< instance count for the current mission */
|
||||
|
||||
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||
|
||||
Geofence _geofence; /**< class that handles the geofence */
|
||||
bool _geofence_violation_warning_sent; /**< prevents spaming to mavlink */
|
||||
bool _geofence_violation_warning_sent{false}; /**< prevents spaming to mavlink */
|
||||
|
||||
bool _inside_fence; /**< vehicle is inside fence */
|
||||
bool _can_loiter_at_sp{false}; /**< flags if current position SP can be used to loiter */
|
||||
bool _pos_sp_triplet_updated{false}; /**< flags if position SP triplet needs to be published */
|
||||
bool _pos_sp_triplet_published_invalid_once{false}; /**< flags if position SP triplet has been published once to UORB */
|
||||
bool _mission_result_updated{false}; /**< flags if mission result has seen an update */
|
||||
|
||||
bool _can_loiter_at_sp; /**< flags if current position SP can be used to loiter */
|
||||
bool _pos_sp_triplet_updated; /**< flags if position SP triplet needs to be published */
|
||||
bool _pos_sp_triplet_published_invalid_once; /**< flags if position SP triplet has been published once to UORB */
|
||||
bool _mission_result_updated; /**< flags if mission result has seen an update */
|
||||
|
||||
NavigatorMode *_navigation_mode; /**< abstract pointer to current navigation mode class */
|
||||
NavigatorMode *_navigation_mode{nullptr}; /**< abstract pointer to current navigation mode class */
|
||||
Mission _mission; /**< class that handles the missions */
|
||||
Loiter _loiter; /**< class that handles loiter */
|
||||
Takeoff _takeoff; /**< class for handling takeoff commands */
|
||||
Land _land; /**< class for handling land commands */
|
||||
RTL _rtl; /**< class that handles RTL */
|
||||
RCLoss _rcLoss; /**< class that handles RTL according to
|
||||
OBC rules (rc loss mode) */
|
||||
RCLoss _rcLoss; /**< class that handles RTL according to OBC rules (rc loss mode) */
|
||||
DataLinkLoss _dataLinkLoss; /**< class that handles the OBC datalink loss mode */
|
||||
EngineFailure _engineFailure; /**< class that handles the engine failure mode
|
||||
(FW only!) */
|
||||
EngineFailure _engineFailure; /**< class that handles the engine failure mode (FW only!) */
|
||||
GpsFailure _gpsFailure; /**< class that handles the OBC gpsfailure loss mode */
|
||||
|
||||
FollowTarget _follow_target;
|
||||
FollowTarget _follow_target;
|
||||
|
||||
NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */
|
||||
|
||||
@ -319,59 +299,20 @@ private:
|
||||
control::BlockParamFloat _param_cruising_speed_plane;
|
||||
control::BlockParamFloat _param_cruising_throttle_plane;
|
||||
|
||||
float _mission_cruising_speed_mc;
|
||||
float _mission_cruising_speed_fw;
|
||||
float _mission_throttle;
|
||||
float _mission_cruising_speed_mc{-1.0f};
|
||||
float _mission_cruising_speed_fw{-1.0f};
|
||||
float _mission_throttle{-1.0f};
|
||||
|
||||
/**
|
||||
* Retrieve global position
|
||||
*/
|
||||
void global_position_update();
|
||||
|
||||
/**
|
||||
* Retrieve local position
|
||||
*/
|
||||
void local_position_update();
|
||||
|
||||
/**
|
||||
* Retrieve gps position
|
||||
*/
|
||||
void gps_position_update();
|
||||
|
||||
/**
|
||||
* Retrieve sensor values
|
||||
*/
|
||||
void sensor_combined_update();
|
||||
|
||||
/**
|
||||
* Retrieve home position
|
||||
*/
|
||||
void home_position_update(bool force = false);
|
||||
|
||||
/**
|
||||
* Retrieve fixed wing navigation capabilities
|
||||
*/
|
||||
// update subscriptions
|
||||
void fw_pos_ctrl_status_update();
|
||||
|
||||
/**
|
||||
* Retrieve vehicle status
|
||||
*/
|
||||
void vehicle_status_update();
|
||||
|
||||
/**
|
||||
* Retrieve vehicle land detected
|
||||
*/
|
||||
void vehicle_land_detected_update();
|
||||
|
||||
/**
|
||||
* Retrieve vehicle control mode
|
||||
*/
|
||||
void vehicle_control_mode_update();
|
||||
|
||||
/**
|
||||
* Update parameters
|
||||
*/
|
||||
void global_position_update();
|
||||
void gps_position_update();
|
||||
void home_position_update(bool force = false);
|
||||
void local_position_update();
|
||||
void params_update();
|
||||
void sensor_combined_update();
|
||||
void vehicle_land_detected_update();
|
||||
void vehicle_status_update();
|
||||
|
||||
/**
|
||||
* Shim for calling task_main from task_create.
|
||||
@ -393,16 +334,9 @@ private:
|
||||
*/
|
||||
void publish_position_setpoint_triplet();
|
||||
|
||||
|
||||
/**
|
||||
* Publish the mission result so commander and mavlink know what is going on
|
||||
*/
|
||||
void publish_mission_result();
|
||||
|
||||
/* this class has ptr data members, so it should not be copied,
|
||||
* consequently the copy constructors are private.
|
||||
*/
|
||||
Navigator(const Navigator &);
|
||||
Navigator operator=(const Navigator &);
|
||||
};
|
||||
#endif
|
||||
|
||||
@ -47,9 +47,7 @@
|
||||
|
||||
#include <cfloat>
|
||||
|
||||
#include <arch/board/board.h>
|
||||
#include <dataman/dataman.h>
|
||||
#include <drivers/device/device.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <geo/geo.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
@ -60,7 +58,6 @@
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/stat.h>
|
||||
#include <sys/types.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <systemlib/mavlink_log.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <uORB/topics/fence.h>
|
||||
@ -82,57 +79,13 @@ extern "C" __EXPORT int navigator_main(int argc, char *argv[]);
|
||||
|
||||
namespace navigator
|
||||
{
|
||||
|
||||
Navigator *g_navigator;
|
||||
}
|
||||
|
||||
Navigator::Navigator() :
|
||||
SuperBlock(nullptr, "NAV"),
|
||||
_task_should_exit(false),
|
||||
_navigator_task(-1),
|
||||
_mavlink_log_pub(nullptr),
|
||||
_global_pos_sub(-1),
|
||||
_local_pos_sub(-1),
|
||||
_gps_pos_sub(-1),
|
||||
_sensor_combined_sub(-1),
|
||||
_home_pos_sub(-1),
|
||||
_vstatus_sub(-1),
|
||||
_land_detected_sub(-1),
|
||||
_fw_pos_ctrl_status_sub(-1),
|
||||
_control_mode_sub(-1),
|
||||
_onboard_mission_sub(-1),
|
||||
_offboard_mission_sub(-1),
|
||||
_param_update_sub(-1),
|
||||
_vehicle_command_sub(-1),
|
||||
_pos_sp_triplet_pub(nullptr),
|
||||
_mission_result_pub(nullptr),
|
||||
_geofence_result_pub(nullptr),
|
||||
_att_sp_pub(nullptr),
|
||||
_vstatus{},
|
||||
_land_detected{},
|
||||
_control_mode{},
|
||||
_global_pos{},
|
||||
_gps_pos{},
|
||||
_sensor_combined{},
|
||||
_home_pos{},
|
||||
_mission_item{},
|
||||
_fw_pos_ctrl_status{},
|
||||
_pos_sp_triplet{},
|
||||
_reposition_triplet{},
|
||||
_takeoff_triplet{},
|
||||
_mission_result{},
|
||||
_att_sp{},
|
||||
_mission_item_valid(false),
|
||||
_mission_instance_count(0),
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
|
||||
_geofence(this),
|
||||
_geofence_violation_warning_sent(false),
|
||||
_inside_fence(true),
|
||||
_can_loiter_at_sp(false),
|
||||
_pos_sp_triplet_updated(false),
|
||||
_pos_sp_triplet_published_invalid_once(false),
|
||||
_mission_result_updated(false),
|
||||
_navigation_mode(nullptr),
|
||||
_mission(this, "MIS"),
|
||||
_loiter(this, "LOI"),
|
||||
_takeoff(this, "TKF"),
|
||||
@ -149,10 +102,7 @@ Navigator::Navigator() :
|
||||
_param_mc_alt_acceptance_radius(this, "MC_ALT_RAD"),
|
||||
_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_mc(-1.0f),
|
||||
_mission_cruising_speed_fw(-1.0f),
|
||||
_mission_throttle(-1.0f)
|
||||
_param_cruising_throttle_plane(this, "FW_THR_CRUISE", false)
|
||||
{
|
||||
/* Create a list of our possible navigation types */
|
||||
_navigation_mode_array[0] = &_mission;
|
||||
@ -232,7 +182,12 @@ Navigator::home_position_update(bool force)
|
||||
void
|
||||
Navigator::fw_pos_ctrl_status_update()
|
||||
{
|
||||
orb_copy(ORB_ID(fw_pos_ctrl_status), _fw_pos_ctrl_status_sub, &_fw_pos_ctrl_status);
|
||||
bool updated = false;
|
||||
orb_check(_fw_pos_ctrl_status_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(fw_pos_ctrl_status), _fw_pos_ctrl_status_sub, &_fw_pos_ctrl_status);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
@ -250,16 +205,6 @@ Navigator::vehicle_land_detected_update()
|
||||
orb_copy(ORB_ID(vehicle_land_detected), _land_detected_sub, &_land_detected);
|
||||
}
|
||||
|
||||
void
|
||||
Navigator::vehicle_control_mode_update()
|
||||
{
|
||||
if (orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode) != OK) {
|
||||
/* in case the commander is not be running */
|
||||
_control_mode.flag_control_auto_enabled = false;
|
||||
_control_mode.flag_armed = false;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
Navigator::params_update()
|
||||
{
|
||||
@ -306,7 +251,6 @@ Navigator::task_main()
|
||||
_fw_pos_ctrl_status_sub = orb_subscribe(ORB_ID(fw_pos_ctrl_status));
|
||||
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
|
||||
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
|
||||
_home_pos_sub = orb_subscribe(ORB_ID(home_position));
|
||||
_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
|
||||
_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
|
||||
@ -316,7 +260,6 @@ Navigator::task_main()
|
||||
/* copy all topics first time */
|
||||
vehicle_status_update();
|
||||
vehicle_land_detected_update();
|
||||
vehicle_control_mode_update();
|
||||
global_position_update();
|
||||
local_position_update();
|
||||
gps_position_update();
|
||||
@ -407,13 +350,6 @@ Navigator::task_main()
|
||||
params_update();
|
||||
}
|
||||
|
||||
/* vehicle control mode updated */
|
||||
orb_check(_control_mode_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
vehicle_control_mode_update();
|
||||
}
|
||||
|
||||
/* vehicle status updated */
|
||||
orb_check(_vstatus_sub, &updated);
|
||||
|
||||
@ -445,7 +381,7 @@ Navigator::task_main()
|
||||
orb_check(_vehicle_command_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
vehicle_command_s cmd;
|
||||
vehicle_command_s cmd = {};
|
||||
orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);
|
||||
|
||||
if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) {
|
||||
@ -532,7 +468,7 @@ Navigator::task_main()
|
||||
/* find NAV_CMD_DO_LAND_START in the mission and
|
||||
* use MAV_CMD_MISSION_START to start the mission there
|
||||
*/
|
||||
unsigned land_start = _mission.find_offboard_land_start();
|
||||
int land_start = _mission.find_offboard_land_start();
|
||||
|
||||
if (land_start != -1) {
|
||||
vehicle_command_s vcmd = {};
|
||||
@ -542,8 +478,10 @@ Navigator::task_main()
|
||||
vcmd.param1 = land_start;
|
||||
vcmd.param2 = 0;
|
||||
|
||||
orb_advert_t pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
|
||||
(void)orb_unadvertise(pub);
|
||||
publish_vehicle_cmd(vcmd);
|
||||
|
||||
} else {
|
||||
PX4_WARN("planned landing not available");
|
||||
}
|
||||
|
||||
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH) {
|
||||
@ -598,12 +536,12 @@ Navigator::task_main()
|
||||
last_geofence_check = hrt_absolute_time();
|
||||
have_geofence_position_data = false;
|
||||
|
||||
_geofence_result.timestamp = hrt_absolute_time();
|
||||
_geofence_result.geofence_action = _geofence.getGeofenceAction();
|
||||
|
||||
if (!inside) {
|
||||
/* inform other apps via the mission result */
|
||||
_geofence_result.geofence_violated = true;
|
||||
publish_geofence_result();
|
||||
|
||||
/* Issue a warning about the geofence violation once */
|
||||
if (!_geofence_violation_warning_sent) {
|
||||
@ -614,10 +552,12 @@ Navigator::task_main()
|
||||
} else {
|
||||
/* inform other apps via the mission result */
|
||||
_geofence_result.geofence_violated = false;
|
||||
publish_geofence_result();
|
||||
|
||||
/* Reset the _geofence_violation_warning_sent field */
|
||||
_geofence_violation_warning_sent = false;
|
||||
}
|
||||
|
||||
publish_geofence_result();
|
||||
}
|
||||
|
||||
/* Do stuff according to navigation state set by commander */
|
||||
@ -748,24 +688,8 @@ Navigator::start()
|
||||
void
|
||||
Navigator::status()
|
||||
{
|
||||
/* TODO: add this again */
|
||||
// PX4_INFO("Global position is %svalid", _global_pos_valid ? "" : "in");
|
||||
|
||||
// if (_global_pos.global_valid) {
|
||||
// PX4_INFO("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat);
|
||||
// PX4_INFO("Altitude %5.5f meters, altitude above home %5.5f meters",
|
||||
// (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt));
|
||||
// PX4_INFO("Ground velocity in m/s, N %5.5f, E %5.5f, D %5.5f",
|
||||
// (double)_global_pos.vel_n, (double)_global_pos.vel_e, (double)_global_pos.vel_d);
|
||||
// PX4_INFO("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F));
|
||||
// }
|
||||
|
||||
if (_geofence.valid()) {
|
||||
PX4_INFO("Geofence is valid");
|
||||
/* TODO: needed? */
|
||||
// PX4_INFO("Vertex longitude latitude");
|
||||
// for (unsigned i = 0; i < _fence.count; i++)
|
||||
// PX4_INFO("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat);
|
||||
|
||||
} else {
|
||||
PX4_INFO("Geofence not set (no /etc/geofence.txt on microsd) or not valid");
|
||||
@ -804,7 +728,7 @@ Navigator::get_acceptance_radius()
|
||||
float
|
||||
Navigator::get_altitude_acceptance_radius()
|
||||
{
|
||||
if (!this->get_vstatus()->is_rotary_wing) {
|
||||
if (!get_vstatus()->is_rotary_wing) {
|
||||
return _param_fw_alt_acceptance_radius.get();
|
||||
|
||||
} else {
|
||||
@ -817,8 +741,7 @@ Navigator::get_cruising_speed()
|
||||
{
|
||||
/* there are three options: The mission-requested cruise speed, or the current hover / plane speed */
|
||||
if (_vstatus.is_rotary_wing) {
|
||||
if (_mission_cruising_speed_mc > 0.0f
|
||||
&& _vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) {
|
||||
if (is_planned_mission() && _mission_cruising_speed_mc > 0.0f) {
|
||||
return _mission_cruising_speed_mc;
|
||||
|
||||
} else {
|
||||
@ -826,8 +749,7 @@ Navigator::get_cruising_speed()
|
||||
}
|
||||
|
||||
} else {
|
||||
if (_mission_cruising_speed_fw > 0.0f
|
||||
&& _vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) {
|
||||
if (is_planned_mission() && _mission_cruising_speed_fw > 0.0f) {
|
||||
return _mission_cruising_speed_fw;
|
||||
|
||||
} else {
|
||||
@ -1008,7 +930,6 @@ Navigator::publish_mission_result()
|
||||
void
|
||||
Navigator::publish_geofence_result()
|
||||
{
|
||||
|
||||
/* lazily publish the geofence result only once available */
|
||||
if (_geofence_result_pub != nullptr) {
|
||||
/* publish mission result */
|
||||
@ -1034,6 +955,17 @@ Navigator::publish_att_sp()
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
Navigator::publish_vehicle_cmd(const struct vehicle_command_s &vcmd)
|
||||
{
|
||||
if (_vehicle_cmd_pub != nullptr) {
|
||||
orb_publish(ORB_ID(vehicle_command), _vehicle_cmd_pub, &vcmd);
|
||||
|
||||
} else {
|
||||
_vehicle_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
Navigator::set_mission_failure(const char *reason)
|
||||
{
|
||||
|
||||
@ -53,10 +53,6 @@ NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) :
|
||||
on_inactive();
|
||||
}
|
||||
|
||||
NavigatorMode::~NavigatorMode()
|
||||
{
|
||||
}
|
||||
|
||||
void
|
||||
NavigatorMode::run(bool active)
|
||||
{
|
||||
|
||||
@ -56,15 +56,10 @@ class Navigator;
|
||||
class NavigatorMode : public control::SuperBlock
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* Constructor
|
||||
*/
|
||||
NavigatorMode(Navigator *navigator, const char *name);
|
||||
|
||||
/**
|
||||
* Destructor
|
||||
*/
|
||||
virtual ~NavigatorMode();
|
||||
virtual ~NavigatorMode() = default;
|
||||
NavigatorMode(const NavigatorMode &) = delete;
|
||||
NavigatorMode operator=(const NavigatorMode &) = delete;
|
||||
|
||||
void run(bool active);
|
||||
|
||||
@ -84,16 +79,10 @@ public:
|
||||
virtual void on_active();
|
||||
|
||||
protected:
|
||||
Navigator *_navigator;
|
||||
Navigator *_navigator{nullptr};
|
||||
|
||||
private:
|
||||
bool _first_run;
|
||||
|
||||
/* this class has ptr data members, so it should not be copied,
|
||||
* consequently the copy constructors are private.
|
||||
*/
|
||||
NavigatorMode(const NavigatorMode &);
|
||||
NavigatorMode operator=(const NavigatorMode &);
|
||||
bool _first_run{true};
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@ -268,9 +268,9 @@ RTL::set_rtl_item()
|
||||
|
||||
_navigator->set_can_loiter_at_sp(true);
|
||||
|
||||
if (autoland && (Navigator::get_time_inside(_mission_item) > FLT_EPSILON)) {
|
||||
if (autoland && (get_time_inside(_mission_item) > FLT_EPSILON)) {
|
||||
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs",
|
||||
(double)Navigator::get_time_inside(_mission_item));
|
||||
(double)get_time_inside(_mission_item));
|
||||
|
||||
} else {
|
||||
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loiter");
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user