fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 04:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixed bug in HIL message handling, operational with actuator outputs now

Browse Source
This commit is contained in:
Lorenz Meier
2012-11-07 23:50:06 +01:00
parent 0f6ec03939
commit 1b322c7764
1 changed files with 45 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/mavlink/orb_listener.c
+45 -24
View File
@@ -419,7 +419,7 @@ l_actuator_outputs(struct listener *l)
/* copy actuator data into local buffer */ /* copy actuator data into local buffer */
orb_copy(ids[l->arg], *l->subp, &act_outputs); 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, mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000,
l->arg /* port number */, l->arg /* port number */,
act_outputs.output[0], act_outputs.output[0],
@@ -430,6 +430,30 @@ l_actuator_outputs(struct listener *l)
act_outputs.output[5], act_outputs.output[5],
act_outputs.output[6], act_outputs.output[6],
act_outputs.output[7]); 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 void
@@ -459,31 +483,28 @@ l_manual_control_setpoint(struct listener *l)
void void
l_vehicle_attitude_controls(struct listener *l) 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 (gcs_link) {
if (mavlink_hil_enabled) { /* send, add spaces so that string buffer is at least 10 chars long */
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
/* translate the current syste state to mavlink state and mode */ last_sensor_timestamp / 1000,
uint8_t mavlink_state = 0; "ctrl0 ",
uint8_t mavlink_mode = 0; actuators.control[0]);
get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
/* HIL message as per MAVLink spec */ "ctrl1 ",
mavlink_msg_hil_controls_send(chan, actuators.control[1]);
hrt_absolute_time(), mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
actuators.output[0], last_sensor_timestamp / 1000,
actuators.output[1], "ctrl2 ",
actuators.output[2], actuators.control[2]);
actuators.output[3], mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
actuators.output[4], last_sensor_timestamp / 1000,
actuators.output[5], "ctrl3 ",
actuators.output[6], actuators.control[3]);
actuators.output[7],
mavlink_mode,
0);
} }
} }