inline void processSendData()
{
if (checkTxPeriod(g_tx_freq_cfg_pkt.rcdata_period, g_tx_freq_cfg_pkt.rcdata_phase))
{
sendRcData();
}
if (checkTxPeriod(g_tx_freq_cfg_pkt.imu_period, g_tx_freq_cfg_pkt.imu_phase))
{
sendImuData();
}
if (checkTxPeriod(g_tx_freq_cfg_pkt.flight_state_period, g_tx_freq_cfg_pkt.flight_state_phase))
{
sendFlightStateData();
}
if (checkTxPeriod(g_tx_freq_cfg_pkt.pose_period, g_tx_freq_cfg_pkt.pose_phase))
{
sendMavPoseData();
}
if (checkTxPeriod(g_tx_freq_cfg_pkt.status_period, g_tx_freq_cfg_pkt.status_phase ))
{
sendStatusData();
}
if (checkTxPeriod(g_tx_freq_cfg_pkt.ctrl_debug_period, g_tx_freq_cfg_pkt.ctrl_debug_phase))
{
sendCtrlDebugData();
}
}
/* and FALSE otherwise */
BOOL processDpCommand(UINT32* arr_in)
{
doDataObject_t __svdmh_in;
DisplayPortStatus_t __stat;
DisplayPortConfig_t __config;
if (DpEnabled == FALSE) return TRUE;
__svdmh_in.object = arr_in[0];
switch (__svdmh_in.SVDM.Command) {
case DP_COMMAND_STATUS:
if (__svdmh_in.SVDM.CommandType == INITIATOR) {
if (DpModeEntered == FALSE) return TRUE;
__stat.word = arr_in[1];
informStatus(__stat);
updateStatusData(); // get updated info from system
sendStatusData(__svdmh_in); // send it out
} else {
__stat.word = arr_in[1];
informStatus(__stat);
}
break;
case DP_COMMAND_CONFIG:
if (__svdmh_in.SVDM.CommandType == INITIATOR) {
if (DpModeEntered == FALSE) return TRUE;
__config.word = arr_in[1];
if (DpReconfigure(__config) == TRUE) {
/* if pin reconfig is successful */
replyToConfig(__svdmh_in, TRUE);
} else {
/* if pin reconfig is NOT successful */
replyToConfig(__svdmh_in, FALSE);
}
} else {
if (__svdmh_in.SVDM.CommandType == RESPONDER_ACK) {
informConfigResult(TRUE);
} else {
informConfigResult(FALSE);
}
}
break;
default:
/* command not recognized */
return TRUE;
}
return FALSE;
}
