diff --git a/msg/actuator_armed.msg b/msg/actuator_armed.msg index 98aa90e7e3..4543e7b8c9 100644 --- a/msg/actuator_armed.msg +++ b/msg/actuator_armed.msg @@ -1,6 +1,5 @@ uint64 timestamp # time since system start (microseconds) -uint32 armed_time_ms # Arming timestamp bool armed # Set to true if system is armed bool prearmed # Set to true if the actuator safety is disabled but motors are not armed bool ready_to_arm # Set to true if system is ready to be armed diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 85b45cf43e..a968bebf0a 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -114,3 +114,7 @@ uint8 ARM_DISARM_REASON_UNIT_TEST = 13 uint8 latest_arming_reason uint8 latest_disarming_reason + +uint64 armed_time # Arming timestamp (microseconds) + +uint64 takeoff_time # Takeoff timestamp (microseconds) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 09572856c7..58d64c63d8 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1718,10 +1718,11 @@ Commander::run() if (_armed.armed) { if (!was_landed && _land_detector.landed) { mavlink_log_info(&_mavlink_log_pub, "Landing detected"); + _status.takeoff_time = 0; } else if (was_landed && !_land_detector.landed) { mavlink_log_info(&_mavlink_log_pub, "Takeoff detected"); - _time_at_takeoff = hrt_absolute_time(); + _status.takeoff_time = hrt_absolute_time(); _have_taken_off_since_arming = true; // Set all position and velocity test probation durations to takeoff value @@ -2453,10 +2454,8 @@ Commander::run() if (_armed.armed) { if (_status.failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) { - const hrt_abstime time_at_arm = _armed.armed_time_ms * 1000; - // 500ms is the PWM spoolup time. Within this timeframe controllers are not affecting actuator_outputs - if (hrt_elapsed_time(&time_at_arm) < 500_ms) { + if (hrt_elapsed_time(&_status.armed_time) < 500_ms) { arm_disarm(false, true, arm_disarm_reason_t::FAILURE_DETECTOR); mavlink_log_critical(&_mavlink_log_pub, "ESCs did not respond to arm request"); } @@ -2464,7 +2463,7 @@ Commander::run() if (_status.failure_detector_status & (vehicle_status_s::FAILURE_ROLL | vehicle_status_s::FAILURE_PITCH | vehicle_status_s::FAILURE_ALT | vehicle_status_s::FAILURE_EXT)) { - const bool is_second_after_takeoff = hrt_elapsed_time(&_time_at_takeoff) < (1_s * _param_com_lkdown_tko.get()); + const bool is_second_after_takeoff = hrt_elapsed_time(&_status.takeoff_time) < (1_s * _param_com_lkdown_tko.get()); if (is_second_after_takeoff && !_lockdown_triggered) { // This handles the case where something fails during the early takeoff phase @@ -3994,7 +3993,7 @@ void Commander::estimator_check() } else { // if nav status is unconfirmed, confirm yaw angle as passed after 30 seconds or achieving 5 m/s of speed - const bool sufficient_time = (hrt_elapsed_time(&_time_at_takeoff) > 30_s); + const bool sufficient_time = (_status.takeoff_time != 0) && (hrt_elapsed_time(&_status.takeoff_time) > 30_s); const bool sufficient_speed = (lpos.vx * lpos.vx + lpos.vy * lpos.vy > 25.0f); bool innovation_pass = estimator_status.vel_test_ratio < 1.0f && estimator_status.pos_test_ratio < 1.0f; diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index cd02f0f18b..a36cf5cfd9 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -311,7 +311,6 @@ private: hrt_abstime _lvel_probation_time_us = POSVEL_PROBATION_MIN; /* class variables used to check for navigation failure after takeoff */ - hrt_abstime _time_at_takeoff{0}; /**< last time we were on the ground */ hrt_abstime _time_last_innov_pass{0}; /**< last time velocity or position innovations passed */ bool _nav_test_passed{false}; /**< true if the post takeoff navigation test has passed */ bool _nav_test_failed{false}; /**< true if the post takeoff navigation test has failed */ diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 6d440b4303..8be08a1375 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -231,10 +231,10 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const safe } if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { - armed->armed_time_ms = hrt_absolute_time() / 1000; + status->armed_time = hrt_absolute_time(); } else { - armed->armed_time_ms = 0; + status->armed_time = 0; } } } diff --git a/src/modules/mavlink/streams/FLIGHT_INFORMATION.hpp b/src/modules/mavlink/streams/FLIGHT_INFORMATION.hpp index ca5a00cc78..5dd9f88c88 100644 --- a/src/modules/mavlink/streams/FLIGHT_INFORMATION.hpp +++ b/src/modules/mavlink/streams/FLIGHT_INFORMATION.hpp @@ -34,7 +34,7 @@ #ifndef FLIGHT_INFORMATION_HPP #define FLIGHT_INFORMATION_HPP -#include +#include class MavlinkStreamFlightInformation : public MavlinkStream { @@ -58,26 +58,31 @@ private: _param_com_flight_uuid = param_find("COM_FLIGHT_UUID"); } - uORB::Subscription _armed_sub{ORB_ID(actuator_armed)}; - param_t _param_com_flight_uuid; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + param_t _param_com_flight_uuid{PARAM_INVALID}; bool send() override { - actuator_armed_s actuator_armed{}; - bool ret = _armed_sub.copy(&actuator_armed); - - if (ret && actuator_armed.timestamp != 0) { - int32_t flight_uuid; - param_get(_param_com_flight_uuid, &flight_uuid); + vehicle_status_s vehicle_status{}; + if (_vehicle_status_sub.copy(&vehicle_status) && vehicle_status.timestamp != 0) { mavlink_flight_information_t flight_info{}; - flight_info.flight_uuid = static_cast(flight_uuid); - flight_info.arming_time_utc = flight_info.takeoff_time_utc = actuator_armed.armed_time_ms; flight_info.time_boot_ms = hrt_absolute_time() / 1000; + flight_info.arming_time_utc = vehicle_status.armed_time; + flight_info.takeoff_time_utc = vehicle_status.takeoff_time; + + int32_t flight_uuid; + + if (param_get(_param_com_flight_uuid, &flight_uuid) == PX4_OK) { + flight_info.flight_uuid = static_cast(flight_uuid); + } + mavlink_msg_flight_information_send_struct(_mavlink->get_channel(), &flight_info); + + return true; } - return ret; + return false; } };