FeatherHAB/wsprhab Changeset - 677c51754ccf

Changeset - 677c51754ccf

Parent rev.

Child rev.

[Not reviewed]

default

0 3 0

Ethan Zonca - 9 years ago 2016-10-11 20:48:46
ez@ethanzonca.com

Refactor GPS code

3 files changed with 61 insertions and 219 deletions:

inc/gps.h

src/gps.c

175

src/main.c

0 comments (0 inline, 0 general)

inc/gps.h

➞

Show inline comments

 /*
  * Master Firmware: NMEA Parser
+ *
  * This file is part of OpenTrack.
+ *
  * OpenTrack is free software: you can redistribute it and/or modify
  * it under the terms of the GNU Affero General Public License as published by
  * the Free Software Foundation, either version 3 of the License, or
  * (at your option) any later version.
+ *
  * OpenTrack is distributed in the hope that it will be useful,
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU Affero General Public License for more details.
+ *
  * You should have received a copy of the GNU Affero General Public License
  * along with OpenTrack. If not, see <http://www.gnu.org/licenses/>.
+ *
  * Ethan Zonca
  * Matthew Kanning
  * Kyle Ripperger
  * Matthew Kroening
+ *
  */
 #ifndef GPS_H_
 #define GPS_H_
 #include <stdint.h>
 // Duration before GPS fix is declared stale
 #define GPS_STALEFIX_MS 60000
-enum gps_state
+typedef struct _gps_data
+{
     GPS_STATE_ACQUIRING = 0,
     GPS_STATE_FRESHFIX,
     GPS_STATE_STALEFIX,
     GPS_STATE_NOFIX
 };
     int32_t pdop;
     int32_t sats_in_solution;
     uint32_t speed;
     //! int32_t heading;
     int32_t latitude;
     int32_t longitude;
     int32_t altitude;
     uint8_t month;
     uint8_t day;
     uint8_t hour;
     uint8_t minute;
     uint8_t second;
     uint8_t valid;
     uint8_t fixtype;
 } gps_data_t;
 void gps_init();
 void gps_update_data(void);
 void gps_update_position();
 void gps_update_time(uint8_t* hour, uint8_t* minute, uint8_t* second);
 void gps_check_lock(uint8_t* lock, uint8_t* sats);
 uint8_t gps_check_nav(void);
 void gps_poweron(void);
 void gps_poweroff(void);
 void gps_acquirefix(void);
 uint8_t gps_getstate(void);
 gps_data_t* gps_getdata(void);
 #endif /* GPS_H_ */

src/gps.c

➞

Show inline comments

 // TODO: Transition to using https://github.com/cuspaceflight/joey-m/blob/master/firmware/gps.c requesting UBX data
 //
 // GPS: communicate with ublox GPS module via ubx protocol
 //
 #include "stm32f0xx_hal.h"
 #include "config.h"
 #include "gpio.h"
 #include "uart.h"
 #include "gps.h"
 typedef struct _gps_data
+{
     int32_t hdop;
     int32_t sats_in_view;
     int32_t speed;
     int32_t heading;
 volatile gps_data_t position;
     int32_t latitude;
     int32_t longitude;
     int32_t altitude;
     uint8_t hour;
     uint8_t minute;
     uint8_t second;
 } gps_data_t;
 gps_data_t position;
 // Private methods
 static void gps_ubx_checksum(uint8_t* data, uint8_t len, uint8_t* cka, uint8_t* ckb);
 static uint8_t _gps_verify_checksum(uint8_t* data, uint8_t len);
+// Initialize GPS module on startup
 void gps_init()
+{
     // Initialize serial port
    // done in poweron uart_init();
 	gps_poweron();
 //	// uart1 ubx only
 //
 //	uint8_t setUBXuart1 = {0xB5, 0x62, 0x06, 0x00, 0x14, 0x00, 0x01, 0x00, 0x00, 0x00, 0xD0, 0x08, 0x00, 0x00, 0x80, 0x25,
 //	0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x9A, 0x79};
 //	HAL_UART_Transmit(uart_gethandle(), setUBXuart1, sizeof(setUBXuart1)/sizeof(uint8_t), 100);
@@ @@ -112,112 +97,26 @@ void gps_init() @@
 //
 //    // Set dynamic model 6 (<1g airborne platform)
 //    uint8_t airborne_model[] = { 0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x06, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00, 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x16, 0xDC };
 //    //gps_sendubx(airborne_model, sizeof(airborne_model)/sizeof(uint8_t));
 //    res = HAL_UART_Transmit(uart_gethandle(), airborne_model, sizeof(airborne_model)/sizeof(uint8_t), 100);
+}
 void gps_update_position()
+{
     // Request a NAV-POSLLH message from the GPS
     uint8_t request[8] = {0xB5, 0x62, 0x01, 0x02, 0x00, 0x00, 0x03, 0x0A};
     volatile uint8_t check_a = 0;
     volatile uint8_t check_b = 0;
     for(uint8_t i = 2; i<6; i++)
+    {
     	check_a += request[i];
     	check_b += check_a;
+    }
     //_gps_send_msg(request, 8);
     uint8_t flushed = uart_gethandle()->Instance->RDR;
     volatile HAL_StatusTypeDef txres = HAL_UART_Transmit(uart_gethandle(), request, 8, 100);
     uint8_t buf[36];
     for(uint8_t i =0; i<36; i++)
     	buf[i] = 0xaa;
     volatile HAL_StatusTypeDef result = HAL_UART_Receive(uart_gethandle(), buf, 36, 1000);
     //for(uint8_t i = 0; i < 36; i++)
     //    buf[i] = _gps_get_byte();
 //    // Verify the sync and header bits
 //    if( buf[0] != 0xB5 || buf[1] != 0x62 )
 //        led_set(LED_RED, 1);
 //    if( buf[2] != 0x01 || buf[3] != 0x02 )
 //        led_set(LED_RED, 1);
     // 4 bytes of longitude (1e-7)
     position.longitude = (int32_t)buf[10] | (int32_t)buf[11] << 8 |
         (int32_t)buf[12] << 16 | (int32_t)buf[13] << 24;
     // 4 bytes of latitude (1e-7)
     position.latitude = (int32_t)buf[14] | (int32_t)buf[15] << 8 |
         (int32_t)buf[16] << 16 | (int32_t)buf[17] << 24;
     // 4 bytes of altitude above MSL (mm)
     position.altitude = (int32_t)buf[22] | (int32_t)buf[23] << 8 |
         (int32_t)buf[24] << 16 | (int32_t)buf[25] << 24;
     if( !_gps_verify_checksum(&buf[2], 32) )
         led_blink(2);
+}
 void gps_update_time(uint8_t* hour, uint8_t* minute, uint8_t* second)
+{
     // Send a NAV-TIMEUTC message to the receiver
     uint8_t request[8] = {0xB5, 0x62, 0x01, 0x21, 0x00, 0x00, 0x22, 0x67};
     volatile uint8_t check_a = 0;
     volatile uint8_t check_b = 0;
     for(uint8_t i = 2; i<6; i++)
+    {
     	check_a += request[i];
     	check_b += check_a;
+    }
     uint8_t flushed = uart_gethandle()->Instance->RDR;
     volatile HAL_StatusTypeDef res = HAL_UART_Transmit(uart_gethandle(), request, 8, 100);
     // Get the message back from the GPS
     uint8_t buf[28];
     res = HAL_UART_Receive(uart_gethandle(), buf, 28, 500);
 //    // Verify the sync and header bits
 //    if( buf[0] != 0xB5 || buf[1] != 0x62 )
 //        led_set(LED_RED, 1);
 //    if( buf[2] != 0x01 || buf[3] != 0x21 )
 //        led_set(LED_RED, 1);
     *hour = buf[22];
     *minute = buf[23];
     *second = buf[24];
 //    if( !_gps_verify_checksum(&buf[2], 24) ) led_set(LED_RED, 1);
+}
 /**
  * Check the navigation status to determine the quality of the
  * fix currently held by the receiver with a NAV-STATUS message.
  */
 void gps_check_lock(uint8_t* lock, uint8_t* sats)
 // Poll for fix data from the GPS and update the internal structure
 void gps_update_data(void)
+{
     // Construct the request to the GPS
     uint8_t request[8] = {0xB5, 0x62, 0x01, 0x07, 0x00, 0x00, 0xFF, 0xFF};
     volatile uint8_t check_a = 0;
     volatile uint8_t check_b = 0;
     for(uint8_t i = 2; i<6; i++)
+    {
     	check_a += request[i];
     	check_b += check_a;
+    }
@@ @@ -233,54 +132,49 @@ void gps_check_lock(uint8_t* lock, uint8 @@
     uint8_t buf[100];
     for(uint8_t i=0; i<100; i++)
     	buf[i] = 0xaa;
     volatile HAL_StatusTypeDef res = HAL_UART_Receive(uart_gethandle(), buf, 100, 3000);
     // Check 60 bytes minus SYNC and CHECKSUM (4 bytes)
     if( !_gps_verify_checksum(&buf[2], 96) )
         led_blink(2);
     //volatile uint32_t gpstime_ms = (buf[6+0] << 24) | (buf[6+1] << 16) | buf[6+2] << 8) | (buf[6+3]);
     volatile uint8_t month = buf[6+6];
     volatile uint8_t day = buf[6+7];
     volatile uint8_t hour = buf[6+8];
     volatile uint8_t minute = buf[6+9];
     volatile uint8_t second = buf[6+10];
     volatile uint8_t valid = buf[6+11] & 0b1111;
     volatile uint8_t fixtype = buf[6+20];
     position.month = buf[6+6];
     position.day = buf[6+7];
     position.hour = buf[6+8];
     position.minute = buf[6+9];
     position.second = buf[6+10];
     position.valid = buf[6+11] & 0b1111;
     position.fixtype = buf[6+20];
-    volatile uint8_t sats_in_solution = buf[6+23];
+    position.sats_in_solution = buf[6+23];
     volatile uint32_t longitude = (buf[6+24] << 24) | (buf[6+25] << 16) | (buf[6+26] << 8) | (buf[6+27]);
     volatile uint32_t latitude = (buf[6+28] << 24) | (buf[6+29] << 16) | (buf[6+30] << 8) | (buf[6+31]);
     volatile uint32_t altitude_sealevel = (buf[6+36] << 24) | (buf[6+37] << 16) | (buf[6+38] << 8) | (buf[6+39]);
     volatile uint32_t groundspeed = (buf[6+60] << 24) | (buf[6+61] << 16) | (buf[6+62] << 8) | (buf[6+63]);
     volatile uint16_t pdop = (buf[6+76] << 8) | (buf[6+77]);
 //
     position.longitude = (buf[6+24] << 24) | (buf[6+25] << 16) | (buf[6+26] << 8) | (buf[6+27]);
     position.latitude =  (buf[6+28] << 24) | (buf[6+29] << 16) | (buf[6+30] << 8) | (buf[6+31]);
     position.altitude = (buf[6+36] << 24) | (buf[6+37] << 16) | (buf[6+38] << 8) | (buf[6+39]);
     position.speed = (buf[6+60] << 24) | (buf[6+61] << 16) | (buf[6+62] << 8) | (buf[6+63]);
     position.pdop = (buf[6+76] << 8) | (buf[6+77]);
 //    // Return the value if GPSfixOK is set in 'flags'
 //    if( buf[17] & 0x01 )
 //        *lock = buf[16];
 //    else
 //        *lock = 0;
     *lock = fixtype;
     *sats = sats_in_solution;
+}
 /**
  * Verify that the uBlox 6 GPS receiver is set to the <1g airborne
  * navigaion mode.
  */
 // Verify that the uBlox 6 GPS receiver is set to the <1g airborne navigaion mode.
 uint8_t gps_check_nav(void)
+{
     uint8_t request[8] = {0xB5, 0x62, 0x06, 0x24, 0x00, 0x00,
 x2A, 0x84};
     uint8_t flushed = uart_gethandle()->Instance->RDR;
     HAL_UART_Transmit(uart_gethandle(), request, 8, 100);
     // Get the message back from the GPS
     uint8_t buf[44];
     HAL_UART_Receive(uart_gethandle(), buf, 44, 100);
 //    // Verify sync and header bytes
@@ @@ -297,106 +191,65 @@ uint8_t gps_check_nav(void) @@
 //    for(uint8_t i = 0; i < 10; i++)
 //        ack[i] = _gps_get_byte();
     HAL_UART_Receive(uart_gethandle(), ack, 10, 100);
     // If we got a NACK, then return 0xFF
     if( ack[3] == 0x00 ) return 0xFF;
     // Return the navigation mode and let the caller analyse it
     return buf[8];
+}
 /**
  * Verify the checksum for the given data and length.
  */
 // Verify the checksum for the given data and length.
 static uint8_t _gps_verify_checksum(uint8_t* data, uint8_t len)
+{
     uint8_t a, b;
     gps_ubx_checksum(data, len, &a, &b);
     if( a != *(data + len) || b != *(data + len + 1))
         return 0;
     else
         return 1;
+}
 // Calculate a UBX checksum using 8-bit Fletcher (RFC1145)
 static void gps_ubx_checksum(uint8_t* data, uint8_t len, uint8_t* cka, uint8_t* ckb)
+{
     *cka = 0;
     *ckb = 0;
     for( uint8_t i = 0; i < len; i++ )
+    {
         *cka += *data;
         *ckb += *cka;
         data++;
+    }
+}
 // Power on GPS module and initialize UART
 void gps_poweron(void)
+{
     // NOTE: pchannel
     HAL_GPIO_WritePin(GPS_NOTEN, 0);
     uart_init();
     // Begin DMA reception
     //HAL_UART_Receive_DMA(uart_gethandle(), nmeaBuffer, NMEABUFFER_SIZE);
+}
 // Power off GPS module
 void gps_poweroff(void)
+{
     // NOTE: pchannel
     uart_deinit();
     HAL_GPIO_WritePin(GPS_NOTEN, 1);
+}
 //
 //uint8_t gps_hadfix = 0;
 //uint8_t gps_hasfix()
 //{
 //	uint8_t hasFix = get_latitudeTrimmed()[0] != 0x00;
 //	gps_hadfix = hasFix;
 //	return hasFix;
 //}
 //void gps_sendubx(uint8_t* dat, uint8_t size)
 //{
 //    uint8_t sendctr;
 //    for(sendctr = 0; sendctr < size; sendctr++)
 //    {
 //        HAL_UART_Transmit(huart1, dat[sendctr]);
 //    }
 //}
 uint8_t gps_acquiring= 0;
 uint32_t gps_lastfix_time;
 void gps_acquirefix(void)
 gps_data_t* gps_getdata(void)
+{
     gps_poweron();
     // Wait for fix
     gps_acquiring = 1;
     return &position;
+}
 uint8_t gps_getstate(void)
+{
     if(gps_acquiring)
         return GPS_STATE_ACQUIRING;
     else if(gps_lastfix_time == 0)
         return GPS_STATE_NOFIX;
     else if(HAL_GetTick() - gps_lastfix_time < GPS_STALEFIX_MS)
         return GPS_STATE_FRESHFIX;
     else
         return GPS_STATE_STALEFIX;
+}
 // vim:softtabstop=4 shiftwidth=4 expandtab

src/main.c

➞

Show inline comments

@@ @@ -40,24 +40,34 @@ int main(void) @@
     led_blink(4);
     uint16_t blink_rate = BLINK_FAST;
     uint8_t state = SYSTEM_GPSACQ;
     uint32_t fixinfo_timer = 0;
     uint8_t fix_ok = 0;
     uint8_t numsats = 0;
     while (1)
+    {
         // Update fix status every 2 seconds
         if(HAL_GetTick() - fixinfo_timer > 2000)
+        {
         	gps_update_data();
         	fixinfo_timer = HAL_GetTick();
+        }
         switch(state)
+        {
             // Idling: sleep and wait for RTC timeslot trigger
             case SYSTEM_IDLE:
+            {
                 blink_rate = BLINK_SLOW;
                 // Wait for RTC wakeup interrupt
                 //wfi();
                 //enter_sleep();
                 // Somehow go to another state when we get an interrupt
@@ @@ -66,32 +76,26 @@ int main(void) @@
             // Attempt to acquire GPS fix
             case SYSTEM_GPSACQ:
+            {
                 blink_rate = BLINK_FAST;
                 // TODO: probably don't power on all the time, just on state transition
 //                gps_poweron();
 //                HAL_Delay(100);
 //                gps_update_position();
                 // Update fix status every 2 seconds
                 if(HAL_GetTick() - fixinfo_timer > 2000)
+                {
                 	gps_check_lock(&fix_ok, &numsats);
                 	fixinfo_timer = HAL_GetTick();
+                }
-                if(fix_ok > 0)
+                if(gps_getdata()->fixtype > 0)
+                {
                     // Disable GPS module
                     //gps_poweroff();
                     // TODO: Set RTC from GPS time
                     // TODO: Set RTC for countdown to next transmission timeslot!
                     // TODO: Set wspr countdown timer for this transmission!
                     state = SYSTEM_WSPRTX;
+                }
 //                else if(fix_timeout)

0 comments (0 inline, 0 general)