diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-03-25 17:37:44 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-03-25 17:37:44 +0100 |
commit | 506c16f12a8420991a0e64c616bbf5833ce1845c (patch) | |
tree | cd47f23224053a8d1d858bff7a653ea645919720 /src | |
parent | 2c2c4af59907a1a2dd9fb0635fd9a179f8c8860f (diff) | |
download | px4-firmware-506c16f12a8420991a0e64c616bbf5833ce1845c.tar.gz px4-firmware-506c16f12a8420991a0e64c616bbf5833ce1845c.tar.bz2 px4-firmware-506c16f12a8420991a0e64c616bbf5833ce1845c.zip |
Bring fixed wing HIL back to normal mode, keep multicopter unchanged
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/mavlink/mavlink_messages.cpp | 31 |
1 files 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); } |