void
l_vehicle_attitude_controls(struct listener *l)
{
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]);
}
/* 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.control[0],
actuators.control[1],
actuators.control[2],
actuators.control[3],
0,
0,
0,
0,
mavlink_mode,
0);
}
}
void
l_actuator_outputs(const struct listener *l)
{
struct actuator_outputs_s act_outputs;
orb_id_t ids[] = {
ORB_ID(actuator_outputs_0),
ORB_ID(actuator_outputs_1),
ORB_ID(actuator_outputs_2),
ORB_ID(actuator_outputs_3)
};
/* copy actuator data into local buffer */
orb_copy(ids[l->arg], *l->subp, &act_outputs);
if (gcs_link) {
mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000,
l->arg /* port number */,
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]);
/* only send in HIL mode */
if (mavlink_hil_enabled && armed.armed) {
/* 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 */
/* scale / assign outputs depending on system type */
if (mavlink_system.type == MAV_TYPE_QUADROTOR) {
mavlink_msg_hil_controls_send(chan,
hrt_absolute_time(),
((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
-1,
-1,
-1,
-1,
mavlink_mode,
0);
} else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) {
mavlink_msg_hil_controls_send(chan,
hrt_absolute_time(),
((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
-1,
-1,
mavlink_mode,
0);
} else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) {
mavlink_msg_hil_controls_send(chan,
hrt_absolute_time(),
((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f,
mavlink_mode,
0);
} else {
mavlink_msg_hil_controls_send(chan,
hrt_absolute_time(),
(act_outputs.output[0] - 1500.0f) / 500.0f,
(act_outputs.output[1] - 1500.0f) / 500.0f,
(act_outputs.output[2] - 1500.0f) / 500.0f,
(act_outputs.output[3] - 1000.0f) / 1000.0f,
(act_outputs.output[4] - 1500.0f) / 500.0f,
(act_outputs.output[5] - 1500.0f) / 500.0f,
(act_outputs.output[6] - 1500.0f) / 500.0f,
(act_outputs.output[7] - 1500.0f) / 500.0f,
mavlink_mode,
0);
}
}
}
}
void send(const hrt_abstime t)
{
struct vehicle_status_s status;
struct position_setpoint_triplet_s pos_sp_triplet;
struct actuator_outputs_s act;
bool updated = act_sub->update(&act_time, &act);
updated |= pos_sp_triplet_sub->update(&pos_sp_triplet_time, &pos_sp_triplet);
updated |= status_sub->update(&status_time, &status);
if (updated && (status.arming_state == ARMING_STATE_ARMED)) {
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state;
uint8_t mavlink_base_mode;
uint32_t mavlink_custom_mode;
get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
if (mavlink_system.type == MAV_TYPE_QUADROTOR ||
mavlink_system.type == MAV_TYPE_HEXAROTOR ||
mavlink_system.type == MAV_TYPE_OCTOROTOR) {
/* set number of valid outputs depending on vehicle type */
unsigned n;
switch (mavlink_system.type) {
case MAV_TYPE_QUADROTOR:
n = 4;
break;
case MAV_TYPE_HEXAROTOR:
n = 6;
break;
default:
n = 8;
break;
}
/* scale / assign outputs depending on system type */
float out[8];
for (unsigned i = 0; i < 8; i++) {
if (i < n) {
if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
/* scale fake PWM out 900..2100 us to 0..1 for normal multirotors */
out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
} else {
/* send 0 when disarmed */
out[i] = 0.0f;
}
} else {
out[i] = -1.0f;
}
}
mavlink_msg_hil_controls_send(_channel,
hrt_absolute_time(),
out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
mavlink_base_mode,
0);
} else {
/* fixed wing: scale all channels except throttle -1 .. 1
* because we know that we set the mixers up this way
*/
float out[8];
const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2;
for (unsigned i = 0; i < 8; i++) {
if (i != 3) {
/* scale fake PWM out 900..2100 us to -1..+1 for normal channels */
out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
} else {
/* scale fake PWM out 900..2100 us to 0..1 for throttle */
out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
}
}
mavlink_msg_hil_controls_send(_channel,
hrt_absolute_time(),
out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
mavlink_base_mode,
0);
}
}
}
void
l_actuator_outputs(struct listener *l)
{
struct actuator_outputs_s act_outputs;
orb_id_t ids[] = {
ORB_ID(actuator_outputs_0),
ORB_ID(actuator_outputs_1),
ORB_ID(actuator_outputs_2),
ORB_ID(actuator_outputs_3)
};
/* copy actuator data into local buffer */
orb_copy(ids[l->arg], *l->subp, &act_outputs);
if (gcs_link) {
mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000,
l->arg /* port number */,
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]);
/* 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);
float rudder, throttle;
// XXX very ugly, needs rework
if (isfinite(act_outputs.output[3])
&& act_outputs.output[3] > 800 && act_outputs.output[3] < 2200) {
/* throttle is fourth output */
rudder = (act_outputs.output[2] - 1500.0f) / 1000.0f;
throttle = (act_outputs.output[3] - 1000.0f) / 1000.0f;
} else {
/* only three outputs, put throttle on position 4 / index 3 */
rudder = 0;
throttle = (act_outputs.output[2] - 1000.0f) / 1000.0f;
}
/* HIL message as per MAVLink spec */
mavlink_msg_hil_controls_send(chan,
hrt_absolute_time(),
(act_outputs.output[0] - 1500.0f) / 1000.0f,
(act_outputs.output[1] - 1500.0f) / 1000.0f,
rudder,
throttle,
(act_outputs.output[4] - 1500.0f) / 1000.0f,
(act_outputs.output[5] - 1500.0f) / 1000.0f,
(act_outputs.output[6] - 1500.0f) / 1000.0f,
(act_outputs.output[7] - 1500.0f) / 1000.0f,
mavlink_mode,
0);
}
}
}