diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-11-07 23:50:06 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-11-07 23:50:06 +0100 |
commit | 1b322c7764e62e4dec17fb2e38723e89e398bdfb (patch) | |
tree | 6e78954891adf654608d9b65716912b4640a5899 | |
parent | 0f6ec039391ee55d1545e90d6ac65f46a7992c23 (diff) | |
download | px4-firmware-1b322c7764e62e4dec17fb2e38723e89e398bdfb.tar.gz px4-firmware-1b322c7764e62e4dec17fb2e38723e89e398bdfb.tar.bz2 px4-firmware-1b322c7764e62e4dec17fb2e38723e89e398bdfb.zip |
Fixed bug in HIL message handling, operational with actuator outputs now
-rw-r--r-- | apps/mavlink/orb_listener.c | 73 |
1 files changed, 47 insertions, 26 deletions
diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index 6342ce979..7a393185f 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -419,7 +419,7 @@ l_actuator_outputs(struct listener *l) /* copy actuator data into local buffer */ orb_copy(ids[l->arg], *l->subp, &act_outputs); - if (gcs_link) + if (gcs_link) { mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, l->arg /* port number */, act_outputs.output[0], @@ -430,6 +430,30 @@ l_actuator_outputs(struct listener *l) act_outputs.output[5], act_outputs.output[6], act_outputs.output[7]); + + /* only send in HIL mode */ + if (mavlink_hil_enabled) { + + /* translate the current syste state to mavlink state and mode */ + uint8_t mavlink_state = 0; + uint8_t mavlink_mode = 0; + get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); + + /* HIL message as per MAVLink spec */ + mavlink_msg_hil_controls_send(chan, + hrt_absolute_time(), + act_outputs.output[0], + act_outputs.output[1], + act_outputs.output[2], + act_outputs.output[3], + act_outputs.output[4], + act_outputs.output[5], + act_outputs.output[6], + act_outputs.output[7], + mavlink_mode, + 0); + } + } } void @@ -459,31 +483,28 @@ l_manual_control_setpoint(struct listener *l) void l_vehicle_attitude_controls(struct listener *l) { - struct actuator_outputs_s actuators; - - orb_copy(ORB_ID_VEHICLE_CONTROLS, mavlink_subs.act_0_sub, &actuators); - - /* Only send in HIL mode */ - if (mavlink_hil_enabled) { - - /* translate the current syste state to mavlink state and mode */ - uint8_t mavlink_state = 0; - uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); - - /* HIL message as per MAVLink spec */ - mavlink_msg_hil_controls_send(chan, - hrt_absolute_time(), - actuators.output[0], - actuators.output[1], - actuators.output[2], - actuators.output[3], - actuators.output[4], - actuators.output[5], - actuators.output[6], - actuators.output[7], - mavlink_mode, - 0); + struct actuator_controls_s actuators; + + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators); + + if (gcs_link) { + /* send, add spaces so that string buffer is at least 10 chars long */ + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl0 ", + actuators.control[0]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl1 ", + actuators.control[1]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl2 ", + actuators.control[2]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl3 ", + actuators.control[3]); } } |