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 7 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
30 changes: 25 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 @@ -3825,6 +3834,7 @@ bool Commander::preflight_check(bool report)
void Commander::data_link_check(bool &status_changed)
{
bool updated = false;
static bool print_msg_once = false;
Copy link
Member

Choose a reason for hiding this comment

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

Please don't use static, but a class member instead.

Copy link
Contributor Author

Choose a reason for hiding this comment

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

Checkout here 99bd6ef


orb_check(_telemetry_status_sub, &updated);

Expand Down Expand Up @@ -3918,6 +3928,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 +3963,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) > _onboard_boot_timeout.get() * 1_s) {
if (!print_msg_once) {
mavlink_log_critical(&mavlink_log_pub, "Avoidance system not available!");
print_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_msg_once = false;
}

//if status changed
Expand All @@ -3974,7 +3991,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 +4001,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
4 changes: 3 additions & 1 deletion 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_ONB_BOOT_T>) _onboard_boot_timeout

)

const int64_t POSVEL_PROBATION_MIN = 1_s; /**< minimum probation duration (usec) */
Expand Down
16 changes: 14 additions & 2 deletions src/modules/commander/commander_params.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -799,9 +799,21 @@ PARAM_DEFINE_INT32(NAV_RCL_ACT, 2);

/**
* Flag to enable obstacle avoidance
* Temporary Parameter to enable interface testing
*
* @boolean
* @group Mission
*/
PARAM_DEFINE_INT32(COM_OBS_AVOID, 0);
PARAM_DEFINE_INT32(COM_OBS_AVOID, 0);

/**
* Set onboard controller bootup timeout
Copy link
Contributor

Choose a reason for hiding this comment

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

I would call it onboard computer or companion computer.

Copy link
Contributor

Choose a reason for hiding this comment

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

Strongly agree. FWIW IMO terminology for onboard/offboard is unhelpful because it is all from perspective of FC, but people think in terms of vehicles.

Is this a general companion computer timeout or an avoidance system running on companion computer timeout?

Assuming the later I'd do something like:

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.

Note, I have tried to explain how it detects the companion computer.

Copy link
Contributor Author

Choose a reason for hiding this comment

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

Sorry, I've lost that comment line during my changes.
@hamishwillee thanks for your suggestion, makes sense.

To answer your question: Yes, it is exactly the time within the avoidance system is expected to be running (formally defined as companion computer boot time + avoidance system startup time).

To this aim i thing that makes sense to change the name parameter to a more consistent too, i.e COM_OA_BOOT_T or COM_OBS_AVOID_T.

I think that the second one would be more consistent with COM_OBS_AVOID.
What do you think? @hamishwillee @julianoes

Copy link
Contributor

Choose a reason for hiding this comment

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

I would probably vote COM_OA_BOOT_T because it's more self explanatory.

*
* This parameter defines the bootup timeout.
Copy link
Contributor

Choose a reason for hiding this comment

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

What about:

The avoidance system is expected to boot within this bootup timeout. If no avoidance system has been detected by this time, a MAVLink warning message is sent.

Copy link
Contributor

Choose a reason for hiding this comment

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

Much better than what was here. See above for my version - it is much the same, but you don't need to mention time or timeout quite so much.

* After the timeout, a mavlink message that tells the user that the avoidance system
Copy link
Contributor

Choose a reason for hiding this comment

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

FYI, MAVLink, not mavlink by preference.

* is not available is sent.
* @group Commander
* @unit s
* @min 0
* @max 200
*/
PARAM_DEFINE_INT32(COM_ONB_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