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

Implement RC and DL failsafe action handling for multirotors 

Move RC and DL failsafe actions handling from navigator to commander (credits to @AndreasAntener)
Separate manual kill switch handling via manual_lockdown to prevent override and release of software lockdown by RC switch

Other changes:
Add failsafe tune
Fix LED blinking for Pixracer
Return back support for rc inputs in simulator but now it is configurable via cmake
This commit is contained in:
Anton Matosov 2016-12-15 10:38:59 -08:00 committed by Lorenz Meier
parent 964dabe179
commit 3a17c07b1e
21 changed files with 579 additions and 211 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@ -79,6 +79,7 @@ vectorcontrol/
# CLion ignores
.idea
cmake-build-*/
# gcov code coverage
coverage-html/

1
msg/actuator_armed.msg
View File

@ -3,5 +3,6 @@ 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
bool lockdown # Set to true if actuators are forced to being disabled (due to emergency or HIL)
bool manual_lockdown # Set to true if manual throttle kill switch is engaged
bool force_failsafe # Set to true if the actuators are forced to the failsafe position
bool in_esc_calibration_mode # IO/FMU should ignore messages from the actuator controls topics

2
src/drivers/ardrone_interface/ardrone_interface.c
View File

@ -333,7 +333,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
/* for now only spin if armed and immediately shut down
* if in failsafe
*/
if (armed.armed && !armed.lockdown) {
if (armed.armed && !(armed.lockdown || armed.manual_lockdown)) {
ardrone_mixing_and_output(ardrone_write, &actuator_controls);
} else {

2
src/drivers/navio_sysfs_pwm_out/navio_sysfs_pwm_out.cpp
View File

@ -373,7 +373,7 @@ void task_main(int argc, char *argv[])
pwm,
&_pwm_limit);
if (_armed.lockdown) {
if (_armed.lockdown || _armed.manual_lockdown) {
send_outputs_pwm(disarmed_pwm);
} else {

23
src/drivers/pwm_out_sim/pwm_out_sim.cpp
View File

@ -88,6 +88,8 @@ class PWMSim : public device::CDev
class PWMSim : public device::VDev
#endif
{
const uint32_t PWM_SIM_DISARMED_MAGIC = 900;
const uint32_t PWM_SIM_FAILSAFE_MAGIC = 600;
public:
enum Mode {
MODE_2PWM,
@ -128,6 +130,8 @@ private:
volatile bool _task_should_exit;
static bool _armed;
static bool _lockdown;
static bool _failsafe;
MixerGroup *_mixers;
@ -170,6 +174,8 @@ PWMSim *g_pwm_sim;
} // namespace
bool PWMSim::_armed = false;
bool PWMSim::_lockdown = false;
bool PWMSim::_failsafe = false;
PWMSim::PWMSim() :
#ifdef __PX4_NUTTX
@ -508,10 +514,23 @@ PWMSim::task_main()
* This will be clearly visible on the servo status and will limit the risk of accidentally
* spinning motors. It would be deadly in flight.
*/
outputs.output[i] = 900;
outputs.output[i] = PWM_SIM_DISARMED_MAGIC;
}
}
/* overwrite outputs in case of force_failsafe */
if (_failsafe) {
for (size_t i = 0; i < num_outputs; i++) {
outputs.output[i] = PWM_SIM_FAILSAFE_MAGIC;
}
}
/* overwrite outputs in case of lockdown */
if (_lockdown) {
for (size_t i = 0; i < num_outputs; i++) {
outputs.output[i] = 0.0;
}
}
/* and publish for anyone that cares to see */
orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs);
@ -526,6 +545,8 @@ PWMSim::task_main()
orb_copy(ORB_ID(actuator_armed), _armed_sub, &aa);
/* do not obey the lockdown value, as lockdown is for PWMSim */
_armed = aa.armed;
_failsafe = aa.force_failsafe;
_lockdown = aa.lockdown || aa.manual_lockdown;
}
}

9
src/drivers/px4fmu/fmu.cpp
View File

@ -1200,8 +1200,15 @@ PX4FMU::cycle()
_disarmed_pwm, _min_pwm, _max_pwm, outputs, pwm_limited, &_pwm_limit);
/* overwrite outputs in case of force_failsafe with _failsafe_pwm PWM values */
if (_armed.force_failsafe) {
for (size_t i = 0; i < num_outputs; i++) {
pwm_limited[i] = _failsafe_pwm[i];
}
}
/* overwrite outputs in case of lockdown with disarmed PWM values */
if (_armed.lockdown) {
if (_armed.lockdown || _armed.manual_lockdown) {
for (size_t i = 0; i < num_outputs; i++) {
pwm_limited[i] = _disarmed_pwm[i];
}

4
src/drivers/px4io/px4io.cpp
View File

@ -1462,11 +1462,11 @@ PX4IO::io_set_arming_state()
_armed = armed.armed;
if (armed.lockdown && !_lockdown_override) {
if ((armed.lockdown || armed.manual_lockdown) && !_lockdown_override) {
set |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
_lockdown_override = true;
} else if (!armed.lockdown && _lockdown_override) {
} else if (!(armed.lockdown || armed.manual_lockdown) && _lockdown_override) {
clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
_lockdown_override = false;
}

43
src/modules/commander/commander.cpp
View File

@ -1670,8 +1670,8 @@ int commander_thread_main(int argc, char *argv[])
transition_result_t arming_ret;
int32_t datalink_loss_enabled = 0;
int32_t rc_loss_enabled = 0;
int32_t datalink_loss_act = 0;
int32_t rc_loss_act = 0;
int32_t datalink_loss_timeout = 10;
float rc_loss_timeout = 0.5;
int32_t datalink_regain_timeout = 0;
@ -1758,8 +1758,8 @@ int commander_thread_main(int argc, char *argv[])
}
/* Safety parameters */
param_get(_param_enable_datalink_loss, &datalink_loss_enabled);
param_get(_param_enable_rc_loss, &rc_loss_enabled);
param_get(_param_enable_datalink_loss, &datalink_loss_act);
param_get(_param_enable_rc_loss, &rc_loss_act);
param_get(_param_datalink_loss_timeout, &datalink_loss_timeout);
param_get(_param_rc_loss_timeout, &rc_loss_timeout);
param_get(_param_rc_in_off, &rc_in_off);
@ -2708,15 +2708,15 @@ int commander_thread_main(int argc, char *argv[])
/* check throttle kill switch */
if (sp_man.kill_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
/* set lockdown flag */
if (!armed.lockdown) {
if (!armed.manual_lockdown) {
mavlink_log_emergency(&mavlink_log_pub, "MANUAL KILL SWITCH ENGAGED");
}
armed.lockdown = true;
armed.manual_lockdown = true;
} else if (sp_man.kill_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
if (armed.lockdown) {
if (armed.manual_lockdown) {
mavlink_log_emergency(&mavlink_log_pub, "MANUAL KILL SWITCH OFF");
}
armed.lockdown = false;
armed.manual_lockdown = false;
}
/* no else case: do not change lockdown flag in unconfigured case */
} else {
@ -2931,18 +2931,20 @@ int commander_thread_main(int argc, char *argv[])
/* now set navigation state according to failsafe and main state */
bool nav_state_changed = set_nav_state(&status,
&armed,
&internal_state,
&mavlink_log_pub,
(datalink_loss_enabled > 0),
(link_loss_actions_t)datalink_loss_act,
_mission_result.finished,
_mission_result.stay_in_failsafe,
&status_flags,
land_detector.landed,
(rc_loss_enabled > 0),
(link_loss_actions_t)rc_loss_act,
offboard_loss_act,
offboard_loss_rc_act);
if (status.failsafe != failsafe_old) {
if (status.failsafe != failsafe_old)
{
status_changed = true;
if (status.failsafe) {
@ -3002,9 +3004,11 @@ int commander_thread_main(int argc, char *argv[])
} else if ((status.hil_state != vehicle_status_s::HIL_STATE_ON) &&
(battery.warning == battery_status_s::BATTERY_WARNING_LOW)) {
/* play tune on battery warning or failsafe */
/* play tune on battery warning */
set_tune(TONE_BATTERY_WARNING_SLOW_TUNE);
} else if (status.failsafe) {
tune_failsafe(true);
} else {
set_tune(TONE_STOP_TUNE);
}
@ -3197,19 +3201,30 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
/* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */
if (actuator_armed->armed) {
if (status.failsafe) {
led_off(LED_BLUE);
if (leds_counter % 5 == 0) {
led_toggle(LED_GREEN);
}
} else {
led_off(LED_GREEN);
/* armed, solid */
led_on(LED_BLUE);
}
} else if (actuator_armed->ready_to_arm) {
led_off(LED_BLUE);
/* ready to arm, blink at 1Hz */
if (leds_counter % 20 == 0) {
led_toggle(LED_BLUE);
led_toggle(LED_GREEN);
}
} else {
led_off(LED_BLUE);
/* not ready to arm, blink at 10Hz */
if (leds_counter % 2 == 0) {
led_toggle(LED_BLUE);
led_toggle(LED_GREEN);
}
}

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

@ -243,6 +243,17 @@ void tune_negative(bool use_buzzer)
}
}
void tune_failsafe(bool use_buzzer)
{
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color(RGBLED_COLOR_PURPLE);
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
if (use_buzzer) {
set_tune(TONE_BATTERY_WARNING_FAST_TUNE);
}
}
int blink_msg_state()
{
if (blink_msg_end == 0) {

1
src/modules/commander/commander_helper.h
View File

@ -64,6 +64,7 @@ void tune_mission_fail(bool use_buzzer);
void tune_positive(bool use_buzzer);
void tune_neutral(bool use_buzzer);
void tune_negative(bool use_buzzer);
void tune_failsafe(bool use_buzzer);
int blink_msg_state();

6
src/modules/commander/commander_tests/state_machine_helper_test.cpp
View File

@ -477,9 +477,9 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
bool StateMachineHelperTest::isSafeTest(void)
{
struct vehicle_status_s current_state;
struct safety_s safety;
struct actuator_armed_s armed;
struct vehicle_status_s current_state = {};
struct safety_s safety = {};
struct actuator_armed_s armed = {};
armed.armed = false;
armed.lockdown = false;

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

@ -118,6 +118,16 @@ static const char *const state_names[vehicle_status_s::ARMING_STATE_MAX] = {
static hrt_abstime last_preflight_check = 0; ///< initialize so it gets checked immediately
static int last_prearm_ret = 1; ///< initialize to fail
void set_link_loss_nav_state(struct vehicle_status_s *status,
struct actuator_armed_s *armed,
status_flags_s *status_flags,
const link_loss_actions_t link_loss_act,
uint8_t auto_recovery_nav_state);
void reset_link_loss_globals(struct actuator_armed_s *armed,
const bool old_failsafe,
const link_loss_actions_t link_loss_act);
transition_result_t arming_state_transition(struct vehicle_status_s *status,
struct battery_status_s *battery,
const struct safety_s *safety,
@ -356,7 +366,8 @@ bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safet
// 1) Not armed
// 2) Armed, but in software lockdown (HIL)
// 3) Safety switch is present AND engaged -> actuators locked
if (!armed->armed || (armed->armed && armed->lockdown) || (safety->safety_switch_available && !safety->safety_off)) {
const bool lockdown = (armed->lockdown || armed->manual_lockdown);
if (!armed->armed || (armed->armed && lockdown) || (safety->safety_switch_available && !safety->safety_off)) {
return true;
} else {
@ -650,19 +661,34 @@ void enable_failsafe(struct vehicle_status_s *status,
/**
* Check failsafe and main status and set navigation status for navigator accordingly
*/
bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *internal_state,
bool set_nav_state(struct vehicle_status_s *status,
struct actuator_armed_s *armed,
struct commander_state_s *internal_state,
orb_advert_t *mavlink_log_pub,
const bool data_link_loss_enabled, const bool mission_finished,
const bool stay_in_failsafe, status_flags_s *status_flags, bool landed,
const bool rc_loss_enabled, const int offb_loss_act, const int offb_loss_rc_act)
const link_loss_actions_t data_link_loss_act,
const bool mission_finished,
const bool stay_in_failsafe,
status_flags_s *status_flags,
bool landed,
const link_loss_actions_t rc_loss_act,
const int offb_loss_act,
const int offb_loss_rc_act)
{
navigation_state_t nav_state_old = status->nav_state;
bool armed = (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED
const bool data_link_loss_act_configured = data_link_loss_act > link_loss_actions_t::DISABLED;
const bool rc_loss_act_configured = rc_loss_act > link_loss_actions_t::DISABLED;
const bool rc_lost = rc_loss_act_configured && (status->rc_signal_lost || status_flags->rc_signal_lost_cmd);
bool is_armed = (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED
|| status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR);
bool old_failsafe = status->failsafe;
status->failsafe = false;
// Safe to do reset flags here, as if loss state persists flags will be restored in the code below
reset_link_loss_globals(armed, old_failsafe, rc_loss_act);
reset_link_loss_globals(armed, old_failsafe, data_link_loss_act);
/* evaluate main state to decide in normal (non-failsafe) mode */
switch (internal_state->main_state) {
case commander_state_s::MAIN_STATE_ACRO:
@ -672,21 +698,10 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
case commander_state_s::MAIN_STATE_ALTCTL:
/* require RC for all manual modes */
if (rc_loss_enabled && (status->rc_signal_lost || status_flags->rc_signal_lost_cmd) && armed) {
if (rc_lost && is_armed) {
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);
if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
} else if (status_flags->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
} else if (status_flags->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
set_rc_loss_nav_state(status, armed, status_flags, rc_loss_act);
} else {
switch (internal_state->main_state) {
@ -719,26 +734,11 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
break;
case commander_state_s::MAIN_STATE_POSCTL: {
const bool rc_lost = rc_loss_enabled && (status->rc_signal_lost || status_flags->rc_signal_lost_cmd);
if (rc_lost && armed) {
if (rc_lost && is_armed) {
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);
if (status_flags->condition_global_position_valid &&
status_flags->condition_home_position_valid &&
!status_flags->gps_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
} else if (status_flags->condition_local_position_valid &&
!status_flags->gps_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
} else if (status_flags->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
set_rc_loss_nav_state(status, armed, status_flags, rc_loss_act);
/* As long as there is RC, we can fallback to ALTCTL, or STAB. */
/* A local position estimate is enough for POSCTL for multirotors,
@ -747,7 +747,7 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
} else if (((status->is_rotary_wing && !status_flags->condition_local_position_valid) ||
(!status->is_rotary_wing && !status_flags->condition_global_position_valid))
&& armed) {
&& is_armed) {
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);
if (status_flags->condition_local_altitude_valid) {
@ -806,41 +806,19 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
/* datalink loss enabled:
* check for datalink lost: this should always trigger RTGS */
} else if (data_link_loss_enabled && status->data_link_lost) {
} else if (data_link_loss_act_configured && status->data_link_lost) {
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
set_data_link_loss_nav_state(status, armed, status_flags, data_link_loss_act);
} else if (status_flags->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
} else if (status_flags->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
/* datalink loss disabled:
/* datalink loss DISABLED:
* check if both, RC and datalink are lost during the mission
* or all links are lost after the mission finishes in air: this should always trigger RCRECOVER */
} else if (!data_link_loss_enabled && status->rc_signal_lost && status->data_link_lost && !landed && mission_finished) {
} else if (!data_link_loss_act_configured && status->rc_signal_lost && status->data_link_lost && !landed && mission_finished) {
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
} else if (status_flags->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
} else if (status_flags->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
set_rc_loss_nav_state(status, armed, status_flags, rc_loss_act);
/* stay where you are if you should stay in failsafe, otherwise everything is perfect */
@ -862,44 +840,23 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
/* also go into failsafe if just datalink is lost */
} else if (status->data_link_lost && data_link_loss_enabled) {
} else if (status->data_link_lost && data_link_loss_act_configured) {
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
set_data_link_loss_nav_state(status, armed, status_flags, data_link_loss_act);
} else if (status_flags->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
/* go into failsafe if RC is lost and datalink loss is not set up and rc loss is not DISABLED */
} else if (status_flags->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
/* go into failsafe if RC is lost and datalink loss is not set up and rc loss is not disabled */
} else if (status->rc_signal_lost && rc_loss_enabled && !data_link_loss_enabled) {
} else if (rc_lost && !data_link_loss_act_configured) {
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);
if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
} else if (status_flags->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
} else if (status_flags->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
set_rc_loss_nav_state(status, armed, status_flags, rc_loss_act);
/* don't bother if RC is lost if datalink is connected */
} else if (status->rc_signal_lost) {
/* this mode is ok, we don't need RC for loitering */
/* this mode is ok, we don't need RC for LOITERing */
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
} else {
@ -1103,6 +1060,93 @@ bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *in
return status->nav_state != nav_state_old;
}
void set_rc_loss_nav_state(struct vehicle_status_s *status,
struct actuator_armed_s *armed,
status_flags_s *status_flags,
const link_loss_actions_t link_loss_act)
{
set_link_loss_nav_state(status, armed, status_flags, link_loss_act, vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER);
}
void set_data_link_loss_nav_state(struct vehicle_status_s *status,
struct actuator_armed_s *armed,
status_flags_s *status_flags,
const link_loss_actions_t link_loss_act)
{
set_link_loss_nav_state(status, armed, status_flags, link_loss_act, vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS);
}
void set_link_loss_nav_state(struct vehicle_status_s *status,
struct actuator_armed_s *armed,
status_flags_s *status_flags,
const link_loss_actions_t link_loss_act,
uint8_t auto_recovery_nav_state)
{
// do the best you can according to the action set
if (link_loss_act == link_loss_actions_t::AUTO_RECOVER
&& status_flags->condition_global_position_valid && status_flags->condition_home_position_valid)
{
status->nav_state = auto_recovery_nav_state;
}
else if (link_loss_act == link_loss_actions_t::AUTO_LOITER && status_flags->condition_global_position_valid)
{
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
}
else if (link_loss_act == link_loss_actions_t::AUTO_RTL
&& status_flags->condition_global_position_valid && status_flags->condition_home_position_valid)
{
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
}
else if (link_loss_act == link_loss_actions_t::AUTO_LAND && status_flags->condition_local_position_valid)
{
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
}
else if (link_loss_act == link_loss_actions_t::TERMINATE)
{
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
armed->force_failsafe = true;
}
else if (link_loss_act == link_loss_actions_t::LOCKDOWN)
{
armed->lockdown = true;
// do the best you can according to the current state
}
else if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid)
{
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
}
else if (status_flags->condition_local_position_valid)
{
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
}
else if (status_flags->condition_local_altitude_valid)
{
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
}
else
{
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
}
void reset_link_loss_globals(struct actuator_armed_s *armed,
const bool old_failsafe,
const link_loss_actions_t link_loss_act)
{
if (old_failsafe)
{
if (link_loss_act == link_loss_actions_t::TERMINATE)
{
armed->force_failsafe = false;
}
else if (link_loss_act == link_loss_actions_t::LOCKDOWN)
{
armed->lockdown = false;
}
}
}
int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report,
status_flags_s *status_flags, battery_status_s *battery, bool can_arm_without_gps, hrt_abstime time_since_boot)
{

36
src/modules/commander/state_machine_helper.h
View File

@ -55,9 +55,17 @@ typedef enum {
TRANSITION_DENIED = -1,
TRANSITION_NOT_CHANGED = 0,
TRANSITION_CHANGED
} transition_result_t;
enum class link_loss_actions_t {
DISABLED = 0,
AUTO_LOITER = 1,
AUTO_RTL = 2,
AUTO_LAND = 3,
AUTO_RECOVER = 4,
TERMINATE = 5,
LOCKDOWN = 6,
};
// This is a struct used by the commander internally.
struct status_flags_s {
@ -117,15 +125,31 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t status_pub, struct vehicle_status_s *current_state, orb_advert_t *mavlink_log_pub);
void enable_failsafe(struct vehicle_status_s *status, bool old_failsafe,
orb_advert_t *mavlink_log_pub, const char *reason);
bool set_nav_state(struct vehicle_status_s *status, struct commander_state_s *internal_state,
bool set_nav_state(struct vehicle_status_s *status,
struct actuator_armed_s *armed,
struct commander_state_s *internal_state,
orb_advert_t *mavlink_log_pub,
const bool data_link_loss_enabled, const bool mission_finished,
const bool stay_in_failsafe, status_flags_s *status_flags, bool landed,
const bool rc_loss_enabled, const int offb_loss_act, const int offb_loss_rc_act);
const link_loss_actions_t data_link_loss_act,
const bool mission_finished,
const bool stay_in_failsafe,
status_flags_s *status_flags,
bool landed,
const link_loss_actions_t rc_loss_act,
const int offb_loss_act,
const int offb_loss_rc_act);
void set_rc_loss_nav_state(struct vehicle_status_s *status,
struct actuator_armed_s *armed,
status_flags_s *status_flags,
const link_loss_actions_t link_loss_act);
void set_data_link_loss_nav_state(struct vehicle_status_s *status,
struct actuator_armed_s *armed,
status_flags_s *status_flags,
const link_loss_actions_t link_loss_act);
int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm,
bool force_report, status_flags_s *status_flags, battery_status_s *battery,

57
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -1144,6 +1144,63 @@ MulticopterAttitudeControl::task_main()
_controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status);
}
}
if (_v_control_mode.flag_control_termination_enabled) {
if (!_vehicle_status.is_vtol) {
_rates_sp.zero();
_rates_int.zero();
_thrust_sp = 0.0f;
_att_control.zero();
/* publish actuator controls */
_actuators.control[0] = 0.0f;
_actuators.control[1] = 0.0f;
_actuators.control[2] = 0.0f;
_actuators.control[3] = 0.0f;
_actuators.timestamp = hrt_absolute_time();
_actuators.timestamp_sample = _ctrl_state.timestamp;
if (!_actuators_0_circuit_breaker_enabled) {
if (_actuators_0_pub != nullptr) {
orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
perf_end(_controller_latency_perf);
} else if (_actuators_id) {
_actuators_0_pub = orb_advertise(_actuators_id, &_actuators);
}
}
_controller_status.roll_rate_integ = _rates_int(0);
_controller_status.pitch_rate_integ = _rates_int(1);
_controller_status.yaw_rate_integ = _rates_int(2);
_controller_status.timestamp = hrt_absolute_time();
/* publish controller status */
if (_controller_status_pub != nullptr) {
orb_publish(ORB_ID(mc_att_ctrl_status), _controller_status_pub, &_controller_status);
} else {
_controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status);
}
/* publish attitude rates setpoint */
_v_rates_sp.roll = _rates_sp(0);
_v_rates_sp.pitch = _rates_sp(1);
_v_rates_sp.yaw = _rates_sp(2);
_v_rates_sp.thrust = _thrust_sp;
_v_rates_sp.timestamp = hrt_absolute_time();
if (_v_rates_sp_pub != nullptr) {
orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
} else if (_rates_sp_id) {
_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
}
}
}
}
perf_end(_loop_perf);

2
src/modules/navigator/navigator.h
View File

@ -297,8 +297,6 @@ private:
control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */
control::BlockParamFloat _param_fw_alt_acceptance_radius; /**< acceptance radius for fixedwing altitude */
control::BlockParamFloat _param_mc_alt_acceptance_radius; /**< acceptance radius for multicopter altitude */
control::BlockParamInt _param_datalinkloss_act; /**< select data link loss action */
control::BlockParamInt _param_rcloss_act; /**< select data link loss action */
control::BlockParamFloat _param_cruising_speed_hover;
control::BlockParamFloat _param_cruising_speed_plane;

20
src/modules/navigator/navigator_main.cpp
View File

@ -158,8 +158,6 @@ Navigator::Navigator() :
_param_acceptance_radius(this, "ACC_RAD"),
_param_fw_alt_acceptance_radius(this, "FW_ALT_RAD"),
_param_mc_alt_acceptance_radius(this, "MC_ALT_RAD"),
_param_datalinkloss_act(this, "DLL_ACT"),
_param_rcloss_act(this, "RCL_ACT"),
_param_cruising_speed_hover(this, "MPC_XY_CRUISE", false),
_param_cruising_speed_plane(this, "FW_AIRSPD_TRIM", false),
_param_cruising_throttle_plane(this, "FW_THR_CRUISE", false),
@ -565,15 +563,7 @@ Navigator::task_main()
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:
_pos_sp_triplet_published_invalid_once = false;
if (_param_rcloss_act.get() == 1) {
_navigation_mode = &_loiter;
} else if (_param_rcloss_act.get() == 3) {
_navigation_mode = &_land;
} else if (_param_rcloss_act.get() == 4) {
_navigation_mode = &_rcLoss;
} else { /* if == 2 or unknown, RTL */
_navigation_mode = &_rtl;
}
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
_pos_sp_triplet_published_invalid_once = false;
@ -592,18 +582,8 @@ Navigator::task_main()
_navigation_mode = &_land;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:
/* Use complex data link loss mode only when enabled via param
* otherwise use rtl */
_pos_sp_triplet_published_invalid_once = false;
if (_param_datalinkloss_act.get() == 1) {
_navigation_mode = &_loiter;
} else if (_param_datalinkloss_act.get() == 3) {
_navigation_mode = &_land;
} else if (_param_datalinkloss_act.get() == 4) {
_navigation_mode = &_dataLinkLoss;
} else { /* if == 2 or unknown, RTL */
_navigation_mode = &_rtl;
}
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
_pos_sp_triplet_published_invalid_once = false;

6
src/modules/navigator/navigator_params.c
View File

@ -110,6 +110,9 @@ PARAM_DEFINE_FLOAT(NAV_MC_ALT_RAD, 3.0f);
* @value 1 Loiter
* @value 2 Return to Land
* @value 3 Land at current position
* @value 4 Data Link Auto Recovery (CASA Outback Challenge rules)
* @value 5 Terminate
* @value 6 Lockdown
*
* @group Mission
*/
@ -128,6 +131,9 @@ PARAM_DEFINE_INT32(NAV_DLL_ACT, 0);
* @value 1 Loiter
* @value 2 Return to Land
* @value 3 Land at current position
* @value 4 RC Auto Recovery (CASA Outback Challenge rules)
* @value 5 Terminate
* @value 6 Lockdown
*
* @group Mission
*/

17
src/modules/simulator/CMakeLists.txt
View File

@ -30,6 +30,23 @@
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
option(ENABLE_UART_RC_INPUT "Enable RC Input from UART mavlink connection" OFF)
if(ENABLE_UART_RC_INPUT)
if (APPLE)
set(PIXHAWK_DEVICE "/dev/cu.usbmodem1")
elseif (UNIX AND NOT APPLE)
set(PIXHAWK_DEVICE "/dev/ttyACM0")
elseif (WIN32)
set(PIXHAWK_DEVICE "COM3")
endif()
set(PIXHAWK_DEVICE_BAUD 115200)
endif()
configure_file(simulator_config.h.in simulator_config.h @ONLY)
include_directories(${CMAKE_CURRENT_BINARY_DIR})
set(SIMULATOR_SRCS simulator.cpp)
if (NOT ${OS} STREQUAL "qurt")
list(APPEND SIMULATOR_SRCS

39
src/modules/simulator/simulator_config.h.in Normal file
View File

@ -0,0 +1,39 @@
/****************************************************************************
*
* Copyright (c) 2016 Anton Matosov. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#cmakedefine ENABLE_UART_RC_INPUT
#cmakedefine PIXHAWK_DEVICE "@PIXHAWK_DEVICE@"
#cmakedefine PIXHAWK_DEVICE_BAUD @PIXHAWK_DEVICE_BAUD@

160
src/modules/simulator/simulator_mavlink.cpp
View File

@ -1,6 +1,7 @@
/****************************************************************************
*
* Copyright (c) 2015 Mark Charlebois. All rights reserved.
* Copyright (c) 2016 Anton Matosov. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -34,6 +35,7 @@
#include <px4_log.h>
#include <px4_time.h>
#include "simulator.h"
#include <simulator_config.h>
#include "errno.h"
#include <geo/geo.h>
#include <drivers/drv_pwm_output.h>
@ -47,8 +49,15 @@ extern "C" __EXPORT hrt_abstime hrt_reset(void);
#define SEND_INTERVAL 20
#define UDP_PORT 14560
#define PIXHAWK_DEVICE "/dev/ttyACM0"
#define PRESS_GROUND 101325.0f
#define DENSITY 1.2041f
static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f;
#ifdef ENABLE_UART_RC_INPUT
#ifndef B460800
#define B460800 460800
#endif
@ -57,12 +66,8 @@ extern "C" __EXPORT hrt_abstime hrt_reset(void);
#define B921600 921600
#endif
#define PRESS_GROUND 101325.0f
#define DENSITY 1.2041f
static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f;
static int openUart(const char *uart_name, int baud);
#endif
static int _fd;
static unsigned char _buf[1024];
@ -657,6 +662,25 @@ void Simulator::pollForMAVLinkMessages(bool publish, int udp_port)
fds[0].fd = _fd;
fds[0].events = POLLIN;
#ifdef ENABLE_UART_RC_INPUT
// setup serial connection to autopilot (used to get manual controls)
int serial_fd = openUart(PIXHAWK_DEVICE, PIXHAWK_DEVICE_BAUD);
char serial_buf[1024];
if (serial_fd >= 0) {
fds[1].fd = serial_fd;
fds[1].events = POLLIN;
fd_count++;
PX4_INFO("Start using %s for radio control input.", PIXHAWK_DEVICE);
} else {
PX4_INFO("Not using %s for radio control input. Assuming joystick input via MAVLink.", PIXHAWK_DEVICE);
}
#endif
int len = 0;
// wait for first data from simulator and respond with first controls
@ -782,8 +806,130 @@ void Simulator::pollForMAVLinkMessages(bool publish, int udp_port)
}
}
}
#ifdef ENABLE_UART_RC_INPUT
// got data from PIXHAWK
if (fd_count > 1 && fds[1].revents & POLLIN) {
len = ::read(serial_fd, serial_buf, sizeof(serial_buf));
if (len > 0) {
mavlink_message_t msg;
mavlink_status_t serial_status = {};
for (int i = 0; i < len; ++i) {
if (mavlink_parse_char(MAVLINK_COMM_1, serial_buf[i], &msg, &serial_status)) {
// have a message, handle it
handle_message(&msg, true);
}
}
}
}
#endif
}
}
#ifdef ENABLE_UART_RC_INPUT
int openUart(const char *uart_name, int baud)
{
/* process baud rate */
int speed;
switch (baud) {
case 0: speed = B0; break;
case 50: speed = B50; break;
case 75: speed = B75; break;
case 110: speed = B110; break;
case 134: speed = B134; break;
case 150: speed = B150; break;
case 200: speed = B200; break;
case 300: speed = B300; break;
case 600: speed = B600; break;
case 1200: speed = B1200; break;
case 1800: speed = B1800; break;
case 2400: speed = B2400; break;
case 4800: speed = B4800; break;
case 9600: speed = B9600; break;
case 19200: speed = B19200; break;
case 38400: speed = B38400; break;
case 57600: speed = B57600; break;
case 115200: speed = B115200; break;
case 230400: speed = B230400; break;
case 460800: speed = B460800; break;
case 921600: speed = B921600; break;
default:
warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n",
baud);
return -EINVAL;
}
/* open uart */
int uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY);
if (uart_fd < 0) {
return uart_fd;
}
/* Try to set baud rate */
struct termios uart_config;
memset(&uart_config, 0, sizeof(uart_config));
int termios_state;
/* Back up the original uart configuration to restore it after exit */
if ((termios_state = tcgetattr(uart_fd, &uart_config)) < 0) {
warnx("ERR GET CONF %s: %d\n", uart_name, termios_state);
::close(uart_fd);
return -1;
}
/* Fill the struct for the new configuration */
tcgetattr(uart_fd, &uart_config);
/* Set baud rate */
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
warnx("ERR SET BAUD %s: %d\n", uart_name, termios_state);
::close(uart_fd);
return -1;
}
// Make raw
cfmakeraw(&uart_config);
if ((termios_state = tcsetattr(uart_fd, TCSANOW, &uart_config)) < 0) {
warnx("ERR SET CONF %s\n", uart_name);
::close(uart_fd);
return -1;
}
return uart_fd;
}
#endif
int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu)
{

2
src/modules/uavcan/uavcan_servers.cpp
View File

@ -536,7 +536,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
struct actuator_armed_s armed;
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
if (armed.armed && !armed.lockdown) {
if (armed.armed && !(armed.lockdown || armed.manual_lockdown)) {
warnx("UAVCAN command bridge: system armed, exiting now.");
break;
}