224 lines
8.4 KiB
C
224 lines
8.4 KiB
C
//
|
|
// Created by cfif on 15.10.2024.
|
|
//
|
|
#include <stdlib.h>
|
|
#include "CanMain.h"
|
|
#include "SystemDelayInterface.h"
|
|
#include "ext_telematica.h"
|
|
#include "string.h"
|
|
#include "Rtc.h"
|
|
#include "EgtsTimestamp.h"
|
|
#include "math.h"
|
|
|
|
#define LOG_SIGN "UDS_MUTE"
|
|
#define LOGGER &env->slog->logger
|
|
|
|
extern eDeviceTestingCode deviceTestingCodeCan;
|
|
|
|
void CanMainMute_Init(
|
|
tCanMainMute *env,
|
|
tSerialPortFrameIO *CanIO,
|
|
SystemMutexCmsis txAccessQueue,
|
|
tGsmWithGnss *gsm,
|
|
tLoggerToSerialPort *slog
|
|
) {
|
|
env->CanIO = CanIO;
|
|
env->txAccessQueue = txAccessQueue;
|
|
env->gsm = gsm;
|
|
env->slog = slog;
|
|
|
|
memset(env->GNSS_NMEA1, 0, sizeof(env->GNSS_NMEA1));
|
|
memset(env->GNSS_NMEA2, 0, sizeof(env->GNSS_NMEA2));
|
|
memset(env->GNSS_NMEA3, 0, sizeof(env->GNSS_NMEA3));
|
|
memset(env->GNSS_NMEA4, 0, sizeof(env->GNSS_NMEA4));
|
|
memset(env->GNSS_NMEA5, 0, sizeof(env->GNSS_NMEA5));
|
|
memset(env->GNSS_NMEA6, 0, sizeof(env->GNSS_NMEA6));
|
|
|
|
InitThreadBlock(env->T_can_MainMute, "CanMainMute", osPriorityNormal);
|
|
InitThreadBlock(env->T_can_MainNav, "CanMainNav", osPriorityNormal);
|
|
}
|
|
|
|
static double nmeaLocationToDeg(double dec) {
|
|
double _dec = dec;
|
|
int deg = (int) (_dec / 100);
|
|
int min = (int) (_dec) - (deg * 100);
|
|
|
|
double sec = (double) (_dec - min - 100 * deg) * 60.0;
|
|
|
|
return deg + min / 60.0 + sec / 3600.0;
|
|
}
|
|
|
|
void CanMainTaskNav(tCanMainMute *env) {
|
|
uint16_t sent = 0;
|
|
uint8_t countCycle = 0;
|
|
uint8_t lcCounter = 0;
|
|
|
|
for (;;) {
|
|
|
|
if (!EXT_ENV_TELE.store.device->factoryMode) {
|
|
if ((EXT_ENV_TELE.DataToCan != NULL) && (EXT_ENV_TELE.DataToCan->nav_data_ToCan == true)) {
|
|
|
|
deviceTestingCodeCan = DEVICE_TESTING_CODE_NOT_ACTIVE;
|
|
|
|
//начало-----------------------------------НАВИГАЦИЯ----------------------------------------------------
|
|
tGNSS_NMEA1 *GNSS_NMEA1 = (tGNSS_NMEA1 *) env->GNSS_NMEA1;
|
|
tGNSS_NMEA2 *GNSS_NMEA2 = (tGNSS_NMEA2 *) env->GNSS_NMEA2;
|
|
tGNSS_NMEA3 *GNSS_NMEA3 = (tGNSS_NMEA3 *) env->GNSS_NMEA3;
|
|
|
|
time_t timestamp = 0;
|
|
RtcGet(env->gsm->Rtc, ×tamp);
|
|
tNmeaRmc nmeaRmc;
|
|
Gnss_GetFullNavData(env->gsm, &nmeaRmc);
|
|
int32_t alt = GnssGgaGetAlt(env->gsm);
|
|
uint8_t countSats = GnssGgaGetSat(env->gsm);
|
|
|
|
GNSS_NMEA1->NTM = Egts_EpochTimestampToEgtsTime(timestamp);
|
|
double lat = (fabs(nmeaLocationToDeg(nmeaRmc.location.latitude)) / 90) * 0xFFFFFFFF;
|
|
GNSS_NMEA1->LAT = (uint32_t) lat;
|
|
|
|
double lon = (fabs(nmeaLocationToDeg(nmeaRmc.location.longitude)) / 90) * 0xFFFFFFFF;
|
|
GNSS_NMEA2->LONG = (uint32_t) lon;
|
|
GNSS_NMEA2->FLG.VLD = (nmeaRmc.status == 'A') ? 1 : 0;
|
|
GNSS_NMEA2->FLG.LAHS = (nmeaRmc.location.nsIndicator == 'S') ? 1 : 0;
|
|
GNSS_NMEA2->FLG.LOHS = (nmeaRmc.location.ewIndicator == 'W') ? 1 : 0;
|
|
GNSS_NMEA2->FLG.ALTH = 1;
|
|
GNSS_NMEA2->FLG.SATH = 1;
|
|
|
|
uint16_t speed = (uint16_t) (nmeaRmc.knotVelocity * 1.82 * 10);
|
|
GNSS_NMEA2->SPD.SPD = speed & 0b0011111111111111;
|
|
|
|
if (alt >= 0) {
|
|
GNSS_NMEA2->SPD.ALTS = 0;
|
|
} else {
|
|
GNSS_NMEA2->SPD.ALTS = 1;
|
|
}
|
|
|
|
uint16_t dir = (uint16_t) nmeaRmc.headingAngle;
|
|
GNSS_NMEA2->SPD.DIRH = dir >> 8;
|
|
|
|
GNSS_NMEA2->DIR = dir;
|
|
|
|
GNSS_NMEA3->ALT = abs(alt);
|
|
GNSS_NMEA3->SAT = countSats;
|
|
|
|
|
|
if (osMutexAcquire(env->txAccessQueue, 3000) == osOK) {
|
|
|
|
CanSerialPortFrameSetId(env->CanIO->env, 0x5A4);
|
|
sent = env->CanIO->transmit(env->CanIO->env, env->GNSS_NMEA1, sizeof(env->GNSS_NMEA1),
|
|
WAIT_FRAME_WRITE);
|
|
|
|
CanSerialPortFrameSetId(env->CanIO->env, 0x5A6);
|
|
sent = env->CanIO->transmit(env->CanIO->env, env->GNSS_NMEA2, sizeof(env->GNSS_NMEA2),
|
|
WAIT_FRAME_WRITE);
|
|
|
|
CanSerialPortFrameSetId(env->CanIO->env, 0x5A8);
|
|
sent = env->CanIO->transmit(env->CanIO->env, env->GNSS_NMEA3, sizeof(env->GNSS_NMEA3),
|
|
WAIT_FRAME_WRITE);
|
|
|
|
CanSerialPortFrameSetId(env->CanIO->env, 0x5AA);
|
|
sent = env->CanIO->transmit(env->CanIO->env, env->GNSS_NMEA4, sizeof(env->GNSS_NMEA4),
|
|
WAIT_FRAME_WRITE);
|
|
|
|
CanSerialPortFrameSetId(env->CanIO->env, 0x5AC);
|
|
sent = env->CanIO->transmit(env->CanIO->env, env->GNSS_NMEA5, sizeof(env->GNSS_NMEA5),
|
|
WAIT_FRAME_WRITE);
|
|
|
|
CanSerialPortFrameSetId(env->CanIO->env, 0x5AE);
|
|
sent = env->CanIO->transmit(env->CanIO->env, env->GNSS_NMEA6, sizeof(env->GNSS_NMEA6),
|
|
WAIT_FRAME_WRITE);
|
|
|
|
osMutexRelease(env->txAccessQueue);
|
|
} else {
|
|
LoggerInfoStatic(LOGGER, LOG_SIGN, "Ошибка доступа CanMainTaskNav (1)")
|
|
}
|
|
|
|
//конец------------------------------------НАВИГАЦИЯ----------------------------------------------------
|
|
|
|
SystemDelayMs(50);
|
|
++countCycle;
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
void CanMainTaskMute(tCanMainMute *env) {
|
|
uint16_t sent = 0;
|
|
uint8_t countCycle = 0;
|
|
uint8_t lcCounter = 0;
|
|
for (;;) {
|
|
if (!EXT_ENV_TELE.store.device->factoryMode) {
|
|
// if( (EXT_ENV_TELE.DataToCan != NULL) && (EXT_ENV_TELE.DataToCan->gnss_general_ToCan == true) ) {
|
|
//начало-----------------------------------ТИШИНА-------------------------------------------------------
|
|
if (osMutexAcquire(env->txAccessQueue, 3000) == osOK) {
|
|
|
|
CanSerialPortFrameSetId(env->CanIO->env, 0x640);
|
|
|
|
if (lcCounter >= 15) {
|
|
lcCounter = 0;
|
|
} else {
|
|
++lcCounter;
|
|
}
|
|
EXT_ENV_TELE.GNSS_General->GNSS_LC = lcCounter;
|
|
|
|
tGNSS_General GNSS_General;
|
|
memcpy(&GNSS_General, EXT_ENV_TELE.GNSS_General, sizeof(tGNSS_General));
|
|
|
|
if (EXT_ENV_TELE.GNSS_General->GNSS_Call_Auto == CAN_GNSS_ROLLOVER_DETECT) {
|
|
GNSS_General.GNSS_Call_By_button = CAN_GNSS_CALL;
|
|
}
|
|
|
|
// if (EXT_ENV_TELE.GNSS_General->GNSS_Call_Test == CAN_GNSS_CALL_TEST_ACTIVE) {
|
|
// GNSS_General.GNSS_Call_By_button = CAN_GNSS_CALL;
|
|
// }
|
|
|
|
// GNSS_General.GNSS_Call_Test = 0;
|
|
GNSS_General.GNSS_Call_Auto = 0;
|
|
GNSS_General.Reserved1 = 0;
|
|
GNSS_General.Reserved2 = 0;
|
|
|
|
sent = env->CanIO->transmit(env->CanIO->env, (uint8_t *) &GNSS_General, 2,
|
|
WAIT_FRAME_WRITE);
|
|
|
|
osMutexRelease(env->txAccessQueue);
|
|
} else {
|
|
LoggerInfoStatic(LOGGER, LOG_SIGN, "Ошибка доступа CanMainTaskMute (1)")
|
|
}
|
|
//конец-----------------------------------ТИШИНА-------------------------------------------------------
|
|
|
|
SystemDelayMs(100);
|
|
++countCycle;
|
|
// }
|
|
} else {
|
|
env->isPassedTestCan = false;
|
|
|
|
if (osMutexAcquire(env->txAccessQueue, 1000) == osOK) {
|
|
|
|
CanSerialPortFrameSetId(env->CanIO->env, 0x88);
|
|
sent = env->CanIO->transmit(env->CanIO->env, (uint8_t *) canTestData, 8,
|
|
WAIT_FRAME_WRITE);
|
|
|
|
osMutexRelease(env->txAccessQueue);
|
|
} else {
|
|
LoggerInfoStatic(LOGGER, LOG_SIGN, "Ошибка доступа CanMainTaskMute (2)")
|
|
}
|
|
|
|
SystemDelayMs(100);
|
|
|
|
if (env->isPassedTestCan) {
|
|
deviceTestingCodeCan = DEVICE_TESTING_CODE_PASSED;
|
|
} else {
|
|
deviceTestingCodeCan = DEVICE_TESTING_CODE_ERROR;
|
|
}
|
|
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
void CanMainMute_Start(tCanMainMute *env) {
|
|
ThreadBlock_Start(env->T_can_MainMute, env, CanMainTaskMute);
|
|
ThreadBlock_Start(env->T_can_MainNav, env, CanMainTaskNav);
|
|
} |