99 lines
3.2 KiB
C
99 lines
3.2 KiB
C
//
|
||
// Created by cfif on 16.05.2024.
|
||
//
|
||
#include "CarPositionUpdate.h"
|
||
#include "math.h"
|
||
#include "stdlib.h"
|
||
#include "SystemDelayInterface.h"
|
||
#include "EgtsOutputCommands.h"
|
||
|
||
#define LOG_SIGN "EGTS MOVE"
|
||
#define LOGGER &env->slog->logger
|
||
|
||
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 CarEventResetPosition(tEgtsProcessing *env) {
|
||
env->carEventPosition.carPosition = CAR_POSITION_FIX;
|
||
}
|
||
|
||
void CarEventMovingPosition(tEgtsProcessing *env) {
|
||
env->carEventPosition.carPosition = CAR_POSITION_MOVE;
|
||
|
||
env->carEventPosition.timerMoving = CAR_EVENT_TIME_MOVING;
|
||
env->carEventPosition.isTimerMoving = true;
|
||
}
|
||
|
||
bool isMovingSpeed(tEgtsProcessing *env, uint16_t speed) {
|
||
|
||
for (uint8_t i = 0; i < CAR_POINT_COUNT_SPEED; ++i) {
|
||
if (env->carEventPosition.dataSpeed[i] < speed)
|
||
return false;
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
void CarEventPosition(tEgtsProcessing *env, tNmeaRmc *position, uint16_t totalSpeed) {
|
||
|
||
if (env->carEventPosition.isTimerMoving) {
|
||
if (env->carEventPosition.timerMoving < SystemGetMs()) {
|
||
LoggerInfoStatic(LOGGER, LOG_SIGN, "Сброс признака движения")
|
||
env->carEventPosition.isTimerMoving = false;
|
||
CarEventResetPosition(env);
|
||
}
|
||
}
|
||
|
||
if (position->status != 'A')
|
||
return;
|
||
|
||
if (env->egtsPointEnv.timeOutCheckTeledata_TimeSpeedUpdate < SystemGetMs()) {
|
||
|
||
env->egtsPointEnv.timeOutCheckTeledata_TimeSpeedUpdate = SystemGetMs() + CAR_EVENT_TIME_SPEED;
|
||
|
||
env->carEventPosition.dataSpeed[env->carEventPosition.dataSpeedStep] = totalSpeed;
|
||
++env->carEventPosition.dataSpeedStep;
|
||
|
||
if (env->carEventPosition.dataSpeedStep >= CAR_POINT_COUNT_SPEED) {
|
||
env->carEventPosition.dataSpeedStep = 0;
|
||
}
|
||
}
|
||
|
||
if (isMovingSpeed(env, CAR_TRESHOLD_DETECT_SPEED)) {
|
||
CarEventMovingPosition(env);
|
||
}
|
||
|
||
|
||
// Установка точки отсчета
|
||
if (env->carEventPosition.stopPositionLimitTime < SystemGetMs()) {
|
||
|
||
env->carEventPosition.beginPosition.location.latitude = nmeaLocationToDeg(position->location.latitude);
|
||
env->carEventPosition.beginPosition.location.longitude = nmeaLocationToDeg(position->location.longitude);
|
||
|
||
env->carEventPosition.stopPositionLimitTime = SystemGetMs() + CAR_EVENT_TIME_DISTANCE;
|
||
}
|
||
|
||
env->carEventPosition.movePosition.location.latitude = nmeaLocationToDeg(position->location.latitude);
|
||
env->carEventPosition.movePosition.location.longitude = nmeaLocationToDeg(position->location.longitude);
|
||
|
||
double latDistance = fabs(env->carEventPosition.movePosition.location.latitude -
|
||
env->carEventPosition.beginPosition.location.latitude);
|
||
double lonDistance = fabs(env->carEventPosition.movePosition.location.longitude -
|
||
env->carEventPosition.beginPosition.location.longitude);
|
||
|
||
double x = latDistance * 0.1;
|
||
double y = lonDistance * 0.1;
|
||
double move = sqrt(x * x + y * y);
|
||
|
||
if ((uint32_t) move > CAR_EVENT_DISTANCE) {
|
||
CarEventMovingPosition(env);
|
||
}
|
||
|
||
} |