diff --git a/.gitignore b/.gitignore
index ac9d0647a6a583cadaaed29bed41a8d0e594a6d2..76ea62d30ca7f1e222fba341bfed0d52bc87050c 100644
--- a/.gitignore
+++ b/.gitignore
@@ -79,6 +79,7 @@ vectorcontrol/
# CLion ignores
.idea
+cmake-build-*/
# gcov code coverage
coverage-html/
diff --git a/msg/actuator_armed.msg b/msg/actuator_armed.msg
index 51a7ab116c00be4d06432037f4d17838aa9cf72c..4a3482afe4f8daad8feeb03b004db11a2b530f62 100644
--- a/msg/actuator_armed.msg
+++ b/msg/actuator_armed.msg
@@ -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
diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c
index f76f0bc639f9ac443e9d41fcf9f1dccf9f1f5020..2d6c67083464fcd25eac24f3250d797a70f955a2 100644
--- a/src/drivers/ardrone_interface/ardrone_interface.c
+++ b/src/drivers/ardrone_interface/ardrone_interface.c
@@ -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 {
diff --git a/src/drivers/navio_sysfs_pwm_out/navio_sysfs_pwm_out.cpp b/src/drivers/navio_sysfs_pwm_out/navio_sysfs_pwm_out.cpp
index 759f103f6352029bc1c71372e17c320ee4419f07..a84244184350914469f94b0850b9048106235079 100644
--- a/src/drivers/navio_sysfs_pwm_out/navio_sysfs_pwm_out.cpp
+++ b/src/drivers/navio_sysfs_pwm_out/navio_sysfs_pwm_out.cpp
@@ -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 {
diff --git a/src/drivers/pwm_out_sim/pwm_out_sim.cpp b/src/drivers/pwm_out_sim/pwm_out_sim.cpp
index 1df5f4bc8bb095395e9be887c565ff4094946b6c..6b01d091105e9970e04b5e30be08350c6e35c04b 100644
--- a/src/drivers/pwm_out_sim/pwm_out_sim.cpp
+++ b/src/drivers/pwm_out_sim/pwm_out_sim.cpp
@@ -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;
}
}
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 2ddba3a2a93409ebbaf8da313d46d504fbd421b0..44f2b4ba7e5a18024350796c7af1e4f6f63279fe 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -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];
}
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 32077f120591aa78f8f593a36f63ae1fd9c89b5b..739adc0f263813ef1478ce6dd96ab161318b0bfa 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -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;
}
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 1d4cc54c66afa11d8279ad72fb9cbd6ddf6f2ddf..529c6bb4fec3c407b44afa99c0b17602f930e2d2 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -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,
- &internal_state,
- &mavlink_log_pub,
- (datalink_loss_enabled > 0),
- _mission_result.finished,
- _mission_result.stay_in_failsafe,
- &status_flags,
- land_detector.landed,
- (rc_loss_enabled > 0),
- offboard_loss_act,
- offboard_loss_rc_act);
-
- if (status.failsafe != failsafe_old) {
+ &armed,
+ &internal_state,
+ &mavlink_log_pub,
+ (link_loss_actions_t)datalink_loss_act,
+ _mission_result.finished,
+ _mission_result.stay_in_failsafe,
+ &status_flags,
+ land_detector.landed,
+ (link_loss_actions_t)rc_loss_act,
+ offboard_loss_act,
+ offboard_loss_rc_act);
+
+ 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) {
- /* armed, solid */
- led_on(LED_BLUE);
+ 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);
}
}
diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
index d1a309e18334444884d5c827f5d9f65652aad9c0..118744f7ecd4d7baa2242bcaac0f5bc5f0777955 100644
--- a/src/modules/commander/commander_helper.cpp
+++ b/src/modules/commander/commander_helper.cpp
@@ -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) {
diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h
index f0f5eb49f3edef61bd4a2a9a37de8a7794528f8a..af2fa7f204dae77b1a265bbf63bc64a5240b4db3 100644
--- a/src/modules/commander/commander_helper.h
+++ b/src/modules/commander/commander_helper.h
@@ -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();
diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
index ac576f4efc224ff1f5530289bdec5a4ffab949ae..79402fc8d673e84ae69411796e83ec9e631cb768 100644
--- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp
+++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
@@ -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;
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index b929961c321d60b4e0562b0d51e782d4730a3bff..9f2d75237827c8655f031c6b799cb2a6251ade3f 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -118,15 +118,25 @@ 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,
- arming_state_t new_arming_state,
- struct actuator_armed_s *armed,
- bool fRunPreArmChecks,
- orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log
+ struct battery_status_s *battery,
+ const struct safety_s *safety,
+ arming_state_t new_arming_state,
+ struct actuator_armed_s *armed,
+ bool fRunPreArmChecks,
+ orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log
status_flags_s *status_flags,
- float avionics_power_rail_voltage,
+ float avionics_power_rail_voltage,
bool can_arm_without_gps,
hrt_abstime time_since_boot)
{
@@ -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 {
@@ -640,7 +651,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
*/
void enable_failsafe(struct vehicle_status_s *status,
bool old_failsafe,
- orb_advert_t *mavlink_log_pub, const char * reason) {
+ orb_advert_t *mavlink_log_pub, const char *reason) {
if (old_failsafe == false) {
mavlink_and_console_log_info(mavlink_log_pub, reason);
}
@@ -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;
-
- } 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;
+ set_data_link_loss_nav_state(status, armed, status_flags, data_link_loss_act);
- } 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;
-
- } 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;
+ set_data_link_loss_nav_state(status, armed, status_flags, data_link_loss_act);
- } 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 */
- /* 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)
{
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index 0dce9ef9b40ef3d15c00f9b7310c74bc21b01de0..2e21dd1de07fe833392fe44d16ac80af3fadcc05 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -52,49 +52,57 @@
#include <uORB/topics/commander_state.h>
typedef enum {
- TRANSITION_DENIED = -1,
- TRANSITION_NOT_CHANGED = 0,
- TRANSITION_CHANGED
-
+ 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 {
- bool condition_calibration_enabled;
- bool condition_system_sensors_initialized;
- bool condition_system_prearm_error_reported; // true if errors have already been reported
- bool condition_system_hotplug_timeout; // true if the hotplug sensor search is over
- bool condition_system_returned_to_home;
- bool condition_auto_mission_available;
- bool condition_global_position_valid; // set to true by the commander app if the quality of the position estimate is good enough to use it for navigation
- bool condition_home_position_valid; // indicates a valid home position (a valid home position is not always a valid launch)
- bool condition_local_position_valid;
- bool condition_local_altitude_valid;
- bool condition_airspeed_valid; // set to true by the commander app if there is a valid airspeed measurement available
- bool condition_power_input_valid; // set if input power is valid
- bool usb_connected; // status of the USB power supply
- bool circuit_breaker_engaged_power_check;
- bool circuit_breaker_engaged_airspd_check;
- bool circuit_breaker_engaged_enginefailure_check;
- bool circuit_breaker_engaged_gpsfailure_check;
- bool circuit_breaker_flight_termination_disabled;
- bool circuit_breaker_engaged_usb_check;
- bool offboard_control_signal_found_once;
- bool offboard_control_signal_lost;
- bool offboard_control_signal_weak;
- bool offboard_control_set_by_command; // true if the offboard mode was set by a mavlink command and should not be overridden by RC
- bool offboard_control_loss_timeout; // true if offboard is lost for a certain amount of time
- bool rc_signal_found_once;
- bool rc_signal_lost_cmd; // true if RC lost mode is commanded
- bool rc_input_blocked; // set if RC input should be ignored temporarily
- bool data_link_lost_cmd; // datalink to GCS lost mode commanded
- bool vtol_transition_failure; // Set to true if vtol transition failed
- bool vtol_transition_failure_cmd; // Set to true if vtol transition failure mode is commanded
- bool gps_failure; // Set to true if a gps failure is detected
- bool gps_failure_cmd; // Set to true if a gps failure mode is commanded
- bool barometer_failure; // Set to true if a barometer failure is detected
- bool ever_had_barometer_data; // Set to true if ever had valid barometer data before
+ bool condition_calibration_enabled;
+ bool condition_system_sensors_initialized;
+ bool condition_system_prearm_error_reported; // true if errors have already been reported
+ bool condition_system_hotplug_timeout; // true if the hotplug sensor search is over
+ bool condition_system_returned_to_home;
+ bool condition_auto_mission_available;
+ bool condition_global_position_valid; // set to true by the commander app if the quality of the position estimate is good enough to use it for navigation
+ bool condition_home_position_valid; // indicates a valid home position (a valid home position is not always a valid launch)
+ bool condition_local_position_valid;
+ bool condition_local_altitude_valid;
+ bool condition_airspeed_valid; // set to true by the commander app if there is a valid airspeed measurement available
+ bool condition_power_input_valid; // set if input power is valid
+ bool usb_connected; // status of the USB power supply
+ bool circuit_breaker_engaged_power_check;
+ bool circuit_breaker_engaged_airspd_check;
+ bool circuit_breaker_engaged_enginefailure_check;
+ bool circuit_breaker_engaged_gpsfailure_check;
+ bool circuit_breaker_flight_termination_disabled;
+ bool circuit_breaker_engaged_usb_check;
+ bool offboard_control_signal_found_once;
+ bool offboard_control_signal_lost;
+ bool offboard_control_signal_weak;
+ bool offboard_control_set_by_command; // true if the offboard mode was set by a mavlink command and should not be overridden by RC
+ bool offboard_control_loss_timeout; // true if offboard is lost for a certain amount of time
+ bool rc_signal_found_once;
+ bool rc_signal_lost_cmd; // true if RC lost mode is commanded
+ bool rc_input_blocked; // set if RC input should be ignored temporarily
+ bool data_link_lost_cmd; // datalink to GCS lost mode commanded
+ bool vtol_transition_failure; // Set to true if vtol transition failed
+ bool vtol_transition_failure_cmd; // Set to true if vtol transition failure mode is commanded
+ bool gps_failure; // Set to true if a gps failure is detected
+ bool gps_failure_cmd; // Set to true if a gps failure mode is commanded
+ bool barometer_failure; // Set to true if a barometer failure is detected
+ bool ever_had_barometer_data; // Set to true if ever had valid barometer data before
};
bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
@@ -105,7 +113,7 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status,
arming_state_t new_arming_state,
struct actuator_armed_s *armed,
bool fRunPreArmChecks,
- orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log
+ orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log
status_flags_s *status_flags,
float avionics_power_rail_voltage,
bool can_arm_without_gps,
@@ -117,18 +125,34 @@ 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);
+ 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,
- bool can_arm_without_gps, hrt_abstime time_since_boot);
+ bool force_report, status_flags_s *status_flags, battery_status_s *battery,
+ bool can_arm_without_gps, hrt_abstime time_since_boot);
#endif /* STATE_MACHINE_HELPER_H_ */
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 39b53c2a4ebbddf413cb2a060fa4f94e7fe09fb3..63031de655b59462493b864839abbce7733ed88c 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -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);
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 80675c22e9e9a9659d3fe37657b14a566d8b167f..df7bee40d9b4b7b7adbd9c690c663269f1efdac2 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -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;
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 28d7c246d8ae9bfa97a2b5e951c403e465bb7267..c88dfd45c339487af5c8b10c1ec903d644a9d117 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -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;
- }
+ _navigation_mode = &_rcLoss;
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;
- }
+ _navigation_mode = &_dataLinkLoss;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
_pos_sp_triplet_published_invalid_once = false;
diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c
index 3b93795b6612de8fe0a6c20b8c0bcd976e89e1b0..1b2ff7b2063dced996c49613746826a1268a140f 100644
--- a/src/modules/navigator/navigator_params.c
+++ b/src/modules/navigator/navigator_params.c
@@ -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
*/
diff --git a/src/modules/simulator/CMakeLists.txt b/src/modules/simulator/CMakeLists.txt
index 98db9d8731f716e9976aef70e731a20ea6bddb04..da01c4084fc31557af571d07f5c8004cb242b3f9 100644
--- a/src/modules/simulator/CMakeLists.txt
+++ b/src/modules/simulator/CMakeLists.txt
@@ -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
diff --git a/src/modules/simulator/simulator_config.h.in b/src/modules/simulator/simulator_config.h.in
new file mode 100644
index 0000000000000000000000000000000000000000..a60b491a7901c55b05621b4749e0ae39ad186d72
--- /dev/null
+++ b/src/modules/simulator/simulator_config.h.in
@@ -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@
diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
index f3343ca747910e4599c58bb46da8941ce73cc54a..e3cecbbd51ca873c3e84cf48a07af50c8231068f 100644
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/src/modules/simulator/simulator_mavlink.cpp
@@ -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)
{
diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp
index 09d2389e00211f5f433274767c77a14db70ff325..014b7989ef040f70714dd5a6ed1b0e6d4ce9d6db 100644
--- a/src/modules/uavcan/uavcan_servers.cpp
+++ b/src/modules/uavcan/uavcan_servers.cpp
@@ -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;
}