From 2c2c4af59907a1a2dd9fb0635fd9a179f8c8860f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 25 Mar 2014 12:21:07 +0100 Subject: mavlink / HIL: Split handling of actuator feedback between fixed wing and multicopters --- src/modules/mavlink/mavlink_messages.cpp | 84 ++++++++++++++++++++------------ 1 file changed, 53 insertions(+), 31 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 96852bb0e..e292f52ba 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -71,6 +71,7 @@ #include #include #include +#include #include "mavlink_messages.h" @@ -762,6 +763,9 @@ private: MavlinkOrbSubscription *act_sub; struct actuator_outputs_s *act; + MavlinkOrbSubscription *ctrl_sub; + struct actuator_controls_s *ctrl; + protected: void subscribe(Mavlink *mavlink) { @@ -773,6 +777,9 @@ protected: act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0)); act = (struct actuator_outputs_s *)act_sub->get_data(); + + ctrl_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0)); + ctrl = (struct actuator_controls_s *)ctrl_sub->get_data(); } void send(const hrt_abstime t) @@ -780,6 +787,7 @@ protected: bool updated = status_sub->update(t); updated |= pos_sp_triplet_sub->update(t); updated |= act_sub->update(t); + updated |= ctrl_sub->update(t); if (updated) { /* translate the current syste state to mavlink state and mode */ @@ -788,47 +796,61 @@ protected: uint32_t mavlink_custom_mode; get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - /* set number of valid outputs depending on vehicle type */ - unsigned n; + if (mavlink_system.type == MAV_TYPE_QUADROTOR || + mavlink_system.type == MAV_TYPE_HEXAROTOR || + mavlink_system.type == MAV_TYPE_OCTOROTOR) { + /* set number of valid outputs depending on vehicle type */ + unsigned n; - switch (mavlink_system.type) { - case MAV_TYPE_QUADROTOR: - n = 4; - break; + switch (mavlink_system.type) { + case MAV_TYPE_QUADROTOR: + n = 4; + break; - case MAV_TYPE_HEXAROTOR: - n = 6; - break; + case MAV_TYPE_HEXAROTOR: + n = 6; + break; - default: - n = 8; - break; - } + default: + n = 8; + break; + } - /* scale / assign outputs depending on system type */ - float out[8]; + /* scale / assign outputs depending on system type */ + float out[8]; - for (unsigned i = 0; i < 8; i++) { - if (i < n) { - if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - /* scale fake PWM out 900..2100 us to -1..1*/ - out[i] = (act->output[i] - 1500.0f) / 600.0f; + for (unsigned i = 0; i < 8; i++) { + if (i < n) { + if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { + /* scale fake PWM out 900..2100 us to 0..1 for normal multirotors */ + out[i] = (act->output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + + } else { + /* send 0 when disarmed */ + out[i] = 0.0f; + } } else { - /* send 0 when disarmed */ - out[i] = 0.0f; + out[i] = -1.0f; } - - } else { - out[i] = -1.0f; } - } - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); + mavlink_msg_hil_controls_send(_channel, + hrt_absolute_time(), + out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], + mavlink_base_mode, + 0); + } else { + + /* no special handling required, just send the controls */ + + mavlink_msg_hil_controls_send(_channel, + hrt_absolute_time(), + ctrl->control[0], ctrl->control[1], ctrl->control[2], ctrl->control[3], + ctrl->control[4], ctrl->control[5], ctrl->control[6], ctrl->control[7], + mavlink_base_mode, + 0); + } } } }; -- cgit v1.2.3 From 506c16f12a8420991a0e64c616bbf5833ce1845c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 25 Mar 2014 17:37:44 +0100 Subject: Bring fixed wing HIL back to normal mode, keep multicopter unchanged --- src/modules/mavlink/mavlink_messages.cpp | 31 +++++++++++++++++++++---------- 1 file changed, 21 insertions(+), 10 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index e292f52ba..4ca3840d4 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -763,9 +763,6 @@ private: MavlinkOrbSubscription *act_sub; struct actuator_outputs_s *act; - MavlinkOrbSubscription *ctrl_sub; - struct actuator_controls_s *ctrl; - protected: void subscribe(Mavlink *mavlink) { @@ -777,9 +774,6 @@ protected: act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0)); act = (struct actuator_outputs_s *)act_sub->get_data(); - - ctrl_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0)); - ctrl = (struct actuator_controls_s *)ctrl_sub->get_data(); } void send(const hrt_abstime t) @@ -787,7 +781,6 @@ protected: bool updated = status_sub->update(t); updated |= pos_sp_triplet_sub->update(t); updated |= act_sub->update(t); - updated |= ctrl_sub->update(t); if (updated) { /* translate the current syste state to mavlink state and mode */ @@ -842,12 +835,30 @@ protected: 0); } else { - /* no special handling required, just send the controls */ + /* fixed wing: scale all channels except throttle -1 .. 1 + * because we know that we set the mixers up this way + */ + + float out[8]; + + const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; + + for (unsigned i = 0; i < 8; i++) { + if (i != 3) { + /* scale fake PWM out 900..2100 us to -1..+1 for normal channels */ + out[i] = (act->output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); + + } else { + + /* scale fake PWM out 900..2100 us to 0..1 for throttle */ + out[i] = (act->output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + } + + } mavlink_msg_hil_controls_send(_channel, hrt_absolute_time(), - ctrl->control[0], ctrl->control[1], ctrl->control[2], ctrl->control[3], - ctrl->control[4], ctrl->control[5], ctrl->control[6], ctrl->control[7], + out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], mavlink_base_mode, 0); } -- cgit v1.2.3 From 66527eea02ca6c8b1e33047f60bce1b832f82071 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 27 Mar 2014 17:54:29 +0100 Subject: commander: workaround to prevent RC loss in HIL --- src/modules/commander/commander.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d114a2e5c..cf7ba757e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -119,6 +119,7 @@ extern struct system_load_s system_load; #define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */ #define RC_TIMEOUT 100000 +#define RC_TIMEOUT_HIL 500000 #define DIFFPRESS_TIMEOUT 2000000 #define PRINT_INTERVAL 5000000 @@ -1108,8 +1109,16 @@ int commander_thread_main(int argc, char *argv[]) } } + + /* + * XXX workaround: + * Prevent RC loss in HIL when sensors.cpp is only publishing sp_man at a low rate (e.g. 30Hz) + * which can trigger RC loss if the computer/simulator lags. + */ + uint64_t rc_timeout = status.hil_state == HIL_STATE_ON ? RC_TIMEOUT_HIL : RC_TIMEOUT; + /* start RC input check */ - if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { + if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + rc_timeout) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; -- cgit v1.2.3