Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Obstacle Avoidance prearm checks and log health status #11638

Merged
merged 10 commits into from
Mar 27, 2019
Merged

Obstacle Avoidance prearm checks and log health status #11638

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
9ad80bb
Added obstacle avoidance healthiness topic in vehicle status msg.
cmic0 Feb 13, 2019
153345d
Added status_change to force message updating.
cmic0 Feb 14, 2019
5bf776e
Commander: Improved logic for OA prearm checks.
cmic0 Mar 12, 2019
62ba7ce
Commander: increased hardcoded timeout time for avoidance to start.
cmic0 Mar 13, 2019
912d902
Commander: Added COM_ONB_BOOT_T parameter.
cmic0 Mar 13, 2019
e004f77
Commander: changed logic for checking OA at boot time.
cmic0 Mar 14, 2019
4b85732
Commander: reset print once variable if OA is lost.
cmic0 Mar 14, 2019
4a15a86
Commander: Changed COM_ONB_BOOT_T parameter to COM_OA_BOOT_T.
cmic0 Mar 19, 2019
99bd6ef
Fixed CI errors. changed _print_msg_once into private class member.
cmic0 Mar 22, 2019
384cfc3
Commander: renamed print_msg_once variable into a more self-explanato…
cmic0 Mar 25, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions msg/vehicle_status_flags.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,3 +33,6 @@ bool rc_input_blocked # set if RC input should be
bool rc_calibration_valid # set if RC calibration is valid
bool vtol_transition_failure # Set to true if vtol transition failed
bool usb_connected # status of the USB power supply

bool avoidance_system_required # Set to true if avoidance system is enabled via COM_OBS_AVOID parameter
bool avoidance_system_valid # Status of the obstacle avoidance system
29 changes: 24 additions & 5 deletions src/modules/commander/Commander.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -580,6 +580,10 @@ Commander::Commander() :

status_flags.condition_power_input_valid = true;
status_flags.rc_calibration_valid = true;

status_flags.avoidance_system_valid = false;


}

Commander::~Commander()
Expand Down Expand Up @@ -1210,6 +1214,8 @@ Commander::run()
/* failsafe response to loss of navigation accuracy */
param_t _param_posctl_nav_loss_act = param_find("COM_POSCTL_NAVL");

status_flags.avoidance_system_required = _obs_avoid.get();

/* pthread for slow low prio thread */
pthread_t commander_low_prio_thread;

Expand Down Expand Up @@ -1478,6 +1484,9 @@ Commander::run()
param_init_forced = false;
}

/* Update OA parameter */
status_flags.avoidance_system_required = _obs_avoid.get();

/* handle power button state */
orb_check(power_button_state_sub, &updated);

Expand Down Expand Up @@ -3918,6 +3927,7 @@ void Commander::data_link_check(bool &status_changed)
mavlink_log_info(&mavlink_log_pub, "Avoidance system regained");
status_changed = true;
_avoidance_system_lost = false;
status_flags.avoidance_system_valid = true;
}
}

Expand Down Expand Up @@ -3952,19 +3962,25 @@ void Commander::data_link_check(bool &status_changed)
}

// AVOIDANCE SYSTEM state check (only if it is enabled)
if (_obs_avoid.get() && !_onboard_controller_lost) {
if (status_flags.avoidance_system_required && !_onboard_controller_lost) {

//if avoidance never started
if (_datalink_last_heartbeat_avoidance_system == 0 && hrt_elapsed_time(&_avoidance_system_not_started) > 5_s) {
_avoidance_system_not_started = hrt_absolute_time();
mavlink_log_info(&mavlink_log_pub, "Waiting for avoidance system to start");
if (_datalink_last_heartbeat_avoidance_system == 0
&& hrt_elapsed_time(&_datalink_last_heartbeat_avoidance_system) > _oa_boot_timeout.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
Expand All @@ -3974,7 +3990,8 @@ void Commander::data_link_check(bool &status_changed)
}

if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_ACTIVE) {
mavlink_log_info(&mavlink_log_pub, "Avoidance system healthy");
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) {
Expand All @@ -3983,6 +4000,8 @@ void Commander::data_link_check(bool &status_changed)

if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_FLIGHT_TERMINATION) {
mavlink_log_critical(&mavlink_log_pub, "Avoidance system abort");
status_flags.avoidance_system_valid = false;
status_changed = true;
}

_avoidance_system_status_change = false;
Expand Down
7 changes: 5 additions & 2 deletions src/modules/commander/Commander.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -117,7 +117,9 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
(ParamInt<px4::params::COM_LOW_BAT_ACT>) _low_bat_action,
(ParamFloat<px4::params::COM_DISARM_LAND>) _disarm_when_landed_timeout,

(ParamInt<px4::params::COM_OBS_AVOID>) _obs_avoid
(ParamInt<px4::params::COM_OBS_AVOID>) _obs_avoid,
(ParamInt<px4::params::COM_OA_BOOT_T>) _oa_boot_timeout

)

const int64_t POSVEL_PROBATION_MIN = 1_s; /**< minimum probation duration (usec) */
Expand Down Expand Up @@ -185,7 +187,6 @@ class Commander : public ModuleBase<Commander>, public ModuleParams

hrt_abstime _datalink_last_heartbeat_avoidance_system{0};
bool _avoidance_system_lost{false};
hrt_abstime _avoidance_system_not_started{0};

bool _avoidance_system_status_change{false};
uint8_t _datalink_last_status_avoidance_system{telemetry_status_s::MAV_STATE_UNINIT};
Expand All @@ -202,6 +203,8 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
systemlib::Hysteresis _auto_disarm_landed{false};
systemlib::Hysteresis _auto_disarm_killed{false};

bool _print_avoidance_msg_once{false};

// Subscriptions
Subscription<estimator_status_s> _estimator_status_sub{ORB_ID(estimator_status)};
Subscription<mission_result_s> _mission_result_sub{ORB_ID(mission_result)};
Expand Down
18 changes: 15 additions & 3 deletions src/modules/commander/commander_params.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -798,10 +798,22 @@ PARAM_DEFINE_INT32(NAV_DLL_ACT, 0);
PARAM_DEFINE_INT32(NAV_RCL_ACT, 2);

/**
* Flag to enable obstacle avoidance
* Temporary Parameter to enable interface testing
* Flag to enable obstacle avoidance.
*
* @boolean
* @group Mission
*/
PARAM_DEFINE_INT32(COM_OBS_AVOID, 0);
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.
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is this true? Is it about trajectory points being sent? Or is it about a heartbeat?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes, it is true. In the actual implementation, as soon as the avoidance node is running sends both heartbeats and trajectory points. On the long term, this behavior will probably be changed.

If we want to maintain a more general statement we could go for:
The avoidance system running on the companion computer is expected to boot within this time, after which its functionalities are fully operative.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

No if it's true it's fine.

* 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);
11 changes: 11 additions & 0 deletions src/modules/commander/state_machine_helper.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,7 @@
#include <systemlib/mavlink_log.h>
#include <drivers/drv_hrt.h>


#include "state_machine_helper.h"
#include "commander_helper.h"
#include "PreflightCheck.h"
Expand Down Expand Up @@ -915,6 +916,7 @@ bool prearm_check(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &s
bool reportFailures = true;
bool prearm_ok = true;


// USB not connected
if (!status_flags.circuit_breaker_engaged_usb_check && status_flags.usb_connected) {
if (reportFailures) {
Expand Down Expand Up @@ -997,6 +999,15 @@ bool prearm_check(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &s
}
}

if (status_flags.avoidance_system_required && !status_flags.avoidance_system_valid) {
if (prearm_ok && reportFailures) {
mavlink_log_critical(mavlink_log_pub, "ARMING DENIED: Avoidance system not ready");
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Right so this makes sense.

}

prearm_ok = false;

}

return prearm_ok;
}

Expand Down