diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-12-22 00:47:52 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-12-22 00:47:52 +0100 |
commit | 4cf2266b79a28445ad0b493c36cf125081900423 (patch) | |
tree | 94f065f916ce7b0a5d4d0c48d40bf9b1086b2049 /apps/mavlink/orb_listener.c | |
parent | fe6496a04dd0a232bb530f57031cfb4f6e65bb44 (diff) | |
download | px4-firmware-4cf2266b79a28445ad0b493c36cf125081900423.tar.gz px4-firmware-4cf2266b79a28445ad0b493c36cf125081900423.tar.bz2 px4-firmware-4cf2266b79a28445ad0b493c36cf125081900423.zip |
Robustified actuator output topic, added number of mixed outputs
Diffstat (limited to 'apps/mavlink/orb_listener.c')
-rw-r--r-- | apps/mavlink/orb_listener.c | 18 |
1 files changed, 16 insertions, 2 deletions
diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index e763e642a..971ba6a8e 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -507,12 +507,26 @@ l_actuator_outputs(struct listener *l) 0); } else { + /* + * Catch the case where no rudder is in use and throttle is not + * on output four + */ + float rudder, throttle; + + if (act_outputs.noutputs < 4) { + rudder = 0.0f; + throttle = (act_outputs.output[2] - 900.0f) / 1200.0f; + } else { + rudder = (act_outputs.output[2] - 1500.0f) / 600.0f; + throttle = (act_outputs.output[3] - 900.0f) / 1200.0f; + } + mavlink_msg_hil_controls_send(chan, hrt_absolute_time(), (act_outputs.output[0] - 1500.0f) / 600.0f, (act_outputs.output[1] - 1500.0f) / 600.0f, - 0.0f/*(act_outputs.output[2] - 1500.0f) / 600.0f*/, - (act_outputs.output[2] - 900.0f) / 1200.0f, + rudder, + throttle, (act_outputs.output[4] - 1500.0f) / 600.0f, (act_outputs.output[5] - 1500.0f) / 600.0f, (act_outputs.output[6] - 1500.0f) / 600.0f, |