int nmea_igen_pos_rmove_loop(nmeaGENERATOR *gen, nmeaINFO *info)
{
nmeaPOS crd;
info->direction += nmea_random(-10, 10);
info->speed += nmea_random(-2, 3);
if(info->direction < 0)
info->direction = 359 + info->direction;
if(info->direction > 359)
info->direction -= 359;
if(info->speed > 40)
info->speed = 40;
if(info->speed < 1)
info->speed = 1;
nmea_info2pos(info, &crd);
nmea_move_horz(&crd, &crd, info->direction, info->speed / 3600);
nmea_pos2info(&crd, info);
info->declination = info->direction;
return 1;
};
/*
portTASK_FUNCTION(gps, pvParameters ) {
vTaskDelay(500);
awal_gps();
//ser2_putstring("Masuk serial 2\r\n");
vTaskDelay(100);
int hasilGPS=0;
for(;;) {
vTaskDelay(2);
//if (len=serPollGPS())
{
if (jgps<2) {
hasilGPS = proses_gps();
#ifdef DEBUG_GPSs
info_gps(hasilGPS);
#endif
}
}
#ifdef DEBUG_GPSs
vTaskDelay(3000);
#endif
}
deinit_gps();
printf("akhir GPS !!\r\n");
}
//*/
int proses_gps() {
int parsenya=0;
parsenya = nmea_parse(&parserGPS, pesanGPS, (int)strlen(pesanGPS), &infoGPS);
//printf("serPoll : %d :%3d: %s\r\n", parsenya, (int)strlen(pesanGPS), pesanGPS);
nmea_info2pos(&infoGPS, &dposGPS);
//info_gps(parsenya);
return parsenya;
}
int main()
{
static const char * filename = "../../samples/parse_file/gpslog.txt";
nmeaINFO info;
nmeaPARSER parser;
FILE *file;
char buff[2048];
int size, it = 0;
nmeaPOS dpos;
file = fopen(filename, "rb");
if(!file) {
printf("Could not open file %s\n", filename);
return -1;
}
nmea_property()->trace_func = &trace;
nmea_property()->error_func = &error;
nmea_zero_INFO(&info);
nmea_parser_init(&parser);
/*
while(1)
{
*/
while(!feof(file))
{
size = (int)fread(&buff[0], 1, 100, file);
nmea_parse(&parser, &buff[0], size, &info);
nmea_info2pos(&info, &dpos);
printf(
"%03d, Lat: %f, Lon: %f, Sig: %d, Fix: %d\n",
it++, dpos.lat, dpos.lon, info.sig, info.fix
);
}
fseek(file, 0, SEEK_SET);
/*
}
*/
nmea_parser_destroy(&parser);
fclose(file);
return 0;
}
void MainWindow::gpsTimer()
{
if (m_info.speed > 0) {
m_lines.last().end();
}
double total_distance = 0.0;
foreach(TravelLine line, m_lines) {
// Get the new position
_nmeaPOS startPos;
_nmeaPOS endPos;
// First get the start position
nmea_info2pos(&m_info, &startPos);
// Calculate the new position based on the bearing and distance
nmea_move_horz(&startPos, &endPos, line.bearing(), line.distance() / 1000.0);
// Update the info struct
nmea_pos2info(&endPos, &m_info);
total_distance += line.distance();
}
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)
}
