From d3fce072551599aa53e7585d901ec3ec90ec22eb Mon Sep 17 00:00:00 2001 From: Anton Matosov Date: Thu, 15 Dec 2016 10:38:59 -0800 Subject: [PATCH] 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 --- .gitignore | 1 + msg/actuator_armed.msg | 1 + .../ardrone_interface/ardrone_interface.c | 2 +- .../navio_sysfs_pwm_out.cpp | 2 +- src/drivers/pwm_out_sim/pwm_out_sim.cpp | 23 +- src/drivers/px4fmu/fmu.cpp | 9 +- src/drivers/px4io/px4io.cpp | 4 +- src/modules/commander/commander.cpp | 65 +++-- src/modules/commander/commander_helper.cpp | 11 + src/modules/commander/commander_helper.h | 1 + .../state_machine_helper_test.cpp | 6 +- .../commander/state_machine_helper.cpp | 242 +++++++++++------- src/modules/commander/state_machine_helper.h | 118 +++++---- .../mc_att_control/mc_att_control_main.cpp | 57 +++++ src/modules/navigator/navigator.h | 2 - src/modules/navigator/navigator_main.cpp | 24 +- src/modules/navigator/navigator_params.c | 6 + src/modules/simulator/CMakeLists.txt | 17 ++ src/modules/simulator/simulator_config.h.in | 39 +++ src/modules/simulator/simulator_mavlink.cpp | 160 +++++++++++- src/modules/uavcan/uavcan_servers.cpp | 2 +- 21 files changed, 580 insertions(+), 212 deletions(-) create mode 100644 src/modules/simulator/simulator_config.h.in diff --git a/.gitignore b/.gitignore index b803f4ae7009..7cab87e8d8f0 100644 --- a/.gitignore +++ b/.gitignore @@ -77,6 +77,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 51a7ab116c00..4a3482afe4f8 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 f76f0bc639f9..2d6c67083464 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 759f103f6352..a84244184350 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 1df5f4bc8bb0..6b01d091105e 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 @@ class PWMSim : public device::VDev 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 52c2b36329f4..053268edae8b 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1192,8 +1192,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 db2cb34905d5..a99988bde3f2 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1454,11 +1454,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 679534690b5c..fa8f833506c1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1665,8 +1665,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; @@ -1753,8 +1753,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); @@ -2685,15 +2685,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 */ @@ -2909,18 +2909,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) { @@ -2980,9 +2982,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); } @@ -3162,19 +3166,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 d1a309e18334..118744f7ecd4 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 f0f5eb49f3ed..af2fa7f204da 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 ac576f4efc22..79402fc8d673 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 b929961c321d..9f2d75237827 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 0dce9ef9b40e..2e21dd1de07f 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -52,49 +52,57 @@ #include 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 39b53c2a4ebb..63031de655b5 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 80675c22e9e9..df7bee40d9b4 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -297,8 +297,6 @@ class Navigator : public control::SuperBlock 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 28d7c246d8ae..c88dfd45c339 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 3b93795b6612..1b2ff7b2063d 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 98db9d8731f7..da01c4084fc3 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 000000000000..a60b491a7901 --- /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 6045a816e9d2..fe07dbfe1548 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 #include #include "simulator.h" +#include #include "errno.h" #include #include @@ -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]; @@ -656,6 +661,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 @@ -781,8 +805,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 b05f4ea32053..ba54186b181f 100644 --- a/src/modules/uavcan/uavcan_servers.cpp +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -537,7 +537,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; }