2016-01-19 13:39:46 +00:00
/ *
Copyright ( c ) 2015 - 2016 Christopher Young
Distributable under the terms of The "BSD New" " License
that can be found in the LICENSE file , herein included
as part of this header .
ry835ai . go : GPS functions , GPS init , AHRS status messages , other external sensor monitoring .
* /
2015-08-15 00:11:04 +00:00
package main
import (
2015-08-20 21:01:42 +00:00
"fmt"
2015-08-17 17:59:03 +00:00
"log"
2015-08-15 03:10:16 +00:00
"strconv"
2015-08-17 17:59:03 +00:00
"strings"
2015-08-20 20:47:05 +00:00
"sync"
2015-08-20 21:01:42 +00:00
"time"
2015-08-17 17:59:03 +00:00
2015-09-14 05:17:18 +00:00
"bufio"
2015-09-24 04:28:16 +00:00
2015-08-16 23:33:34 +00:00
"github.com/kidoman/embd"
_ "github.com/kidoman/embd/host/all"
2015-08-17 17:59:03 +00:00
"github.com/kidoman/embd/sensor/bmp180"
"github.com/tarm/serial"
2015-08-20 20:47:05 +00:00
2015-09-16 04:04:23 +00:00
"os"
2015-09-23 03:51:56 +00:00
"os/exec"
2015-09-24 04:28:16 +00:00
"../mpu6050"
2015-08-15 00:11:04 +00:00
)
2016-05-05 04:01:10 +00:00
const (
2016-05-05 05:16:16 +00:00
SAT_TYPE_UNKNOWN = 0 // default type
SAT_TYPE_GPS = 1 // GPxxx; NMEA IDs 1-32
SAT_TYPE_GLONASS = 2 // GLxxx; NMEA IDs 65-88
SAT_TYPE_GALILEO = 3 // GAxxx; NMEA IDs unknown
SAT_TYPE_BEIDOU = 4 // GBxxx; NMEA IDs 201-235
SAT_TYPE_SBAS = 10 // NMEA IDs 33-54
2016-05-05 04:01:10 +00:00
)
type SatelliteInfo struct {
2016-05-08 05:14:20 +00:00
SatelliteNMEA uint8 // NMEA ID of the satellite. 1-32 is GPS, 33-54 is SBAS, 65-88 is Glonass.
SatelliteID string // Formatted code indicating source and PRN code. e.g. S138==WAAS satellite 138, G2==GPS satellites 2
Elevation int16 // Angle above local horizon, -xx to +90
Azimuth int16 // Bearing (degrees true), 0-359
Signal int8 // Signal strength, 0 - 99; -99 indicates no reception
Type uint8 // Type of satellite (GPS, GLONASS, Galileo, SBAS)
TimeLastSolution time . Time // Time (system ticker) a solution was last calculated using this satellite
TimeLastSeen time . Time // Time (system ticker) a signal was last received from this satellite
TimeLastTracked time . Time // Time (system ticker) this satellite was tracked (almanac data)
InSolution bool // True if satellite is used in the position solution (reported by GSA message or PUBX,03)
2016-05-05 04:01:10 +00:00
}
2015-08-20 20:47:05 +00:00
type SituationData struct {
2015-08-20 21:01:42 +00:00
mu_GPS * sync . Mutex
2015-08-20 20:47:05 +00:00
// From GPS.
2016-03-24 14:37:23 +00:00
LastFixSinceMidnightUTC float32
Lat float32
Lng float32
Quality uint8
HeightAboveEllipsoid float32 // GPS height above WGS84 ellipsoid, ft. This is specified by the GDL90 protocol, but most EFBs use MSL altitude instead. HAE is about 70-100 ft below GPS MSL altitude over most of the US.
GeoidSep float32 // geoid separation, ft, MSL minus HAE (used in altitude calculation)
Satellites uint16 // satellites used in solution
SatellitesTracked uint16 // satellites tracked (almanac data received)
SatellitesSeen uint16 // satellites seen (signal received)
Accuracy float32 // 95% confidence for horizontal position, meters.
NACp uint8 // NACp categories are defined in AC 20-165A
Alt float32 // Feet MSL
AccuracyVert float32 // 95% confidence for vertical position, meters
GPSVertVel float32 // GPS vertical velocity, feet per second
LastFixLocalTime time . Time
2016-04-07 05:24:12 +00:00
TrueCourse float32
2016-03-24 14:37:23 +00:00
GroundSpeed uint16
LastGroundTrackTime time . Time
GPSTime time . Time
LastGPSTimeTime time . Time // stratuxClock time since last GPS time received.
LastValidNMEAMessageTime time . Time // time valid NMEA message last seen
LastValidNMEAMessage string // last NMEA message processed.
2015-08-20 20:47:05 +00:00
2015-08-20 21:01:42 +00:00
mu_Attitude * sync . Mutex
2015-08-20 20:47:05 +00:00
// From BMP180 pressure sensor.
2015-09-24 20:26:51 +00:00
Temp float64
Pressure_alt float64
2016-03-24 04:26:56 +00:00
LastTempPressTime time . Time
2015-08-20 20:47:05 +00:00
// From MPU6050 accel/gyro.
2015-09-24 20:26:51 +00:00
Pitch float64
Roll float64
Gyro_heading float64
LastAttitudeTime time . Time
2015-08-15 00:11:04 +00:00
}
2015-08-17 17:59:03 +00:00
var serialConfig * serial . Config
var serialPort * serial . Port
2015-08-15 00:11:04 +00:00
2016-02-10 07:06:52 +00:00
var readyToInitGPS bool // TO-DO: replace with channel control to terminate goroutine when complete
2016-05-05 04:01:10 +00:00
var satelliteMutex * sync . Mutex
2016-05-07 05:18:10 +00:00
var Satellites map [ string ] SatelliteInfo
2016-05-05 04:01:10 +00:00
2015-09-18 19:18:58 +00:00
/ *
2016-03-10 17:33:03 +00:00
u - blox5_Referenzmanual . pdf
2015-09-18 19:18:58 +00:00
Platform settings
Airborne < 2 g Recommended for typical airborne environment . No 2 D position fixes supported .
p .91 - CFG - MSG
Navigation / Measurement Rate Settings
Header 0xB5 0x62
ID 0x06 0x08
0x0064 ( 100 ms )
0x0001
0x0001 ( GPS time )
{ 0xB5 , 0x62 , 0x06 , 0x08 , 0x00 , 0x64 , 0x00 , 0x01 , 0x00 , 0x01 }
p .109 CFG - NAV5 ( 0x06 0x24 )
Poll Navigation Engine Settings
* /
func chksumUBX ( msg [ ] byte ) [ ] byte {
ret := make ( [ ] byte , 2 )
for i := 0 ; i < len ( msg ) ; i ++ {
ret [ 0 ] = ret [ 0 ] + msg [ i ]
ret [ 1 ] = ret [ 1 ] + ret [ 0 ]
}
return ret
}
// p.62
func makeUBXCFG ( class , id byte , msglen uint16 , msg [ ] byte ) [ ] byte {
ret := make ( [ ] byte , 6 )
ret [ 0 ] = 0xB5
ret [ 1 ] = 0x62
ret [ 2 ] = class
ret [ 3 ] = id
ret [ 4 ] = byte ( msglen & 0xFF )
2015-09-24 04:28:16 +00:00
ret [ 5 ] = byte ( ( msglen >> 8 ) & 0xFF )
2015-09-18 19:18:58 +00:00
ret = append ( ret , msg ... )
chk := chksumUBX ( ret [ 2 : ] )
ret = append ( ret , chk [ 0 ] )
ret = append ( ret , chk [ 1 ] )
return ret
}
2016-02-05 07:41:09 +00:00
func makeNMEACmd ( cmd string ) [ ] byte {
2016-02-08 05:12:14 +00:00
chk_sum := byte ( 0 )
for i := range cmd {
chk_sum = chk_sum ^ byte ( cmd [ i ] )
}
return [ ] byte ( fmt . Sprintf ( "$%s*%02x\x0d\x0a" , cmd , chk_sum ) )
2016-02-05 07:41:09 +00:00
}
2015-08-20 20:47:05 +00:00
func initGPSSerial ( ) bool {
2015-09-15 14:04:44 +00:00
var device string
2016-02-02 06:35:48 +00:00
baudrate := int ( 9600 )
isSirfIV := bool ( false )
2016-05-19 03:48:58 +00:00
if _ , err := os . Stat ( "/dev/ublox8" ) ; err == nil { // u-blox 8 (RY83xAI over USB).
device = "/dev/ublox8"
} else if _ , err := os . Stat ( "/dev/ublox7" ) ; err == nil { // u-blox 7 (VK-172, RY725AI over USB).
device = "/dev/ublox7"
} else if _ , err := os . Stat ( "/dev/ublox6" ) ; err == nil { // u-blox 6 (VK-162).
device = "/dev/ublox6"
2016-05-17 21:17:01 +00:00
} else if _ , err := os . Stat ( "/dev/prolific0" ) ; err == nil { // Assume it's a BU-353-S4 SIRF IV.
//TODO: Check a "serialout" flag and/or deal with multiple prolific devices.
2016-02-08 05:12:14 +00:00
isSirfIV = true
baudrate = 4800
2016-05-17 21:17:01 +00:00
device = "/dev/prolific0"
2016-04-29 03:18:34 +00:00
} else if _ , err := os . Stat ( "/dev/ttyAMA0" ) ; err == nil { // ttyAMA0 is PL011 UART (GPIO pins 8 and 10) on all RPi.
2016-04-12 02:52:07 +00:00
device = "/dev/ttyAMA0"
2016-02-02 06:35:48 +00:00
} else {
2016-02-08 05:12:14 +00:00
log . Printf ( "No suitable device found.\n" )
return false
2015-09-15 14:04:44 +00:00
}
2016-03-23 13:19:11 +00:00
if globalSettings . DEBUG {
log . Printf ( "Using %s for GPS\n" , device )
}
2015-12-27 08:47:37 +00:00
2015-12-29 22:00:59 +00:00
/ * Developer option -- uncomment to allow "hot" configuration of GPS ( assuming 38.4 kpbs on warm start )
serialConfig = & serial . Config { Name : device , Baud : 38400 }
p , err := serial . OpenPort ( serialConfig )
if err != nil {
log . Printf ( "serial port err: %s\n" , err . Error ( ) )
return false
} else { // reset port to 9600 baud for configuration
cfg1 := make ( [ ] byte , 20 )
cfg1 [ 0 ] = 0x01 // portID.
cfg1 [ 1 ] = 0x00 // res0.
cfg1 [ 2 ] = 0x00 // res1.
cfg1 [ 3 ] = 0x00 // res1.
// [ 7 ] [ 6 ] [ 5 ] [ 4 ]
// 0000 0000 0000 0000 1000 0000 1100 0000
// UART mode. 0 stop bits, no parity, 8 data bits. Little endian order.
cfg1 [ 4 ] = 0xC0
cfg1 [ 5 ] = 0x08
cfg1 [ 6 ] = 0x00
cfg1 [ 7 ] = 0x00
// Baud rate. Little endian order.
bdrt1 := uint32 ( 9600 )
cfg1 [ 11 ] = byte ( ( bdrt1 >> 24 ) & 0xFF )
cfg1 [ 10 ] = byte ( ( bdrt1 >> 16 ) & 0xFF )
cfg1 [ 9 ] = byte ( ( bdrt1 >> 8 ) & 0xFF )
cfg1 [ 8 ] = byte ( bdrt1 & 0xFF )
// inProtoMask. NMEA and UBX. Little endian.
cfg1 [ 12 ] = 0x03
cfg1 [ 13 ] = 0x00
// outProtoMask. NMEA. Little endian.
cfg1 [ 14 ] = 0x02
cfg1 [ 15 ] = 0x00
cfg1 [ 16 ] = 0x00 // flags.
cfg1 [ 17 ] = 0x00 // flags.
cfg1 [ 18 ] = 0x00 //pad.
cfg1 [ 19 ] = 0x00 //pad.
p . Write ( makeUBXCFG ( 0x06 , 0x00 , 20 , cfg1 ) )
p . Close ( )
}
-- End developer option * /
2015-09-18 19:18:58 +00:00
2016-02-02 06:35:48 +00:00
// Open port at default baud for config.
serialConfig = & serial . Config { Name : device , Baud : baudrate }
2015-12-27 08:47:37 +00:00
p , err := serial . OpenPort ( serialConfig )
2015-09-18 19:18:58 +00:00
if err != nil {
log . Printf ( "serial port err: %s\n" , err . Error ( ) )
return false
}
2016-02-02 06:35:48 +00:00
if isSirfIV {
2016-02-08 05:12:14 +00:00
log . Printf ( "Using SiRFIV config.\n" )
2016-02-05 07:41:09 +00:00
// Enable 38400 baud.
p . Write ( makeNMEACmd ( "PSRF100,1,38400,8,1,0" ) )
2016-02-04 10:05:05 +00:00
baudrate = 38400
p . Close ( )
time . Sleep ( 250 * time . Millisecond )
// Re-open port at newly configured baud so we can configure 5Hz messages.
serialConfig = & serial . Config { Name : device , Baud : baudrate }
p , err = serial . OpenPort ( serialConfig )
2016-02-05 07:41:09 +00:00
// Enable 5Hz. (To switch back to 1Hz: $PSRF103,00,7,00,0*22)
p . Write ( makeNMEACmd ( "PSRF103,00,6,00,0" ) )
// Enable GGA.
p . Write ( makeNMEACmd ( "PSRF103,00,00,01,01" ) )
// Enable GSA.
p . Write ( makeNMEACmd ( "PSRF103,02,00,01,01" ) )
// Enable RMC.
p . Write ( makeNMEACmd ( "PSRF103,04,00,01,01" ) )
// Enable VTG.
p . Write ( makeNMEACmd ( "PSRF103,05,00,01,01" ) )
2016-05-05 03:28:28 +00:00
// Enable GSV (once every 5 position updates)
p . Write ( makeNMEACmd ( "PSRF103,03,00,05,01" ) )
2016-02-17 03:13:21 +00:00
2016-03-23 13:18:28 +00:00
if globalSettings . DEBUG {
log . Printf ( "Finished writing SiRF GPS config to %s. Opening port to test connection.\n" , device )
}
2016-02-08 05:12:14 +00:00
} else {
2016-05-08 20:41:49 +00:00
// Set 5 Hz update. Little endian order.
2016-05-08 01:53:12 +00:00
//p.Write(makeUBXCFG(0x06, 0x08, 6, []byte{0x64, 0x00, 0x01, 0x00, 0x01, 0x00})) // 10 Hz
p . Write ( makeUBXCFG ( 0x06 , 0x08 , 6 , [ ] byte { 0xc8 , 0x00 , 0x01 , 0x00 , 0x01 , 0x00 } ) ) // 5 Hz
2016-02-02 06:35:48 +00:00
// Set navigation settings.
nav := make ( [ ] byte , 36 )
nav [ 0 ] = 0x05 // Set dyn and fixMode only.
nav [ 1 ] = 0x00
// dyn.
nav [ 2 ] = 0x07 // "Airborne with >2g Acceleration".
nav [ 3 ] = 0x02 // 3D only.
2016-02-03 05:44:54 +00:00
2016-02-02 06:35:48 +00:00
p . Write ( makeUBXCFG ( 0x06 , 0x24 , 36 , nav ) )
// GNSS configuration CFG-GNSS for ublox 7 higher, p. 125 (v8)
// NOTE: Max position rate = 5 Hz if GPS+GLONASS used.
2016-05-08 01:53:12 +00:00
// TESTING: 5Hz unified GPS + GLONASS
2016-02-02 06:35:48 +00:00
// Disable GLONASS to enable 10 Hz solution rate. GLONASS is not used
// for SBAS (WAAS), so little real-world impact.
cfgGnss := [ ] byte { 0x00 , 0x20 , 0x20 , 0x05 }
2016-05-08 01:53:12 +00:00
gps := [ ] byte { 0x00 , 0x08 , 0x10 , 0x00 , 0x01 , 0x00 , 0x01 , 0x01 } // enable GPS with 8-16 tracking channels
sbas := [ ] byte { 0x01 , 0x02 , 0x03 , 0x00 , 0x01 , 0x00 , 0x01 , 0x01 } // enable SBAS (WAAS) with 2-3 tracking channels
2016-02-02 06:35:48 +00:00
beidou := [ ] byte { 0x03 , 0x00 , 0x10 , 0x00 , 0x00 , 0x00 , 0x01 , 0x01 }
qzss := [ ] byte { 0x05 , 0x00 , 0x03 , 0x00 , 0x00 , 0x00 , 0x01 , 0x01 }
2016-05-08 01:53:12 +00:00
//glonass := []byte{0x06, 0x04, 0x0E, 0x00, 0x00, 0x00, 0x01, 0x01} // this disables GLONASS
glonass := [ ] byte { 0x06 , 0x08 , 0x0E , 0x00 , 0x01 , 0x00 , 0x01 , 0x01 } // this enables GLONASS with 8-14 tracking channels
2016-02-02 06:35:48 +00:00
cfgGnss = append ( cfgGnss , gps ... )
cfgGnss = append ( cfgGnss , sbas ... )
cfgGnss = append ( cfgGnss , beidou ... )
cfgGnss = append ( cfgGnss , qzss ... )
cfgGnss = append ( cfgGnss , glonass ... )
p . Write ( makeUBXCFG ( 0x06 , 0x3E , uint16 ( len ( cfgGnss ) ) , cfgGnss ) )
// SBAS configuration for ublox 6 and higher
p . Write ( makeUBXCFG ( 0x06 , 0x16 , 8 , [ ] byte { 0x01 , 0x07 , 0x03 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 } ) )
2016-05-08 20:41:49 +00:00
// Message output configuration: UBX,00 (position) on each calculated fix; UBX,03 (satellite info) every 5th fix,
// UBX,04 (timing) every 10th, GGA (NMEA position) every 5th. All other NMEA messages disabled.
2016-02-02 06:35:48 +00:00
// Msg DDC UART1 UART2 USB I2C Res
2016-05-08 20:41:49 +00:00
p . Write ( makeUBXCFG ( 0x06 , 0x01 , 8 , [ ] byte { 0xF0 , 0x00 , 0x00 , 0x05 , 0x00 , 0x05 , 0x00 , 0x01 } ) ) // GGA enabled every 5th message
p . Write ( makeUBXCFG ( 0x06 , 0x01 , 8 , [ ] byte { 0xF0 , 0x01 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x01 } ) ) // GLL disabled
2016-05-08 16:36:05 +00:00
p . Write ( makeUBXCFG ( 0x06 , 0x01 , 8 , [ ] byte { 0xF0 , 0x02 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x01 } ) ) // GSA disabled
2016-05-08 20:41:49 +00:00
//p.Write(makeUBXCFG(0x06, 0x01, 8, []byte{0xF0, 0x02, 0x00, 0x05, 0x00, 0x05, 0x00, 0x01})) // GSA enabled disabled every 5th position (used for testing only)
2016-05-08 16:36:05 +00:00
p . Write ( makeUBXCFG ( 0x06 , 0x01 , 8 , [ ] byte { 0xF0 , 0x03 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x01 } ) ) // GSV disabled
2016-05-08 20:41:49 +00:00
//p.Write(makeUBXCFG(0x06, 0x01, 8, []byte{0xF0, 0x03, 0x00, 0x05, 0x00, 0x05, 0x00, 0x01})) // GSV enabled for every 5th position (used for testing only)
2016-02-02 06:35:48 +00:00
p . Write ( makeUBXCFG ( 0x06 , 0x01 , 8 , [ ] byte { 0xF0 , 0x04 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x01 } ) ) // RMC
p . Write ( makeUBXCFG ( 0x06 , 0x01 , 8 , [ ] byte { 0xF0 , 0x05 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x01 } ) ) // VGT
p . Write ( makeUBXCFG ( 0x06 , 0x01 , 8 , [ ] byte { 0xF0 , 0x06 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 } ) ) // GRS
p . Write ( makeUBXCFG ( 0x06 , 0x01 , 8 , [ ] byte { 0xF0 , 0x07 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 } ) ) // GST
p . Write ( makeUBXCFG ( 0x06 , 0x01 , 8 , [ ] byte { 0xF0 , 0x08 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 } ) ) // ZDA
p . Write ( makeUBXCFG ( 0x06 , 0x01 , 8 , [ ] byte { 0xF0 , 0x09 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 } ) ) // GBS
p . Write ( makeUBXCFG ( 0x06 , 0x01 , 8 , [ ] byte { 0xF0 , 0x0A , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 } ) ) // DTM
p . Write ( makeUBXCFG ( 0x06 , 0x01 , 8 , [ ] byte { 0xF0 , 0x0D , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 } ) ) // GNS
p . Write ( makeUBXCFG ( 0x06 , 0x01 , 8 , [ ] byte { 0xF0 , 0x0E , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 } ) ) // ???
p . Write ( makeUBXCFG ( 0x06 , 0x01 , 8 , [ ] byte { 0xF0 , 0x0F , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00 } ) ) // VLW
p . Write ( makeUBXCFG ( 0x06 , 0x01 , 8 , [ ] byte { 0xF1 , 0x00 , 0x01 , 0x01 , 0x01 , 0x01 , 0x01 , 0x00 } ) ) // Ublox,0
2016-05-08 16:36:05 +00:00
p . Write ( makeUBXCFG ( 0x06 , 0x01 , 8 , [ ] byte { 0xF1 , 0x03 , 0x05 , 0x05 , 0x05 , 0x05 , 0x05 , 0x00 } ) ) // Ublox,3
2016-02-02 06:35:48 +00:00
p . Write ( makeUBXCFG ( 0x06 , 0x01 , 8 , [ ] byte { 0xF1 , 0x04 , 0x0A , 0x0A , 0x0A , 0x0A , 0x0A , 0x00 } ) ) // Ublox,4
2016-02-03 05:44:54 +00:00
2016-02-02 06:35:48 +00:00
// Reconfigure serial port.
cfg := make ( [ ] byte , 20 )
cfg [ 0 ] = 0x01 // portID.
cfg [ 1 ] = 0x00 // res0.
cfg [ 2 ] = 0x00 // res1.
cfg [ 3 ] = 0x00 // res1.
2016-02-03 05:44:54 +00:00
2016-02-02 06:35:48 +00:00
// [ 7 ] [ 6 ] [ 5 ] [ 4 ]
// 0000 0000 0000 0000 0000 10x0 1100 0000
// UART mode. 0 stop bits, no parity, 8 data bits. Little endian order.
cfg [ 4 ] = 0xC0
cfg [ 5 ] = 0x08
cfg [ 6 ] = 0x00
cfg [ 7 ] = 0x00
2016-02-03 05:44:54 +00:00
2016-02-02 06:35:48 +00:00
// Baud rate. Little endian order.
bdrt := uint32 ( 38400 )
cfg [ 11 ] = byte ( ( bdrt >> 24 ) & 0xFF )
cfg [ 10 ] = byte ( ( bdrt >> 16 ) & 0xFF )
cfg [ 9 ] = byte ( ( bdrt >> 8 ) & 0xFF )
cfg [ 8 ] = byte ( bdrt & 0xFF )
2016-02-03 05:44:54 +00:00
2016-02-02 06:35:48 +00:00
// inProtoMask. NMEA and UBX. Little endian.
cfg [ 12 ] = 0x03
cfg [ 13 ] = 0x00
2016-02-03 05:44:54 +00:00
2016-02-02 06:35:48 +00:00
// outProtoMask. NMEA. Little endian.
cfg [ 14 ] = 0x02
cfg [ 15 ] = 0x00
cfg [ 16 ] = 0x00 // flags.
cfg [ 17 ] = 0x00 // flags.
cfg [ 18 ] = 0x00 //pad.
cfg [ 19 ] = 0x00 //pad.
2016-02-03 05:44:54 +00:00
2016-02-02 06:35:48 +00:00
p . Write ( makeUBXCFG ( 0x06 , 0x00 , 20 , cfg ) )
// time.Sleep(100* time.Millisecond) // pause and wait for the GPS to finish configuring itself before closing / reopening the port
baudrate = 38400
2016-02-17 03:13:21 +00:00
2016-03-23 13:18:28 +00:00
if globalSettings . DEBUG {
log . Printf ( "Finished writing u-blox GPS config to %s. Opening port to test connection.\n" , device )
}
2016-02-03 05:44:54 +00:00
}
2015-09-18 19:18:58 +00:00
p . Close ( )
2015-12-29 22:52:10 +00:00
time . Sleep ( 250 * time . Millisecond )
2016-02-10 07:06:52 +00:00
// Re-open port at newly configured baud so we can read messages. ReadTimeout is set to keep from blocking the gpsSerialReader() on misconfigures or ttyAMA disconnects
serialConfig = & serial . Config { Name : device , Baud : baudrate , ReadTimeout : time . Millisecond * 2500 }
2015-12-27 03:19:35 +00:00
p , err = serial . OpenPort ( serialConfig )
if err != nil {
log . Printf ( "serial port err: %s\n" , err . Error ( ) )
return false
}
serialPort = p
2015-08-15 00:11:04 +00:00
return true
}
2015-12-27 03:19:35 +00:00
// func validateNMEAChecksum determines if a string is a properly formatted NMEA sentence with a valid checksum.
//
// If the input string is valid, output is the input stripped of the "$" token and checksum, along with a boolean 'true'
// If the input string is the incorrect format, the checksum is missing/invalid, or checksum calculation fails, an error string and
// boolean 'false' are returned
//
// Checksum is calculated as XOR of all bytes between "$" and "*"
func validateNMEAChecksum ( s string ) ( string , bool ) {
//validate format. NMEA sentences start with "$" and end in "*xx" where xx is the XOR value of all bytes between
if ! ( strings . HasPrefix ( s , "$" ) && strings . Contains ( s , "*" ) ) {
return "Invalid NMEA message" , false
}
2015-12-29 22:00:59 +00:00
// strip leading "$" and split at "*"
2015-12-27 03:19:35 +00:00
s_split := strings . Split ( strings . TrimPrefix ( s , "$" ) , "*" )
s_out := s_split [ 0 ]
s_cs := s_split [ 1 ]
2015-12-29 22:00:59 +00:00
if len ( s_cs ) < 2 {
2015-12-27 03:19:35 +00:00
return "Missing checksum. Fewer than two bytes after asterisk" , false
}
2015-12-29 22:00:59 +00:00
2015-12-27 03:19:35 +00:00
cs , err := strconv . ParseUint ( s_cs [ : 2 ] , 16 , 8 )
if err != nil {
return "Invalid checksum" , false
}
cs_calc := byte ( 0 )
for i := range s_out {
cs_calc = cs_calc ^ byte ( s_out [ i ] )
}
2015-12-29 22:00:59 +00:00
if cs_calc != byte ( cs ) {
2015-12-27 03:19:35 +00:00
return fmt . Sprintf ( "Checksum failed. Calculated %#X; expected %#X" , cs_calc , cs ) , false
}
2015-12-29 22:00:59 +00:00
2015-12-27 03:19:35 +00:00
return s_out , true
}
2015-12-31 07:10:10 +00:00
// Only count this heading if a "sustained" >7 kts is obtained. This filters out a lot of heading
2015-12-29 22:14:03 +00:00
// changes while on the ground and "movement" is really only changes in GPS fix as it settles down.
//TODO: Some more robust checking above current and last speed.
2015-12-31 07:10:10 +00:00
//TODO: Dynamic adjust for gain based on groundspeed
2016-04-07 05:24:12 +00:00
func setTrueCourse ( groundSpeed uint16 , trueCourse float64 ) {
2015-12-29 22:14:03 +00:00
if myMPU6050 != nil && globalStatus . RY835AI_connected && globalSettings . AHRS_Enabled {
2015-12-31 07:10:10 +00:00
if mySituation . GroundSpeed >= 7 && groundSpeed >= 7 {
2016-04-07 05:24:12 +00:00
myMPU6050 . ResetHeading ( trueCourse , 0.10 )
2015-12-29 22:14:03 +00:00
}
}
}
2016-01-05 16:08:16 +00:00
func calculateNACp ( accuracy float32 ) uint8 {
ret := uint8 ( 0 )
if accuracy < 3 {
ret = 11
} else if accuracy < 10 {
ret = 10
} else if accuracy < 30 {
ret = 9
} else if accuracy < 92.6 {
ret = 8
} else if accuracy < 185.2 {
ret = 7
} else if accuracy < 555.6 {
ret = 6
}
return ret
}
2016-02-12 03:20:35 +00:00
/ *
processNMEALine parses NMEA - 01 83 formatted strings against several message types .
Standard messages supported : RMC GGA VTG GSA
U - blox proprietary messages : PUBX , 00 PUBX , 03 PUBX , 04
return is false if errors occur during parse , or if GPS position is invalid
return is true if parse occurs correctly and position is valid .
* /
2016-04-01 22:25:53 +00:00
func processNMEALine ( l string ) ( sentenceUsed bool ) {
2016-03-24 14:23:29 +00:00
mySituation . mu_GPS . Lock ( )
2016-04-01 22:25:53 +00:00
2016-03-24 14:37:23 +00:00
defer func ( ) {
2016-04-01 22:25:53 +00:00
if sentenceUsed || globalSettings . DEBUG {
logSituation ( )
}
2016-03-24 14:37:23 +00:00
mySituation . mu_GPS . Unlock ( )
} ( )
2016-03-24 14:23:29 +00:00
2015-12-27 03:19:35 +00:00
l_valid , validNMEAcs := validateNMEAChecksum ( l )
2015-12-29 22:00:59 +00:00
if ! validNMEAcs {
2015-12-27 03:19:35 +00:00
log . Printf ( "GPS error. Invalid NMEA string: %s\n" , l_valid ) // remove log message once validation complete
return false
}
x := strings . Split ( l_valid , "," )
2016-03-24 14:37:23 +00:00
mySituation . LastValidNMEAMessageTime = stratuxClock . Time
mySituation . LastValidNMEAMessage = l
2016-02-05 01:08:12 +00:00
2015-12-29 22:00:59 +00:00
if x [ 0 ] == "PUBX" { // UBX proprietary message
2016-03-24 14:23:29 +00:00
if x [ 1 ] == "00" { // Position fix.
2015-12-29 22:00:59 +00:00
if len ( x ) < 20 {
return false
}
2016-03-24 14:23:29 +00:00
tmpSituation := mySituation // If we decide to not use the data in this message, then don't make incomplete changes in mySituation.
2015-12-27 03:19:35 +00:00
2016-02-12 03:20:35 +00:00
// Do the accuracy / quality fields first to prevent invalid position etc. from being sent downstream
// field 8 = nav status
// DR = dead reckoning, G2= 2D GPS, G3 = 3D GPS, D2= 2D diff, D3 = 3D diff, RK = GPS+DR, TT = time only
if x [ 8 ] == "D2" || x [ 8 ] == "D3" {
2016-03-24 14:23:29 +00:00
tmpSituation . Quality = 2
2016-02-12 03:20:35 +00:00
} else if x [ 8 ] == "G2" || x [ 8 ] == "G3" {
2016-03-24 14:23:29 +00:00
tmpSituation . Quality = 1
2016-02-12 03:20:35 +00:00
} else if x [ 8 ] == "DR" || x [ 8 ] == "RK" {
2016-03-24 14:23:29 +00:00
tmpSituation . Quality = 6
2016-02-12 03:20:35 +00:00
} else if x [ 8 ] == "NF" {
2016-03-24 14:23:29 +00:00
tmpSituation . Quality = 0 // Just a note.
return false
2016-02-12 03:20:35 +00:00
} else {
2016-03-24 14:23:29 +00:00
tmpSituation . Quality = 0 // Just a note.
return false
2016-02-12 03:20:35 +00:00
}
// field 9 = horizontal accuracy, m
hAcc , err := strconv . ParseFloat ( x [ 9 ] , 32 )
if err != nil {
2016-03-24 14:23:29 +00:00
return false
2016-02-12 03:20:35 +00:00
}
2016-03-24 14:23:29 +00:00
tmpSituation . Accuracy = float32 ( hAcc * 2 ) // UBX reports 1-sigma variation; NACp is 95% confidence (2-sigma)
2016-02-12 03:20:35 +00:00
// NACp estimate.
2016-03-24 14:23:29 +00:00
tmpSituation . NACp = calculateNACp ( tmpSituation . Accuracy )
2016-02-12 03:20:35 +00:00
// field 10 = vertical accuracy, m
vAcc , err := strconv . ParseFloat ( x [ 10 ] , 32 )
if err != nil {
return false
}
2016-03-24 14:23:29 +00:00
tmpSituation . AccuracyVert = float32 ( vAcc * 2 ) // UBX reports 1-sigma variation; we want 95% confidence
2016-02-12 03:20:35 +00:00
2015-12-27 03:19:35 +00:00
// field 2 = time
2016-02-17 03:13:21 +00:00
if len ( x [ 2 ] ) < 8 {
2015-12-27 03:19:35 +00:00
return false
}
hr , err1 := strconv . Atoi ( x [ 2 ] [ 0 : 2 ] )
min , err2 := strconv . Atoi ( x [ 2 ] [ 2 : 4 ] )
2016-02-17 03:13:21 +00:00
sec , err3 := strconv . ParseFloat ( x [ 2 ] [ 4 : ] , 32 )
2015-12-27 03:19:35 +00:00
if err1 != nil || err2 != nil || err3 != nil {
return false
}
2016-03-24 14:23:29 +00:00
tmpSituation . LastFixSinceMidnightUTC = float32 ( 3600 * hr + 60 * min ) + float32 ( sec )
2015-12-27 03:19:35 +00:00
// field 3-4 = lat
if len ( x [ 3 ] ) < 10 {
return false
}
hr , err1 = strconv . Atoi ( x [ 3 ] [ 0 : 2 ] )
2016-02-17 03:13:21 +00:00
minf , err2 := strconv . ParseFloat ( x [ 3 ] [ 2 : ] , 32 )
2015-12-27 03:19:35 +00:00
if err1 != nil || err2 != nil {
return false
}
2016-03-24 14:23:29 +00:00
tmpSituation . Lat = float32 ( hr ) + float32 ( minf / 60.0 )
2015-12-27 03:19:35 +00:00
if x [ 4 ] == "S" { // South = negative.
2016-03-24 14:23:29 +00:00
tmpSituation . Lat = - tmpSituation . Lat
2015-12-27 03:19:35 +00:00
}
// field 5-6 = lon
if len ( x [ 5 ] ) < 11 {
return false
}
hr , err1 = strconv . Atoi ( x [ 5 ] [ 0 : 3 ] )
2016-02-17 03:13:21 +00:00
minf , err2 = strconv . ParseFloat ( x [ 5 ] [ 3 : ] , 32 )
2015-12-27 03:19:35 +00:00
if err1 != nil || err2 != nil {
return false
}
2016-03-24 14:23:29 +00:00
tmpSituation . Lng = float32 ( hr ) + float32 ( minf / 60.0 )
2015-12-27 03:19:35 +00:00
if x [ 6 ] == "W" { // West = negative.
2016-03-24 14:23:29 +00:00
tmpSituation . Lng = - tmpSituation . Lng
2015-12-27 03:19:35 +00:00
}
2015-12-28 05:57:31 +00:00
// field 7 = height above ellipsoid, m
2015-12-27 03:19:35 +00:00
2015-12-28 05:57:31 +00:00
hae , err1 := strconv . ParseFloat ( x [ 7 ] , 32 )
2015-12-27 03:19:35 +00:00
if err1 != nil {
return false
}
2016-03-24 14:23:29 +00:00
alt := float32 ( hae * 3.28084 ) - tmpSituation . GeoidSep // convert to feet and offset by geoid separation
tmpSituation . HeightAboveEllipsoid = float32 ( hae * 3.28084 ) // feet
tmpSituation . Alt = alt
2015-12-27 03:19:35 +00:00
2016-03-24 14:23:29 +00:00
tmpSituation . LastFixLocalTime = stratuxClock . Time
2015-12-27 03:19:35 +00:00
// field 11 = groundspeed, km/h
groundspeed , err := strconv . ParseFloat ( x [ 11 ] , 32 )
if err != nil {
return false
}
groundspeed = groundspeed * 0.540003 // convert to knots
2016-03-24 14:23:29 +00:00
tmpSituation . GroundSpeed = uint16 ( groundspeed )
2015-12-29 22:00:59 +00:00
2015-12-27 03:19:35 +00:00
// field 12 = track, deg
2016-04-07 05:24:12 +00:00
trueCourse := float32 ( 0.0 )
2016-02-12 02:00:57 +00:00
tc , err := strconv . ParseFloat ( x [ 12 ] , 32 )
if err != nil {
return false
}
if groundspeed > 3 { // TO-DO: use average groundspeed over last n seconds to avoid random "jumps"
2016-04-07 05:24:12 +00:00
trueCourse = float32 ( tc )
setTrueCourse ( uint16 ( groundspeed ) , tc )
tmpSituation . TrueCourse = trueCourse
2015-12-27 03:19:35 +00:00
} else {
2016-02-12 02:00:57 +00:00
// Negligible movement. Don't update course, but do use the slow speed.
// TO-DO: use average course over last n seconds?
2015-12-27 03:19:35 +00:00
}
2016-03-24 14:23:29 +00:00
tmpSituation . LastGroundTrackTime = stratuxClock . Time
2015-12-27 03:19:35 +00:00
// field 13 = vertical velocity, m/s
2015-12-29 22:00:59 +00:00
vv , err := strconv . ParseFloat ( x [ 13 ] , 32 )
if err != nil {
return false
}
2016-03-24 14:23:29 +00:00
tmpSituation . GPSVertVel = float32 ( vv * - 3.28084 ) // convert to ft/sec and positive = up
2015-12-27 03:19:35 +00:00
// field 14 = age of diff corrections
// field 18 = number of satellites
sat , err1 := strconv . Atoi ( x [ 18 ] )
if err1 != nil {
return false
}
2016-05-08 16:36:05 +00:00
tmpSituation . Satellites = uint16 ( sat ) // this seems to be reliable. UBX,03 handles >12 satellites solutions correctly.
2015-12-27 03:19:35 +00:00
2016-03-24 14:23:29 +00:00
// We've made it this far, so that means we've processed "everything" and can now make the change to mySituation.
mySituation = tmpSituation
return true
2016-05-08 20:41:49 +00:00
} else if x [ 1 ] == "03" { // satellite status message. Only the first 20 satellites will be reported in this message for UBX firmware older than v3.0. Order seems to be GPS, then SBAS, then GLONASS.
2015-12-27 08:47:37 +00:00
2016-05-14 02:32:11 +00:00
if len ( x ) < 3 { // malformed UBX,03 message that somehow passed checksum verification but is missing all of its fields
return false
}
2015-12-27 08:47:37 +00:00
// field 2 = number of satellites tracked
2016-05-08 01:53:12 +00:00
//satSeen := 0 // satellites seen (signal present)
2015-12-27 08:47:37 +00:00
satTracked , err := strconv . Atoi ( x [ 2 ] )
2015-12-29 22:00:59 +00:00
if err != nil {
return false
2015-12-28 05:57:31 +00:00
}
2016-05-14 02:32:11 +00:00
2016-05-08 20:41:49 +00:00
if globalSettings . DEBUG {
2016-05-14 02:32:11 +00:00
log . Printf ( "GPS PUBX,03 message with %d satellites is %d fields long. (Should be %d fields long)\n" , satTracked , len ( x ) , satTracked * 6 + 3 )
}
if len ( x ) < ( satTracked * 6 + 3 ) { // malformed UBX,03 message that somehow passed checksum verification but is missing some of its fields
if globalSettings . DEBUG {
log . Printf ( "GPS PUBX,03 message is missing fields\n" )
}
return false
2016-05-08 05:33:05 +00:00
}
2016-05-08 20:41:49 +00:00
mySituation . SatellitesTracked = uint16 ( satTracked ) // requires UBX M8N firmware v3.01 or later to report > 20 satellites
2015-12-29 22:00:59 +00:00
// fields 3-8 are repeated block
2016-02-12 03:20:35 +00:00
2016-05-08 01:53:12 +00:00
var sv , elev , az , cno int
var svType uint8
var svStr string
/ * Reference for constellation tracking
for i := 0 ; i < satTracked ; i ++ {
x [ 3 + 6 * i ] // sv number
x [ 4 + 6 * i ] // status [ U | e | - ] indicates [U]sed in solution, [e]phemeris data only, [-] not used
x [ 5 + 6 * i ] // azimuth, deg, 0-359
x [ 6 + 6 * i ] // elevation, deg, 0-90
x [ 7 + 6 * i ] // signal strength dB-Hz
x [ 8 + 6 * i ] // lock time, sec, 0-64
}
* /
2015-12-29 22:00:59 +00:00
for i := 0 ; i < satTracked ; i ++ {
2016-05-08 01:53:12 +00:00
//field 3+6i is sv number. GPS NMEA = PRN. GLONASS NMEA = PRN + 65. SBAS is PRN; needs to be converted to NMEA for compatiblity with GSV messages.
sv , err = strconv . Atoi ( x [ 3 + 6 * i ] ) // sv number
if err != nil {
return false
}
if sv < 33 { // indicates GPS
svType = SAT_TYPE_GPS
svStr = fmt . Sprintf ( "G%d" , sv )
} else if sv < 65 { // indicates SBAS: WAAS, EGNOS, MSAS, etc.
svType = SAT_TYPE_SBAS
svStr = fmt . Sprintf ( "S%d" , sv + 87 ) // add 87 to convert from NMEA to PRN.
} else if sv < 97 { // GLONASS
svType = SAT_TYPE_GLONASS
svStr = fmt . Sprintf ( "R%d" , sv - 64 ) // subtract 64 to convert from NMEA to PRN.
} else if ( sv >= 120 ) && ( sv < 162 ) { // indicates SBAS: WAAS, EGNOS, MSAS, etc.
svType = SAT_TYPE_SBAS
svStr = fmt . Sprintf ( "S%d" , sv )
sv -= 87 // subtract 87 to convert to NMEA from PRN.
} else { // TO-DO: Galileo
svType = SAT_TYPE_UNKNOWN
svStr = fmt . Sprintf ( "U%d" , sv )
}
var thisSatellite SatelliteInfo
// START OF PROTECTED BLOCK
satelliteMutex . Lock ( )
// Retrieve previous information on this satellite code.
if val , ok := Satellites [ svStr ] ; ok { // if we've already seen this satellite identifier, copy it in to do updates
thisSatellite = val
2016-05-08 20:41:49 +00:00
//log.Printf("UBX,03: Satellite %s already seen. Retrieving from 'Satellites'.\n", svStr) // DEBUG
2016-05-08 01:53:12 +00:00
} else { // this satellite isn't in the Satellites data structure
thisSatellite . SatelliteID = svStr
thisSatellite . SatelliteNMEA = uint8 ( sv )
thisSatellite . Type = uint8 ( svType )
2016-05-08 20:41:49 +00:00
//log.Printf("UBX,03: Creating new satellite %s\n", svStr) // DEBUG
2016-05-08 01:53:12 +00:00
}
thisSatellite . TimeLastTracked = stratuxClock . Time
// Field 6+6*i is elevation, deg, 0-90
elev , err = strconv . Atoi ( x [ 6 + 6 * i ] ) // elevation
if err != nil { // could be blank if no position fix. Represent as -999.
elev = - 999
}
thisSatellite . Elevation = int16 ( elev )
// Field 5+6*i is azimuth, deg, 0-359
az , err = strconv . Atoi ( x [ 5 + 6 * i ] ) // azimuth
if err != nil { // could be blank if no position fix. Represent as -999.
az = - 999
}
thisSatellite . Azimuth = int16 ( az )
// Field 7+6*i is signal strength dB-Hz
cno , err = strconv . Atoi ( x [ 7 + 6 * i ] ) // signal
if err != nil { // will be blank if satellite isn't being received. Represent as -99.
cno = - 99
} else if cno > 0 {
thisSatellite . TimeLastSeen = stratuxClock . Time // Is this needed?
2015-12-29 22:00:59 +00:00
}
2016-05-08 01:53:12 +00:00
thisSatellite . Signal = int8 ( cno )
// Field 4+6*i is status: [ U | e | - ]: [U]sed in solution, [e]phemeris data only, [-] not used
if x [ 4 + 6 * i ] == "U" {
thisSatellite . InSolution = true
2016-05-08 05:14:20 +00:00
thisSatellite . TimeLastSolution = stratuxClock . Time
2016-05-08 01:53:12 +00:00
} else if x [ 4 + 6 * i ] == "e" {
thisSatellite . InSolution = false
2016-05-08 05:33:05 +00:00
//log.Printf("Satellite %s is no longer in solution but has ephemeris - UBX,03\n", svStr) // DEBUG
2016-05-08 01:53:12 +00:00
// do anything that needs to be done for ephemeris
} else {
thisSatellite . InSolution = false
2016-05-08 05:33:05 +00:00
//log.Printf("Satellite %s is no longer in solution and has no ephemeris - UBX,03\n", svStr) // DEBUG
2016-05-08 01:53:12 +00:00
}
if globalSettings . DEBUG {
2016-05-14 02:32:11 +00:00
inSolnStr := " "
if thisSatellite . InSolution {
inSolnStr = "+"
}
log . Printf ( "UBX: Satellite %s%s at index %d. Type = %d, NMEA-ID = %d, Elev = %d, Azimuth = %d, Cno = %d\n" , inSolnStr , svStr , i , svType , sv , elev , az , cno ) // remove later?
2016-05-08 01:53:12 +00:00
}
Satellites [ thisSatellite . SatelliteID ] = thisSatellite // Update constellation with this satellite
updateConstellation ( )
satelliteMutex . Unlock ( )
// END OF PROTECTED BLOCK
// end of satellite iteration loop
2015-12-29 22:00:59 +00:00
}
2015-12-28 05:57:31 +00:00
2016-03-24 14:23:29 +00:00
return true
2015-12-29 22:00:59 +00:00
} else if x [ 1 ] == "04" { // clock message
2016-01-01 05:46:25 +00:00
// field 5 is UTC week (epoch = 1980-JAN-06). If this is invalid, do not parse date / time
utcWeek , err0 := strconv . Atoi ( x [ 5 ] )
if err0 != nil {
// log.Printf("Error reading GPS week\n")
return false
}
if utcWeek < 1877 || utcWeek >= 32767 { // unless we're in a flying Delorean, UTC dates before 2016-JAN-01 are not valid. Check underflow condition as well.
2016-06-30 15:51:56 +00:00
if globalSettings . DEBUG {
log . Printf ( "GPS week # %v out of scope; not setting time and date\n" , utcWeek )
}
2016-01-01 05:46:25 +00:00
return false
} / * else {
log . Printf ( "GPS week # %v valid; evaluate time and date\n" , utcWeek ) //debug option
} * /
2015-12-27 08:47:37 +00:00
// field 2 is UTC time
2016-02-17 04:36:56 +00:00
if len ( x [ 2 ] ) < 7 {
2015-12-27 08:47:37 +00:00
return false
}
hr , err1 := strconv . Atoi ( x [ 2 ] [ 0 : 2 ] )
min , err2 := strconv . Atoi ( x [ 2 ] [ 2 : 4 ] )
2016-02-17 04:36:56 +00:00
sec , err3 := strconv . ParseFloat ( x [ 2 ] [ 4 : ] , 32 )
2015-12-27 08:47:37 +00:00
if err1 != nil || err2 != nil || err3 != nil {
return false
}
2015-12-29 22:00:59 +00:00
2015-12-27 08:47:37 +00:00
// field 3 is date
if len ( x [ 3 ] ) == 6 {
// Date of Fix, i.e 191115 = 19 November 2015 UTC field 9
2016-02-17 04:36:56 +00:00
gpsTimeStr := fmt . Sprintf ( "%s %02d:%02d:%06.3f" , x [ 3 ] , hr , min , sec )
gpsTime , err := time . Parse ( "020106 15:04:05.000" , gpsTimeStr )
2015-12-27 08:47:37 +00:00
if err == nil {
2016-03-24 14:23:29 +00:00
// We only update ANY of the times if all of the time parsing is complete.
2016-02-17 03:13:21 +00:00
mySituation . LastGPSTimeTime = stratuxClock . Time
2016-03-24 12:59:34 +00:00
mySituation . GPSTime = gpsTime
2016-03-24 14:23:29 +00:00
mySituation . LastFixSinceMidnightUTC = float32 ( 3600 * hr + 60 * min ) + float32 ( sec )
2016-01-01 05:46:25 +00:00
// log.Printf("GPS time is: %s\n", gpsTime) //debug
if time . Since ( gpsTime ) > 3 * time . Second || time . Since ( gpsTime ) < - 3 * time . Second {
2016-01-09 18:15:21 +00:00
setStr := gpsTime . Format ( "20060102 15:04:05.000" ) + " UTC"
log . Printf ( "setting system time to: '%s'\n" , setStr )
2015-12-27 08:47:37 +00:00
if err := exec . Command ( "date" , "-s" , setStr ) . Run ( ) ; err != nil {
log . Printf ( "Set Date failure: %s error\n" , err )
2016-01-09 18:15:21 +00:00
} else {
log . Printf ( "Time set from GPS. Current time is %v\n" , time . Now ( ) )
2015-12-27 08:47:37 +00:00
}
}
2016-03-24 21:26:59 +00:00
setDataLogTimeWithGPS ( mySituation )
2016-03-24 14:23:29 +00:00
return true // All possible successes lead here.
2015-12-27 08:47:37 +00:00
}
}
2015-12-29 22:00:59 +00:00
}
2015-12-27 03:19:35 +00:00
2016-02-12 02:00:57 +00:00
// otherwise parse the NMEA standard messages as a compatibility option for SIRF, generic NMEA, etc.
2015-12-27 03:19:35 +00:00
} else if ( x [ 0 ] == "GNVTG" ) || ( x [ 0 ] == "GPVTG" ) { // Ground track information.
2016-03-24 14:23:29 +00:00
tmpSituation := mySituation // If we decide to not use the data in this message, then don't make incomplete changes in mySituation.
if len ( x ) < 9 { // Reduce from 10 to 9 to allow parsing by devices pre-NMEA v2.3
2016-02-12 02:00:57 +00:00
return false
}
groundspeed , err := strconv . ParseFloat ( x [ 5 ] , 32 ) // Knots.
if err != nil {
2015-08-15 00:11:04 +00:00
return false
}
2016-03-24 14:23:29 +00:00
tmpSituation . GroundSpeed = uint16 ( groundspeed )
2016-02-12 02:00:57 +00:00
2016-04-07 05:24:12 +00:00
trueCourse := float32 ( 0 )
2016-02-12 02:00:57 +00:00
tc , err := strconv . ParseFloat ( x [ 1 ] , 32 )
2015-08-15 00:11:04 +00:00
if err != nil {
return false
}
2016-02-12 02:00:57 +00:00
if groundspeed > 3 { // TO-DO: use average groundspeed over last n seconds to avoid random "jumps"
2016-04-07 05:24:12 +00:00
trueCourse = float32 ( tc )
setTrueCourse ( uint16 ( groundspeed ) , tc )
tmpSituation . TrueCourse = trueCourse
2016-02-12 02:00:57 +00:00
} else {
// Negligible movement. Don't update course, but do use the slow speed.
// TO-DO: use average course over last n seconds?
}
2016-03-24 14:23:29 +00:00
tmpSituation . LastGroundTrackTime = stratuxClock . Time
// We've made it this far, so that means we've processed "everything" and can now make the change to mySituation.
mySituation = tmpSituation
return true
} else if ( x [ 0 ] == "GNGGA" ) || ( x [ 0 ] == "GPGGA" ) { // Position fix.
tmpSituation := mySituation // If we decide to not use the data in this message, then don't make incomplete changes in mySituation.
2015-08-20 20:47:05 +00:00
2015-08-15 00:11:04 +00:00
if len ( x ) < 15 {
return false
}
2016-02-12 03:20:35 +00:00
// Quality indicator.
q , err1 := strconv . Atoi ( x [ 6 ] )
if err1 != nil {
return false
}
2016-03-24 14:23:29 +00:00
tmpSituation . Quality = uint8 ( q ) // 1 = 3D GPS; 2 = DGPS (SBAS /WAAS)
2016-02-12 03:20:35 +00:00
2015-08-15 00:11:04 +00:00
// Timestamp.
2016-02-17 03:13:21 +00:00
if len ( x [ 1 ] ) < 7 {
2015-08-15 00:11:04 +00:00
return false
}
hr , err1 := strconv . Atoi ( x [ 1 ] [ 0 : 2 ] )
min , err2 := strconv . Atoi ( x [ 1 ] [ 2 : 4 ] )
2016-02-17 03:13:21 +00:00
sec , err3 := strconv . ParseFloat ( x [ 1 ] [ 4 : ] , 32 )
2015-08-15 00:11:04 +00:00
if err1 != nil || err2 != nil || err3 != nil {
return false
}
2016-03-24 14:23:29 +00:00
tmpSituation . LastFixSinceMidnightUTC = float32 ( 3600 * hr + 60 * min ) + float32 ( sec )
2015-08-15 00:11:04 +00:00
// Latitude.
2016-01-06 03:09:42 +00:00
if len ( x [ 2 ] ) < 4 {
2015-08-15 00:11:04 +00:00
return false
}
2015-09-23 03:51:56 +00:00
2015-08-15 00:11:04 +00:00
hr , err1 = strconv . Atoi ( x [ 2 ] [ 0 : 2 ] )
2016-01-06 03:09:42 +00:00
minf , err2 := strconv . ParseFloat ( x [ 2 ] [ 2 : ] , 32 )
2015-08-15 00:11:04 +00:00
if err1 != nil || err2 != nil {
return false
}
2016-03-24 14:23:29 +00:00
tmpSituation . Lat = float32 ( hr ) + float32 ( minf / 60.0 )
2015-08-15 00:11:04 +00:00
if x [ 3 ] == "S" { // South = negative.
2016-03-24 14:23:29 +00:00
tmpSituation . Lat = - tmpSituation . Lat
2015-08-15 00:11:04 +00:00
}
// Longitude.
2016-01-06 03:09:42 +00:00
if len ( x [ 4 ] ) < 5 {
2015-08-15 00:11:04 +00:00
return false
}
hr , err1 = strconv . Atoi ( x [ 4 ] [ 0 : 3 ] )
2016-01-06 03:09:42 +00:00
minf , err2 = strconv . ParseFloat ( x [ 4 ] [ 3 : ] , 32 )
2015-08-15 00:11:04 +00:00
if err1 != nil || err2 != nil {
return false
}
2016-03-24 14:23:29 +00:00
tmpSituation . Lng = float32 ( hr ) + float32 ( minf / 60.0 )
2015-08-15 00:11:04 +00:00
if x [ 5 ] == "W" { // West = negative.
2016-03-24 14:23:29 +00:00
tmpSituation . Lng = - tmpSituation . Lng
2015-08-15 00:11:04 +00:00
}
// Altitude.
alt , err1 := strconv . ParseFloat ( x [ 9 ] , 32 )
if err1 != nil {
return false
}
2016-03-24 14:23:29 +00:00
tmpSituation . Alt = float32 ( alt * 3.28084 ) // Convert to feet.
2015-08-15 00:11:04 +00:00
2015-12-28 05:57:31 +00:00
// Geoid separation (Sep = HAE - MSL)
// (needed for proper MSL offset on PUBX,00 altitudes)
2015-12-29 22:00:59 +00:00
geoidSep , err1 := strconv . ParseFloat ( x [ 11 ] , 32 )
if err1 != nil {
return false
}
2016-03-24 14:23:29 +00:00
tmpSituation . GeoidSep = float32 ( geoidSep * 3.28084 ) // Convert to feet.
tmpSituation . HeightAboveEllipsoid = tmpSituation . GeoidSep + tmpSituation . Alt
2015-08-15 00:11:04 +00:00
// Timestamp.
2016-03-24 14:23:29 +00:00
tmpSituation . LastFixLocalTime = stratuxClock . Time
// We've made it this far, so that means we've processed "everything" and can now make the change to mySituation.
mySituation = tmpSituation
return true
} else if ( x [ 0 ] == "GNRMC" ) || ( x [ 0 ] == "GPRMC" ) { // Recommended Minimum data. FIXME: Is this needed anymore?
tmpSituation := mySituation // If we decide to not use the data in this message, then don't make incomplete changes in mySituation.
2015-08-15 00:11:04 +00:00
2015-09-23 03:51:56 +00:00
//$GPRMC,123519,A,4807.038,N,01131.000,E,022.4,084.4,230394,003.1,W*6A
/ * check RY835 man for NMEA version , if > 2.2 , add mode field
Where :
RMC Recommended Minimum sentence C
123519 Fix taken at 12 : 35 : 19 UTC
A Status A = active or V = Void .
4807.038 , N Latitude 48 deg 07.038 ' N
01131.000 , E Longitude 11 deg 31.000 ' E
022.4 Speed over the ground in knots
084.4 Track angle in degrees True
230394 Date - 23 rd of March 1994
003.1 , W Magnetic Variation
D mode field ( nmea 2.3 and higher )
* 6 A The checksum data , always begins with *
* /
2016-02-17 03:13:21 +00:00
if len ( x ) < 11 {
2015-09-23 03:51:56 +00:00
return false
}
2015-10-19 17:18:48 +00:00
2016-02-12 03:20:35 +00:00
if x [ 2 ] != "A" { // invalid fix
2016-03-24 14:23:29 +00:00
tmpSituation . Quality = 0 // Just a note.
2016-02-12 03:20:35 +00:00
return false
}
2015-09-23 03:51:56 +00:00
// Timestamp.
2016-02-17 03:13:21 +00:00
if len ( x [ 1 ] ) < 7 {
2015-09-23 03:51:56 +00:00
return false
}
hr , err1 := strconv . Atoi ( x [ 1 ] [ 0 : 2 ] )
min , err2 := strconv . Atoi ( x [ 1 ] [ 2 : 4 ] )
2016-02-17 03:13:21 +00:00
sec , err3 := strconv . ParseFloat ( x [ 1 ] [ 4 : ] , 32 )
2015-09-23 03:51:56 +00:00
if err1 != nil || err2 != nil || err3 != nil {
return false
}
2016-03-24 14:23:29 +00:00
tmpSituation . LastFixSinceMidnightUTC = float32 ( 3600 * hr + 60 * min ) + float32 ( sec )
2015-10-19 17:18:48 +00:00
if len ( x [ 9 ] ) == 6 {
// Date of Fix, i.e 191115 = 19 November 2015 UTC field 9
2016-02-18 17:53:43 +00:00
gpsTimeStr := fmt . Sprintf ( "%s %02d:%02d:%06.3f" , x [ 9 ] , hr , min , sec )
2016-02-17 04:36:56 +00:00
gpsTime , err := time . Parse ( "020106 15:04:05.000" , gpsTimeStr )
2015-10-19 17:18:48 +00:00
if err == nil {
2016-03-24 14:23:29 +00:00
tmpSituation . LastGPSTimeTime = stratuxClock . Time
tmpSituation . GPSTime = gpsTime
2016-01-01 05:46:25 +00:00
if time . Since ( gpsTime ) > 3 * time . Second || time . Since ( gpsTime ) < - 3 * time . Second {
2016-01-09 18:15:21 +00:00
setStr := gpsTime . Format ( "20060102 15:04:05.000" ) + " UTC"
log . Printf ( "setting system time to: '%s'\n" , setStr )
2015-10-19 17:18:48 +00:00
if err := exec . Command ( "date" , "-s" , setStr ) . Run ( ) ; err != nil {
log . Printf ( "Set Date failure: %s error\n" , err )
2016-01-09 18:15:21 +00:00
} else {
log . Printf ( "Time set from GPS. Current time is %v\n" , time . Now ( ) )
2015-10-19 17:18:48 +00:00
}
}
}
}
2015-09-23 03:51:56 +00:00
// Latitude.
2016-01-06 03:09:42 +00:00
if len ( x [ 3 ] ) < 4 {
2015-09-23 03:51:56 +00:00
return false
}
hr , err1 = strconv . Atoi ( x [ 3 ] [ 0 : 2 ] )
2016-01-06 03:09:42 +00:00
minf , err2 := strconv . ParseFloat ( x [ 3 ] [ 2 : ] , 32 )
2015-09-23 03:51:56 +00:00
if err1 != nil || err2 != nil {
return false
}
2016-03-24 14:23:29 +00:00
tmpSituation . Lat = float32 ( hr ) + float32 ( minf / 60.0 )
2015-09-23 03:51:56 +00:00
if x [ 4 ] == "S" { // South = negative.
2016-03-24 14:23:29 +00:00
tmpSituation . Lat = - tmpSituation . Lat
2015-09-23 03:51:56 +00:00
}
// Longitude.
2016-01-06 03:09:42 +00:00
if len ( x [ 5 ] ) < 5 {
2015-09-23 03:51:56 +00:00
return false
}
hr , err1 = strconv . Atoi ( x [ 5 ] [ 0 : 3 ] )
2016-01-06 03:09:42 +00:00
minf , err2 = strconv . ParseFloat ( x [ 5 ] [ 3 : ] , 32 )
2015-09-23 03:51:56 +00:00
if err1 != nil || err2 != nil {
return false
}
2016-03-24 14:23:29 +00:00
tmpSituation . Lng = float32 ( hr ) + float32 ( minf / 60.0 )
2015-09-23 03:51:56 +00:00
if x [ 6 ] == "W" { // West = negative.
2016-03-24 14:23:29 +00:00
tmpSituation . Lng = - tmpSituation . Lng
2015-09-23 03:51:56 +00:00
}
2016-02-12 03:20:35 +00:00
2016-03-24 14:23:29 +00:00
tmpSituation . LastFixLocalTime = stratuxClock . Time
2016-02-12 03:20:35 +00:00
2015-09-23 03:51:56 +00:00
// ground speed in kts (field 7)
groundspeed , err := strconv . ParseFloat ( x [ 7 ] , 32 )
if err != nil {
return false
}
2016-03-24 14:23:29 +00:00
tmpSituation . GroundSpeed = uint16 ( groundspeed )
2016-02-12 03:20:35 +00:00
// ground track "True" (field 8)
2016-04-07 05:24:12 +00:00
trueCourse := float32 ( 0 )
2015-09-23 03:51:56 +00:00
tc , err := strconv . ParseFloat ( x [ 8 ] , 32 )
if err != nil {
return false
}
2016-02-12 03:20:35 +00:00
if groundspeed > 3 { // TO-DO: use average groundspeed over last n seconds to avoid random "jumps"
2016-04-07 05:24:12 +00:00
trueCourse = float32 ( tc )
setTrueCourse ( uint16 ( groundspeed ) , tc )
tmpSituation . TrueCourse = trueCourse
2016-02-12 03:20:35 +00:00
} else {
// Negligible movement. Don't update course, but do use the slow speed.
// TO-DO: use average course over last n seconds?
}
2016-03-24 14:23:29 +00:00
tmpSituation . LastGroundTrackTime = stratuxClock . Time
// We've made it this far, so that means we've processed "everything" and can now make the change to mySituation.
mySituation = tmpSituation
2016-03-24 21:26:59 +00:00
setDataLogTimeWithGPS ( mySituation )
2016-03-24 14:23:29 +00:00
return true
} else if ( x [ 0 ] == "GNGSA" ) || ( x [ 0 ] == "GPGSA" ) { // Satellite data.
tmpSituation := mySituation // If we decide to not use the data in this message, then don't make incomplete changes in mySituation.
2016-01-04 00:14:29 +00:00
2016-01-03 23:54:42 +00:00
if len ( x ) < 18 {
return false
}
2016-01-04 00:14:29 +00:00
2016-01-03 23:54:42 +00:00
// field 1: operation mode
2016-02-02 06:35:48 +00:00
// M: manual forced to 2D or 3D mode
// A: automatic switching between 2D and 3D modes
2016-05-08 05:14:20 +00:00
/ *
2016-05-08 20:41:49 +00:00
if ( x [ 1 ] != "A" ) && ( x [ 1 ] != "M" ) { // invalid fix ... but x[2] is a better indicator of fix quality. Deprecating this.
2016-05-08 05:14:20 +00:00
tmpSituation . Quality = 0 // Just a note.
return false
}
* /
2016-01-04 00:14:29 +00:00
2016-01-03 23:54:42 +00:00
// field 2: solution type
// 1 = no solution; 2 = 2D fix, 3 = 3D fix. WAAS status is parsed from GGA message, so no need to get here
2016-05-08 05:14:20 +00:00
if ( x [ 2 ] == "" ) || ( x [ 2 ] == "1" ) { // missing or no solution
tmpSituation . Quality = 0 // Just a note.
return false
}
2016-01-03 23:54:42 +00:00
// fields 3-14: satellites in solution
2016-05-07 15:19:42 +00:00
var svStr string
var svType uint8
2016-05-16 02:17:01 +00:00
var svSBAS bool // used to indicate whether this GSA message contains a SBAS satellite
var svGLONASS bool // used to indicate whether this GSA message contains GLONASS satellites
2016-01-03 23:54:42 +00:00
sat := 0
2016-05-07 15:19:42 +00:00
2016-01-03 23:54:42 +00:00
for _ , svtxt := range x [ 3 : 15 ] {
2016-05-07 15:19:42 +00:00
sv , err := strconv . Atoi ( svtxt )
2016-01-03 23:54:42 +00:00
if err == nil {
sat ++
2016-05-07 15:19:42 +00:00
if sv < 33 { // indicates GPS
svType = SAT_TYPE_GPS
svStr = fmt . Sprintf ( "G%d" , sv )
} else if sv < 65 { // indicates SBAS: WAAS, EGNOS, MSAS, etc.
svType = SAT_TYPE_SBAS
2016-05-08 01:53:12 +00:00
svStr = fmt . Sprintf ( "S%d" , sv + 87 ) // add 87 to convert from NMEA to PRN.
2016-05-16 02:17:01 +00:00
svSBAS = true
2016-05-07 15:19:42 +00:00
} else if sv < 97 { // GLONASS
svType = SAT_TYPE_GLONASS
svStr = fmt . Sprintf ( "R%d" , sv - 64 ) // subtract 64 to convert from NMEA to PRN.
2016-05-16 02:17:01 +00:00
svGLONASS = true
2016-05-07 20:48:02 +00:00
} else { // TO-DO: Galileo
2016-05-07 15:19:42 +00:00
svType = SAT_TYPE_UNKNOWN
svStr = fmt . Sprintf ( "U%d" , sv )
}
var thisSatellite SatelliteInfo
// START OF PROTECTED BLOCK
satelliteMutex . Lock ( )
// Retrieve previous information on this satellite code.
if val , ok := Satellites [ svStr ] ; ok { // if we've already seen this satellite identifier, copy it in to do updates
thisSatellite = val
//log.Printf("Satellite %s already seen. Retrieving from 'Satellites'.\n", svStr)
} else { // this satellite isn't in the Satellites data structure, so create it
thisSatellite . SatelliteID = svStr
thisSatellite . SatelliteNMEA = uint8 ( sv )
thisSatellite . Type = uint8 ( svType )
2016-05-08 05:33:05 +00:00
//log.Printf("Creating new satellite %s from GSA message\n", svStr) // DEBUG
2016-05-07 15:19:42 +00:00
}
thisSatellite . InSolution = true
2016-05-08 05:14:20 +00:00
thisSatellite . TimeLastSolution = stratuxClock . Time
2016-05-07 15:19:42 +00:00
thisSatellite . TimeLastSeen = stratuxClock . Time // implied, since this satellite is used in the position solution
thisSatellite . TimeLastTracked = stratuxClock . Time // implied, since this satellite is used in the position solution
Satellites [ thisSatellite . SatelliteID ] = thisSatellite // Update constellation with this satellite
updateConstellation ( )
satelliteMutex . Unlock ( )
// END OF PROTECTED BLOCK
2016-01-03 23:54:42 +00:00
}
}
2016-05-08 21:55:48 +00:00
if sat < 12 || tmpSituation . Satellites < 13 { // GSA only reports up to 12 satellites in solution, so we don't want to overwrite higher counts based on updateConstellation().
tmpSituation . Satellites = uint16 ( sat )
2016-05-16 02:17:01 +00:00
if ( tmpSituation . Quality == 2 ) && ! svSBAS && ! svGLONASS { // add one to the satellite count if we have a SBAS solution, but the GSA message doesn't track a SBAS satellite
tmpSituation . Satellites ++
}
2016-05-08 20:41:49 +00:00
}
//log.Printf("There are %d satellites in solution from this GSA message\n", sat) // TESTING - DEBUG
2016-01-04 00:14:29 +00:00
2016-01-03 23:54:42 +00:00
// field 16: HDOP
// Accuracy estimate
hdop , err1 := strconv . ParseFloat ( x [ 16 ] , 32 )
if err1 != nil {
return false
}
2016-03-24 14:23:29 +00:00
if tmpSituation . Quality == 2 {
tmpSituation . Accuracy = float32 ( hdop * 4.0 ) // Rough 95% confidence estimate for WAAS / DGPS solution
2016-01-03 23:54:42 +00:00
} else {
2016-03-24 14:23:29 +00:00
tmpSituation . Accuracy = float32 ( hdop * 8.0 ) // Rough 95% confidence estimate for 3D non-WAAS solution
2016-01-03 23:54:42 +00:00
}
// NACp estimate.
2016-03-24 14:23:29 +00:00
tmpSituation . NACp = calculateNACp ( tmpSituation . Accuracy )
2016-01-03 23:54:42 +00:00
// field 17: VDOP
// accuracy estimate
vdop , err1 := strconv . ParseFloat ( x [ 17 ] , 32 )
if err1 != nil {
return false
}
2016-03-24 14:23:29 +00:00
tmpSituation . AccuracyVert = float32 ( vdop * 5 ) // rough estimate for 95% confidence
// We've made it this far, so that means we've processed "everything" and can now make the change to mySituation.
mySituation = tmpSituation
return true
2015-08-15 00:11:04 +00:00
}
2016-05-05 03:28:28 +00:00
2016-05-08 20:41:49 +00:00
if ( x [ 0 ] == "GPGSV" ) || ( x [ 0 ] == "GLGSV" ) { // GPS + SBAS or GLONASS satellites in view message. Galileo is TBD.
2016-05-05 03:28:28 +00:00
if len ( x ) < 4 {
return false
}
2016-05-07 06:27:53 +00:00
2016-05-08 20:41:49 +00:00
// field 1 = number of GSV messages of this type
2016-05-05 05:16:16 +00:00
msgNum , err := strconv . Atoi ( x [ 2 ] )
if err != nil {
return false
}
2016-05-08 20:41:49 +00:00
// field 2 = index of this GSV message
msgIndex , err := strconv . Atoi ( x [ 2 ] )
if err != nil {
return false
}
2016-05-05 03:28:28 +00:00
// field 3 = number of GPS satellites tracked
2016-05-06 05:50:45 +00:00
/ * Is this redundant if parsing from full constellation ?
2016-05-05 03:28:28 +00:00
satTracked , err := strconv . Atoi ( x [ 3 ] )
if err != nil {
return false
}
2016-05-06 05:50:45 +00:00
* /
2016-05-05 03:28:28 +00:00
2016-05-07 06:27:53 +00:00
//mySituation.SatellitesTracked = uint16(satTracked) // Replaced with parsing of 'Satellites' data structure
2016-05-05 03:28:28 +00:00
// field 4-7 = repeating block with satellite id, elevation, azimuth, and signal strengh (Cno)
2016-05-05 04:01:10 +00:00
2016-05-05 05:16:16 +00:00
lenGSV := len ( x )
satsThisMsg := ( lenGSV - 4 ) / 4
2016-05-07 06:27:53 +00:00
if globalSettings . DEBUG {
2016-05-08 20:41:49 +00:00
log . Printf ( "%s message [%d of %d] is %v fields long and describes %v satellites\n" , x [ 0 ] , msgIndex , msgNum , lenGSV , satsThisMsg )
2016-05-07 06:27:53 +00:00
}
2016-05-05 05:16:16 +00:00
var sv , elev , az , cno int
var svType uint8
2016-05-07 05:18:10 +00:00
var svStr string
2016-05-05 05:16:16 +00:00
for i := 0 ; i < satsThisMsg ; i ++ {
sv , err = strconv . Atoi ( x [ 4 + 4 * i ] ) // sv number
if err != nil {
return false
}
if sv < 33 { // indicates GPS
svType = SAT_TYPE_GPS
2016-05-07 05:18:10 +00:00
svStr = fmt . Sprintf ( "G%d" , sv )
} else if sv < 65 { // indicates SBAS: WAAS, EGNOS, MSAS, etc.
2016-05-05 05:16:16 +00:00
svType = SAT_TYPE_SBAS
2016-05-08 01:53:12 +00:00
svStr = fmt . Sprintf ( "S%d" , sv + 87 ) // add 87 to convert from NMEA to PRN.
2016-05-07 20:48:02 +00:00
} else if sv < 97 { // GLONASS
svType = SAT_TYPE_GLONASS
svStr = fmt . Sprintf ( "R%d" , sv - 64 ) // subtract 64 to convert from NMEA to PRN.
} else { // TO-DO: Galileo
2016-05-05 05:16:16 +00:00
svType = SAT_TYPE_UNKNOWN
2016-05-07 05:18:10 +00:00
svStr = fmt . Sprintf ( "U%d" , sv )
2016-05-05 05:16:16 +00:00
}
2016-05-06 05:50:45 +00:00
var thisSatellite SatelliteInfo
// START OF PROTECTED BLOCK
satelliteMutex . Lock ( )
2016-05-07 15:19:42 +00:00
// Retrieve previous information on this satellite code.
2016-05-07 05:18:10 +00:00
if val , ok := Satellites [ svStr ] ; ok { // if we've already seen this satellite identifier, copy it in to do updates
2016-05-06 05:50:45 +00:00
thisSatellite = val
2016-05-08 20:41:49 +00:00
//log.Printf("Satellite %s already seen. Retrieving from 'Satellites'.\n", svStr) // DEBUG
} else { // this satellite isn't in the Satellites data structure, so create it new
2016-05-07 05:18:10 +00:00
thisSatellite . SatelliteID = svStr
thisSatellite . SatelliteNMEA = uint8 ( sv )
2016-05-06 05:50:45 +00:00
thisSatellite . Type = uint8 ( svType )
2016-05-08 20:41:49 +00:00
//log.Printf("Creating new satellite %s\n", svStr) // DEBUG
2016-05-06 05:50:45 +00:00
}
thisSatellite . TimeLastTracked = stratuxClock . Time
2016-05-05 05:16:16 +00:00
elev , err = strconv . Atoi ( x [ 5 + 4 * i ] ) // elevation
2016-05-08 20:41:49 +00:00
if err != nil { // some firmwares leave this blank if there's no position fix. Represent as -999.
2016-05-06 05:50:45 +00:00
elev = - 999
2016-05-05 05:16:16 +00:00
}
2016-05-06 05:50:45 +00:00
thisSatellite . Elevation = int16 ( elev )
2016-05-05 05:16:16 +00:00
az , err = strconv . Atoi ( x [ 6 + 4 * i ] ) // azimuth
2016-05-08 20:41:49 +00:00
if err != nil { // UBX allows tracking up to 5(?) degrees below horizon. Some firmwares leave this blank if no position fix. Represent invalid as -999.
2016-05-06 05:50:45 +00:00
az = - 999
2016-05-05 05:16:16 +00:00
}
2016-05-06 05:50:45 +00:00
thisSatellite . Azimuth = int16 ( az )
2016-05-05 05:16:16 +00:00
cno , err = strconv . Atoi ( x [ 7 + 4 * i ] ) // signal
2016-05-06 05:50:45 +00:00
if err != nil { // will be blank if satellite isn't being received. Represent as -99.
cno = - 99
2016-05-08 01:53:12 +00:00
thisSatellite . InSolution = false // resets the "InSolution" status if the satellite disappears out of solution due to no signal. FIXME
2016-05-08 05:33:05 +00:00
//log.Printf("Satellite %s is no longer in solution due to cno parse error - GSV\n", svStr) // DEBUG
2016-05-06 05:50:45 +00:00
} else if cno > 0 {
2016-05-07 06:27:53 +00:00
thisSatellite . TimeLastSeen = stratuxClock . Time // Is this needed?
2016-05-05 05:16:16 +00:00
}
2016-05-08 20:41:49 +00:00
if cno > 127 { // make sure strong signals don't overflow. Normal range is 0-99 so it shouldn't, but take no chances.
cno = 127
}
2016-05-06 05:50:45 +00:00
thisSatellite . Signal = int8 ( cno )
2016-05-05 05:16:16 +00:00
2016-05-16 02:17:01 +00:00
// hack workaround for GSA 12-sv limitation... if this is a SBAS satellite, we have a SBAS solution, and signal is greater than some arbitrary threshold, set InSolution
// drawback is this will show all tracked SBAS satellites as being in solution.
2016-05-07 15:19:42 +00:00
if thisSatellite . Type == SAT_TYPE_SBAS {
if mySituation . Quality == 2 {
2016-05-16 02:17:01 +00:00
if thisSatellite . Signal > 16 {
2016-05-07 15:19:42 +00:00
thisSatellite . InSolution = true
2016-05-16 02:17:01 +00:00
thisSatellite . TimeLastSolution = stratuxClock . Time
2016-05-07 15:19:42 +00:00
}
} else { // quality == 0 or 1
thisSatellite . InSolution = false
2016-05-08 05:33:05 +00:00
//log.Printf("WAAS satellite %s is marked as out of solution GSV\n", svStr) // DEBUG
2016-05-07 15:19:42 +00:00
}
}
2016-05-07 06:27:53 +00:00
if globalSettings . DEBUG {
2016-05-14 02:32:11 +00:00
inSolnStr := " "
if thisSatellite . InSolution {
inSolnStr = "+"
}
log . Printf ( "GSV: Satellite %s%s at index %d. Type = %d, NMEA-ID = %d, Elev = %d, Azimuth = %d, Cno = %d\n" , inSolnStr , svStr , i , svType , sv , elev , az , cno ) // remove later?
2016-05-07 06:27:53 +00:00
}
2016-05-06 05:50:45 +00:00
Satellites [ thisSatellite . SatelliteID ] = thisSatellite // Update constellation with this satellite
updateConstellation ( )
satelliteMutex . Unlock ( )
// END OF PROTECTED BLOCK
2016-05-05 05:16:16 +00:00
}
2016-05-05 03:28:28 +00:00
return true
}
// if we've gotten this far, the message isn't one that we want to parse
2016-03-24 14:23:29 +00:00
return false
2015-08-15 00:11:04 +00:00
}
func gpsSerialReader ( ) {
defer serialPort . Close ( )
2016-02-10 07:06:52 +00:00
readyToInitGPS = false // TO-DO: replace with channel control to terminate goroutine when complete
2015-09-14 05:17:18 +00:00
2016-02-10 07:06:52 +00:00
i := 0 //debug monitor
2016-02-05 01:08:12 +00:00
scanner := bufio . NewScanner ( serialPort )
for scanner . Scan ( ) && globalStatus . GPS_connected && globalSettings . GPS_Enabled {
2016-02-10 07:06:52 +00:00
i ++
2016-03-10 15:20:46 +00:00
if globalSettings . DEBUG && i % 100 == 0 {
2016-02-15 22:11:19 +00:00
log . Printf ( "gpsSerialReader() scanner loop iteration i=%d\n" , i ) // debug monitor
2016-02-10 07:06:52 +00:00
}
2016-02-05 01:08:12 +00:00
s := scanner . Text ( )
2016-03-10 17:33:03 +00:00
if ! processNMEALine ( s ) {
if globalSettings . DEBUG {
fmt . Printf ( "processNMEALine() exited early -- %s\n" , s )
}
2016-02-12 04:07:30 +00:00
}
2015-08-15 00:11:04 +00:00
}
2016-02-05 01:08:12 +00:00
if err := scanner . Err ( ) ; err != nil {
log . Printf ( "reading standard input: %s\n" , err . Error ( ) )
}
2016-02-10 07:06:52 +00:00
2016-03-21 20:16:12 +00:00
if globalSettings . DEBUG {
log . Printf ( "Exiting gpsSerialReader() after i=%d loops\n" , i ) // debug monitor
}
2015-08-20 20:47:05 +00:00
globalStatus . GPS_connected = false
2016-02-10 07:06:52 +00:00
readyToInitGPS = true // TO-DO: replace with channel control to terminate goroutine when complete
return
2015-08-15 00:11:04 +00:00
}
2015-08-20 20:47:05 +00:00
var i2cbus embd . I2CBus
var myBMP180 * bmp180 . BMP180
var myMPU6050 * mpu6050 . MPU6050
2015-08-16 23:33:34 +00:00
func readBMP180 ( ) ( float64 , float64 , error ) { // ºCelsius, Meters
2015-08-20 20:47:05 +00:00
temp , err := myBMP180 . Temperature ( )
2015-08-16 23:33:34 +00:00
if err != nil {
return temp , 0.0 , err
}
2015-08-20 20:47:05 +00:00
altitude , err := myBMP180 . Altitude ( )
2015-08-20 23:47:05 +00:00
altitude = float64 ( 1 / 0.3048 ) * altitude // Convert meters to feet.
2015-08-16 23:33:34 +00:00
if err != nil {
return temp , altitude , err
}
return temp , altitude , nil
}
2015-08-20 21:01:42 +00:00
func readMPU6050 ( ) ( float64 , float64 , error ) { //TODO: error checking.
2015-08-20 20:47:05 +00:00
pitch , roll := myMPU6050 . PitchAndRoll ( )
return pitch , roll , nil
}
func initBMP180 ( ) error {
myBMP180 = bmp180 . New ( i2cbus ) //TODO: error checking.
return nil
}
func initMPU6050 ( ) error {
2015-12-27 21:11:21 +00:00
myMPU6050 = mpu6050 . New ( ) //TODO: error checking.
2015-08-20 20:47:05 +00:00
return nil
}
func initI2C ( ) error {
i2cbus = embd . NewI2CBus ( 1 ) //TODO: error checking.
return nil
}
// Unused at the moment. 5 second update, since read functions in bmp180 are slow.
func tempAndPressureReader ( ) {
timer := time . NewTicker ( 5 * time . Second )
for globalStatus . RY835AI_connected && globalSettings . AHRS_Enabled {
<- timer . C
// Read temperature and pressure altitude.
temp , alt , err_bmp180 := readBMP180 ( )
// Process.
if err_bmp180 != nil {
log . Printf ( "readBMP180(): %s\n" , err_bmp180 . Error ( ) )
globalStatus . RY835AI_connected = false
} else {
2015-09-24 20:26:51 +00:00
mySituation . Temp = temp
mySituation . Pressure_alt = alt
2016-03-24 04:26:56 +00:00
mySituation . LastTempPressTime = stratuxClock . Time
2015-08-20 21:01:42 +00:00
}
2015-08-20 20:47:05 +00:00
}
globalStatus . RY835AI_connected = false
}
2015-08-24 00:49:24 +00:00
func makeFFAHRSSimReport ( ) {
2015-09-24 20:26:51 +00:00
s := fmt . Sprintf ( "XATTStratux,%f,%f,%f" , mySituation . Gyro_heading , mySituation . Pitch , mySituation . Roll )
2015-08-24 00:49:24 +00:00
2015-09-01 20:16:31 +00:00
sendMsg ( [ ] byte ( s ) , NETWORK_AHRS_FFSIM , false )
2015-08-24 00:49:24 +00:00
}
func makeAHRSGDL90Report ( ) {
msg := make ( [ ] byte , 16 )
msg [ 0 ] = 0x4c
msg [ 1 ] = 0x45
msg [ 2 ] = 0x01
msg [ 3 ] = 0x00
2015-09-24 20:26:51 +00:00
pitch := int16 ( float64 ( mySituation . Pitch ) * float64 ( 10.0 ) )
roll := int16 ( float64 ( mySituation . Roll ) * float64 ( 10.0 ) )
2016-02-18 05:34:21 +00:00
hdg := uint16 ( float64 ( mySituation . Gyro_heading ) * float64 ( 10.0 ) )
slip_skid := int16 ( float64 ( 0 ) * float64 ( 10.0 ) )
yaw_rate := int16 ( float64 ( 0 ) * float64 ( 10.0 ) )
g := int16 ( float64 ( 1.0 ) * float64 ( 10.0 ) )
2015-08-24 00:49:24 +00:00
// Roll.
msg [ 4 ] = byte ( ( roll >> 8 ) & 0xFF )
msg [ 5 ] = byte ( roll & 0xFF )
// Pitch.
msg [ 6 ] = byte ( ( pitch >> 8 ) & 0xFF )
msg [ 7 ] = byte ( pitch & 0xFF )
// Heading.
msg [ 8 ] = byte ( ( hdg >> 8 ) & 0xFF )
msg [ 9 ] = byte ( hdg & 0xFF )
// Slip/skid.
msg [ 10 ] = byte ( ( slip_skid >> 8 ) & 0xFF )
msg [ 11 ] = byte ( slip_skid & 0xFF )
// Yaw rate.
msg [ 12 ] = byte ( ( yaw_rate >> 8 ) & 0xFF )
msg [ 13 ] = byte ( yaw_rate & 0xFF )
// "G".
msg [ 14 ] = byte ( ( g >> 8 ) & 0xFF )
msg [ 15 ] = byte ( g & 0xFF )
2015-09-01 20:16:31 +00:00
sendMsg ( prepareMessage ( msg ) , NETWORK_AHRS_GDL90 , false )
2015-08-24 00:49:24 +00:00
}
2015-08-20 20:47:05 +00:00
func attitudeReaderSender ( ) {
timer := time . NewTicker ( 100 * time . Millisecond ) // ~10Hz update.
for globalStatus . RY835AI_connected && globalSettings . AHRS_Enabled {
<- timer . C
// Read pitch and roll.
pitch , roll , err_mpu6050 := readMPU6050 ( )
if err_mpu6050 != nil {
log . Printf ( "readMPU6050(): %s\n" , err_mpu6050 . Error ( ) )
globalStatus . RY835AI_connected = false
break
}
2015-10-19 00:27:49 +00:00
mySituation . mu_Attitude . Lock ( )
mySituation . Pitch = pitch
mySituation . Roll = roll
mySituation . Gyro_heading = myMPU6050 . Heading ( ) //FIXME. Experimental.
2016-01-07 16:47:01 +00:00
mySituation . LastAttitudeTime = stratuxClock . Time
2015-10-19 00:27:49 +00:00
2015-08-20 20:47:05 +00:00
// Send, if valid.
2015-08-20 23:47:05 +00:00
// if isGPSGroundTrackValid(), etc.
2015-08-20 20:47:05 +00:00
2016-02-15 01:27:02 +00:00
// makeFFAHRSSimReport() // simultaneous use of GDL90 and FFSIM not supported in FF 7.5.1 or later. Function definition will be kept for AHRS debugging and future workarounds.
2015-08-24 00:49:24 +00:00
makeAHRSGDL90Report ( )
2015-08-20 20:47:05 +00:00
mySituation . mu_Attitude . Unlock ( )
}
globalStatus . RY835AI_connected = false
}
2016-05-06 05:50:45 +00:00
/ *
2016-05-16 02:17:01 +00:00
updateConstellation ( ) : Periodic cleanup and statistics calculation for ' Satellites '
2016-05-06 05:50:45 +00:00
data structure . Calling functions must protect this in a satelliteMutex .
* /
func updateConstellation ( ) {
2016-05-08 16:36:05 +00:00
var sats , tracked , seen uint8
2016-05-07 05:18:10 +00:00
for svStr , thisSatellite := range Satellites {
2016-05-06 05:50:45 +00:00
if stratuxClock . Since ( thisSatellite . TimeLastTracked ) > 10 * time . Second { // remove stale satellites if they haven't been tracked for 10 seconds
2016-05-07 05:18:10 +00:00
delete ( Satellites , svStr )
2016-05-06 05:50:45 +00:00
} else { // satellite almanac data is "fresh" even if it isn't being received.
tracked ++
if thisSatellite . Signal > 0 {
seen ++
}
2016-05-08 05:14:20 +00:00
if stratuxClock . Since ( thisSatellite . TimeLastSolution ) > 5 * time . Second {
thisSatellite . InSolution = false
Satellites [ svStr ] = thisSatellite
}
2016-05-08 16:36:05 +00:00
if thisSatellite . InSolution { // TESTING: Determine "In solution" from structure (fix for multi-GNSS overflow)
sats ++
}
2016-05-08 05:14:20 +00:00
// do any other calculations needed for this satellite
2016-05-06 05:50:45 +00:00
}
}
2016-05-07 06:27:53 +00:00
//log.Printf("Satellite counts: %d tracking channels, %d with >0 dB-Hz signal\n", tracked, seen) // DEBUG - REMOVE
//log.Printf("Satellite struct: %v\n", Satellites) // DEBUG - REMOVE
2016-05-08 16:36:05 +00:00
mySituation . Satellites = uint16 ( sats )
2016-05-06 05:50:45 +00:00
mySituation . SatellitesTracked = uint16 ( tracked )
mySituation . SatellitesSeen = uint16 ( seen )
}
2016-02-05 01:08:12 +00:00
func isGPSConnected ( ) bool {
2016-03-24 14:37:23 +00:00
return stratuxClock . Since ( mySituation . LastValidNMEAMessageTime ) < 5 * time . Second
2016-02-05 01:08:12 +00:00
}
2016-04-29 03:18:34 +00:00
/ *
isGPSValid returns true only if a valid position fix has been seen in the last 15 seconds ,
and if the GPS subsystem has recently detected a GPS device .
If false , ' Quality ` is set to 0 ( "No fix" ) , as is the number of satellites in solution .
* /
2015-08-20 20:47:05 +00:00
func isGPSValid ( ) bool {
2016-04-29 03:18:34 +00:00
isValid := false
2016-04-29 13:54:04 +00:00
if ( stratuxClock . Since ( mySituation . LastFixLocalTime ) < 15 * time . Second ) && globalStatus . GPS_connected && mySituation . Quality > 0 {
2016-04-29 03:18:34 +00:00
isValid = true
} else {
mySituation . Quality = 0
mySituation . Satellites = 0
}
return isValid
2015-08-20 20:47:05 +00:00
}
func isGPSGroundTrackValid ( ) bool {
2016-01-07 16:47:01 +00:00
return stratuxClock . Since ( mySituation . LastGroundTrackTime ) < 15 * time . Second
2015-08-20 20:47:05 +00:00
}
2016-02-17 03:13:21 +00:00
func isGPSClockValid ( ) bool {
return stratuxClock . Since ( mySituation . LastGPSTimeTime ) < 15 * time . Second
}
2015-08-20 20:47:05 +00:00
func isAHRSValid ( ) bool {
2016-01-07 16:47:01 +00:00
return stratuxClock . Since ( mySituation . LastAttitudeTime ) < 1 * time . Second // If attitude information gets to be over 1 second old, declare invalid.
2015-08-20 20:47:05 +00:00
}
2015-08-20 21:01:42 +00:00
func isTempPressValid ( ) bool {
2016-03-24 04:26:56 +00:00
return stratuxClock . Since ( mySituation . LastTempPressTime ) < 15 * time . Second
2015-08-20 21:01:42 +00:00
}
2015-08-20 20:47:05 +00:00
func initAHRS ( ) error {
if err := initI2C ( ) ; err != nil { // I2C bus.
return err
}
if err := initBMP180 ( ) ; err != nil { // I2C temperature and pressure altitude.
i2cbus . Close ( )
return err
}
if err := initMPU6050 ( ) ; err != nil { // I2C accel/gyro.
i2cbus . Close ( )
myBMP180 . Close ( )
return err
}
globalStatus . RY835AI_connected = true
go attitudeReaderSender ( )
go tempAndPressureReader ( )
return nil
}
func pollRY835AI ( ) {
2016-02-10 07:06:52 +00:00
readyToInitGPS = true //TO-DO: Implement more robust method (channel control) to kill zombie serial readers
timer := time . NewTicker ( 4 * time . Second )
2015-08-20 20:47:05 +00:00
for {
<- timer . C
// GPS enabled, was not connected previously?
2016-02-10 07:06:52 +00:00
if globalSettings . GPS_Enabled && ! globalStatus . GPS_connected && readyToInitGPS { //TO-DO: Implement more robust method (channel control) to kill zombie serial readers
globalStatus . GPS_connected = initGPSSerial ( )
2015-08-20 20:47:05 +00:00
if globalStatus . GPS_connected {
go gpsSerialReader ( )
}
}
// RY835AI I2C enabled, was not connected previously?
if globalSettings . AHRS_Enabled && ! globalStatus . RY835AI_connected {
err := initAHRS ( )
if err != nil {
log . Printf ( "initAHRS(): %s\ndisabling AHRS sensors.\n" , err . Error ( ) )
globalStatus . RY835AI_connected = false
}
}
}
}
func initRY835AI ( ) {
mySituation . mu_GPS = & sync . Mutex { }
mySituation . mu_Attitude = & sync . Mutex { }
2016-05-05 04:01:10 +00:00
satelliteMutex = & sync . Mutex { }
2016-05-07 05:18:10 +00:00
Satellites = make ( map [ string ] SatelliteInfo )
2015-08-20 20:47:05 +00:00
go pollRY835AI ( )
2015-08-17 17:59:03 +00:00
}
