From d4f713b2861efa9dce79cb814396370904d9a2ac Mon Sep 17 00:00:00 2001 From: Daniel Agar 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 +#include +#include #include #include -#include +#include // publications #include #include #include -#include #include #include #include @@ -56,6 +56,7 @@ #include #include #include +#include #include #include #include @@ -72,6 +73,7 @@ class Commander : public ModuleBase, 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_sub{ORB_ID(mission_result)}; Subscription _global_position_sub{ORB_ID(vehicle_global_position)}; Subscription _local_position_sub{ORB_ID(vehicle_local_position)}; + + Publication _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; };