From 1b322c7764e62e4dec17fb2e38723e89e398bdfb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 7 Nov 2012 23:50:06 +0100 Subject: [PATCH] Fixed bug in HIL message handling, operational with actuator outputs now --- apps/mavlink/orb_listener.c | 69 ++++++++++++++++++++++++------------- 1 file changed, 45 insertions(+), 24 deletions(-) diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index 6342ce9793..7a393185f4 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; + struct actuator_controls_s actuators; - orb_copy(ORB_ID_VEHICLE_CONTROLS, mavlink_subs.act_0_sub, &actuators); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_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); + 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]); } }