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

Commit

Permalink
Merge pull request #82 from OpenStratos/feature/new-gps
Browse files Browse the repository at this point in the history 
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.
  • Loading branch information
@Razican
Razican committed May 20, 2016
2 parents 4bce425 + 419bd31 commit 1090ffc
Show file tree
Hide file tree
Showing 6 changed files with 237 additions and 0 deletions.
211 changes: 211 additions & 0 deletions gps/GPS.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -88,6 +88,9 @@ bool GPS::initialize()
}
this->logger->log("Serial connection started.");

this->logger->log("Setting GPS to pedestrian mode");
this->enter_pedestrian_mode();

this->logger->log("Starting GPS frame thread...");
thread t(&GPS::gps_thread, this);
t.detach();
Expand Down Expand Up @@ -338,3 +341,211 @@ void GPS::parse_RMC(const string& frame)
this->velocity.course = stof(s_data[8]);
}
}

void GPS::enter_airborne_1g_mode()
{
int gps_dynamic_model_set_success = 0;
struct timeval time_now, time_start;
long ms_now, ms_start;

unsigned char setdm6[] = {
0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x06, //Byte at offset 2
0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, //determines new
0x05, 0x00, 0xFA, 0x00, 0xFA, 0x00, 0x64, 0x00, 0x2C, //operation mode.
0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x16, 0xDC
};
uint8_t sz_setdm6 = 44;

gettimeofday(&time_start, NULL);
ms_start = (long)((time_start.tv_sec)*1000 + (time_start.tv_usec)/1000);
ms_now = ms_start;

while(!gps_dynamic_model_set_success && (ms_now - ms_start)<6000) //Prevent lock and
{ //timeout if not set
gettimeofday(&time_now, NULL); //after six seconds
ms_now = (long)((time_now.tv_sec)*1000 + (time_now.tv_usec)/1000);

this->send_ublox_packet(setdm6, sz_setdm6);
gps_dynamic_model_set_success = this->receive_check_ublox_ack(setdm6);
}
if (gps_dynamic_model_set_success)
{
this->logger->log("GPS entered airborne (<1g) mode successfully");
}
else
{
this->logger->log("GPS failed to enter airborne (<1g) mode");
}
}

void GPS::enter_stationary_mode()
{
int gps_dynamic_model_set_success = 0;
struct timeval time_now, time_start;
long ms_now, ms_start;

unsigned char setdm2[] = {
0xB5, 0x62, 0x02, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x06, //Byte at offset 2
0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, //determines new
0x05, 0x00, 0xFA, 0x00, 0xFA, 0x00, 0x64, 0x00, 0x2C, //operation mode.
0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x16, 0xDC
};
uint8_t sz_setdm2 = 44;

gettimeofday(&time_start, NULL);
ms_start = (long)((time_start.tv_sec)*1000 + (time_start.tv_usec)/1000);
ms_now = ms_start;

while(!gps_dynamic_model_set_success && (ms_now - ms_start)<6000) //Prevent lock and
{ //timeout if not set
gettimeofday(&time_now, NULL); //after six seconds
ms_now = (long)((time_now.tv_sec)*1000 + (time_now.tv_usec)/1000);

this->send_ublox_packet(setdm2, sz_setdm2);
gps_dynamic_model_set_success = this->receive_check_ublox_ack(setdm2);
}

if (gps_dynamic_model_set_success)
{
this->logger->log("GPS entered stationary mode successfully");
}
else
{
this->logger->log("GPS failed to enter stationary mode");
}
}

void GPS::enter_pedestrian_mode()
{
int gps_dynamic_model_set_success = 0;
struct timeval time_now, time_start;
long ms_now, ms_start;

unsigned char setdm3[] = {
0xB5, 0x62, 0x03, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x06, //Byte at offset 2
0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, //determines new
0x05, 0x00, 0xFA, 0x00, 0xFA, 0x00, 0x64, 0x00, 0x2C, //operation mode.
0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x16, 0xDC
};
uint8_t sz_setdm3 = 44;

gettimeofday(&time_start, NULL);
ms_start = (long)((time_start.tv_sec)*1000 + (time_start.tv_usec)/1000);
ms_now = ms_start;

while(!gps_dynamic_model_set_success && (ms_now - ms_start)<6000) //Prevent lock and
{ //timeout if not set
gettimeofday(&time_now, NULL); //after six seconds
ms_now = (long)((time_now.tv_sec)*1000 + (time_now.tv_usec)/1000);

this->send_ublox_packet(setdm3, sz_setdm3);
gps_dynamic_model_set_success = this->receive_check_ublox_ack(setdm3);
}

if (gps_dynamic_model_set_success)
{
this->logger->log("GPS entered pedestrian mode successfully");
}
else
{
this->logger->log("GPS failed to enter pedestrian mode");
}
}


void GPS::send_ublox_packet(unsigned char *message, uint8_t len)
{
if (this->serial->is_open())
{
this->serial->flush();
this->serial->write((unsigned char)0xFF);
this_thread::sleep_for(500ms);
for (unsigned int i = 0; i<len; i++){
this->serial->write(message[i]);
}
}else
{
this->logger->log("Tried to send a packet, but the serial is closed");
}
}

bool GPS::receive_check_ublox_ack(unsigned char *message)
{
unsigned char ack_packet[10];
unsigned int bytes_ordered;
unsigned char byte;
struct timeval time_now, time_start;
long ms_now, ms_start;

ack_packet[0] = 0xB5;
ack_packet[1] = 0x62;
ack_packet[2] = 0x05;
ack_packet[3] = 0x01;
ack_packet[4] = 0x02;
ack_packet[5] = 0x00;
ack_packet[6] = message[2];
ack_packet[7] = message[3];
ack_packet[8] = 0x00;
ack_packet[9] = 0x00;

for (unsigned int i = 0; i<8; i++){
ack_packet[8]+= ack_packet[i];
ack_packet[9]+= ack_packet[8];
}
bytes_ordered = 0;

gettimeofday(&time_start, NULL);
ms_start = (long)((time_start.tv_sec)*1000 + (time_start.tv_usec)/1000);
ms_now = ms_start;

while (ms_now - ms_start <= 3000){ //Avoid lock by
gettimeofday(&time_now, NULL); //timing out after
ms_now = (long)((time_now.tv_sec)*1000 + (time_now.tv_usec)/1000); //3s

if (bytes_ordered > 9)
{
return true;
}

if (this->serial->available())
{
byte = (unsigned char)this->serial->read_char();
if (byte == ack_packet[bytes_ordered])
{
bytes_ordered++;
}
else
{
bytes_ordered = 0;
}
}
}
return false;
}

void GPS::notify_initialization()
{
this->logger->log("Initialization notified. Switching to pedestrian mode");
this->enter_pedestrian_mode();
}

void GPS::notify_takeoff()
{
this->logger->log("Takeoff notified. Switching to airborne mode");
this->enter_airborne_1g_mode();
}

void GPS::notify_safe_mode()
{
this->logger->log("Safe mode entry notified. Switching to airborne mode");
this->enter_airborne_1g_mode();
}

void GPS::notify_landing()
{
this->logger->log("Landing notified. Switching to stationary mode");
this->enter_stationary_mode();
}
12 changes: 12 additions & 0 deletions gps/GPS.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,13 @@ namespace os {
void parse_GSA(const string& frame);
void parse_RMC(const string& frame);

void enter_pedestrian_mode(); /*Before launch*/
void enter_airborne_1g_mode(); /*While in flight*/
void enter_stationary_mode(); /*When landed*/

void send_ublox_packet(unsigned char *, uint8_t);
bool receive_check_ublox_ack(unsigned char*);

public:
GPS(GPS& copy) = delete;
~GPS();
Expand All @@ -69,6 +76,11 @@ namespace os {
bool turn_on() const;
bool turn_off() const;
void parse(const string& frame);

void notify_takeoff();
void notify_landing();
void notify_initialization();
void notify_safe_mode();
};
}

Expand Down
4 changes: 4 additions & 0 deletions logic/initialize.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,10 @@ if ( ! GPS::get_instance().initialize())
}
logger.log("GPS initialized.");

//Notify the GPS about its initialization, so that it sets itself to the appropriate mode
logger.log("Notifying GPS about initialization...");
GPS::get_instance().notify_initialization();

logger.log("Initializing GSM...");
if ( ! GSM::get_instance().initialize())
{
Expand Down
3 changes: 3 additions & 0 deletions logic/land.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,9 @@ if ( ! Camera::get_instance().stop())
else
logger->log("Video stopped.");

logger->log("Notifying GPS about landing...");
GPS::get_instance().notify_landing();

logger->log("Waiting 1 minute before sending landed SMS...");
this_thread::sleep_for(1min);

Expand Down
4 changes: 4 additions & 0 deletions logic/safe_mode.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -161,6 +161,10 @@ switch (last_state)
}

logger->log("GPS fix acquired.");

logger->log("Notifying GPS about entering safe mode...");
GPS::get_instance().notify_safe_mode();

this_thread::sleep_for(5s);
for (int i = 0;
GPS::get_instance().get_HDOP() > 5 && i < 10;
Expand Down
3 changes: 3 additions & 0 deletions logic/wait_launch.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,3 +11,6 @@ while ( ! has_launched(launch_altitude))
this_thread::sleep_for(1s);

logger->log("Balloon launched.");

logger->log("Notifying GPS about launch...");
GPS::get_instance().notify_takeoff();

0 comments on commit 1090ffc

Please sign in to comment.