/** * Module thread, should not return. */ static void ahrscommsTask(void* parameters) { portTickType lastSysTime; AhrsStatusData data; AlarmsSet(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_CRITICAL); /*Until AHRS connects, assume it doesn't know home */ AhrsStatusGet(&data); data.HomeSet = AHRSSTATUS_HOMESET_FALSE; //data.CalibrationSet = AHRSSTATUS_CALIBRATIONSET_FALSE; data.AlgorithmSet = AHRSSTATUS_CALIBRATIONSET_FALSE; AhrsStatusSet(&data); // Main task loop while (1) { AHRSSettingsData settings; AHRSSettingsGet(&settings); AhrsSendObjects(); AhrsCommStatus stat = AhrsGetStatus(); if(stat.linkOk) { AlarmsClear(SYSTEMALARMS_ALARM_AHRSCOMMS); }else { AlarmsSet(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_WARNING); } AhrsStatusData sData; AhrsStatusGet(&sData); sData.LinkRunning = stat.linkOk; sData.AhrsKickstarts = stat.remote.kickStarts; sData.AhrsCrcErrors = stat.remote.crcErrors; sData.AhrsRetries = stat.remote.retries; sData.AhrsInvalidPackets = stat.remote.invalidPacket; sData.OpCrcErrors = stat.local.crcErrors; sData.OpRetries = stat.local.retries; sData.OpInvalidPackets = stat.local.invalidPacket; AhrsStatusSet(&sData); /* Wait for the next update interval */ vTaskDelayUntil(&lastSysTime, settings.UpdatePeriod / portTICK_RATE_MS ); } }
/** * Module thread, should not return. */ static void ahrscommsTask(void *parameters) { portTickType lastSysTime; AlarmsSet(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_CRITICAL); // Main task loop while (1) { PIOS_WDG_UpdateFlag(PIOS_WDG_AHRS); AhrsCommStatus stat; AhrsSendObjects(); AhrsGetStatus(&stat); if (stat.linkOk) { AlarmsClear(SYSTEMALARMS_ALARM_AHRSCOMMS); } else { AlarmsSet(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_WARNING); } InsStatusData sData; InsStatusGet(&sData); sData.LinkRunning = stat.linkOk; sData.AhrsKickstarts = stat.remote.kickStarts; sData.AhrsCrcErrors = stat.remote.crcErrors; sData.AhrsRetries = stat.remote.retries; sData.AhrsInvalidPackets = stat.remote.invalidPacket; sData.OpCrcErrors = stat.local.crcErrors; sData.OpRetries = stat.local.retries; sData.OpInvalidPackets = stat.local.invalidPacket; InsStatusSet(&sData); /* Wait for the next update interval */ vTaskDelayUntil(&lastSysTime, 2 / portTICK_RATE_MS); } }