bool GCS_MAVLINK_Sub::send_info() { // Just do this all at once, hopefully the hard-wire telemetry requirement means this is ok // Name is char[10] CHECK_PAYLOAD_SIZE(NAMED_VALUE_FLOAT); send_named_float("CamTilt", 1 - (SRV_Channels::get_output_norm(SRV_Channel::k_mount_tilt) / 2.0f + 0.5f)); CHECK_PAYLOAD_SIZE(NAMED_VALUE_FLOAT); send_named_float("CamPan", 1 - (SRV_Channels::get_output_norm(SRV_Channel::k_mount_pan) / 2.0f + 0.5f)); CHECK_PAYLOAD_SIZE(NAMED_VALUE_FLOAT); send_named_float("TetherTrn", sub.quarter_turn_count/4); CHECK_PAYLOAD_SIZE(NAMED_VALUE_FLOAT); send_named_float("Lights1", SRV_Channels::get_output_norm(SRV_Channel::k_rcin9) / 2.0f + 0.5f); CHECK_PAYLOAD_SIZE(NAMED_VALUE_FLOAT); send_named_float("Lights2", SRV_Channels::get_output_norm(SRV_Channel::k_rcin10) / 2.0f + 0.5f); CHECK_PAYLOAD_SIZE(NAMED_VALUE_FLOAT); send_named_float("PilotGain", sub.gain); CHECK_PAYLOAD_SIZE(NAMED_VALUE_FLOAT); send_named_float("InputHold", sub.input_hold_engaged); return true; }
void GCS::send_named_float(const char *name, float value) const { FOR_EACH_ACTIVE_CHANNEL(send_named_float(name, value)); }