task_return_t scheduler_send_rt_stats(scheduler_t* scheduler) { const mavlink_stream_t* mavlink_stream = scheduler->mavlink_stream; task_entry_t* stab_task = scheduler_get_task_by_id(scheduler,0); mavlink_message_t msg; mavlink_msg_named_value_float_pack( mavlink_stream->sysid, mavlink_stream->compid, &msg, time_keeper_get_millis(), "stabAvgDelay", stab_task->delay_avg); mavlink_stream_send(mavlink_stream, &msg); mavlink_msg_named_value_float_pack( mavlink_stream->sysid, mavlink_stream->compid, &msg, time_keeper_get_millis(), "stabDelayVar", sqrt(stab_task->delay_var_squared)); mavlink_stream_send(mavlink_stream, &msg); mavlink_msg_named_value_float_pack( mavlink_stream->sysid, mavlink_stream->compid, &msg, time_keeper_get_millis(), "stabMaxDelay", stab_task->delay_max); mavlink_stream_send(mavlink_stream, &msg); mavlink_msg_named_value_float_pack( mavlink_stream->sysid, mavlink_stream->compid, &msg, time_keeper_get_millis(), "stabRTvio", stab_task->rt_violations); mavlink_stream_send(mavlink_stream, &msg); mavlink_msg_named_value_float_pack( mavlink_stream->sysid, mavlink_stream->compid, &msg, time_keeper_get_millis(), "stabExTime", stab_task->execution_time); mavlink_stream_send(mavlink_stream, &msg); stab_task->rt_violations = 0; stab_task->delay_max = 0; return TASK_RUN_SUCCESS; }
void analog_monitor_telemetry_send_sonar(const analog_monitor_t* analog_monitor, const Mavlink_stream* mavlink_stream, mavlink_message_t* msg) { mavlink_msg_named_value_float_pack(mavlink_stream->sysid(), mavlink_stream->compid(), msg, time_keeper_get_ms(), "sonar", 1000.0f / 9.8f * 2.54f * analog_monitor->avg[ANALOG_RAIL_12]); }