GONEC_NEW
/
GONEC_CONVERT
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
GONEC_CONVERT/APP/EraGlonassMsd.c

535 lines
20 KiB
C
Raw Blame History

/*
* msd.c
*
* Created on: OCT 21, 2020
* Author: FICOM-IT LTD
*/
#include "EraGlonassMsd.h"
#include "BitBuffer.h"
#include <memory.h>
void EraGlonassMsdInit(tEraGlonassMsd *env) {
// env->lastPos = (tEraGlonassMsd_GpsPos) {512, 512};
//init not changeable fields
env->MSD_Data.msgId = 0;
env->MSD_Data.msdId = MSD_V_2; // MSD format version
env->MSD_Data.MSD_Control.extraData = 0x00; // Additional information not included
// aGpsStatus
env->MSD_Data.MSD_Control.recentPos1 = 0x01; // Recent vehicle position 1 available
env->MSD_Data.MSD_Control.recentPos2 = 0x01; // Recent vehicle position 2 available
env->MSD_Data.additionalData.coordSystem = ERA_GLONASS_MSD_COORDINATE_SYSTEM_TYPE_WGS84;
env->MSD_Data.additionalData.diagnosticResult = NULL;
}
void EraGlonassMsd_ClearDiagnostic(tMSD_DiagnosticResult *env) {
memset(env, 0, sizeof(tMSD_DiagnosticResult));
}
// In case we don't have valid GPS data set defaults as described in EN15722
void EraGlonassMsdNoGnssSetDefaults(tEraGlonassMsd *env) {
env->MSD_Data.timestamp = 0x00;
env->MSD_Data.pos.lat = 0xFFFFFFFF;
env->MSD_Data.pos.lon = 0xFFFFFFFF;
// env->MSD_Data.pos.lat = 0xFFFFFFF;
// env->MSD_Data.pos.lon = 0xFFFFFFF;
env->MSD_Data.direction = 0xFF;
env->MSD_Data.MSD_PosDelta[0].lat = 512;
env->MSD_Data.MSD_PosDelta[0].lon = 512;
env->MSD_Data.MSD_PosDelta[1].lat = 512;
env->MSD_Data.MSD_PosDelta[1].lon = 512;
// aGpsStatus
env->MSD_Data.MSD_Control.recentPos1 = 0x00; // Recent vehicle position 1 available
env->MSD_Data.MSD_Control.recentPos2 = 0x00; // Recent vehicle position 2 available
}
void EraGlonassMsdSetDataEmergencySituationFlags(
tEraGlonassMsd *env,
int MSD_MSG_ID,
eEcallActivationType eCallType,
eEcallTestMode eCallTestMode
) {
// Message ID
env->MSD_Data.msgId = MSD_MSG_ID;
// Automatic or manual eCall activation
if (eCallType == AUTOMATIC_ACTIVATION) {
env->MSD_Data.MSD_Control.activation = 1;
}
if (eCallType == MANUAL_ACTIVATION) {
env->MSD_Data.MSD_Control.activation = 0;
}
// Test eCall
if (eCallTestMode == TEST_CALL) {
env->MSD_Data.MSD_Control.testCall = 1;
}
if (eCallTestMode == EMERGENCY_CALL) {
env->MSD_Data.MSD_Control.testCall = 0;
}
}
void EraGlonassMsdCalcPosDelta(tEraGlonassMsd *env, tEraGlonassMsd_GpsPos lastPos) {
int16_t deltaLat = 0, deltaLon = 0;
deltaLat = (lastPos.lat - env->MSD_Data.pos.lat) / 100;
deltaLon = (lastPos.lon - env->MSD_Data.pos.lon) / 100;
if (deltaLat <= -512) {
deltaLat = -512;
} else if (deltaLat >= 511) {
deltaLat = 511;
}
deltaLat += 512; // Make offset
if (deltaLon <= -512) {
deltaLon = -512;
} else if (deltaLon >= 511) {
deltaLon = 511;
}
deltaLon += 512;
env->MSD_Data.MSD_PosDelta[1].lat = env->MSD_Data.MSD_PosDelta[0].lat;
env->MSD_Data.MSD_PosDelta[1].lon = env->MSD_Data.MSD_PosDelta[0].lon;
env->MSD_Data.MSD_Control.recentPos2 = env->MSD_Data.MSD_Control.recentPos1;
env->MSD_Data.MSD_PosDelta[0].lat = deltaLat;
env->MSD_Data.MSD_PosDelta[0].lon = deltaLon;
env->MSD_Data.MSD_Control.recentPos1 = 0x01;
//Update to the latest position in order to calculate
//correctly the deltas on the next iteration
// env->lastPos.lat = env->MSD_Data.pos.lat;
// env->lastPos.lon = env->MSD_Data.pos.lon;
}
void EraGlonassMsdSetTimeStamp(tEraGlonassMsd *env, uint32_t timestamp) {
env->MSD_Data.timestamp = timestamp;
}
void EraGlonassMsdSetPositionValue(
tEraGlonassMsd *env,
int32_t lonArc,
int32_t latArc,
uint16_t track,
uint8_t valid
) {
tEraGlonassMsd_GpsPos lastPos = env->MSD_Data.pos;
env->MSD_Data.pos.lat = latArc;
env->MSD_Data.pos.lon = lonArc;
if (valid == 0) {
env->MSD_Data.MSD_Control.posStatus = 0x00; // False if position is not valid
env->MSD_Data.direction = 0xFF;
} else if (valid == 1){
EraGlonassMsdCalcPosDelta(env, lastPos);
env->MSD_Data.MSD_Control.posStatus = 0x00;
env->MSD_Data.direction = 0xFF;
} else if (valid == 2){
EraGlonassMsdCalcPosDelta(env, lastPos);
env->MSD_Data.MSD_Control.posStatus = 0x01;
// env->MSD_Data.direction = (track / 2); //(*255)/2
env->MSD_Data.direction = track; //(*255)/2
}
env->MSD_Data.pos.lat += 0x80000000; // Make offset
env->MSD_Data.pos.lon += 0x80000000;
}
void EraGlonassMsdSetPassengersNumber(tEraGlonassMsd *env, uint16_t value) {
// Check passengers number
if (value >= 1) {
env->MSD_Data.MSD_Control.passengers = value; // Passengers number available
} else {
env->MSD_Data.MSD_Control.passengers = 0x00; // Passengers number not available
}
}
void EraGlonassMsdSetVehicleType(tEraGlonassMsd *env, eUveosGostVehicleType value) {
env->MSD_Data.MSD_Control.vehicleType = value;
}
void EraGlonassMsdSetVIN(tEraGlonassMsd *env, char *vin, uint8_t vinLen) {
unsigned char ch = 0, i = 0;
// Set VIN
for (i = 0; i < 17; i++) {
if (i < vinLen) {
ch = vin[i];
} else {
ch = '0';
}
if (ch >= '0' && ch <= '9') {
ch -= 0x30; // Numbers are encoded with valuese 0x00 to 0x09
} else if (ch >= 'A' && ch <= 'H') {
ch -= 0x37;
} else if (ch >= 'J' && ch <= 'N') {
ch -= 0x38;
} else if (ch == 'P') {
ch = 0x17;
} else if (ch >= 'R' && ch <= 'Z') {
ch -= 0x3A;
} else {
ch = '0' - 0x30;
}
env->MSD_Data.vin[i] = ch;
}
}
void EraGlonassMsdSetPropulsionStorageType(
tEraGlonassMsd *env,
eUveosGostVehiclePropulsionStorageType value
) {
env->MSD_Data.MSD_Control.includedTanks = 0b01111111; // Enable all tank type to be transmitted
// Set propulsion system storage
env->MSD_Data.propulsion = value;
}
uint16_t sEraGlonassMsdEncodeOfControl(tEraGlonassMsd_DataTypedef *data, unsigned char *raw, uint16_t lastBit) {
lastBit += vBitBufferAppendByte(raw, lastBit, data->msgId, 8);
lastBit += vBitBufferAppendByte(raw, lastBit, data->MSD_Control.activation ? 0x01 : 0x00, 1);
lastBit += vBitBufferAppendByte(raw, lastBit, data->MSD_Control.testCall ? 0x01 : 0x00, 1);
lastBit += vBitBufferAppendByte(raw, lastBit, data->MSD_Control.posStatus ? 0x01 : 0x00, 1);
lastBit += vBitBufferAppendByte(raw, lastBit, data->MSD_Control.vehicleType, 5);
return lastBit;
}
uint16_t sEraGlonassMsdEncodeOfVin(tEraGlonassMsd_DataTypedef *data, unsigned char *raw, uint16_t lastBit) {
for (int i = 0; i < 17; ++i) {
lastBit += vBitBufferAppendByte(raw, lastBit, ((uint8_t) data->vin[i]), 6);
}
return lastBit;
}
uint16_t sEraGlonassMsdEncodeOfPropulsion(tEraGlonassMsd_DataTypedef *data, unsigned char *raw, uint16_t lastBit) {
uint8_t maxCountOfTanks;
if (data->msdId == MSD_V_1) {
maxCountOfTanks = 6;
} else if (data->msdId == MSD_V_2) {
maxCountOfTanks = 7;
}
uint8_t tanksInfoIncluded = !(data->MSD_Control.includedTanks >> maxCountOfTanks) && 0x1;
if (tanksInfoIncluded) {
lastBit += vBitBufferAppendByte(raw, lastBit, data->MSD_Control.includedTanks, maxCountOfTanks + 1);
for (int i = 0; i < maxCountOfTanks; ++i) {
if ((data->MSD_Control.includedTanks >> i) && 0x1) {
lastBit += vBitBufferAppendByte(raw, lastBit, (data->propulsion >> i), 1);
}
}
} else {
lastBit += vBitBufferAppendByte(raw, lastBit, 0x1, 1);
}
return lastBit;
}
uint16_t sEraGlonassMsdEncodeOfTimestamp(tEraGlonassMsd_DataTypedef *data, unsigned char *raw, uint16_t lastBit) {
lastBit += vBitBufferAppendBitsReverseBytes(raw, lastBit, (uint8_t *) &data->timestamp, 32);
return lastBit;
}
uint16_t sEraGlonassMsdEncodeOfLocation(tEraGlonassMsd_DataTypedef *data, unsigned char *raw, uint16_t lastBit) {
lastBit += vBitBufferAppendBitsReverseBytes(raw, lastBit, (uint8_t *) &data->pos.lat, 32);
lastBit += vBitBufferAppendBitsReverseBytes(raw, lastBit, (uint8_t *) &data->pos.lon, 32);
return lastBit;
}
uint16_t sEraGlonassMsdEncodeOfDirection(tEraGlonassMsd_DataTypedef *data, unsigned char *raw, uint16_t lastBit) {
\
lastBit += vBitBufferAppendByte(raw, lastBit, data->direction, 8);
return lastBit;
}
uint16_t sEraGlonassMsdEncodeOfRecentPositions(tEraGlonassMsd_DataTypedef *data, unsigned char *raw, uint16_t lastBit) {
\
if (data->MSD_Control.recentPos1) {
lastBit += vBitBufferAppendBits(raw, lastBit, (uint8_t *) &data->MSD_PosDelta[0].lat, 10);
lastBit += vBitBufferAppendBits(raw, lastBit, (uint8_t *) &data->MSD_PosDelta[0].lon, 10);
}
if (data->MSD_Control.recentPos2) {
lastBit += vBitBufferAppendBits(raw, lastBit, (uint8_t *) &data->MSD_PosDelta[1].lat, 10);
lastBit += vBitBufferAppendBits(raw, lastBit, (uint8_t *) &data->MSD_PosDelta[1].lon, 10);
}
return lastBit;
}
uint16_t sEraGlonassMsdEncodeOfPassengersCount(tEraGlonassMsd_DataTypedef *data, unsigned char *raw, uint16_t lastBit) {
if (data->MSD_Control.passengers) {
lastBit += vBitBufferAppendBits(raw, lastBit, (uint8_t *) &data->MSD_Control.passengers, 8);
}
return lastBit;
}
uint16_t sEraGlonassMsdEncodeOfV1FreeSpaceFix(tEraGlonassMsd_DataTypedef *data, unsigned char *raw, uint16_t lastBit) {
if (data->msdId == MSD_V_1) {
lastBit += vBitBufferAppendByte(raw, lastBit, 0x00, 8);
}
return lastBit;
}
uint16_t
sEraGlonassMsdEncodeOfAdditionalDataOID(tEraGlonassMsd_DataTypedef *data, unsigned char *raw, uint16_t lastBit) {
lastBit += vBitBufferAppendByte(raw, lastBit, 0x03, 8);//lent of OID
lastBit += vBitBufferAppendByte(raw, lastBit, 0x01, 8);//OID 1
lastBit += vBitBufferAppendByte(raw, lastBit, 0x04, 8);//OID 2
if (data->msdId == MSD_V_1) {
lastBit += vBitBufferAppendByte(raw, lastBit, 0x01, 8);//OID 3
} else if (data->msdId == MSD_V_2) {
lastBit += vBitBufferAppendByte(raw, lastBit, 0x02, 8);//OID 3
}
return lastBit;
}
#define MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(FLAG) lastBit += vBitBufferAppendByte(raw, lastBit, (FLAG) == ERA_GLONASS_MSD_OPTIONAL_FLAG_ABSENT ? 0b0 : 0b1, 1);
#define MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(FLAG) \
if((FLAG) == ERA_GLONASS_MSD_OPTIONAL_FLAG_PRESENT_TRUE) { lastBit += vBitBufferAppendByte(raw, lastBit, 0b1 , 1);} \
else if((FLAG) == ERA_GLONASS_MSD_OPTIONAL_FLAG_PRESENT_FALSE) { lastBit += vBitBufferAppendByte(raw, lastBit, 0b0, 1);}
uint16_t sEraGlonassMsdEncodeOfAdditionalDataPayload(
tEraGlonassMsd_DataTypedef *data, unsigned char *raw, uint16_t lastBit
) {
lastBit += vBitBufferAppendByte(raw, lastBit, 0b0, 1); // enable extendable
//enable what present
lastBit += vBitBufferAppendByte(raw, lastBit, data->additionalData.asi15Present ? 0b1 : 0b0, 1);
lastBit += vBitBufferAppendByte(raw, lastBit, data->additionalData.diagnosticResult ? 0b1 : 0b0, 1);
lastBit += vBitBufferAppendByte(raw, lastBit, data->additionalData.crashInfoPresent ? 0b1 : 0b0, 1);
if (data->msdId == MSD_V_2) {
lastBit += vBitBufferAppendByte(raw, lastBit, data->additionalData.coordSystem ? 0b1 : 0b0,
1); // enable extendable
}
if (data->additionalData.asi15Present) {
lastBit += vBitBufferAppendByte(raw, lastBit, ((uint8_t *) &data->additionalData.asi15)[1], 3);
lastBit += vBitBufferAppendByte(raw, lastBit, ((uint8_t *) &data->additionalData.asi15)[0], 8);
}
if (data->additionalData.diagnosticResult) {
tMSD_DiagnosticResult *diagRes = data->additionalData.diagnosticResult;
MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->micConnectionFailure);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->micFailure);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->rightSpeakerFailure);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->leftSpeakerFailure);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->speakersFailure);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->ignitionLineFailure);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->uimFailure);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->statusIndicatorFailure);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->batteryFailure);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->batteryVoltageLow);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->crashSensorFailure);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->firmwareImageCorruption);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->commModuleInterfaceFailure);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->gnssReceiverFailure);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->raimProblem);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->gnssAntennaFailure);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->commModuleFailure);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->eventsMemoryOverflow);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->crashProfileMemoryOverflow);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->otherCriticalFailures);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(diagRes->otherNotCriticalFailures);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->micConnectionFailure);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->micFailure);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->rightSpeakerFailure);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->leftSpeakerFailure);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->speakersFailure);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->ignitionLineFailure);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->uimFailure);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->statusIndicatorFailure);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->batteryFailure);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->batteryVoltageLow);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->crashSensorFailure);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->firmwareImageCorruption);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->commModuleInterfaceFailure);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->gnssReceiverFailure);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->raimProblem);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->gnssAntennaFailure);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->commModuleFailure);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->eventsMemoryOverflow);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->crashProfileMemoryOverflow);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->otherCriticalFailures);
MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(diagRes->otherNotCriticalFailures);
}
if (data->additionalData.crashInfoPresent) {
MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(data->additionalData.crashInfo.CrashFront)
MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(data->additionalData.crashInfo.CrashLeftSide)
MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(data->additionalData.crashInfo.CrashRightSide)
MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(data->additionalData.crashInfo.CrashRear)
MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(data->additionalData.crashInfo.CrashWithRollover)
MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(data->additionalData.crashInfo.CrashSide)
MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(data->additionalData.crashInfo.CrashFrontOrSide)
MSD_TO_RAW_ADD_OPTIONAL_FLAG_PRESENT(data->additionalData.crashInfo.CrashOtherType)
MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(data->additionalData.crashInfo.CrashFront)
MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(data->additionalData.crashInfo.CrashLeftSide)
MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(data->additionalData.crashInfo.CrashRightSide)
MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(data->additionalData.crashInfo.CrashRear)
MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(data->additionalData.crashInfo.CrashWithRollover)
MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(data->additionalData.crashInfo.CrashSide)
MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(data->additionalData.crashInfo.CrashFrontOrSide)
MSD_TO_RAW_ADD_OPTIONAL_FLAG_VALUE(data->additionalData.crashInfo.CrashOtherType)
}
if (data->msdId == MSD_V_2) {
lastBit += vBitBufferAppendByte(raw, lastBit, data->additionalData.coordSystem, 2);
}
return lastBit;
}
uint16_t sEraGlonassMsdEncodeOfAdditionalDataOctetString(
tEraGlonassMsd_DataTypedef *data, unsigned char *raw, uint16_t lastBit
) {
uint16_t octetStringOffset = lastBit + 8;
uint16_t octetStringBitsLength =
sEraGlonassMsdEncodeOfAdditionalDataPayload(data, raw, octetStringOffset) - (octetStringOffset);
uint8_t octetsCount = octetStringBitsLength / 8 + (octetStringBitsLength % 8 ? 1 : 0);
lastBit += vBitBufferAppendByte(raw, lastBit, octetsCount, 8);
lastBit += octetsCount * 8;
return lastBit;
}
uint16_t sEraGlonassMsdEncodeOfAdditionalData(tEraGlonassMsd_DataTypedef *data, unsigned char *raw, uint16_t lastBit) {
lastBit = sEraGlonassMsdEncodeOfAdditionalDataOID(data, raw, lastBit);
lastBit = sEraGlonassMsdEncodeOfAdditionalDataOctetString(data, raw, lastBit);
return lastBit;
}
uint16_t sEraGlonassMsdEncodeOfMsdMainBody(tEraGlonassMsd_DataTypedef *data, unsigned char *raw, uint16_t lastBit) {
lastBit += vBitBufferAppendByte(raw, lastBit, 0b0, 1);
lastBit += vBitBufferAppendByte(raw, lastBit, data->additionalDataPresent ? 0b1 : 0b0, 1);
lastBit += vBitBufferAppendByte(raw, lastBit, 0b0, 1);
// lastBit+= vBitBufferAppendByte(raw,lastBit,data->MSD_Control.extraData,3);
lastBit += vBitBufferAppendByte(raw, lastBit, data->MSD_Control.recentPos1 ? 0x01 : 0x00, 1);
lastBit += vBitBufferAppendByte(raw, lastBit, data->MSD_Control.recentPos2 ? 0x01 : 0x00, 1);
lastBit += vBitBufferAppendByte(raw, lastBit, data->MSD_Control.passengers ? 0x01 : 0x00, 1);
lastBit = sEraGlonassMsdEncodeOfControl(data, raw, lastBit);
lastBit = sEraGlonassMsdEncodeOfVin(data, raw, lastBit);
lastBit = sEraGlonassMsdEncodeOfPropulsion(data, raw, lastBit);
lastBit = sEraGlonassMsdEncodeOfV1FreeSpaceFix(data, raw, lastBit);
lastBit = sEraGlonassMsdEncodeOfTimestamp(data, raw, lastBit);
lastBit = sEraGlonassMsdEncodeOfLocation(data, raw, lastBit);
lastBit = sEraGlonassMsdEncodeOfDirection(data, raw, lastBit);
lastBit = sEraGlonassMsdEncodeOfRecentPositions(data, raw, lastBit);
lastBit = sEraGlonassMsdEncodeOfPassengersCount(data, raw, lastBit);
if (data->additionalDataPresent) {
lastBit = sEraGlonassMsdEncodeOfAdditionalData(data, raw, lastBit);
}
return lastBit;
}
uint16_t sEraGlonassMsdEncodeV2BodyOctetString(tEraGlonassMsd_DataTypedef *data, unsigned char *raw, uint16_t lastBit) {
//length of msd block
uint16_t octetStringOffset = lastBit + 8;
uint16_t octetStringBitsLength =
sEraGlonassMsdEncodeOfMsdMainBody(data, raw, octetStringOffset) - (octetStringOffset);
uint8_t octetsCount = octetStringBitsLength / 8 + (octetStringBitsLength % 8 ? 1 : 0);
lastBit += vBitBufferAppendByte(raw, lastBit, octetsCount, 8);
lastBit += octetsCount * 8;
return lastBit;
}
uint16_t EraGlonassMsdEncode(tEraGlonassMsd *env, eEraGlonassMsd_Version msdVersion, uint8_t *encoded) {
env->MSD_Data.msdId = msdVersion;
memset(encoded, '\0', ERA_GLONASS_MSD_ENCODED_BUFFER_LENGTH); // Clear
uint16_t lastBit = 0;
lastBit += vBitBufferAppendByte(encoded, lastBit, env->MSD_Data.msdId, 8);
if (env->MSD_Data.msdId == MSD_V_1) {
lastBit = sEraGlonassMsdEncodeOfMsdMainBody(&env->MSD_Data, encoded, lastBit);
} else if (env->MSD_Data.msdId == MSD_V_2) {
lastBit = sEraGlonassMsdEncodeV2BodyOctetString(&env->MSD_Data, encoded, lastBit);
}
size_t byteLength = lastBit / 8 + (lastBit % 8 ? 1 : 0);
return byteLength;
}
Reference in New Issue View Git Blame Copy Permalink