From 7a424244114a5a17e92af705f26eb456361edc2e Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 8 Sep 2017 12:18:59 -0400
Subject: [PATCH] Navigator resurrect FW GPS failure navigation (#7762)
---
.../commander/state_machine_helper.cpp | 103 +++++++++++++-----
src/modules/navigator/gpsfailure.cpp | 70 ++++++------
src/modules/navigator/gpsfailure.h | 20 ++--
src/modules/navigator/gpsfailure_params.c | 21 ++--
src/modules/navigator/navigator.h | 2 -
src/modules/navigator/navigator_main.cpp | 69 +++---------
6 files changed, 138 insertions(+), 147 deletions(-)
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index a50435b17c..a686941fb4 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -72,7 +72,6 @@ static const char reason_no_rc[] = "no RC";
static const char reason_no_offboard[] = "no offboard";
static const char reason_no_rc_and_no_offboard[] = "no RC and no offboard";
static const char reason_no_gps[] = "no gps";
-static const char reason_no_gps_cmd[] = "no gps cmd";
static const char reason_no_local_position[] = "no local position";
static const char reason_no_global_position[] = "no global position";
static const char reason_no_datalink[] = "no datalink";
@@ -518,6 +517,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
if (hitl_on) {
mavlink_log_info(mavlink_log_pub, "Enabled Hardware-in-the-loop simulation.");
+
} else {
mavlink_log_critical(mavlink_log_pub, "Set parameter SYS_HITL to 1 before starting HITL.");
ret = TRANSITION_DENIED;
@@ -648,6 +648,7 @@ bool set_nav_state(struct vehicle_status_s *status,
} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, !(posctl_nav_loss_act == 1), !status->is_rotary_wing)) {
// nothing to do - everything done in check_invalid_pos_nav_state
+
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
}
@@ -669,20 +670,21 @@ bool set_nav_state(struct vehicle_status_s *status,
} else if (status_flags->data_link_lost_cmd) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
- } else if (status_flags->gps_failure_cmd) {
- status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
- enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_gps_cmd);
-
} else if (status_flags->rc_signal_lost_cmd) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
} else if (status_flags->vtol_transition_failure_cmd) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
- /* finished handling commands which have priority, now handle failures */
+ } else if (status_flags->gps_failure || status_flags->gps_failure_cmd) {
+ if (status->is_rotary_wing) {
+ status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+
+ } else {
+ // TODO: FW position controller doesn't run without condition_global_position_valid
+ status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
+ }
- } else if (status_flags->gps_failure) {
- status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_gps);
} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
@@ -696,28 +698,24 @@ bool set_nav_state(struct vehicle_status_s *status,
} else if (status->mission_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
+ } else if (data_link_loss_act_configured && status->data_link_lost) {
/* datalink loss enabled:
* check for datalink lost: this should always trigger RTGS */
-
- } else if (data_link_loss_act_configured && status->data_link_lost) {
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
set_data_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act);
+ } else if (!data_link_loss_act_configured && status->rc_signal_lost && status->data_link_lost && !landed
+ && mission_finished) {
/* datalink loss DISABLED:
* check if both, RC and datalink are lost during the mission
* or all links are lost after the mission finishes in air: this should always trigger RCRECOVER */
-
- } else if (!data_link_loss_act_configured && status->rc_signal_lost && status->data_link_lost && !landed
- && mission_finished) {
-
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
set_rc_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act);
- /* stay where you are if you should stay in failsafe, otherwise everything is perfect */
-
} else if (!stay_in_failsafe) {
+ /* stay where you are if you should stay in failsafe, otherwise everything is perfect */
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
}
@@ -730,28 +728,31 @@ bool set_nav_state(struct vehicle_status_s *status,
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (status_flags->gps_failure) {
- status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
- enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_gps);
+ if (status->is_rotary_wing) {
+ status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
- /* also go into failsafe if just datalink is lost, and we're actually in air */
+ } else {
+ status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
+ }
+
+ enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_gps);
- enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
// nothing to do - everything done in check_invalid_pos_nav_state
} else if (status->data_link_lost && data_link_loss_act_configured && !landed) {
-
+ /* also go into failsafe if just datalink is lost, and we're actually in air */
set_data_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act);
- /* go into failsafe if RC is lost and datalink loss is not set up and rc loss is not DISABLED */
+ enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
} else if (rc_lost && !data_link_loss_act_configured) {
+ /* go into failsafe if RC is lost and datalink loss is not set up and rc loss is not DISABLED */
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);
set_rc_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act);
- /* don't bother if RC is lost if datalink is connected */
-
} else if (status->rc_signal_lost) {
+ /* don't bother if RC is lost if datalink is connected */
/* this mode is ok, we don't need RC for LOITERing */
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
@@ -771,7 +772,14 @@ bool set_nav_state(struct vehicle_status_s *status,
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (status_flags->gps_failure) {
- status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+ if (status->is_rotary_wing) {
+ status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+
+ } else {
+ // TODO: FW position controller doesn't run without condition_global_position_valid
+ status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
+ }
+
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_gps);
} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
@@ -857,7 +865,13 @@ bool set_nav_state(struct vehicle_status_s *status,
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
} else if (status_flags->condition_local_altitude_valid) {
- status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+ if (status->is_rotary_wing) {
+ status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+
+ } else {
+ // TODO: FW position controller doesn't run without condition_global_position_valid
+ status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
+ }
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
@@ -890,7 +904,13 @@ bool set_nav_state(struct vehicle_status_s *status,
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
} else if (status_flags->condition_local_altitude_valid) {
- status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+ if (status->is_rotary_wing) {
+ status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+
+ } else {
+ // TODO: FW position controller doesn't run without condition_global_position_valid
+ status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
+ }
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
@@ -901,7 +921,13 @@ bool set_nav_state(struct vehicle_status_s *status,
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
} else if (status_flags->condition_local_altitude_valid) {
- status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+ if (status->is_rotary_wing) {
+ status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+
+ } else {
+ // TODO: FW position controller doesn't run without condition_global_position_valid
+ status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
+ }
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
@@ -948,17 +974,27 @@ bool check_invalid_pos_nav_state(struct vehicle_status_s *status,
// fallback to a mode that gives the operator stick control
if (status->is_rotary_wing && status_flags->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
+
} else if (status_flags->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
+
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_STAB;
}
+
} else {
// go into a descent that does not require stick control
if (status_flags->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
+
} else if (status_flags->condition_local_altitude_valid) {
- status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+ if (status->is_rotary_wing) {
+ status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+
+ } else {
+ // TODO: FW position controller doesn't run without condition_global_position_valid
+ status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
+ }
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
@@ -966,6 +1002,7 @@ bool check_invalid_pos_nav_state(struct vehicle_status_s *status,
if (using_global_pos) {
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_global_position);
+
} else {
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_local_position);
}
@@ -1026,7 +1063,13 @@ void set_link_loss_nav_state(vehicle_status_s *status,
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
} else if (status_flags->condition_local_altitude_valid) {
- status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+ if (status->is_rotary_wing) {
+ status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+
+ } else {
+ // TODO: FW position controller doesn't run without condition_global_position_valid
+ status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
+ }
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp
index 8d57319160..a1f7c8cc50 100644
--- a/src/modules/navigator/gpsfailure.cpp
+++ b/src/modules/navigator/gpsfailure.cpp
@@ -37,24 +37,21 @@
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
-#include <string.h>
-#include <stdlib.h>
-#include <math.h>
-#include <fcntl.h>
+#include "gpsfailure.h"
+#include "navigator.h"
#include <systemlib/mavlink_log.h>
-#include <systemlib/err.h>
#include <geo/geo.h>
#include <navigator/navigation.h>
-
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/home_position.h>
+#include <mathlib/mathlib.h>
-#include "navigator.h"
-#include "gpsfailure.h"
+using matrix::Eulerf;
+using matrix::Quatf;
-#define DELAY_SIGMA 0.01f
+static constexpr float DELAY_SIGMA = 0.01f;
GpsFailure::GpsFailure(Navigator *navigator, const char *name) :
MissionBlock(navigator, name),
@@ -67,14 +64,11 @@ GpsFailure::GpsFailure(Navigator *navigator, const char *name) :
{
/* load initial params */
updateParams();
+
/* initial reset */
on_inactive();
}
-GpsFailure::~GpsFailure()
-{
-}
-
void
GpsFailure::on_inactive()
{
@@ -96,24 +90,34 @@ GpsFailure::on_activation()
void
GpsFailure::on_active()
{
-
switch (_gpsf_state) {
case GPSF_STATE_LOITER: {
/* Position controller does not run in this mode:
* navigator has to publish an attitude setpoint */
- _navigator->get_att_sp()->roll_body = M_DEG_TO_RAD_F * _param_openlooploiter_roll.get();
- _navigator->get_att_sp()->pitch_body = M_DEG_TO_RAD_F * _param_openlooploiter_pitch.get();
- _navigator->get_att_sp()->thrust = _param_openlooploiter_thrust.get();
- _navigator->publish_att_sp();
+ vehicle_attitude_setpoint_s att_sp = {};
+ att_sp.timestamp = hrt_absolute_time();
+ att_sp.roll_body = math::radians(_param_openlooploiter_roll.get());
+ att_sp.pitch_body = math::radians(_param_openlooploiter_pitch.get());
+ att_sp.thrust = _param_openlooploiter_thrust.get();
+
+ Quatf q(Eulerf(att_sp.roll_body, att_sp.pitch_body, 0.0f));
+ q.copyTo(att_sp.q_d);
+ att_sp.q_d_valid = true;
+
+ if (_att_sp_pub != nullptr) {
+ /* publish att sp*/
+ orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp);
+
+ } else {
+ /* advertise and publish */
+ _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
+ }
/* Measure time */
- hrt_abstime elapsed = hrt_elapsed_time(&_timestamp_activation);
-
- //warnx("open loop loiter, posctl enabled %u, elapsed %.1fs, thrust %.2f",
- //_navigator->get_control_mode()->flag_control_position_enabled, elapsed * 1e-6, (double)_param_openlooploiter_thrust.get());
- if (elapsed > _param_loitertime.get() * 1e6f) {
- /* no recovery, adavance the state machine */
- warnx("gps not recovered, switch to next state");
+ if ((_param_loitertime.get() > FLT_EPSILON) &&
+ (hrt_elapsed_time(&_timestamp_activation) > _param_loitertime.get() * 1e6f)) {
+ /* no recovery, advance the state machine */
+ PX4_WARN("GPS not recovered, switching to next failure state");
advance_gpsf();
}
@@ -142,17 +146,17 @@ GpsFailure::set_gpsf_item()
switch (_gpsf_state) {
case GPSF_STATE_TERMINATE: {
- /* Request flight termination from the commander */
+ /* Request flight termination from commander */
_navigator->get_mission_result()->flight_termination = true;
_navigator->set_mission_result_updated();
- warnx("gps fail: request flight termination");
+ PX4_WARN("GPS failure: request flight termination");
}
+ break;
default:
break;
}
- reset_mission_item_reached();
_navigator->set_position_setpoint_triplet_updated();
}
@@ -162,20 +166,18 @@ GpsFailure::advance_gpsf()
switch (_gpsf_state) {
case GPSF_STATE_NONE:
_gpsf_state = GPSF_STATE_LOITER;
- warnx("gpsf loiter");
- mavlink_log_critical(_navigator->get_mavlink_log_pub(), "GPS failed: open loop loiter");
+ mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Global position failure: fixed bank loiter");
break;
case GPSF_STATE_LOITER:
_gpsf_state = GPSF_STATE_TERMINATE;
- warnx("gpsf terminate");
- mavlink_log_emergency(_navigator->get_mavlink_log_pub(), "no gps recovery, termination");
- warnx("mavlink sent");
+ mavlink_log_emergency(_navigator->get_mavlink_log_pub(), "no GPS recovery, terminating flight");
break;
case GPSF_STATE_TERMINATE:
- warnx("gpsf end");
+ PX4_WARN("terminate");
_gpsf_state = GPSF_STATE_END;
+ break;
default:
break;
diff --git a/src/modules/navigator/gpsfailure.h b/src/modules/navigator/gpsfailure.h
index e9e72babf8..dcc9dce887 100644
--- a/src/modules/navigator/gpsfailure.h
+++ b/src/modules/navigator/gpsfailure.h
@@ -47,9 +47,6 @@
#include <uORB/Publication.hpp>
#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <drivers/drv_hrt.h>
-
-#include "navigator_mode.h"
#include "mission_block.h"
class Navigator;
@@ -58,14 +55,11 @@ class GpsFailure : public MissionBlock
{
public:
GpsFailure(Navigator *navigator, const char *name);
+ ~GpsFailure() = default;
- ~GpsFailure();
-
- virtual void on_inactive();
-
- virtual void on_activation();
-
- virtual void on_active();
+ void on_inactive() override;
+ void on_activation() override;
+ void on_active() override;
private:
/* Params */
@@ -79,9 +73,11 @@ private:
GPSF_STATE_LOITER = 1,
GPSF_STATE_TERMINATE = 2,
GPSF_STATE_END = 3,
- } _gpsf_state;
+ } _gpsf_state{GPSF_STATE_NONE};
+
+ hrt_abstime _timestamp_activation{0}; //*< timestamp when this mode was activated */
- hrt_abstime _timestamp_activation; //*< timestamp when this mode was activated */
+ orb_advert_t _att_sp_pub{nullptr};
/**
* Set the GPSF item
diff --git a/src/modules/navigator/gpsfailure_params.c b/src/modules/navigator/gpsfailure_params.c
index 7ee5e77080..d671308341 100644
--- a/src/modules/navigator/gpsfailure_params.c
+++ b/src/modules/navigator/gpsfailure_params.c
@@ -36,18 +36,13 @@
*
* Parameters for GPSF navigation mode
*
- * @author Thomas Gubler <thomasgubler@gmail.com>
- */
-
-/*
- * GPS Failure Navigation Mode parameters, accessible via MAVLink
*/
/**
* Loiter time
*
- * The amount of time in seconds the system should do open loop loiter and wait for gps recovery
- * before it goes into flight termination.
+ * The time in seconds the system should do open loop loiter and wait for GPS recovery
+ * before it goes into flight termination. Set to 0 to disable.
*
* @unit s
* @min 0.0
@@ -56,12 +51,12 @@
* @increment 1
* @group GPS Failure Navigation
*/
-PARAM_DEFINE_FLOAT(NAV_GPSF_LT, 30.0f);
+PARAM_DEFINE_FLOAT(NAV_GPSF_LT, 0.0f);
/**
- * Open loop loiter roll
+ * Fixed bank angle
*
- * Roll in degrees during the open loop loiter
+ * Roll in degrees during the loiter
*
* @unit deg
* @min 0.0
@@ -73,7 +68,7 @@ PARAM_DEFINE_FLOAT(NAV_GPSF_LT, 30.0f);
PARAM_DEFINE_FLOAT(NAV_GPSF_R, 15.0f);
/**
- * Open loop loiter pitch
+ * Fixed pitch angle
*
* Pitch in degrees during the open loop loiter
*
@@ -87,7 +82,7 @@ PARAM_DEFINE_FLOAT(NAV_GPSF_R, 15.0f);
PARAM_DEFINE_FLOAT(NAV_GPSF_P, 0.0f);
/**
- * Open loop loiter thrust
+ * Thrust
*
* Thrust value which is set during the open loop loiter
*
@@ -98,4 +93,4 @@ PARAM_DEFINE_FLOAT(NAV_GPSF_P, 0.0f);
* @increment 0.05
* @group GPS Failure Navigation
*/
-PARAM_DEFINE_FLOAT(NAV_GPSF_TR, 0.7f);
+PARAM_DEFINE_FLOAT(NAV_GPSF_TR, 0.0f);
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 135a482ba3..2b3939d724 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -130,7 +130,6 @@ public:
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 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; }
@@ -239,7 +238,6 @@ private:
int _vehicle_command_sub{-1}; /**< vehicle commands (onboard and offboard) */
int _vstatus_sub{-1}; /**< vehicle status subscription */
- 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};
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index b1d53fef51..e5b40fe27a 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -57,14 +57,9 @@
#include <px4_tasks.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
-
#include <sys/types.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/systemlib.h>
-#include <drivers/device/device.h>
-#include <arch/board/board.h>
-
-#include <uORB/uORB.h>
#include <uORB/topics/fw_pos_ctrl_status.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/mission.h>
@@ -267,13 +262,11 @@ Navigator::task_main()
px4_pollfd_struct_t fds[1] = {};
/* Setup of loop */
- fds[0].fd = _global_pos_sub;
+ fds[0].fd = _local_pos_sub;
fds[0].events = POLLIN;
- bool global_pos_available_once = false;
-
- /* rate-limit global pos subscription to 20 Hz / 50 ms */
- orb_set_interval(_global_pos_sub, 49);
+ /* rate-limit position subscription to 20 Hz / 50 ms */
+ orb_set_interval(_local_pos_sub, 50);
while (!_task_should_exit) {
@@ -281,11 +274,6 @@ Navigator::task_main()
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000);
if (pret == 0) {
- /* timed out - periodic check for _task_should_exit, etc. */
- if (global_pos_available_once) {
- global_pos_available_once = false;
- }
-
/* Let the loop run anyway, don't do `continue` here. */
} else if (pret < 0) {
@@ -295,16 +283,9 @@ Navigator::task_main()
continue;
} else {
-
if (fds[0].revents & POLLIN) {
- /* success, global pos is available */
- global_position_update();
-
- if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
- have_geofence_position_data = true;
- }
-
- global_pos_available_once = true;
+ /* success, local pos is available */
+ local_position_update();
}
}
@@ -323,11 +304,15 @@ Navigator::task_main()
}
}
- /* local position updated */
- orb_check(_local_pos_sub, &updated);
+ /* global position updated */
+ orb_check(_global_pos_sub, &updated);
if (updated) {
- local_position_update();
+ global_position_update();
+
+ if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
+ have_geofence_position_data = true;
+ }
}
/* sensors combined updated */
@@ -372,6 +357,7 @@ Navigator::task_main()
home_position_update();
}
+ /* vehicle_command updated */
orb_check(_vehicle_command_sub, &updated);
if (updated) {
@@ -509,9 +495,6 @@ Navigator::task_main()
_mission.set_current_offboard_mission_index(cmd.param1);
}
- } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_PAUSE_CONTINUE) {
- 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
@@ -680,29 +663,17 @@ Navigator::task_main()
}
orb_unsubscribe(_global_pos_sub);
- _global_pos_sub = -1;
orb_unsubscribe(_local_pos_sub);
- _local_pos_sub = -1;
orb_unsubscribe(_gps_pos_sub);
- _gps_pos_sub = -1;
orb_unsubscribe(_sensor_combined_sub);
- _sensor_combined_sub = -1;
orb_unsubscribe(_fw_pos_ctrl_status_sub);
- _fw_pos_ctrl_status_sub = -1;
orb_unsubscribe(_vstatus_sub);
- _vstatus_sub = -1;
orb_unsubscribe(_land_detected_sub);
- _land_detected_sub = -1;
orb_unsubscribe(_home_pos_sub);
- _home_pos_sub = -1;
orb_unsubscribe(_onboard_mission_sub);
- _onboard_mission_sub = -1;
orb_unsubscribe(_offboard_mission_sub);
- _offboard_mission_sub = -1;
orb_unsubscribe(_param_update_sub);
- _param_update_sub = -1;
orb_unsubscribe(_vehicle_command_sub);
- _vehicle_command_sub = -1;
PX4_INFO("exiting");
@@ -985,20 +956,6 @@ Navigator::publish_geofence_result()
}
}
-void
-Navigator::publish_att_sp()
-{
- /* lazily publish the attitude sp only once available */
- if (_att_sp_pub != nullptr) {
- /* publish att sp*/
- orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
-
- } else {
- /* advertise and publish */
- _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
- }
-}
-
void
Navigator::publish_vehicle_cmd(const struct vehicle_command_s &vcmd)
{
--
GitLab