fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

69
apps/mavlink/orb_listener.c
View File

@ -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]);
}
}