void send(const hrt_abstime t)
{
struct vehicle_status_s status;
struct position_setpoint_triplet_s pos_sp_triplet;
/* always send the heartbeat, independent of the update status of the topics */
if (!status_sub->update(&status)) {
/* if topic update failed fill it with defaults */
memset(&status, 0, sizeof(status));
}
if (!pos_sp_triplet_sub->update(&pos_sp_triplet)) {
/* if topic update failed fill it with defaults */
memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet));
}
uint8_t mavlink_state = 0;
uint8_t mavlink_base_mode = 0;
uint32_t mavlink_custom_mode = 0;
get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
mavlink_msg_heartbeat_send(_channel,
mavlink_system.type,
MAV_AUTOPILOT_PX4,
mavlink_base_mode,
mavlink_custom_mode,
mavlink_state);
}
void send(const hrt_abstime t)
{
struct vehicle_attitude_s att;
struct vehicle_global_position_s pos;
struct actuator_armed_s armed;
struct actuator_controls_s act;
struct airspeed_s airspeed;
bool updated = att_sub->update(&att_time, &att);
updated |= pos_sub->update(&pos_time, &pos);
updated |= armed_sub->update(&armed_time, &armed);
updated |= act_sub->update(&act_time, &act);
updated |= airspeed_sub->update(&airspeed_time, &airspeed);
if (updated) {
float groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e);
uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
float throttle = armed.armed ? act.control[3] * 100.0f : 0.0f;
mavlink_msg_vfr_hud_send(_channel,
airspeed.true_airspeed_m_s,
groundspeed,
heading,
throttle,
pos.alt,
-pos.vel_d);
}
}
void send(const hrt_abstime t)
{
bool updated = pos_sub->update(t);
updated |= home_sub->update(t);
if (updated) {
mavlink_msg_global_position_int_send(_channel,
pos->timestamp / 1000,
pos->lat * 1e7,
pos->lon * 1e7,
pos->alt * 1000.0f,
(pos->alt - home->alt) * 1000.0f,
pos->vel_n * 100.0f,
pos->vel_e * 100.0f,
pos->vel_d * 100.0f,
_wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f);
}
}
void send(const hrt_abstime t)
{
if (att_sub->update(t)) {
mavlink_msg_attitude_send(_channel,
att->timestamp / 1000,
att->roll, att->pitch, att->yaw,
att->rollspeed, att->pitchspeed, att->yawspeed);
}
}
void send(const hrt_abstime t)
{
(void)status_sub->update(t);
(void)pos_sp_triplet_sub->update(t);
uint8_t mavlink_state = 0;
uint8_t mavlink_base_mode = 0;
uint32_t mavlink_custom_mode = 0;
get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
mavlink_msg_heartbeat_send(_channel,
mavlink_system.type,
MAV_AUTOPILOT_PX4,
mavlink_base_mode,
mavlink_custom_mode,
mavlink_state);
}
void send(const hrt_abstime t)
{
if (att_rates_sp_sub->update(t)) {
mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel,
att_rates_sp->timestamp / 1000,
att_rates_sp->roll,
att_rates_sp->pitch,
att_rates_sp->yaw,
att_rates_sp->thrust);
}
}
void send(const hrt_abstime t)
{
if (pos_sp_triplet_sub->update(t)) {
mavlink_msg_global_position_setpoint_int_send(_channel,
MAV_FRAME_GLOBAL,
(int32_t)(pos_sp_triplet->current.lat * 1e7),
(int32_t)(pos_sp_triplet->current.lon * 1e7),
(int32_t)(pos_sp_triplet->current.alt * 1000),
(int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f));
}
}
void send(const hrt_abstime t)
{
if (pos_sp_sub->update(t)) {
mavlink_msg_local_position_setpoint_send(_channel,
MAV_FRAME_LOCAL_NED,
pos_sp->x,
pos_sp->y,
pos_sp->z,
pos_sp->yaw);
}
}
void send(const hrt_abstime t)
{
struct vehicle_attitude_s att;
if (att_sub->update(&att_time, &att)) {
mavlink_msg_attitude_send(_channel,
att.timestamp / 1000,
att.roll, att.pitch, att.yaw,
att.rollspeed, att.pitchspeed, att.yawspeed);
}
}
void send(const hrt_abstime t)
{
struct vehicle_global_position_s pos;
struct home_position_s home;
bool updated = pos_sub->update(&pos_time, &pos);
updated |= home_sub->update(&home_time, &home);
if (updated) {
mavlink_msg_global_position_int_send(_channel,
pos.timestamp / 1000,
pos.lat * 1e7,
pos.lon * 1e7,
pos.alt * 1000.0f,
(pos.alt - home.alt) * 1000.0f,
pos.vel_n * 100.0f,
pos.vel_e * 100.0f,
pos.vel_d * 100.0f,
_wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f);
}
}
void send(const hrt_abstime t)
{
if (debug_sub->update(t)) {
/* enforce null termination */
debug->key[sizeof(debug->key) - 1] = '\0';
mavlink_msg_named_value_float_send(_channel,
debug->timestamp_ms,
debug->key,
debug->value);
}
}
void send(const hrt_abstime t)
{
if (flow_sub->update(t)) {
mavlink_msg_optical_flow_send(_channel,
flow->timestamp,
flow->sensor_id,
flow->flow_raw_x, flow->flow_raw_y,
flow->flow_comp_x_m, flow->flow_comp_y_m,
flow->quality,
flow->ground_distance_m);
}
}
void send(const hrt_abstime t)
{
if (manual_sub->update(t)) {
mavlink_msg_manual_control_send(_channel,
mavlink_system.sysid,
manual->x * 1000,
manual->y * 1000,
manual->z * 1000,
manual->r * 1000,
0);
}
}
void send(const hrt_abstime t)
{
if (manual_sub->update(t)) {
mavlink_msg_manual_control_send(_channel,
mavlink_system.sysid,
manual->roll * 1000,
manual->pitch * 1000,
manual->yaw * 1000,
manual->throttle * 1000,
0);
}
}
void send(const hrt_abstime t)
{
if (pos_sub->update(t)) {
mavlink_msg_vicon_position_estimate_send(_channel,
pos->timestamp / 1000,
pos->x,
pos->y,
pos->z,
pos->roll,
pos->pitch,
pos->yaw);
}
}
void send(const hrt_abstime t)
{
struct vehicle_rates_setpoint_s att_rates_sp;
if (att_rates_sp_sub->update(&att_rates_sp_time, &att_rates_sp)) {
mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel,
att_rates_sp.timestamp / 1000,
att_rates_sp.roll,
att_rates_sp.pitch,
att_rates_sp.yaw,
att_rates_sp.thrust);
}
}
void send(const hrt_abstime t)
{
struct vehicle_local_position_setpoint_s pos_sp;
if (pos_sp_sub->update(&pos_sp_time, &pos_sp)) {
mavlink_msg_local_position_setpoint_send(_channel,
MAV_FRAME_LOCAL_NED,
pos_sp.x,
pos_sp.y,
pos_sp.z,
pos_sp.yaw);
}
}
void send(const hrt_abstime t)
{
bool updated = att_sub->update(t);
updated |= pos_sub->update(t);
updated |= armed_sub->update(t);
updated |= act_sub->update(t);
updated |= airspeed_sub->update(t);
if (updated) {
float groundspeed = sqrtf(pos->vel_n * pos->vel_n + pos->vel_e * pos->vel_e);
uint16_t heading = _wrap_2pi(att->yaw) * M_RAD_TO_DEG_F;
float throttle = armed->armed ? act->control[3] * 100.0f : 0.0f;
mavlink_msg_vfr_hud_send(_channel,
airspeed->true_airspeed_m_s,
groundspeed,
heading,
throttle,
pos->alt,
-pos->vel_d);
}
}
void send(const hrt_abstime t)
{
if (pos_sub->update(t)) {
mavlink_msg_local_position_ned_send(_channel,
pos->timestamp / 1000,
pos->x,
pos->y,
pos->z,
pos->vx,
pos->vy,
pos->vz);
}
}
void send(const hrt_abstime t)
{
struct debug_key_value_s debug;
if (debug_sub->update(&debug_time, &debug)) {
/* enforce null termination */
debug.key[sizeof(debug.key) - 1] = '\0';
mavlink_msg_named_value_float_send(_channel,
debug.timestamp_ms,
debug.key,
debug.value);
}
}
void send(const hrt_abstime t)
{
struct optical_flow_s flow;
if (flow_sub->update(&flow_time, &flow)) {
mavlink_msg_optical_flow_send(_channel,
flow.timestamp,
flow.sensor_id,
flow.flow_raw_x, flow.flow_raw_y,
flow.flow_comp_x_m, flow.flow_comp_y_m,
flow.quality,
flow.ground_distance_m);
}
}
void send(const hrt_abstime t)
{
struct manual_control_setpoint_s manual;
if (manual_sub->update(&manual_time, &manual)) {
mavlink_msg_manual_control_send(_channel,
mavlink_system.sysid,
manual.x * 1000,
manual.y * 1000,
manual.z * 1000,
manual.r * 1000,
0);
}
}
void send(const hrt_abstime t)
{
if (att_sub->update(t)) {
mavlink_msg_attitude_quaternion_send(_channel,
att->timestamp / 1000,
att->q[0],
att->q[1],
att->q[2],
att->q[3],
att->rollspeed,
att->pitchspeed,
att->yawspeed);
}
}
void send(const hrt_abstime t)
{
/* we're sending the GPS home periodically to ensure the
* the GCS does pick it up at one point */
if (home_sub->is_published()) {
home_sub->update(t);
mavlink_msg_gps_global_origin_send(_channel,
(int32_t)(home->lat * 1e7),
(int32_t)(home->lon * 1e7),
(int32_t)(home->alt) * 1000.0f);
}
}
void send(const hrt_abstime t)
{
struct rc_input_values rc;
if (rc_sub->update(&rc_time, &rc)) {
const unsigned port_width = 8;
// Deprecated message (but still needed for compatibility!)
for (unsigned i = 0; (i * port_width) < rc.channel_count; i++) {
/* Channels are sent in MAVLink main loop at a fixed interval */
mavlink_msg_rc_channels_raw_send(_channel,
rc.timestamp_publication / 1000,
i,
(rc.channel_count > (i * port_width) + 0) ? rc.values[(i * port_width) + 0] : UINT16_MAX,
(rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX,
(rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX,
(rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX,
(rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX,
(rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX,
(rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX,
(rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX,
rc.rssi);
}
// New message
mavlink_msg_rc_channels_send(_channel,
rc.timestamp_publication / 1000,
rc.channel_count,
((rc.channel_count > 0) ? rc.values[0] : UINT16_MAX),
((rc.channel_count > 1) ? rc.values[1] : UINT16_MAX),
((rc.channel_count > 2) ? rc.values[2] : UINT16_MAX),
((rc.channel_count > 3) ? rc.values[3] : UINT16_MAX),
((rc.channel_count > 4) ? rc.values[4] : UINT16_MAX),
((rc.channel_count > 5) ? rc.values[5] : UINT16_MAX),
((rc.channel_count > 6) ? rc.values[6] : UINT16_MAX),
((rc.channel_count > 7) ? rc.values[7] : UINT16_MAX),
((rc.channel_count > 8) ? rc.values[8] : UINT16_MAX),
((rc.channel_count > 9) ? rc.values[9] : UINT16_MAX),
((rc.channel_count > 10) ? rc.values[10] : UINT16_MAX),
((rc.channel_count > 11) ? rc.values[11] : UINT16_MAX),
((rc.channel_count > 12) ? rc.values[12] : UINT16_MAX),
((rc.channel_count > 13) ? rc.values[13] : UINT16_MAX),
((rc.channel_count > 14) ? rc.values[14] : UINT16_MAX),
((rc.channel_count > 15) ? rc.values[15] : UINT16_MAX),
((rc.channel_count > 16) ? rc.values[16] : UINT16_MAX),
((rc.channel_count > 17) ? rc.values[17] : UINT16_MAX),
rc.rssi);
}
}
void send(const hrt_abstime t)
{
(void)status_sub->update(t);
if (status->arming_state == ARMING_STATE_ARMED
|| status->arming_state == ARMING_STATE_ARMED_ERROR) {
/* send camera capture on */
mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0);
} else {
/* send camera capture off */
mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0);
}
}
void send(const hrt_abstime t)
{
struct vehicle_vicon_position_s pos;
if (pos_sub->update(&pos_time, &pos)) {
mavlink_msg_vicon_position_estimate_send(_channel,
pos.timestamp / 1000,
pos.x,
pos.y,
pos.z,
pos.roll,
pos.pitch,
pos.yaw);
}
}
void send(const hrt_abstime t)
{
struct vehicle_local_position_s pos;
if (pos_sub->update(&pos_time, &pos)) {
mavlink_msg_local_position_ned_send(_channel,
pos.timestamp / 1000,
pos.x,
pos.y,
pos.z,
pos.vx,
pos.vy,
pos.vz);
}
}
void send(const hrt_abstime t)
{
if (act_sub->update(t)) {
mavlink_msg_servo_output_raw_send(_channel,
act->timestamp / 1000,
_n,
act->output[0],
act->output[1],
act->output[2],
act->output[3],
act->output[4],
act->output[5],
act->output[6],
act->output[7]);
}
}
void send(const hrt_abstime t)
{
if (gps_sub->update(t)) {
mavlink_msg_gps_raw_int_send(_channel,
gps->timestamp_position,
gps->fix_type,
gps->lat,
gps->lon,
gps->alt,
cm_uint16_from_m_float(gps->eph_m),
cm_uint16_from_m_float(gps->epv_m),
gps->vel_m_s * 100.0f,
_wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f,
gps->satellites_visible);
}
}
