dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

check avoidance status in commander and set sys status 

remove mavlink log
This commit is contained in:
Martina Rivizzigno 2020-02-18 17:35:12 +01:00 committed by Julian Kent
parent 48b7cd926e
commit aee1e70642
4 changed files with 55 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/subsystem_info.msg
View File

@ -28,6 +28,7 @@ uint64 SUBSYSTEM_TYPE_LOGGING = 16777216
uint64 SUBSYSTEM_TYPE_SENSORBATTERY = 33554432
uint64 SUBSYSTEM_TYPE_SENSORPROXIMITY = 67108864
uint64 SUBSYSTEM_TYPE_SATCOM = 134217728
uint64 SUBSYSTEM_TYPE_OBSTACLE_AVOIDANCE = 536870912
bool present
bool enabled

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

@ -1976,6 +1976,8 @@ Commander::run()
// data link checks which update the status
data_link_check();
avoidance_check();
// engine failure detection
// TODO: move out of commander
if (_actuator_controls_sub.updated()) {
@ -3614,7 +3616,6 @@ void Commander::data_link_check()
_datalink_last_status_avoidance_system = telemetry.remote_system_status;
if (_avoidance_system_lost) {
mavlink_log_info(&mavlink_log_pub, "Avoidance system regained");
_status_changed = true;
_avoidance_system_lost = false;
status_flags.avoidance_system_valid = true;
@ -3654,42 +3655,29 @@ void Commander::data_link_check()
// AVOIDANCE SYSTEM state check (only if it is enabled)
if (status_flags.avoidance_system_required && !_onboard_controller_lost) {
//if avoidance never started
if (_datalink_last_heartbeat_avoidance_system == 0
&& hrt_elapsed_time(&_datalink_last_heartbeat_avoidance_system) > _param_com_oa_boot_t.get() * 1_s) {
if (!_print_avoidance_msg_once) {
mavlink_log_critical(&mavlink_log_pub, "Avoidance system not available");
_print_avoidance_msg_once = true;
}
}
//if heartbeats stop
if (!_avoidance_system_lost && (_datalink_last_heartbeat_avoidance_system > 0)
&& (hrt_elapsed_time(&_datalink_last_heartbeat_avoidance_system) > 5_s)) {
_avoidance_system_lost = true;
mavlink_log_critical(&mavlink_log_pub, "Avoidance system lost");
status_flags.avoidance_system_valid = false;
_print_avoidance_msg_once = false;
}
//if status changed
if (_avoidance_system_status_change) {
if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_BOOT) {
mavlink_log_info(&mavlink_log_pub, "Avoidance system starting");
status_flags.avoidance_system_valid = false;
}
if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_ACTIVE) {
mavlink_log_info(&mavlink_log_pub, "Avoidance system connected");
status_flags.avoidance_system_valid = true;
}
if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_CRITICAL) {
mavlink_log_info(&mavlink_log_pub, "Avoidance system timed out");
status_flags.avoidance_system_valid = false;
_status_changed = true;
}
if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_FLIGHT_TERMINATION) {
mavlink_log_critical(&mavlink_log_pub, "Avoidance system rejected");
status_flags.avoidance_system_valid = false;
_status_changed = true;
}
@ -3712,6 +3700,46 @@ void Commander::data_link_check()
}
}
void Commander::avoidance_check()
{
for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
if (_sub_distance_sensor[i].updated()) {
distance_sensor_s distance_sensor {};
_sub_distance_sensor[i].copy(&distance_sensor);
if ((distance_sensor.orientation != distance_sensor_s::ROTATION_DOWNWARD_FACING) &&
(distance_sensor.orientation != distance_sensor_s::ROTATION_UPWARD_FACING)) {
_valid_distance_sensor_time_us = distance_sensor.timestamp;
}
}
}
const bool cp_enabled = _param_cp_dist.get() > 0.f;
const bool distance_sensor_valid = hrt_elapsed_time(&_valid_distance_sensor_time_us) < 500_ms;
const bool cp_healthy = status_flags.avoidance_system_valid || distance_sensor_valid;
const bool sensor_oa_present = cp_healthy || status_flags.avoidance_system_required || cp_enabled;
const bool auto_mode = _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_MISSION
|| _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER
|| _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL
|| _internal_state.main_state == commander_state_s::MAIN_STATE_OFFBOARD
|| _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF
|| _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND;
const bool pos_ctl_mode = _internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL;
const bool sensor_oa_enabled = ((auto_mode && status_flags.avoidance_system_required) || (pos_ctl_mode
&& cp_enabled)) ? true : false;
const bool sensor_oa_healthy = ((auto_mode && status_flags.avoidance_system_valid) || (pos_ctl_mode
&& cp_healthy)) ? true : false;
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_OBSTACLE_AVOIDANCE, sensor_oa_present, sensor_oa_enabled,
sensor_oa_healthy, status);
}
void Commander::battery_status_check()
{
bool battery_sub_updated = false;

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

@ -61,6 +61,7 @@
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/cpuload.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/esc_status.h>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/geofence_result.h>
@ -132,6 +133,8 @@ private:
*/
void data_link_check();
void avoidance_check();
void esc_status_check(const esc_status_s &esc_status);
void estimator_check();
@ -196,7 +199,6 @@ private:
(ParamFloat<px4::params::COM_DISARM_PRFLT>) _param_com_disarm_preflight,
(ParamBool<px4::params::COM_OBS_AVOID>) _param_com_obs_avoid,
(ParamInt<px4::params::COM_OA_BOOT_T>) _param_com_oa_boot_t,
(ParamInt<px4::params::COM_FLT_PROFILE>) _param_com_flt_profile,
@ -251,7 +253,9 @@ private:
// Mavlink
(ParamInt<px4::params::MAV_COMP_ID>) _param_mav_comp_id,
(ParamInt<px4::params::MAV_SYS_ID>) _param_mav_sys_id,
(ParamInt<px4::params::MAV_TYPE>) _param_mav_type
(ParamInt<px4::params::MAV_TYPE>) _param_mav_type,
(ParamFloat<px4::params::CP_DIST>) _param_cp_dist
)
enum class PrearmedMode {
@ -282,6 +286,8 @@ private:
PreFlightCheck::arm_requirements_t _arm_requirements{};
hrt_abstime _valid_distance_sensor_time_us{0}; /**< Last time that distance sensor data arrived (usec) */
hrt_abstime _last_gpos_fail_time_us{0}; /**< Last time that the global position validity recovery check failed (usec) */
hrt_abstime _last_lpos_fail_time_us{0}; /**< Last time that the local position validity recovery check failed (usec) */
hrt_abstime _last_lvel_fail_time_us{0}; /**< Last time that the local velocity validity recovery check failed (usec) */
@ -325,8 +331,6 @@ private:
Hysteresis _auto_disarm_landed{false};
Hysteresis _auto_disarm_killed{false};
bool _print_avoidance_msg_once{false};
hrt_abstime _last_print_mode_reject_time{0}; ///< To remember when last notification was sent
float _eph_threshold_adj{INFINITY}; ///< maximum allowable horizontal position uncertainty after adjustment for flight condition
@ -388,6 +392,7 @@ private:
#endif
uORB::Subscription _cmd_sub {ORB_ID(vehicle_command)};
uORB::Subscription _cpuload_sub{ORB_ID(cpuload)};
uORB::Subscription _sub_distance_sensor[ORB_MULTI_MAX_INSTANCES] {{ORB_ID(distance_sensor), 0}, {ORB_ID(distance_sensor), 1}, {ORB_ID(distance_sensor), 2}, {ORB_ID(distance_sensor), 3}}; /**< distance data received from onboard rangefinders */
uORB::Subscription _esc_status_sub{ORB_ID(esc_status)};
uORB::Subscription _geofence_result_sub{ORB_ID(geofence_result)};
uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};

13
src/modules/commander/commander_params.c
View File

@ -844,19 +844,6 @@ PARAM_DEFINE_INT32(NAV_RCL_ACT, 2);
*/
PARAM_DEFINE_INT32(COM_OBS_AVOID, 0);
/**
* Set avoidance system bootup timeout.
*
* The avoidance system running on the companion computer is expected to boot
* within this time and start providing trajectory points.
* If no avoidance system is detected a MAVLink warning message is sent.
* @group Commander
* @unit s
* @min 0
* @max 200
*/
PARAM_DEFINE_INT32(COM_OA_BOOT_T, 100);
/**
* User Flight Profile
*