/**
* 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);
}
}
