TEST(misc, testMalfunctionCentral) {
testMalfunctionCentralRemoveNonExistent();
testMalfunctionCentralSameElementAgain();
testMalfunctionCentralRemoveFirstElement();
print("******************************************* testMalfunctionCentral\r\n");
initMalfunctionCentral();
error_codes_set_s localCopy;
// on start-up error storage should be empty
getErrorCodes(&localCopy);
ASSERT_EQ(0, localCopy.count);
obd_code_e code = OBD_Engine_Coolant_Temperature_Circuit_Malfunction;
// let's add one error and validate
addError(code);
getErrorCodes(&localCopy);
ASSERT_EQ( 1, localCopy.count) << "count #1";
ASSERT_EQ(code, localCopy.error_codes[0]);
// let's remove value which is not in the collection
removeError((obd_code_e) 22);
// element not present - nothing to removed
ASSERT_EQ(1, localCopy.count);
ASSERT_EQ(code, localCopy.error_codes[0]);
code = OBD_Intake_Air_Temperature_Circuit_Malfunction;
addError(code);
getErrorCodes(&localCopy);
// todo: ASSERT_EQ(2, localCopy.count);
for (int code = 0; code < 100; code++) {
addError((obd_code_e) code);
}
getErrorCodes(&localCopy);
ASSERT_EQ(MAX_ERROR_CODES_COUNT, localCopy.count);
// now we have full array and code below present
removeError(code);
getErrorCodes(&localCopy);
ASSERT_EQ(MAX_ERROR_CODES_COUNT - 1, localCopy.count);
}
static void testMalfunctionCentralSameElementAgain() {
initMalfunctionCentral();
print("******************************************* testMalfunctionCentralSameElementAgain\r\n");
error_codes_set_s localCopy;
addError(OBD_Engine_Coolant_Temperature_Circuit_Malfunction);
addError(OBD_Engine_Coolant_Temperature_Circuit_Malfunction);
getErrorCodes(&localCopy);
ASSERT_EQ(1, localCopy.count);
}
static void testMalfunctionCentralRemoveFirstElement() {
initMalfunctionCentral();
print("******************************************* testMalfunctionCentralRemoveFirstElement\r\n");
error_codes_set_s localCopy;
obd_code_e firstElement = OBD_Engine_Coolant_Temperature_Circuit_Malfunction;
addError(firstElement);
obd_code_e secondElement = OBD_Intake_Air_Temperature_Circuit_Malfunction;
addError(secondElement);
getErrorCodes(&localCopy);
ASSERT_EQ(2, localCopy.count);
// let's remove first element - code
removeError(firstElement);
getErrorCodes(&localCopy);
ASSERT_EQ(1, localCopy.count);
ASSERT_EQ(secondElement, localCopy.error_codes[0]);
}
static void checkForFirstGPSLock(){
static char gpsLockFlag = 1;
if (gpsLockFlag){
unsigned int error_codes = getErrorCodes();
if((error_codes & STARTUP_ERROR_BROWN_OUT_RESET) || (error_codes & STARTUP_ERROR_POWER_ON_RESET)){
home.latitude = gps_data.latitude;
home.longitude = gps_data.longitude;
home.altitude = gps_data.altitude;
gpsLockFlag = 0;
}
}
}
static msg_t mfiThread(void)
#endif
{
chRegSetThreadName("MFIndicator");
error_codes_set_s localErrorCopy;
while (true) {
chThdSleepSeconds(10);
getErrorCodes(&localErrorCopy);
for (int p = 0; p < localErrorCopy.count; p++) {
// Calculate how many digits in this integer and display error code from start to end
int code = localErrorCopy.error_codes[p];
DisplayErrorCode(DigitLength(code), code);
}
}
}
int writeDatalink(float frequency){
if (time - lastTime > frequency) {
lastTime = time;
struct telem_block* statusData = getDebugTelemetryBlock();//createTelemetryBlock();
if (statusData == 0){
return 0;
}
statusData->lat = getLatitude();
statusData->lon = getLongitude();
statusData->millis = getUTCTime();
statusData->groundSpeed = getSpeed();
statusData->heading = getHeading();
statusData->lastCommandSent = lastCommandSentCode;
statusData->errorCodes = getErrorCodes();
statusData->gpsStatus = (char)(getSatellites() + (isGPSLocked() << 4));
statusData->steeringSetpoint = getPWM(1);
statusData->throttleSetpoint = getPWM(2);
statusData->steeringOutput = sData;
statusData->throttleOutput = tData;
statusData->debugChar = debugChar;
statusData->debugInt = debugInt;
statusData->debugFloat = debugFloat;
if (BLOCKING_MODE) {
sendTelemetryBlock(statusData);
destroyTelemetryBlock(statusData);
} else {
return pushOutboundTelemetryQueue(statusData);
}
}
else if (time < lastTime){
lastTime = time;
return 0;
}
return 0;
}
