int IsPosIn_Circle(Circle_Struct *circle, GpsInfo *pGps)
{
double distance;
Circle_PosNormalize(circle, pGps);
distance = nmea_distance(&Center, &CurPoint);
#ifdef _debug_
fprintf(stderr, "distance = %f\n",distance);
fprintf(stderr, "distance = %d\n",circle->Radius);
#endif
if((distance - circle->Radius) <= 0.0){
return 1;
}
return 0;
}
int main()
{
const char *buff[] = {
"$GPRMC,213916.199,A,4221.0377,N,07102.9778,W,0.00,,010207,,,A*6A\r\n",
"$GPRMC,213917.199,A,4221.0510,N,07102.9549,W,0.23,175.43,010207,,,A*77\r\n",
"$GPRMC,213925.000,A,4221.1129,N,07102.9146,W,0.00,,010207,,,A*68\r\n",
"$GPRMC,111609.14,A,5001.27,N,3613.06,E,11.2,0.0,261206,0.0,E*50\r\n"
};
nmeaPOS pos[NUM_POINTS], pos_moved[NUM_POINTS][2];
double dist[NUM_POINTS][2];
double azimuth[NUM_POINTS][2], azimuth_moved[NUM_POINTS];
int result[2];
int it = 0;
nmeaPARSER parser;
nmea_parser_init(&parser);
for(it = 0; it < NUM_POINTS; ++it)
{
nmeaINFO info;
nmea_zero_INFO(&info);
(void)nmea_parse(&parser, buff[it], (int)strlen(buff[it]), &info);
nmea_info2pos(&info, &pos[it]);
}
nmea_parser_destroy(&parser);
for(it = 0; it < NUM_POINTS; ++it)
{
dist[it][0] = nmea_distance(&pos[0], &pos[it]);
dist[it][1] = nmea_distance_ellipsoid(
&pos[0], &pos[it], &azimuth[it][0], &azimuth[it][1]
);
}
for(it = 0; it < NUM_POINTS; ++it)
{
result[0] = nmea_move_horz(&pos[0], &pos_moved[it][0], azimuth[it][0], dist[it][0]);
result[1] = nmea_move_horz_ellipsoid(
&pos[0], &pos_moved[it][1], azimuth[it][0], dist[it][0], &azimuth_moved[it]
);
}
/* Output of results */
printf("Coordinate points:\n");
for(it = 0; it < NUM_POINTS; ++it)
{
printf(
"P%d in radians: lat:%9.6lf lon:%9.6lf \tin degree: lat:%+010.6lf° lon:%+011.6lf°\n",
it, pos[it].lat, pos[it].lon, nmea_radian2degree(pos[it].lat), nmea_radian2degree(pos[it].lon)
);
}
printf("\nCalculation results:\n");
for(it = 0; it < NUM_POINTS; ++it)
{
printf("\n");
printf("Distance P0 to P%d\ton spheroid: %14.3lf m\n", it, dist[it][0]);
printf("Distance P0 to P%d\ton ellipsoid: %14.3lf m\n", it, dist[it][1]);
printf("Azimuth P0 to P%d\tat start: %8.3lf°\tat end: %8.3lf°\n", it, nmea_radian2degree(azimuth[it][0]), nmea_radian2degree(azimuth[it][1]));
printf("Move P0 to P%d\t \tAzimuth at end: %8.3lf°\n", it, nmea_radian2degree(azimuth_moved[it]));
printf("Move P0 to P%d\ton spheroid: %3s lat:%+010.6lf° lon:%+011.6lf°\n", it, result[0] == 1 ? "OK" : "nOK", nmea_radian2degree(pos_moved[it][0].lat), nmea_radian2degree(pos_moved[it][0].lon));
printf("Move P0 to P%d\ton ellipsoid: %3s lat:%+010.6lf° lon:%+011.6lf°\n", it, result[0] == 1 ? "OK" : "nOK", nmea_radian2degree(pos_moved[it][1].lat), nmea_radian2degree(pos_moved[it][1].lon));
printf("Move P0 to P%d\toriginal: lat:%+010.6lf° lon:%+011.6lf°\n", it, nmea_radian2degree(pos[it].lat), nmea_radian2degree(pos[it].lon));
}
return 0;
}
int main(void)
{
//***********************************DEBUG********************************************/
#if 0
nmeaPOS p1,p2;
nmeaINFO info;
info.lat = 5547.1206;
info.lon = 4906.2111;
nmea_info2pos(&info, &p1);
info.lat = 5547.1221;
info.lon = 4906.208;
nmea_info2pos(&info, &p2);
m += 23;
u32 t = nmea_distance(&p1, &p2);
if(m)
#endif
//***********************************END OF DEBUG********************************************/
NVIC_Configuration(); //for all peripheria
if (SysTick_Config(SystemCoreClock / 1000)) //1ms
{
/* Capture error */
while (1);
}
Delay(500);
USBIniPin();
signUSBMass = USBDetectPin();
signUSBMass = 0; //deb
#if not defined (VER_3)
USBCommonIni();
#endif
if(signUSBMass)
{
#if defined (VER_3)
USBCommonIni();
#endif
while (bDeviceState != CONFIGURED);
}
else //if(!signUSBMass)
{
/* Flash unlock */
FLASH_Unlock();
/* Clear All pending flags */
FLASH_ClearFlag(FLASH_FLAG_BSY | FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR);
//***********************************DEBUG********************************************/
//***********************************END OF DEBUG********************************************/
/* Enable CRC clock */
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE);
ledDioGPIOInit();
led_dn(BOARD_LED_ON);
led_mid(BOARD_LED_ON);
#if defined (VER_3)
led_up(BOARD_LED_ON);
ibuttonInit();
rfmodemInit();
#endif
Delay(1000);
alarmInit();
BKPInit();
//timer6Init();
//rs485Init();
#if not defined (VER_3)
ais326dq_init();
#endif
//ais326dq_data(&ais326dq_out);
/*ADC*/
adcInit();
/*GPS*/
gpsInit();
/* reading settings */
readConfig();
/*MODEM*/
gprsModemInit();
gprsModemOn(innerState.activeSIMCard);
//***********************************DEBUG********************************************/
//GSMSpeaker(1);
//***********************************END OF DEBUG********************************************/
#ifndef BRIDGE_USB_GSM
setupGSM();
ftpGSMPrepare();
packetsIni();
#endif
led_dn(BOARD_LED_OFF);
led_mid(BOARD_LED_OFF);
#if defined (VER_3)
led_up(BOARD_LED_OFF);
#endif
rtc_init();
rtc_gettime(&rtc);
#if 1 /* WATCH DOG */
/* IWDG timeout equal to 3.27 sec (the timeout may varies due to LSI frequency dispersion) */
/* Enable write access to IWDG_PR and IWDG_RLR registers */
IWDG_WriteAccessCmd(IWDG_WriteAccess_Enable);
/* IWDG counter clock: 40KHz(LSI) / 32 = 1.25 KHz */
IWDG_SetPrescaler(IWDG_Prescaler_64); //32
/* Set counter reload value to 0xFFF */
IWDG_SetReload(0xFFF);
/* Reload IWDG counter */
IWDG_ReloadCounter();
/* Enable IWDG (the LSI oscillator will be enabled by hardware) */
IWDG_Enable();
setTimerIWDG(ONE_SEC);
#endif
initSD();
#if defined (VER_3)
//DACInit();
#endif
/* Log */
saveSDInfo((u8 *)"TURN ON BLOCK ",strlen((u8 *)"TURN ON BLOCK "), SD_SENDED, SD_TYPE_MSG_LOG );
//saveSDInfo((u8 *)readRTCTime(&rtc),strlen((const char *)readRTCTime(&rtc)), SD_SENDED, SD_TYPE_MSG_LOG );
#if defined (VER_3)
//DACSpeaker(1);
//wp_play("0:/sound.wav");
//DACSpeaker(0);
#endif
} //if(!signUSBMass)
while (1)
{
if(!signUSBMass)
{
monitorWatchDog();
#ifndef BRIDGE_USB_GPS
if(!innerState.bootFTPStarted)
gpsHandling();
#endif
#ifndef BRIDGE_USB_GSM
if(!innerState.flagTmpDebug)
loopGSM();
loopFTP();
UpdatingFlash();
if(!innerState.bootFTPStarted)
naviPacketHandle();
rcvPacketHandle();
rcvPacketHandleUSB();
#endif
#if !defined (VER_3)
buttonScan();
accelScan();
#endif
//rs485Analyse();
handleFOS();
executeDelayedCmd();
#if defined (VER_3)
#if 0
if(innerState.flagDebug)
{
DACSpeaker(1);
/* Start DAC Channel1 conversion by software */
//a += 300;
//DAC_SetChannel1Data(DAC_Align_12b_R, 4000);
//DAC_SetChannel1Data(DAC_Align_12b_L, a); //for saw
//DAC_SetChannel1Data(DAC_Align_8b_R, a);
//DAC_SetChannel1Data(DAC_Align_12b_R, 4095);
//DAC_SetChannel1Data(DAC_Align_12b_R, 0);
//for (a = 0; a<4095; ++a)
//for(;;)
// DAC_SetChannel1Data(DAC_Align_12b_R, 0);
//DAC_SetChannel1Data(DAC_Align_12b_R, 0);
//for ( ; ; )
//{
// DAC_SoftwareTriggerCmd(DAC_Channel_1, ENABLE);
//}
DAC_SoftwareTriggerCmd(DAC_Channel_1, ENABLE); //debugga
}
else
{
DACSpeaker(0);
}
#endif
#endif
}
else
handleUSBPresent();
} //while(1)
}
