130 lines
4.9 KiB
C
130 lines
4.9 KiB
C
#include <math.h>
|
||
#include <stdlib.h>
|
||
#include "EraGlonassUveos.h"
|
||
#include "AsciiStringAssmeblingUtils.h"
|
||
#include "SystemDelayInterface.h"
|
||
|
||
#define LOGGER env->logger
|
||
#define LOG_SIGN "УВЭОС,DO_NOT_DISTAN"
|
||
|
||
uint64_t tim= 0;
|
||
|
||
void EraGlonassUveos_DoNotingTestingStarted(tEraGlonassUveos *env) {
|
||
env->doNothing.mode = UVEOS_DO_NOTHING_TESTING;
|
||
NavDataProvider_GetNavData(env->navDataProvider, &env->doNothing.beginPosition, true);
|
||
}
|
||
|
||
void EraGlonassUveos_GarageStarted(tEraGlonassUveos *env) {
|
||
env->doNothing.mode = UVEOS_DO_NOTHING_GARAGE;
|
||
NavDataProvider_GetNavData(env->navDataProvider, &env->doNothing.beginPosition, true);
|
||
}
|
||
|
||
void EraGlonassUveos_DoNotingReset(tEraGlonassUveos *env) {
|
||
env->doNothing.mode = UVEOS_DO_NOTHING_DISABLED;
|
||
NavDataProvider_GetNavData(env->navDataProvider, &env->doNothing.beginPosition, true);
|
||
}
|
||
|
||
void EraGlonassUveos_DoNotingSetStartPos(tEraGlonassUveos *env) {
|
||
env->doNothing.mode = UVEOS_DO_NOTHING_DISABLED;
|
||
NavDataProvider_GetNavData(env->navDataProvider, &env->doNothing.movePosition, true);
|
||
}
|
||
|
||
uint32_t getAbsOfPosition(EraGlonassUveosNavData *now, tEraGlonassUveos *env, bool flagSet){
|
||
if(flagSet == true) {
|
||
return abs(now->latitude - env->doNothing.beginPosition.latitude);
|
||
} else {
|
||
return abs(now->longitude - env->doNothing.beginPosition.longitude);
|
||
}
|
||
}
|
||
|
||
void EraGlonassUveos_DoNothingModeDistance(tEraGlonassUveos *env, bool sourseNaData) {
|
||
uint32_t latDistance = 0;
|
||
uint32_t lonDistance = 0;
|
||
size_t str_len = 0;
|
||
char step[5] = {0};
|
||
|
||
if (env->doNothing.stopLimitTime < SystemGetMs()) {
|
||
NavDataProvider_GetNavData(env->navDataProvider, &env->doNothing.movePosition, sourseNaData);
|
||
LoggerInfoStatic(LOGGER, LOG_SIGN, "Установлена новая точка отсчёта движения ТС");
|
||
env->doNothing.stopLimitTime = SystemGetMs() + (40 * 1000);
|
||
}
|
||
|
||
EraGlonassUveosNavData movDinst;
|
||
NavDataProvider_GetNavData(env->navDataProvider, &movDinst, sourseNaData);
|
||
if (
|
||
(env->doNothing.mode != UVEOS_DO_NOTHING_GARAGE) &&
|
||
(env->doNothing.mode != UVEOS_DO_NOTHING_TESTING) &&
|
||
(env->doNothing.movePosition.valid > 1)
|
||
) {
|
||
|
||
latDistance = abs(movDinst.latitude - env->doNothing.movePosition.latitude);
|
||
lonDistance = abs(movDinst.longitude - env->doNothing.movePosition.longitude);
|
||
|
||
double x = latDistance * 0.1;
|
||
double y = lonDistance * 0.1;
|
||
double move = sqrt(x * x + y * y);
|
||
env->doNothing.movDist = (uint32_t) move;
|
||
|
||
if ( env->doNothing.movDist > 25) {
|
||
env->doNothing.mode = UVEOS_DO_NOTHING_MOVE_TO;
|
||
} else {
|
||
env->doNothing.mode = UVEOS_DO_NOTHING_DISABLED;
|
||
}
|
||
}
|
||
|
||
///===================
|
||
|
||
EraGlonassUveosNavData now;
|
||
NavDataProvider_GetNavData(env->navDataProvider, &now, sourseNaData);
|
||
if(now.valid > 0) {
|
||
if(env->doNothing.beginPosition.valid<1){
|
||
env->doNothing.beginPosition = now;
|
||
}
|
||
|
||
latDistance = abs(now.latitude - env->doNothing.beginPosition.latitude);
|
||
lonDistance = abs(now.longitude - env->doNothing.beginPosition.longitude);
|
||
|
||
double x = latDistance * 0.1;
|
||
double y = lonDistance * 0.1;
|
||
double move = sqrt( (x * x) + (y * y) );
|
||
uint32_t res = (uint32_t) move;
|
||
|
||
if((env->doNothing.mode == UVEOS_DO_NOTHING_GARAGE) && (tim < SystemGetMs())) {
|
||
vAsciiStringInit(step, &str_len, 5);
|
||
vAsciiStringAddDecimalInt64(step, &str_len, res, 5);
|
||
LoggerInfoStatic(LOGGER, LOG_SIGN, "Текущее отклонение (режим ГАРАЖ) = ");
|
||
LoggerInfo(LOGGER, LOG_SIGN, step, str_len);
|
||
|
||
if (env->settings->GARAGE_MODE_END_DISTANCE < res) {
|
||
env->doNothing.mode = UVEOS_DO_NOTHING_DISABLED;
|
||
}
|
||
tim = SystemGetMs() + 10000;
|
||
}
|
||
|
||
if(env->doNothing.mode == UVEOS_DO_NOTHING_TESTING) {
|
||
vAsciiStringInit(step, &str_len, 5);
|
||
vAsciiStringAddDecimalInt64(step, &str_len, res, 5);
|
||
LoggerInfoStatic(LOGGER, LOG_SIGN, "Текущее отклонение (режим тестирования) = ");
|
||
LoggerInfo(LOGGER, LOG_SIGN, step, str_len);
|
||
if (env->settings->TEST_MODE_END_DISTANCE < res) {
|
||
env->doNothing.mode = UVEOS_DO_NOTHING_DISABLED;
|
||
}
|
||
}
|
||
|
||
if (
|
||
(env->settings->GARAGE_MODE_END_DISTANCE < res) &&
|
||
(env->doNothing.mode = UVEOS_DO_NOTHING_GARAGE)
|
||
) {
|
||
env->doNothing.mode = UVEOS_DO_NOTHING_DISABLED;
|
||
}
|
||
if (
|
||
(env->settings->TEST_MODE_END_DISTANCE < res) &&
|
||
(env->doNothing.mode = UVEOS_DO_NOTHING_TESTING)
|
||
) {
|
||
env->doNothing.mode = UVEOS_DO_NOTHING_DISABLED;
|
||
}
|
||
|
||
env->offsetToDistance = res;
|
||
}
|
||
}
|