aboutsummaryrefslogtreecommitdiff
path: root/src/modules/navigator
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/navigator')
-rw-r--r--src/modules/navigator/datalinkloss.cpp8
-rw-r--r--src/modules/navigator/geofence.cpp17
-rw-r--r--src/modules/navigator/gpsfailure.cpp2
-rw-r--r--src/modules/navigator/mission.cpp29
-rw-r--r--src/modules/navigator/mission.h6
-rw-r--r--src/modules/navigator/navigator.h18
-rw-r--r--src/modules/navigator/navigator_main.cpp37
-rw-r--r--src/modules/navigator/navigator_mode.cpp2
-rw-r--r--src/modules/navigator/rcloss.cpp6
9 files changed, 93 insertions, 32 deletions
diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp
index e789fd10d..87a6e023a 100644
--- a/src/modules/navigator/datalinkloss.cpp
+++ b/src/modules/navigator/datalinkloss.cpp
@@ -155,7 +155,7 @@ DataLinkLoss::set_dll_item()
case DLL_STATE_TERMINATE: {
/* Request flight termination from the commander */
_navigator->get_mission_result()->flight_termination = true;
- _navigator->publish_mission_result();
+ _navigator->set_mission_result_updated();
reset_mission_item_reached();
warnx("not switched to manual: request flight termination");
pos_sp_triplet->previous.valid = false;
@@ -188,7 +188,7 @@ DataLinkLoss::advance_dll()
_navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get());
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to airfield home");
_navigator->get_mission_result()->stay_in_failsafe = true;
- _navigator->publish_mission_result();
+ _navigator->set_mission_result_updated();
reset_mission_item_reached();
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
} else {
@@ -209,7 +209,7 @@ DataLinkLoss::advance_dll()
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home");
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
_navigator->get_mission_result()->stay_in_failsafe = true;
- _navigator->publish_mission_result();
+ _navigator->set_mission_result_updated();
reset_mission_item_reached();
break;
case DLL_STATE_FLYTOAIRFIELDHOMEWP:
@@ -217,7 +217,7 @@ DataLinkLoss::advance_dll()
warnx("time is up, state should have been changed manually by now");
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating");
_navigator->get_mission_result()->stay_in_failsafe = true;
- _navigator->publish_mission_result();
+ _navigator->set_mission_result_updated();
reset_mission_item_reached();
break;
case DLL_STATE_TERMINATE:
diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp
index 0f431ded2..4482fb36b 100644
--- a/src/modules/navigator/geofence.cpp
+++ b/src/modules/navigator/geofence.cpp
@@ -279,8 +279,14 @@ Geofence::loadFromFile(const char *filename)
while((textStart < sizeof(line)/sizeof(char)) && isspace(line[textStart])) textStart++;
/* if the line starts with #, skip */
- if (line[textStart] == commentChar)
+ if (line[textStart] == commentChar) {
continue;
+ }
+
+ /* if there is only a linefeed, skip it */
+ if (line[0] == '\n') {
+ continue;
+ }
if (gotVertical) {
/* Parse the line as a geofence point */
@@ -291,8 +297,10 @@ Geofence::loadFromFile(const char *filename)
/* Handle degree minute second format */
float lat_d, lat_m, lat_s, lon_d, lon_m, lon_s;
- if (sscanf(line, "DMS %f %f %f %f %f %f", &lat_d, &lat_m, &lat_s, &lon_d, &lon_m, &lon_s) != 6)
+ if (sscanf(line, "DMS %f %f %f %f %f %f", &lat_d, &lat_m, &lat_s, &lon_d, &lon_m, &lon_s) != 6) {
+ warnx("Scanf to parse DMS geofence vertex failed.");
return ERROR;
+ }
// warnx("Geofence DMS: %.5f %.5f %.5f ; %.5f %.5f %.5f", (double)lat_d, (double)lat_m, (double)lat_s, (double)lon_d, (double)lon_m, (double)lon_s);
@@ -301,9 +309,10 @@ Geofence::loadFromFile(const char *filename)
} else {
/* Handle decimal degree format */
-
- if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2)
+ if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2) {
+ warnx("Scanf to parse geofence vertex failed.");
return ERROR;
+ }
}
if (dm_write(DM_KEY_FENCE_POINTS, pointCounter, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) != sizeof(vertex))
diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp
index cd55f60b0..e370796c0 100644
--- a/src/modules/navigator/gpsfailure.cpp
+++ b/src/modules/navigator/gpsfailure.cpp
@@ -141,7 +141,7 @@ GpsFailure::set_gpsf_item()
case GPSF_STATE_TERMINATE: {
/* Request flight termination from the commander */
_navigator->get_mission_result()->flight_termination = true;
- _navigator->publish_mission_result();
+ _navigator->set_mission_result_updated();
warnx("gps fail: request flight termination");
}
default:
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 0765e9b7c..9b0a092da 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -38,6 +38,7 @@
* @author Julian Oes <julian@oes.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Ban Siesta <bansiesta@gmail.com>
*/
#include <sys/types.h>
@@ -149,18 +150,12 @@ Mission::on_active()
/* lets check if we reached the current mission item */
if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
+ set_mission_item_reached();
if (_mission_item.autocontinue) {
/* switch to next waypoint if 'autocontinue' flag set */
advance_mission();
set_mission_items();
- } else {
- /* else just report that item reached */
- if (_mission_type == MISSION_TYPE_OFFBOARD) {
- if (!(_navigator->get_mission_result()->seq_reached == _current_offboard_mission_index && _navigator->get_mission_result()->reached)) {
- set_mission_item_reached();
- }
- }
}
} else if (_mission_type != MISSION_TYPE_NONE &&_param_altmode.get() == MISSION_ALTMODE_FOH) {
@@ -395,7 +390,6 @@ Mission::set_mission_items()
/* reuse setpoint for LOITER only if it's not IDLE */
_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER);
- reset_mission_item_reached();
set_mission_finished();
_navigator->set_position_setpoint_triplet_updated();
@@ -636,6 +630,8 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
"ERROR DO JUMP waypoint could not be written");
return false;
}
+ report_do_jump_mission_changed(*mission_index_ptr,
+ mission_item_tmp.do_jump_repeat_count);
}
/* set new mission item index and repeat
* we don't have to validate here, if it's invalid, we should realize this later .*/
@@ -707,22 +703,31 @@ Mission::save_offboard_mission_state()
}
void
+Mission::report_do_jump_mission_changed(int index, int do_jumps_remaining)
+{
+ /* inform about the change */
+ _navigator->get_mission_result()->item_do_jump_changed = true;
+ _navigator->get_mission_result()->item_changed_index = index;
+ _navigator->get_mission_result()->item_do_jump_remaining = do_jumps_remaining;
+ _navigator->set_mission_result_updated();
+}
+
+void
Mission::set_mission_item_reached()
{
_navigator->get_mission_result()->reached = true;
_navigator->get_mission_result()->seq_reached = _current_offboard_mission_index;
- _navigator->publish_mission_result();
+ _navigator->set_mission_result_updated();
reset_mission_item_reached();
}
void
Mission::set_current_offboard_mission_item()
{
- warnx("current offboard mission index: %d", _current_offboard_mission_index);
_navigator->get_mission_result()->reached = false;
_navigator->get_mission_result()->finished = false;
_navigator->get_mission_result()->seq_current = _current_offboard_mission_index;
- _navigator->publish_mission_result();
+ _navigator->set_mission_result_updated();
save_offboard_mission_state();
}
@@ -731,5 +736,5 @@ void
Mission::set_mission_finished()
{
_navigator->get_mission_result()->finished = true;
- _navigator->publish_mission_result();
+ _navigator->set_mission_result_updated();
}
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index ea7cc0927..a8a644b0f 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -38,6 +38,7 @@
* @author Julian Oes <julian@oes.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Ban Siesta <bansiesta@gmail.com>
*/
#ifndef NAVIGATOR_MISSION_H
@@ -131,6 +132,11 @@ private:
void save_offboard_mission_state();
/**
+ * Inform about a changed mission item after a DO_JUMP
+ */
+ void report_do_jump_mission_changed(int index, int do_jumps_remaining);
+
+ /**
* Set a mission item as reached
*/
void set_mission_item_reached();
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 9cd609955..d9d911d9c 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -54,6 +54,7 @@
#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 "navigator_mode.h"
@@ -107,9 +108,9 @@ public:
void load_fence_from_file(const char *filename);
/**
- * Publish the mission result so commander and mavlink know what is going on
+ * Publish the geofence result
*/
- void publish_mission_result();
+ void publish_geofence_result();
/**
* Publish the attitude sp, only to be used in very special modes when position control is deactivated
@@ -122,6 +123,7 @@ public:
*/
void set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; }
void set_position_setpoint_triplet_updated() { _pos_sp_triplet_updated = true; }
+ void set_mission_result_updated() { _mission_result_updated = true; }
/**
* Getters
@@ -134,6 +136,7 @@ public:
struct home_position_s* get_home_position() { return &_home_pos; }
struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_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; }
int get_onboard_mission_sub() { return _onboard_mission_sub; }
@@ -164,6 +167,7 @@ private:
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 */
@@ -179,7 +183,8 @@ private:
position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */
mission_result_s _mission_result;
- vehicle_attitude_setpoint_s _att_sp;
+ geofence_result_s _geofence_result;
+ vehicle_attitude_setpoint_s _att_sp;
bool _mission_item_valid; /**< flags if the current mission item is valid */
@@ -206,6 +211,7 @@ private:
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 */
control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */
control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */
@@ -271,6 +277,12 @@ 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.
*/
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index df620e5e7..3f7670ec4 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -110,6 +110,7 @@ Navigator::Navigator() :
_param_update_sub(-1),
_pos_sp_triplet_pub(-1),
_mission_result_pub(-1),
+ _geofence_result_pub(-1),
_att_sp_pub(-1),
_vstatus{},
_control_mode{},
@@ -138,6 +139,7 @@ Navigator::Navigator() :
_can_loiter_at_sp(false),
_pos_sp_triplet_updated(false),
_pos_sp_triplet_published_invalid_once(false),
+ _mission_result_updated(false),
_param_loiter_radius(this, "LOITER_RAD"),
_param_acceptance_radius(this, "ACC_RAD"),
_param_datalinkloss_obc(this, "DLL_OBC"),
@@ -398,8 +400,8 @@ Navigator::task_main()
have_geofence_position_data = false;
if (!inside) {
/* inform other apps via the mission result */
- _mission_result.geofence_violated = true;
- publish_mission_result();
+ _geofence_result.geofence_violated = true;
+ publish_geofence_result();
/* Issue a warning about the geofence violation once */
if (!_geofence_violation_warning_sent) {
@@ -408,8 +410,8 @@ Navigator::task_main()
}
} else {
/* inform other apps via the mission result */
- _mission_result.geofence_violated = false;
- publish_mission_result();
+ _geofence_result.geofence_violated = false;
+ publish_geofence_result();
/* Reset the _geofence_violation_warning_sent field */
_geofence_violation_warning_sent = false;
}
@@ -490,6 +492,11 @@ Navigator::task_main()
_pos_sp_triplet_updated = false;
}
+ if (_mission_result_updated) {
+ publish_mission_result();
+ _mission_result_updated = false;
+ }
+
perf_end(_loop_perf);
}
warnx("exiting.");
@@ -639,6 +646,28 @@ Navigator::publish_mission_result()
/* advertise and publish */
_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_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;
+ _mission_result.item_do_jump_remaining = 0;
+}
+
+void
+Navigator::publish_geofence_result()
+{
+
+ /* lazily publish the geofence result only once available */
+ if (_geofence_result_pub > 0) {
+ /* publish mission result */
+ orb_publish(ORB_ID(geofence_result), _geofence_result_pub, &_geofence_result);
+
+ } else {
+ /* advertise and publish */
+ _geofence_result_pub = orb_advertise(ORB_ID(geofence_result), &_geofence_result);
+ }
}
void
diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp
index 3807c5ea8..2f322031c 100644
--- a/src/modules/navigator/navigator_mode.cpp
+++ b/src/modules/navigator/navigator_mode.cpp
@@ -65,7 +65,7 @@ NavigatorMode::run(bool active) {
_first_run = false;
/* Reset stay in failsafe flag */
_navigator->get_mission_result()->stay_in_failsafe = false;
- _navigator->publish_mission_result();
+ _navigator->set_mission_result_updated();
on_activation();
} else {
diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp
index 42392e739..a7cde6325 100644
--- a/src/modules/navigator/rcloss.cpp
+++ b/src/modules/navigator/rcloss.cpp
@@ -128,7 +128,7 @@ RCLoss::set_rcl_item()
case RCL_STATE_TERMINATE: {
/* Request flight termination from the commander */
_navigator->get_mission_result()->flight_termination = true;
- _navigator->publish_mission_result();
+ _navigator->set_mission_result_updated();
warnx("rc not recovered: request flight termination");
pos_sp_triplet->previous.valid = false;
pos_sp_triplet->current.valid = false;
@@ -162,7 +162,7 @@ RCLoss::advance_rcl()
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, terminating");
_rcl_state = RCL_STATE_TERMINATE;
_navigator->get_mission_result()->stay_in_failsafe = true;
- _navigator->publish_mission_result();
+ _navigator->set_mission_result_updated();
reset_mission_item_reached();
}
break;
@@ -171,7 +171,7 @@ RCLoss::advance_rcl()
warnx("time is up, no RC regain, terminating");
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RC not regained, terminating");
_navigator->get_mission_result()->stay_in_failsafe = true;
- _navigator->publish_mission_result();
+ _navigator->set_mission_result_updated();
reset_mission_item_reached();
break;
case RCL_STATE_TERMINATE:
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-13 19:59:41 +0000