# HG changeset patch
# User Ethan Zonca <e@ethanzonca.com>
# Date 2017-01-12 09:16:48
# Node ID 041562172b4a2aea23719062e4e27a9df69fc1f5
# Parent  67d1f9d02048712f185a1fe8d9920f8ac4884195

Doubleinit radio for guaranteed init; GPS fixes

diff --git a/Libraries/Si446x/si446x.c b/Libraries/Si446x/si446x.c
--- a/Libraries/Si446x/si446x.c
+++ b/Libraries/Si446x/si446x.c
@@ -43,6 +43,7 @@ void si446x_init(void)
 
 	// Perform PoR (takes 20ms) and turn device on
 	si446x_reset();
+	si446x_reset();
 
     // Divide SI446x_VCXO_FREQ into its bytes; MSB first
     uint16_t x3 = SI446x_VCXO_FREQ / 0x1000000;
diff --git a/Libraries/aprs/aprs.c b/Libraries/aprs/aprs.c
--- a/Libraries/aprs/aprs.c
+++ b/Libraries/aprs/aprs.c
@@ -91,6 +91,10 @@ void aprs_send(void)
   snprintf(tmpBuffer, 128, "%u,", gps_getdata()->pdop);
   ax25_send_string(tmpBuffer);
 
+  // Heading
+  snprintf(tmpBuffer, 128, "%u,", gps_getdata()->heading);
+  ax25_send_string(tmpBuffer);
+
   // Vbatt
   snprintf(tmpBuffer, 128, "%u,", adc_get_vbatt());
   ax25_send_string(tmpBuffer);
diff --git a/Source/gps.c b/Source/gps.c
--- a/Source/gps.c
+++ b/Source/gps.c
@@ -77,12 +77,12 @@ void gps_update_data(void)
     position.altitude /= 1000; // mm => m
 
     position.speed = (buf[6+60] << 0) | (buf[6+61] << 8) | (buf[6+62] << 16) | (buf[6+63] << 24); // mm/second
-    position.speed /= 1000; // mm/s -> m/s
+    position.speed /= 10; // mm/s -> cm/s
     
     position.pdop = (buf[6+76] << 0) | (buf[6+77] << 8);
     position.pdop /= 100; // scale to dop units
 
-    position.heading = (buf[6+84] << 0) | (buf[6+85] << 8) | (buf[6+86] << 16) | (buf[6+87] << 24); // mm above sealevel
+    position.heading = (buf[6+64] << 0) | (buf[6+65] << 8) | (buf[6+66] << 16) | (buf[6+67] << 24);
     position.heading /= 100000; // 1e-5
 
 //    // Return the value if GPSfixOK is set in 'flags'
@@ -93,44 +93,6 @@ void gps_update_data(void)
 
 }
 
-// TODO: Add data valid flag: invalidate data when GPS powered off
-
-
-// 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,
-        0x2A, 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
-//    if( buf[0] != 0xB5 || buf[1] != 0x62 )
-//        led_set(LED_RED, 1);
-//    if( buf[2] != 0x06 || buf[3] != 0x24 )
-//        led_set(LED_RED, 1);
-
-    // Check 40 bytes of message checksum
-//    if( !_gps_verify_checksum(&buf[2], 40) ) led_set(LED_RED, 1);
-
-    // Clock in and verify the ACK/NACK
-    uint8_t ack[10];
-//    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.
 static uint8_t _gps_verify_checksum(uint8_t* data, uint8_t len)
@@ -202,22 +164,21 @@ void gps_poweron(void)
 
 //    // Disable GLONASS mode
 //    uint8_t disable_glonass[20] = {0xB5, 0x62, 0x06, 0x3E, 0x0C, 0x00, 0x00, 0x00, 0x20, 0x01, 0x06, 0x08, 0x0E, 0x00, 0x00, 0x00, 0x01, 0x01, 0x8F, 0xB2};
-//
-//    //gps_sendubx(disable_glonass, 20);
-//    volatile HAL_StatusTypeDef res = HAL_UART_Transmit(uart_gethandle(), disable_glonass, 20, 100);
+//    HAL_UART_Transmit(uart_gethandle(), disable_glonass, sizeof(disable_glonass)/sizeof(uint8_t), 100);
+//	HAL_Delay(100);
 //
 //    // Enable power saving
 //    uint8_t enable_powersave[10] = {0xB5, 0x62, 0x06, 0x11, 0x02, 0x00, 0x08, 0x01, 0x22, 0x92};
-//    //gps_sendubx(enable_powersave, 10);
-//    res = HAL_UART_Transmit(uart_gethandle(), enable_powersave, 10, 100);
+//    HAL_UART_Transmit(uart_gethandle(), enable_powersave, sizeof(enable_powersave)/sizeof(uint8_t), 100);
+//	HAL_Delay(100);
 //
 //
 //    // 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);
-
-
+//    HAL_UART_Transmit(uart_gethandle(), airborne_model, sizeof(airborne_model)/sizeof(uint8_t), 100);
+//	HAL_Delay(100);
+//
+//
 
 
 
diff --git a/Source/main.c b/Source/main.c
--- a/Source/main.c
+++ b/Source/main.c
@@ -24,11 +24,19 @@ int main(void)
   sysclock_init();
   gpio_init();
 
+  HAL_Delay(100);
+
   adc_init();
 
   afsk_init();
   si446x_init();
+  si446x_init();
+
+  HAL_Delay(100);
+
   gps_poweron();
+  HAL_Delay(100);
+
   pressure_init();
 
   // Software timers