fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Mavlink FLIGHT_INFORMATION fix arming time (ms -> us) and add takeoff time

Browse Source 
- fixes https://github.com/PX4/PX4-Autopilot/issues/16393
This commit is contained in:
Daniel Agar 2020-12-15 18:40:07 -05:00
parent b23d8244f0
commit 336176b2f0
6 changed files with 28 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/actuator_armed.msg
View File

@ -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

4
msg/vehicle_status.msg
View File

@ -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)

11
src/modules/commander/Commander.cpp
View File

@ -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;

1
src/modules/commander/Commander.hpp
View File

@ -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 */

4
src/modules/commander/state_machine_helper.cpp
View File

@ -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;
}
}
}

29
src/modules/mavlink/streams/FLIGHT_INFORMATION.hpp
View File

@ -34,7 +34,7 @@
#ifndef FLIGHT_INFORMATION_HPP
#define FLIGHT_INFORMATION_HPP
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/vehicle_status.h>
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<uint64_t>(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<uint64_t>(flight_uuid);
}
mavlink_msg_flight_information_send_struct(_mavlink->get_channel(), &flight_info);
return true;
}
return ret;
return false;
}
};