Skip to content
This repository was archived by the owner on Feb 16, 2024. It is now read-only.

Commit 1090ffc

Browse files
RazicanRazican
committed
Merge pull request #82 from OpenStratos/feature/new-gps
New GPS script. This fixes #77 for the new GPS. We still have some issues to fix, but this was probably the most extensive one.
2 parents 4bce425 + 419bd31 commit 1090ffc

6 files changed

Lines changed: 237 additions & 0 deletions

File tree

‎gps/GPS.cc‎

Lines changed: 211 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -88,6 +88,9 @@ bool GPS::initialize()
8888
}
8989
this->logger->log("Serial connection started.");
9090

91+
this->logger->log("Setting GPS to pedestrian mode");
92+
this->enter_pedestrian_mode();
93+
9194
this->logger->log("Starting GPS frame thread...");
9295
thread t(&GPS::gps_thread, this);
9396
t.detach();
@@ -338,3 +341,211 @@ void GPS::parse_RMC(const string& frame)
338341
this->velocity.course = stof(s_data[8]);
339342
}
340343
}
344+
345+
void GPS::enter_airborne_1g_mode()
346+
{
347+
int gps_dynamic_model_set_success = 0;
348+
struct timeval time_now, time_start;
349+
long ms_now, ms_start;
350+
351+
unsigned char setdm6[] = {
352+
0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x06, //Byte at offset 2
353+
0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, //determines new
354+
0x05, 0x00, 0xFA, 0x00, 0xFA, 0x00, 0x64, 0x00, 0x2C, //operation mode.
355+
0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
356+
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x16, 0xDC
357+
};
358+
uint8_t sz_setdm6 = 44;
359+
360+
gettimeofday(&time_start, NULL);
361+
ms_start = (long)((time_start.tv_sec)*1000 + (time_start.tv_usec)/1000);
362+
ms_now = ms_start;
363+
364+
while(!gps_dynamic_model_set_success && (ms_now - ms_start)<6000) //Prevent lock and
365+
{ //timeout if not set
366+
gettimeofday(&time_now, NULL); //after six seconds
367+
ms_now = (long)((time_now.tv_sec)*1000 + (time_now.tv_usec)/1000);
368+
369+
this->send_ublox_packet(setdm6, sz_setdm6);
370+
gps_dynamic_model_set_success = this->receive_check_ublox_ack(setdm6);
371+
}
372+
if (gps_dynamic_model_set_success)
373+
{
374+
this->logger->log("GPS entered airborne (<1g) mode successfully");
375+
}
376+
else
377+
{
378+
this->logger->log("GPS failed to enter airborne (<1g) mode");
379+
}
380+
}
381+
382+
void GPS::enter_stationary_mode()
383+
{
384+
int gps_dynamic_model_set_success = 0;
385+
struct timeval time_now, time_start;
386+
long ms_now, ms_start;
387+
388+
unsigned char setdm2[] = {
389+
0xB5, 0x62, 0x02, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x06, //Byte at offset 2
390+
0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, //determines new
391+
0x05, 0x00, 0xFA, 0x00, 0xFA, 0x00, 0x64, 0x00, 0x2C, //operation mode.
392+
0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
393+
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x16, 0xDC
394+
};
395+
uint8_t sz_setdm2 = 44;
396+
397+
gettimeofday(&time_start, NULL);
398+
ms_start = (long)((time_start.tv_sec)*1000 + (time_start.tv_usec)/1000);
399+
ms_now = ms_start;
400+
401+
while(!gps_dynamic_model_set_success && (ms_now - ms_start)<6000) //Prevent lock and
402+
{ //timeout if not set
403+
gettimeofday(&time_now, NULL); //after six seconds
404+
ms_now = (long)((time_now.tv_sec)*1000 + (time_now.tv_usec)/1000);
405+
406+
this->send_ublox_packet(setdm2, sz_setdm2);
407+
gps_dynamic_model_set_success = this->receive_check_ublox_ack(setdm2);
408+
}
409+
410+
if (gps_dynamic_model_set_success)
411+
{
412+
this->logger->log("GPS entered stationary mode successfully");
413+
}
414+
else
415+
{
416+
this->logger->log("GPS failed to enter stationary mode");
417+
}
418+
}
419+
420+
void GPS::enter_pedestrian_mode()
421+
{
422+
int gps_dynamic_model_set_success = 0;
423+
struct timeval time_now, time_start;
424+
long ms_now, ms_start;
425+
426+
unsigned char setdm3[] = {
427+
0xB5, 0x62, 0x03, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x06, //Byte at offset 2
428+
0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, //determines new
429+
0x05, 0x00, 0xFA, 0x00, 0xFA, 0x00, 0x64, 0x00, 0x2C, //operation mode.
430+
0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
431+
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x16, 0xDC
432+
};
433+
uint8_t sz_setdm3 = 44;
434+
435+
gettimeofday(&time_start, NULL);
436+
ms_start = (long)((time_start.tv_sec)*1000 + (time_start.tv_usec)/1000);
437+
ms_now = ms_start;
438+
439+
while(!gps_dynamic_model_set_success && (ms_now - ms_start)<6000) //Prevent lock and
440+
{ //timeout if not set
441+
gettimeofday(&time_now, NULL); //after six seconds
442+
ms_now = (long)((time_now.tv_sec)*1000 + (time_now.tv_usec)/1000);
443+
444+
this->send_ublox_packet(setdm3, sz_setdm3);
445+
gps_dynamic_model_set_success = this->receive_check_ublox_ack(setdm3);
446+
}
447+
448+
if (gps_dynamic_model_set_success)
449+
{
450+
this->logger->log("GPS entered pedestrian mode successfully");
451+
}
452+
else
453+
{
454+
this->logger->log("GPS failed to enter pedestrian mode");
455+
}
456+
}
457+
458+
459+
void GPS::send_ublox_packet(unsigned char *message, uint8_t len)
460+
{
461+
if (this->serial->is_open())
462+
{
463+
this->serial->flush();
464+
this->serial->write((unsigned char)0xFF);
465+
this_thread::sleep_for(500ms);
466+
for (unsigned int i = 0; i<len; i++){
467+
this->serial->write(message[i]);
468+
}
469+
}else
470+
{
471+
this->logger->log("Tried to send a packet, but the serial is closed");
472+
}
473+
}
474+
475+
bool GPS::receive_check_ublox_ack(unsigned char *message)
476+
{
477+
unsigned char ack_packet[10];
478+
unsigned int bytes_ordered;
479+
unsigned char byte;
480+
struct timeval time_now, time_start;
481+
long ms_now, ms_start;
482+
483+
ack_packet[0] = 0xB5;
484+
ack_packet[1] = 0x62;
485+
ack_packet[2] = 0x05;
486+
ack_packet[3] = 0x01;
487+
ack_packet[4] = 0x02;
488+
ack_packet[5] = 0x00;
489+
ack_packet[6] = message[2];
490+
ack_packet[7] = message[3];
491+
ack_packet[8] = 0x00;
492+
ack_packet[9] = 0x00;
493+
494+
for (unsigned int i = 0; i<8; i++){
495+
ack_packet[8]+= ack_packet[i];
496+
ack_packet[9]+= ack_packet[8];
497+
}
498+
bytes_ordered = 0;
499+
500+
gettimeofday(&time_start, NULL);
501+
ms_start = (long)((time_start.tv_sec)*1000 + (time_start.tv_usec)/1000);
502+
ms_now = ms_start;
503+
504+
while (ms_now - ms_start <= 3000){ //Avoid lock by
505+
gettimeofday(&time_now, NULL); //timing out after
506+
ms_now = (long)((time_now.tv_sec)*1000 + (time_now.tv_usec)/1000); //3s
507+
508+
if (bytes_ordered > 9)
509+
{
510+
return true;
511+
}
512+
513+
if (this->serial->available())
514+
{
515+
byte = (unsigned char)this->serial->read_char();
516+
if (byte == ack_packet[bytes_ordered])
517+
{
518+
bytes_ordered++;
519+
}
520+
else
521+
{
522+
bytes_ordered = 0;
523+
}
524+
}
525+
}
526+
return false;
527+
}
528+
529+
void GPS::notify_initialization()
530+
{
531+
this->logger->log("Initialization notified. Switching to pedestrian mode");
532+
this->enter_pedestrian_mode();
533+
}
534+
535+
void GPS::notify_takeoff()
536+
{
537+
this->logger->log("Takeoff notified. Switching to airborne mode");
538+
this->enter_airborne_1g_mode();
539+
}
540+
541+
void GPS::notify_safe_mode()
542+
{
543+
this->logger->log("Safe mode entry notified. Switching to airborne mode");
544+
this->enter_airborne_1g_mode();
545+
}
546+
547+
void GPS::notify_landing()
548+
{
549+
this->logger->log("Landing notified. Switching to stationary mode");
550+
this->enter_stationary_mode();
551+
}

‎gps/GPS.h‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -48,6 +48,13 @@ namespace os {
4848
void parse_GSA(const string& frame);
4949
void parse_RMC(const string& frame);
5050

51+
void enter_pedestrian_mode(); /*Before launch*/
52+
void enter_airborne_1g_mode(); /*While in flight*/
53+
void enter_stationary_mode(); /*When landed*/
54+
55+
void send_ublox_packet(unsigned char *, uint8_t);
56+
bool receive_check_ublox_ack(unsigned char*);
57+
5158
public:
5259
GPS(GPS& copy) = delete;
5360
~GPS();
@@ -69,6 +76,11 @@ namespace os {
6976
bool turn_on() const;
7077
bool turn_off() const;
7178
void parse(const string& frame);
79+
80+
void notify_takeoff();
81+
void notify_landing();
82+
void notify_initialization();
83+
void notify_safe_mode();
7284
};
7385
}
7486

‎logic/initialize.cc‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -37,6 +37,10 @@ if ( ! GPS::get_instance().initialize())
3737
}
3838
logger.log("GPS initialized.");
3939

40+
//Notify the GPS about its initialization, so that it sets itself to the appropriate mode
41+
logger.log("Notifying GPS about initialization...");
42+
GPS::get_instance().notify_initialization();
43+
4044
logger.log("Initializing GSM...");
4145
if ( ! GSM::get_instance().initialize())
4246
{

‎logic/land.cc‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,9 @@ if ( ! Camera::get_instance().stop())
44
else
55
logger->log("Video stopped.");
66

7+
logger->log("Notifying GPS about landing...");
8+
GPS::get_instance().notify_landing();
9+
710
logger->log("Waiting 1 minute before sending landed SMS...");
811
this_thread::sleep_for(1min);
912

‎logic/safe_mode.cc‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -161,6 +161,10 @@ switch (last_state)
161161
}
162162

163163
logger->log("GPS fix acquired.");
164+
165+
logger->log("Notifying GPS about entering safe mode...");
166+
GPS::get_instance().notify_safe_mode();
167+
164168
this_thread::sleep_for(5s);
165169
for (int i = 0;
166170
GPS::get_instance().get_HDOP() > 5 && i < 10;

‎logic/wait_launch.cc‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -11,3 +11,6 @@ while ( ! has_launched(launch_altitude))
1111
this_thread::sleep_for(1s);
1212

1313
logger->log("Balloon launched.");
14+
15+
logger->log("Notifying GPS about launch...");
16+
GPS::get_instance().notify_takeoff();

0 commit comments

Comments
 (0)