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

WIP on HIL

Browse Source
This commit is contained in:
Lorenz Meier
2012-09-22 20:55:44 +02:00
parent a9b21886f3
commit 8b951ec417
6 changed files with 78 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/mavlink/mavlink.c
+24 -23
View File
@@ -936,29 +936,6 @@ static void *uorb_receiveloop(void *arg)
orb_copy(ORB_ID(vehicle_attitude_setpoint), subs->spa_sub, &buf.att_sp);
mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, buf.att_sp.timestamp/1000,
buf.att_sp.roll_body, buf.att_sp.pitch_body, buf.att_sp.yaw_body, buf.att_sp.thrust);
/* 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(&v_status, &armed, &mavlink_state, &mavlink_mode);
/* HIL message as per MAVLink spec */
mavlink_msg_hil_controls_send(chan,
hrt_absolute_time(),
buf.att_sp.roll_body, /* this may be replaced by ctrl0 later */
buf.att_sp.pitch_body,
buf.att_sp.yaw_body,
buf.att_sp.thrust,
0,
0,
0,
0,
mavlink_mode,
0);
}
}
/* --- ACTUATOR OUTPUTS 0 --- */
@@ -975,6 +952,7 @@ static void *uorb_receiveloop(void *arg)
buf.act_outputs.output[5],
buf.act_outputs.output[6],
buf.act_outputs.output[7]);
// if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) {
// mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
// 1 /* port 1 */,
@@ -1093,6 +1071,29 @@ static void *uorb_receiveloop(void *arg)
mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl1 ", buf.actuators.control[1]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl2 ", buf.actuators.control[2]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl3 ", buf.actuators.control[3]);
/* 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(&v_status, &armed, &mavlink_state, &mavlink_mode);
/* HIL message as per MAVLink spec */
mavlink_msg_hil_controls_send(chan,
hrt_absolute_time(),
buf.actuators.control[0],
buf.actuators.control[1],
buf.actuators.control[2],
buf.actuators.control[3],
0,
0,
0,
0,
mavlink_mode,
0);
}
}
/* --- DEBUG KEY/VALUE --- */