From d4f713b2861efa9dce79cb814396370904d9a2ac Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 9 Nov 2018 14:03:24 -0500
Subject: [PATCH] commander cleanup home position handling
---
src/modules/commander/Commander.cpp | 228 +++++++++++-----------------
src/modules/commander/Commander.hpp | 22 ++-
src/modules/uORB/Publication.hpp | 6 +
3 files changed, 113 insertions(+), 143 deletions(-)
diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp
index d484acf0ed..79f89158b3 100644
--- a/src/modules/commander/Commander.cpp
+++ b/src/modules/commander/Commander.cpp
@@ -156,7 +156,6 @@ static struct actuator_armed_s armed = {};
static struct safety_s safety = {};
static struct vehicle_control_mode_s control_mode = {};
static struct offboard_control_mode_s offboard_control_mode = {};
-static struct home_position_s _home = {};
static int32_t _flight_mode_slots[manual_control_setpoint_s::MODE_SLOT_MAX];
static struct commander_state_s internal_state = {};
@@ -500,19 +499,9 @@ void usage(const char *reason)
void print_status()
{
- PX4_INFO("type: %s", (status.is_rotary_wing) ? "symmetric motion" : "forward motion");
- PX4_INFO("safety: USB enabled: %s, power state valid: %s", (status_flags.usb_connected) ? "[OK]" : "[NO]",
- (status_flags.condition_power_input_valid) ? " [OK]" : "[NO]");
- PX4_INFO("home: lat = %.7f, lon = %.7f, alt = %.2f, yaw: %.2f", _home.lat, _home.lon, (double)_home.alt, (double)_home.yaw);
- PX4_INFO("home: x = %.7f, y = %.7f, z = %.2f ", (double)_home.x, (double)_home.y, (double)_home.z);
- PX4_INFO("datalink: %s %s", (status.data_link_lost) ? "LOST" : "OK", (status.high_latency_data_link_active) ? "(high latency)" : " ");
- PX4_INFO("main state: %d", internal_state.main_state);
- PX4_INFO("nav state: %d", status.nav_state);
PX4_INFO("arming: %s", arming_state_names[status.arming_state]);
}
-static orb_advert_t status_pub;
-
transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, const char *armedBy)
{
transition_result_t arming_res = TRANSITION_NOT_CHANGED;
@@ -546,9 +535,14 @@ Commander::Commander() :
_battery_sub = orb_subscribe(ORB_ID(battery_status));
}
+Commander::~Commander()
+{
+ orb_unsubscribe(_battery_sub);
+}
+
bool
Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_s& cmd, actuator_armed_s *armed_local,
- home_position_s *home, orb_advert_t *home_pub, orb_advert_t *command_ack_pub, bool *changed)
+ orb_advert_t *command_ack_pub, bool *changed)
{
/* only handle commands that are meant to be handled by this system and component */
if (cmd.target_system != status_local->system_id || ((cmd.target_component != status_local->component_id)
@@ -760,10 +754,9 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
/* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */
if (cmd_arms && (arming_res == TRANSITION_CHANGED) &&
- (hrt_absolute_time() > (commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) &&
- !home->manual_home) {
+ (hrt_absolute_time() > (commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) && !_home_pub.get().manual_home) {
- set_home_position(*home_pub, *home, false);
+ set_home_position();
}
}
}
@@ -800,7 +793,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
if (use_current) {
/* use current position */
- if (set_home_position(*home_pub, *home, false)) {
+ if (set_home_position()) {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
@@ -817,32 +810,25 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
if (local_pos.xy_global && local_pos.z_global) {
/* use specified position */
- home->timestamp = hrt_absolute_time();
+ home_position_s home{};
+ home.timestamp = hrt_absolute_time();
- home->lat = lat;
- home->lon = lon;
- home->alt = alt;
+ home.lat = lat;
+ home.lon = lon;
+ home.alt = alt;
- home->manual_home = true;
- home->valid_alt = true;
- home->valid_hpos = true;
+ home.manual_home = true;
+ home.valid_alt = true;
+ home.valid_hpos = true;
// update local projection reference including altitude
struct map_projection_reference_s ref_pos;
map_projection_init(&ref_pos, local_pos.ref_lat, local_pos.ref_lon);
- map_projection_project(&ref_pos, lat, lon, &home->x, &home->y);
- home->z = -(alt - local_pos.ref_alt);
-
- /* announce new home position */
- if (*home_pub != nullptr) {
- orb_publish(ORB_ID(home_position), *home_pub, home);
-
- } else {
- *home_pub = orb_advertise(ORB_ID(home_position), home);
- }
+ map_projection_project(&ref_pos, lat, lon, &home.x, &home.y);
+ home.z = -(alt - local_pos.ref_alt);
/* mark home position as set */
- status_flags.condition_home_position_valid = true;
+ status_flags.condition_home_position_valid = _home_pub.update(home);
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
@@ -1041,65 +1027,69 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
* time the vehicle is armed with a good GPS fix.
**/
bool
-Commander::set_home_position(orb_advert_t &homePub, home_position_s &home, bool set_alt_only_to_lpos_ref)
+Commander::set_home_position()
{
- const vehicle_local_position_s &localPosition = _local_position_sub.get();
- const vehicle_global_position_s &globalPosition = _global_position_sub.get();
+ // Need global and local position fix to be able to set home
+ if (status_flags.condition_global_position_valid && status_flags.condition_local_position_valid) {
- if (!set_alt_only_to_lpos_ref) {
- //Need global and local position fix to be able to set home
- if (!status_flags.condition_global_position_valid || !status_flags.condition_local_position_valid) {
- return false;
- }
+ const vehicle_global_position_s &gpos = _global_position_sub.get();
- //Ensure that the GPS accuracy is good enough for intializing home
- if (globalPosition.eph > _home_eph_threshold.get() || globalPosition.epv > _home_epv_threshold.get()) {
- return false;
- }
+ // Ensure that the GPS accuracy is good enough for intializing home
+ if ((gpos.eph <= _home_eph_threshold.get()) && (gpos.epv <= _home_epv_threshold.get())) {
- // Set home position
- home.lat = globalPosition.lat;
- home.lon = globalPosition.lon;
- home.valid_hpos = true;
+ const vehicle_local_position_s &lpos = _local_position_sub.get();
- home.alt = globalPosition.alt;
- home.valid_alt = true;
+ // Set home position
+ home_position_s &home = _home_pub.get();
- home.x = localPosition.x;
- home.y = localPosition.y;
- home.z = localPosition.z;
+ home.lat = gpos.lat;
+ home.lon = gpos.lon;
+ home.valid_hpos = true;
- home.yaw = localPosition.yaw;
+ home.alt = gpos.alt;
+ home.valid_alt = true;
- //Play tune first time we initialize HOME
- if (!status_flags.condition_home_position_valid) {
- tune_home_set(true);
- }
+ home.x = lpos.x;
+ home.y = lpos.y;
+ home.z = lpos.z;
- /* mark home position as set */
- status_flags.condition_home_position_valid = true;
+ home.yaw = lpos.yaw;
- } else if (!home.valid_alt && localPosition.z_global) {
- // handle special case where we are setting only altitude using local position reference
- home.alt = localPosition.ref_alt;
- home.valid_alt = true;
+ //Play tune first time we initialize HOME
+ if (!status_flags.condition_home_position_valid) {
+ tune_home_set(true);
+ }
- } else {
- return false;
+ /* mark home position as set */
+ status_flags.condition_home_position_valid = _home_pub.update();
+
+ home.timestamp = hrt_absolute_time();
+ home.manual_home = false;
+
+ return status_flags.condition_home_position_valid;
+ }
}
- home.timestamp = hrt_absolute_time();
- home.manual_home = false;
+ return false;
+}
- /* announce new home position */
- if (homePub != nullptr) {
- orb_publish(ORB_ID(home_position), homePub, &home);
+bool
+Commander::set_home_position_alt_only()
+{
+ const vehicle_local_position_s &lpos = _local_position_sub.get();
- } else {
- homePub = orb_advertise(ORB_ID(home_position), &home);
+ if (!_home_pub.get().valid_alt && lpos.z_global) {
+ // handle special case where we are setting only altitude using local position reference
+ home_position_s home{};
+ home.alt = lpos.ref_alt;
+ home.valid_alt = true;
+
+ home.timestamp = hrt_absolute_time();
+
+ return _home_pub.update(home);
}
- return true;
+ return false;
}
void
@@ -1180,50 +1170,28 @@ Commander::run()
}
// We want to accept RC inputs as default
- status_flags.rc_input_blocked = false;
status.rc_input_mode = vehicle_status_s::RC_IN_MODE_DEFAULT;
internal_state.main_state = commander_state_s::MAIN_STATE_MANUAL;
internal_state.timestamp = hrt_absolute_time();
status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
status.arming_state = vehicle_status_s::ARMING_STATE_INIT;
- status.failsafe = false;
-
- /* neither manual nor offboard control commands have been received */
- status_flags.offboard_control_signal_found_once = false;
- status_flags.rc_signal_found_once = false;
-
/* mark all signals lost as long as they haven't been found */
status.rc_signal_lost = true;
status_flags.offboard_control_signal_lost = true;
status.data_link_lost = true;
- status_flags.offboard_control_loss_timeout = false;
-
- status_flags.condition_system_hotplug_timeout = false;
status.timestamp = hrt_absolute_time();
status_flags.condition_power_input_valid = true;
- status_flags.usb_connected = false;
status_flags.rc_calibration_valid = true;
- // CIRCUIT BREAKERS
- status_flags.circuit_breaker_engaged_power_check = false;
- status_flags.circuit_breaker_engaged_airspd_check = false;
- status_flags.circuit_breaker_engaged_enginefailure_check = false;
- status_flags.circuit_breaker_engaged_gpsfailure_check = false;
get_circuit_breaker_params();
- /* Set position and velocity validty to false */
- status_flags.condition_global_position_valid = false;
- status_flags.condition_local_position_valid = false;
- status_flags.condition_local_velocity_valid = false;
- status_flags.condition_local_altitude_valid = false;
-
/* publish initial state */
- status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
+ _status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
- if (status_pub == nullptr) {
+ if (_status_pub == nullptr) {
warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n");
warnx("exiting.");
px4_task_exit(PX4_ERROR);
@@ -1239,10 +1207,6 @@ Commander::run()
memset(&control_mode, 0, sizeof(control_mode));
orb_advert_t control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode);
- /* home position */
- orb_advert_t home_pub = nullptr;
- memset(&_home, 0, sizeof(_home));
-
/* command ack */
orb_advert_t command_ack_pub = nullptr;
orb_advert_t commander_state_pub = nullptr;
@@ -1379,7 +1343,6 @@ Commander::run()
uint64_t timestamp_engine_healthy = 0; /**< absolute time when engine was healty */
/* check which state machines for changes, clear "changed" flag */
- bool main_state_changed = false;
bool failsafe_old = false;
bool have_taken_off_since_arming = false;
@@ -2080,7 +2043,7 @@ Commander::run()
/* play tune on mode change only if armed, blink LED always */
if (main_res == TRANSITION_CHANGED || first_rc_eval) {
tune_positive(armed.armed);
- main_state_changed = true;
+ status_changed = true;
} else if (main_res == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */
@@ -2207,7 +2170,7 @@ Commander::run()
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
/* handle it */
- if (handle_command(&status, cmd, &armed, &_home, &home_pub, &command_ack_pub, &status_changed)) {
+ if (handle_command(&status, cmd, &armed, &command_ack_pub, &status_changed)) {
status_changed = true;
}
}
@@ -2296,7 +2259,7 @@ Commander::run()
const hrt_abstime now = hrt_absolute_time();
// automatically set or update home position
- if (!_home.manual_home) {
+ if (!_home_pub.get().manual_home) {
const vehicle_local_position_s &local_position = _local_position_sub.get();
if (armed.armed) {
@@ -2304,7 +2267,7 @@ Commander::run()
(hrt_elapsed_time(&commander_boot_timestamp) > INAIR_RESTART_HOLDOFF_INTERVAL)) {
/* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */
- set_home_position(home_pub, _home, false);
+ set_home_position();
}
} else {
@@ -2313,29 +2276,28 @@ Commander::run()
/* distance from home */
float home_dist_xy = -1.0f;
float home_dist_z = -1.0f;
- mavlink_wpm_distance_to_point_local(_home.x, _home.y, _home.z,
+ mavlink_wpm_distance_to_point_local(_home_pub.get().x, _home_pub.get().y, _home_pub.get().z,
local_position.x, local_position.y, local_position.z,
&home_dist_xy, &home_dist_z);
- if ((home_dist_xy > local_position.eph * 2) || (home_dist_z > local_position.epv * 2)) {
+ if ((home_dist_xy > local_position.eph * 2.0f) || (home_dist_z > local_position.epv * 2.0f)) {
/* update when disarmed, landed and moved away from current home position */
- set_home_position(home_pub, _home, false);
+ set_home_position();
}
}
} else {
/* First time home position update - but only if disarmed */
- set_home_position(home_pub, _home, false);
- }
- }
-
- /* Set home position altitude to EKF origin height if home is not set and the EKF has a global origin.
- * This allows home altitude to be used in the calculation of height above takeoff location when GPS
- * use has commenced after takeoff. */
- if (!_home.valid_alt && local_position.z_global) {
- set_home_position(home_pub, _home, true);
+ set_home_position();
+ /* Set home position altitude to EKF origin height if home is not set and the EKF has a global origin.
+ * This allows home altitude to be used in the calculation of height above takeoff location when GPS
+ * use has commenced after takeoff. */
+ if (!status_flags.condition_home_position_valid) {
+ set_home_position_alt_only();
+ }
+ }
}
}
@@ -2381,21 +2343,15 @@ Commander::run()
failsafe_old = status.failsafe;
}
- // TODO handle mode changes by commands
- if (main_state_changed || nav_state_changed) {
- status_changed = true;
- main_state_changed = false;
- }
-
/* publish states (armed, control_mode, vehicle_status, commander_state, vehicle_status_flags) at 1 Hz or immediately when changed */
- if (hrt_elapsed_time(&status.timestamp) >= 1_s || status_changed) {
+ if (hrt_elapsed_time(&status.timestamp) >= 1_s || status_changed || nav_state_changed) {
set_control_mode();
control_mode.timestamp = now;
orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
status.timestamp = now;
- orb_publish(ORB_ID(vehicle_status), status_pub, &status);
+ orb_publish(ORB_ID(vehicle_status), _status_pub, &status);
armed.timestamp = now;
@@ -2523,13 +2479,13 @@ Commander::run()
/* close fds */
led_deinit();
buzzer_deinit();
- px4_close(sp_man_sub);
- px4_close(offboard_control_mode_sub);
- px4_close(safety_sub);
- px4_close(cmd_sub);
- px4_close(subsys_sub);
- px4_close(param_changed_sub);
- px4_close(land_detector_sub);
+ orb_unsubscribe(sp_man_sub);
+ orb_unsubscribe(offboard_control_mode_sub);
+ orb_unsubscribe(safety_sub);
+ orb_unsubscribe(cmd_sub);
+ orb_unsubscribe(subsys_sub);
+ orb_unsubscribe(param_changed_sub);
+ orb_unsubscribe(land_detector_sub);
thread_running = false;
}
diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp
index f51ab5a959..1cb4d171af 100644
--- a/src/modules/commander/Commander.hpp
+++ b/src/modules/commander/Commander.hpp
@@ -37,16 +37,16 @@
#include "state_machine_helper.h"
#include "failure_detector/FailureDetector.hpp"
-#include <controllib/blocks.hpp>
+#include <lib/controllib/blocks.hpp>
+#include <lib/mathlib/mathlib.h>
#include <px4_module.h>
#include <px4_module_params.h>
-#include <mathlib/mathlib.h>
+#include <systemlib/hysteresis/hysteresis.h>
// publications
#include <uORB/Publication.hpp>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/home_position.h>
-#include <uORB/topics/iridiumsbd_status.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
@@ -56,6 +56,7 @@
#include <uORB/Subscription.hpp>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/geofence_result.h>
+#include <uORB/topics/iridiumsbd_status.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/vehicle_command.h>
@@ -72,6 +73,7 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
{
public:
Commander();
+ ~Commander();
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
@@ -132,10 +134,11 @@ private:
FailureDetector _failure_detector;
bool _failure_detector_termination_printed{false};
- bool handle_command(vehicle_status_s *status, const vehicle_command_s &cmd,
- actuator_armed_s *armed, home_position_s *home, orb_advert_t *home_pub, orb_advert_t *command_ack_pub, bool *changed);
+ bool handle_command(vehicle_status_s *status, const vehicle_command_s &cmd, actuator_armed_s *armed,
+ orb_advert_t *command_ack_pub, bool *changed);
- bool set_home_position(orb_advert_t &homePub, home_position_s &home, bool set_alt_only_to_lpos_ref);
+ bool set_home_position();
+ bool set_home_position_alt_only();
// Set the main system state based on RC and override device inputs
transition_result_t set_main_state(const vehicle_status_s &status, bool *changed);
@@ -146,7 +149,8 @@ private:
// Set the system main state based on the current RC inputs
transition_result_t set_main_state_rc(const vehicle_status_s &status, bool *changed);
- void check_valid(const hrt_abstime ×tamp, const hrt_abstime &timeout, const bool valid_in, bool *valid_out, bool *changed);
+ void check_valid(const hrt_abstime ×tamp, const hrt_abstime &timeout, const bool valid_in, bool *valid_out,
+ bool *changed);
bool check_posvel_validity(const bool data_valid, const float data_accuracy, const float required_accuracy,
const hrt_abstime &data_timestamp_us, hrt_abstime *last_fail_time_us, hrt_abstime *probation_time_us, bool *valid_state,
@@ -192,6 +196,10 @@ private:
Subscription<mission_result_s> _mission_result_sub{ORB_ID(mission_result)};
Subscription<vehicle_global_position_s> _global_position_sub{ORB_ID(vehicle_global_position)};
Subscription<vehicle_local_position_s> _local_position_sub{ORB_ID(vehicle_local_position)};
+
+ Publication<home_position_s> _home_pub{ORB_ID(home_position)};
+
+ orb_advert_t _status_pub{nullptr};
};
#endif /* COMMANDER_HPP_ */
diff --git a/src/modules/uORB/Publication.hpp b/src/modules/uORB/Publication.hpp
index 9aab16dda3..c9547c7b03 100644
--- a/src/modules/uORB/Publication.hpp
+++ b/src/modules/uORB/Publication.hpp
@@ -158,6 +158,12 @@ public:
return PublicationBase::update((void *)(&_data));
}
+ bool update(const T &data)
+ {
+ _data = data;
+ return update();
+ }
+
private:
T _data;
};
--
GitLab