From dcbcb1bf164fcc664c6461f5273bfcc2c725bf12 Mon Sep 17 00:00:00 2001 From: Christopher Young Date: Thu, 20 Aug 2015 16:47:05 -0400 Subject: [PATCH] Integrating RY835AI sensors. Network handling. --- gen_gdl90.go | 71 +++++++------- mpu6050/mpu6050.go | 79 ++++++++-------- network.go | 52 +++++++--- ry835ai.go | 230 ++++++++++++++++++++++++++++++++++++--------- sensortest.go | 6 +- traffic.go | 4 +- 6 files changed, 295 insertions(+), 147 deletions(-) diff --git a/gen_gdl90.go b/gen_gdl90.go index 933edd49..ac4100b7 100644 --- a/gen_gdl90.go +++ b/gen_gdl90.go @@ -47,7 +47,7 @@ const ( var Crc16Table [256]uint16 -var myGPS GPSData +var mySituation SituationData type msg struct { MessageClass uint @@ -121,14 +121,6 @@ func makeLatLng(v float32) []byte { return ret } -func isGPSValid() bool { - return time.Since(myGPS.lastFixLocalTime).Seconds() < 15 -} - -func isGPSGroundTrackValid() bool { - return time.Since(myGPS.lastGroundTrackTime).Seconds() < 15 -} - //TODO func makeOwnshipReport() bool { if !isGPSValid() { @@ -144,12 +136,12 @@ func makeOwnshipReport() bool { msg[3] = 1 // Address. msg[4] = 1 // Address. - tmp := makeLatLng(myGPS.lat) + tmp := makeLatLng(mySituation.lat) msg[5] = tmp[0] // Latitude. msg[6] = tmp[1] // Latitude. msg[7] = tmp[2] // Latitude. - tmp = makeLatLng(myGPS.lng) + tmp = makeLatLng(mySituation.lng) msg[8] = tmp[0] // Longitude. msg[9] = tmp[1] // Longitude. msg[10] = tmp[2] // Longitude. @@ -157,7 +149,7 @@ func makeOwnshipReport() bool { //TODO: 0xFFF "invalid altitude." //FIXME: This is **PRESSURE ALTITUDE** - alt := uint16(myGPS.alt) + alt := uint16(mySituation.alt) alt = (alt + 1000) / 25 alt = alt & 0xFFF // Should fit in 12 bits. @@ -173,7 +165,7 @@ func makeOwnshipReport() bool { gdSpeed := uint16(0) // 1kt resolution. if isGPSGroundTrackValid() { - gdSpeed = myGPS.groundSpeed + gdSpeed = mySituation.groundSpeed } gdSpeed = gdSpeed & 0x0FFF // Should fit in 12 bits. @@ -189,7 +181,7 @@ func makeOwnshipReport() bool { // Showing magnetic (corrected) on ForeFlight. Needs to be True Heading. groundTrack := uint16(0) if isGPSGroundTrackValid() { - groundTrack = myGPS.trueCourse + groundTrack = mySituation.trueCourse } trk := uint8(float32(groundTrack) / TRACK_RESOLUTION) // Resolution is ~1.4 degrees. @@ -197,7 +189,7 @@ func makeOwnshipReport() bool { msg[18] = 0x01 // "Light (ICAO) < 15,500 lbs" - sendMsg(prepareMessage(msg)) + sendGDL90(prepareMessage(msg)) return true } @@ -208,8 +200,8 @@ func makeOwnshipGeometricAltitudeReport() bool { } msg := make([]byte, 5) // See p.28. - msg[0] = 0x0B // Message type "Ownship Geo Alt". - alt := int16(myGPS.alt) + msg[0] = 0x0B // Message type "Ownship Geo Alt". + alt := int16(mySituation.alt) //FIXME. alt = alt / 5 msg[1] = byte(alt >> 8) // Altitude. msg[2] = byte(alt & 0x00FF) // Altitude. @@ -218,7 +210,7 @@ func makeOwnshipGeometricAltitudeReport() bool { msg[3] = 0x00 msg[4] = 0x0A - sendMsg(prepareMessage(msg)) + sendGDL90(prepareMessage(msg)) return true } @@ -235,8 +227,12 @@ func makeHeartbeat() []byte { msg := make([]byte, 7) // See p.10. msg[0] = 0x00 // Message type "Heartbeat". - msg[1] = 0x01 // "UAT Initialized". //FIXME - msg[1] = 0x91 //FIXME: GPS valid. Addr talkback. + msg[1] = 0x01 // "UAT Initialized". + if isGPSValid() { + msg[1] = msg[1] | 0x80 + } + msg[1] = msg[1] | 0x10 //FIXME: Addr talkback. + nowUTC := time.Now().UTC() // Seconds since 0000Z. midnightUTC := time.Date(nowUTC.Year(), nowUTC.Month(), nowUTC.Day(), 0, 0, 0, 0, time.UTC) @@ -265,18 +261,18 @@ func relayMessage(msgtype uint16, msg []byte) { ret[i+4] = msg[i] } - sendMsg(prepareMessage(ret)) + sendGDL90(prepareMessage(ret)) } func heartBeatSender() { - timer := time.Tick(1 * time.Second) + timer := time.NewTicker(1 * time.Second) for { - <-timer - sendMsg(makeHeartbeat()) - // sendMsg(makeTrafficReport()) + <-timer.C + sendGDL90(makeHeartbeat()) + // sendGDL90(makeTrafficReport()) makeOwnshipReport() makeOwnshipGeometricAltitudeReport() - sendMsg(makeInitializationMessage()) + sendGDL90(makeInitializationMessage()) sendTrafficUpdates() updateStatus() } @@ -302,7 +298,7 @@ func updateStatus() { globalStatus.ES_messages_last_minute = ES_messages_last_minute if isGPSValid() { - globalStatus.GPS_satellites_locked = myGPS.satellites + globalStatus.GPS_satellites_locked = mySituation.satellites } } @@ -359,7 +355,8 @@ type settings struct { UAT_Enabled bool ES_Enabled bool GPS_Enabled bool - GDLOutputPorts []uint16 + NetworkOutputs []networkConnection + AHRS_Enabled bool } type status struct { @@ -370,6 +367,8 @@ type status struct { ES_messages_last_minute uint ES_messages_max uint GPS_satellites_locked uint16 + GPS_connected bool + RY835AI_connected bool } var globalSettings settings @@ -401,9 +400,6 @@ func handleManagementConnection(conn net.Conn) { log.Printf("%s - error: %s\n", s, err.Error()) } else { log.Printf("new settings: %s\n", s) - if !globalSettings.GPS_Enabled && newSettings.GPS_Enabled { // GPS was enabled, restart the reader thread. - go gpsReader() - } globalSettings = newSettings saveSettings() } @@ -428,10 +424,11 @@ func managementInterface() { } func defaultSettings() { - globalSettings.UAT_Enabled = true //TODO - globalSettings.ES_Enabled = false //TODO - globalSettings.GPS_Enabled = false //TODO - globalSettings.GDLOutputPorts = []uint16{4000, 43211} + globalSettings.UAT_Enabled = true //TODO + globalSettings.ES_Enabled = false //TODO + globalSettings.GPS_Enabled = true //TODO + globalSettings.NetworkOutputs = []networkConnection{{nil, "", 4000, NETWORK_GDL90}, {nil, "", 43211, NETWORK_GDL90}, {nil, "", 49002, NETWORK_AHRS}} + globalSettings.AHRS_Enabled = true } func readSettings() { @@ -486,9 +483,7 @@ func main() { readSettings() - if globalSettings.GPS_Enabled { - go gpsReader() - } + initRY835AI() //TODO: network stuff diff --git a/mpu6050/mpu6050.go b/mpu6050/mpu6050.go index f596acd3..7b3f9c33 100644 --- a/mpu6050/mpu6050.go +++ b/mpu6050/mpu6050.go @@ -1,33 +1,33 @@ -// Package bmp180 allows interfacing with Bosch BMP180 barometric pressure sensor. This sensor +// Package mpu6050 allows interfacing with InvenSense mpu6050 barometric pressure sensor. This sensor // has the ability to provided compensated temperature and pressure readings. package mpu6050 import ( - "time" "math" -// "github.com/golang/glog" + "time" + // "github.com/golang/glog" "github.com/kidoman/embd" - "fmt" + "log" ) //https://www.olimex.com/Products/Modules/Sensors/MOD-MPU6050/resources/RM-MPU-60xxA_rev_4.pdf const ( address = 0x68 - GYRO_XOUT_H = 0x43 - GYRO_YOUT_H = 0x45 - GYRO_ZOUT_H = 0x47 + GYRO_XOUT_H = 0x43 + GYRO_YOUT_H = 0x45 + GYRO_ZOUT_H = 0x47 - ACCEL_XOUT_H = 0x3B - ACCEL_YOUT_H = 0x3D - ACCEL_ZOUT_H = 0x3F + ACCEL_XOUT_H = 0x3B + ACCEL_YOUT_H = 0x3D + ACCEL_ZOUT_H = 0x3F - PWR_MGMT_1 = 0x6B + PWR_MGMT_1 = 0x6B - ACCEL_SCALE = 16384.0 // Assume AFS_SEL = 0. - GYRO_SCALE = 131.0 // Assume FS_SEL = 0. + ACCEL_SCALE = 16384.0 // Assume AFS_SEL = 0. + GYRO_SCALE = 131.0 // Assume FS_SEL = 0. - pollDelay = 500 * time.Microsecond // 2000Hz + pollDelay = 500 * time.Microsecond // 2000Hz ) type XYZ struct { @@ -41,24 +41,24 @@ type MPU6050 struct { Bus embd.I2CBus Poll time.Duration - started bool - -//TODO - gyro_reading XYZ // "Integrated". - accel_reading XYZ // Directly from sensor. + started bool - pitch_history []float64 - roll_history []float64 + //TODO + gyro_reading XYZ // "Integrated". + accel_reading XYZ // Directly from sensor. - pitch_resting float64 - roll_resting float64 + pitch_history []float64 + roll_history []float64 - pitch float64 - roll float64 -// gyro chan XYZ -// accel chan XYZ + pitch_resting float64 + roll_resting float64 - quit chan struct{} + pitch float64 + roll float64 + // gyro chan XYZ + // accel chan XYZ + + quit chan struct{} } // New returns a handle to a MPU6050 sensor. @@ -81,7 +81,6 @@ func (d *MPU6050) StartUp() error { return nil } - func (d *MPU6050) calibrate() { //TODO: Error checking to make sure that the histories are extensive enough to be significant. //TODO: Error checking to do continuous calibrations. @@ -98,7 +97,7 @@ func (d *MPU6050) calibrate() { } roll_adjust = roll_adjust / float64(len(d.roll_history)) d.roll_resting = roll_adjust - fmt.Printf("calibrate: pitch %f, roll %f\n", pitch_adjust, roll_adjust) + log.Printf("calibrate: pitch %f, roll %f\n", pitch_adjust, roll_adjust) } func (d *MPU6050) readGyro() (XYZ, error) { @@ -149,18 +148,16 @@ func (d *MPU6050) readAccel() (XYZ, error) { func (d *MPU6050) calculatePitchAndRoll() { accel := d.accel_reading -// fmt.Printf("accel: %f, %f, %f\n", accel.x, accel.y, accel.z) + // log.Printf("accel: %f, %f, %f\n", accel.x, accel.y, accel.z) // Accel. p1 := math.Atan2(float64(accel.y), dist(accel.x, accel.z)) p1_deg := p1 * (180 / math.Pi) - r1 := math.Atan2(float64(accel.x), dist(accel.y, accel.z)) r1_deg := -r1 * (180 / math.Pi) - // Gyro. p2 := float64(d.gyro_reading.x) @@ -168,13 +165,13 @@ func (d *MPU6050) calculatePitchAndRoll() { // "Noise filter". ft := float64(0.98) - sample_period := float64(1/2000.0) - d.pitch = float64( ft*( sample_period*p2 + d.pitch) + (1-ft)*p1_deg) - d.roll = float64( (ft*( sample_period*r2 + d.roll) + (1-ft)*r1_deg)) + sample_period := float64(1 / 2000.0) + d.pitch = float64(ft*(sample_period*p2+d.pitch) + (1-ft)*p1_deg) + d.roll = float64((ft*(sample_period*r2+d.roll) + (1-ft)*r1_deg)) d.pitch_history = append(d.pitch_history, d.pitch) d.roll_history = append(d.roll_history, d.roll) - + } func (d *MPU6050) measure() error { @@ -187,8 +184,8 @@ func (d *MPU6050) measure() error { return err } -// glog.V(1).Infof("mpu6050: scaled gyro: (%f, %f, %f)", XYZ_gyro.x, XYZ_gyro.y, XYZ_gyro.z) -// glog.V(1).Infof("mpu6050: scaled accel: (%f, %f, %f)", XYZ_accel.x, XYZ_accel.y, XYZ_accel.z) + // glog.V(1).Infof("mpu6050: scaled gyro: (%f, %f, %f)", XYZ_gyro.x, XYZ_gyro.y, XYZ_gyro.z) + // glog.V(1).Infof("mpu6050: scaled accel: (%f, %f, %f)", XYZ_accel.x, XYZ_accel.y, XYZ_accel.z) d.accel_reading = XYZ_accel d.gyro_reading = XYZ_gyro @@ -199,7 +196,7 @@ func (d *MPU6050) measure() error { func dist(a, b float32) float64 { a64 := float64(a) b64 := float64(b) - return math.Sqrt((a64*a64) + (b64*b64)) + return math.Sqrt((a64 * a64) + (b64 * b64)) } // Temperature returns the current temperature reading. @@ -234,4 +231,4 @@ func (d *MPU6050) Close() { if d.quit != nil { d.quit <- struct{}{} } -} \ No newline at end of file +} diff --git a/network.go b/network.go index 50517dec..0b0b83b9 100644 --- a/network.go +++ b/network.go @@ -10,11 +10,28 @@ import ( "time" ) -var messageQueue chan []byte -var outSockets map[string]*net.UDPConn +type networkMessage struct { + msg []byte + msgType uint8 +} + +type networkConnection struct { + conn *net.UDPConn + ip string + port uint32 + capability uint8 +} + +var messageQueue chan networkMessage +var outSockets map[string]networkConnection var dhcpLeases map[string]string var netMutex *sync.Mutex +const ( + NETWORK_GDL90 = 1 + NETWORK_AHRS = 2 +) + // Read the "dhcpd.leases" file and parse out IP/hostname. func getDHCPLeases() (map[string]string, error) { dat, err := ioutil.ReadFile("/var/lib/dhcp/dhcpd.leases") @@ -39,11 +56,13 @@ func getDHCPLeases() (map[string]string, error) { return ret, nil } -func sendToAllConnectedClients(msg []byte) { +func sendToAllConnectedClients(msg networkMessage) { netMutex.Lock() defer netMutex.Unlock() - for _, sock := range outSockets { - sock.Write(msg) + for _, netconn := range outSockets { + if (netconn.capability & msg.msgType) != 0 { // Check if this port is able to accept the type of message we're sending. + netconn.conn.Write(msg.msg) + } } } @@ -65,10 +84,10 @@ func refreshConnectedClients() { dhcpLeases = t // Client connected that wasn't before. for ip, hostname := range dhcpLeases { - for _, port := range globalSettings.GDLOutputPorts { - ipAndPort := ip + ":" + strconv.Itoa(int(port)) + for _, networkOutput := range globalSettings.NetworkOutputs { + ipAndPort := ip + ":" + strconv.Itoa(int(networkOutput.port)) if _, ok := outSockets[ipAndPort]; !ok { - log.Printf("client connected: %s:%d (%s).\n", ip, port, hostname) + log.Printf("client connected: %s:%d (%s).\n", ip, networkOutput.port, hostname) addr, err := net.ResolveUDPAddr("udp", ipAndPort) if err != nil { log.Printf("ResolveUDPAddr(%s): %s\n", ipAndPort, err.Error()) @@ -79,7 +98,7 @@ func refreshConnectedClients() { log.Printf("DialUDP(%s): %s\n", ipAndPort, err.Error()) continue } - outSockets[ipAndPort] = outConn + outSockets[ipAndPort] = networkConnection{outConn, ip, networkOutput.port, networkOutput.capability} } validConnections[ipAndPort] = true } @@ -88,7 +107,7 @@ func refreshConnectedClients() { for ipAndPort, conn := range outSockets { if _, ok := validConnections[ipAndPort]; !ok { log.Printf("removed connection %s.\n", ipAndPort) - conn.Close() + conn.conn.Close() delete(outSockets, ipAndPort) } } @@ -106,17 +125,20 @@ func messageQueueSender() { case <-dhcpRefresh.C: refreshConnectedClients() } - } } -func sendMsg(msg []byte) { - messageQueue <- msg +func sendMsg(msg []byte, msgType uint8) { + messageQueue <- networkMessage{msg, msgType} +} + +func sendGDL90(msg []byte) { + sendMsg(msg, NETWORK_GDL90) } func initNetwork() { - messageQueue = make(chan []byte, 1024) // Buffered channel, 1024 messages. - outSockets = make(map[string]*net.UDPConn) + messageQueue = make(chan networkMessage, 1024) // Buffered channel, 1024 messages. + outSockets = make(map[string]networkConnection) netMutex = &sync.Mutex{} refreshConnectedClients() go messageQueueSender() diff --git a/ry835ai.go b/ry835ai.go index fc345675..75d16511 100644 --- a/ry835ai.go +++ b/ry835ai.go @@ -5,14 +5,21 @@ import ( "strconv" "strings" "time" + "sync" + "fmt" "github.com/kidoman/embd" _ "github.com/kidoman/embd/host/all" "github.com/kidoman/embd/sensor/bmp180" "github.com/tarm/serial" + + "./mpu6050" ) -type GPSData struct { +type SituationData struct { + mu_GPS *sync.Mutex + + // From GPS. lastFixSinceMidnightUTC uint32 lat float32 lng float32 @@ -25,12 +32,24 @@ type GPSData struct { trueCourse uint16 groundSpeed uint16 lastGroundTrackTime time.Time + + mu_Attitude *sync.Mutex + + // From BMP180 pressure sensor. + temp float64 + pressure_alt float64 + lastTempPressTime time.Time + + // From MPU6050 accel/gyro. + pitch float64 + roll float64 + lastAttitudeTime time.Time } var serialConfig *serial.Config var serialPort *serial.Port -func initGPSSerialReader() bool { +func initGPSSerial() bool { serialConfig = &serial.Config{Name: "/dev/ttyACM0", Baud: 9600} p, err := serial.OpenPort(serialConfig) if err != nil { @@ -44,6 +63,8 @@ func initGPSSerialReader() bool { func processNMEALine(l string) bool { x := strings.Split(l, ",") if x[0] == "$GNVTG" { // Ground track information. + mySituation.mu_GPS.Lock() + defer mySituation.mu_GPS.Unlock() if len(x) < 10 { return false } @@ -56,9 +77,9 @@ func processNMEALine(l string) bool { trueCourse = uint16(tc) } else { // No movement. - myGPS.trueCourse = 0 - myGPS.groundSpeed = 0 - myGPS.lastGroundTrackTime = time.Time{} + mySituation.trueCourse = 0 + mySituation.groundSpeed = 0 + mySituation.lastGroundTrackTime = time.Time{} return true } groundSpeed, err := strconv.ParseFloat(x[5], 32) // Knots. @@ -66,17 +87,16 @@ func processNMEALine(l string) bool { return false } - myGPS.trueCourse = uint16(trueCourse) - myGPS.groundSpeed = uint16(groundSpeed) - myGPS.lastGroundTrackTime = time.Now() + mySituation.trueCourse = uint16(trueCourse) + mySituation.groundSpeed = uint16(groundSpeed) + mySituation.lastGroundTrackTime = time.Now() + } else if x[0] == "$GNGGA" { // GPS fix. if len(x) < 15 { return false } - var fix GPSData - - fix = myGPS - + mySituation.mu_GPS.Lock() + defer mySituation.mu_GPS.Unlock() // Timestamp. if len(x[1]) < 9 { return false @@ -88,7 +108,7 @@ func processNMEALine(l string) bool { return false } - fix.lastFixSinceMidnightUTC = uint32((hr * 60 * 60) + (min * 60) + sec) + mySituation.lastFixSinceMidnightUTC = uint32((hr * 60 * 60) + (min * 60) + sec) // Latitude. if len(x[2]) < 10 { @@ -100,9 +120,9 @@ func processNMEALine(l string) bool { return false } - fix.lat = float32(hr) + float32(minf/60.0) + mySituation.lat = float32(hr) + float32(minf/60.0) if x[3] == "S" { // South = negative. - fix.lat = -fix.lat + mySituation.lat = -mySituation.lat } // Longitude. @@ -115,9 +135,9 @@ func processNMEALine(l string) bool { return false } - fix.lng = float32(hr) + float32(minf/60.0) + mySituation.lng = float32(hr) + float32(minf/60.0) if x[5] == "W" { // West = negative. - fix.lng = -fix.lng + mySituation.lng = -mySituation.lng } // Quality indicator. @@ -125,36 +145,34 @@ func processNMEALine(l string) bool { if err1 != nil { return false } - fix.quality = uint8(q) + mySituation.quality = uint8(q) // Satellites. sat, err1 := strconv.Atoi(x[7]) if err1 != nil { return false } - fix.satellites = uint16(sat) + mySituation.satellites = uint16(sat) // Accuracy. hdop, err1 := strconv.ParseFloat(x[8], 32) if err1 != nil { return false } - fix.accuracy = float32(hdop * 5.0) //FIXME: 5 meters ~ 1.0 HDOP? + mySituation.accuracy = float32(hdop * 5.0) //FIXME: 5 meters ~ 1.0 HDOP? // Altitude. alt, err1 := strconv.ParseFloat(x[9], 32) if err1 != nil { return false } - fix.alt = float32(alt * 3.28084) // Covnert to feet. + mySituation.alt = float32(alt * 3.28084) // Covnert to feet. //TODO: Altitude accuracy. - fix.alt_accuracy = 0 + mySituation.alt_accuracy = 0 // Timestamp. - fix.lastFixLocalTime = time.Now() - - myGPS = fix + mySituation.lastFixLocalTime = time.Now() } return true @@ -162,15 +180,13 @@ func processNMEALine(l string) bool { func gpsSerialReader() { defer serialPort.Close() - buf := make([]byte, 1024) - for { - if !globalSettings.GPS_Enabled { // GPS was turned off. Shut down. - break - } + for globalSettings.GPS_Enabled && globalStatus.GPS_connected { + buf := make([]byte, 1024) n, err := serialPort.Read(buf) if err != nil { - log.Printf("gps unit read error: %s\n", err.Error()) - return + log.Printf("gps serial read error: %s\n", err.Error()) + globalStatus.GPS_connected = false + break } s := string(buf[:n]) x := strings.Split(s, "\n") @@ -178,32 +194,152 @@ func gpsSerialReader() { processNMEALine(l) } } + globalStatus.GPS_connected = false } -func gpsReader() { - if initGPSSerialReader() { - gpsSerialReader() - } else { - globalSettings.GPS_Enabled = false - } -} - -var bus embd.I2CBus -var i2csensor *bmp180.BMP180 +var i2cbus embd.I2CBus +var myBMP180 *bmp180.BMP180 +var myMPU6050 *mpu6050.MPU6050 func readBMP180() (float64, float64, error) { // ºCelsius, Meters - temp, err := i2csensor.Temperature() + temp, err := myBMP180.Temperature() if err != nil { return temp, 0.0, err } - altitude, err := i2csensor.Altitude() + altitude, err := myBMP180.Altitude() if err != nil { return temp, altitude, err } return temp, altitude, nil } -func initBMP180() { - bus = embd.NewI2CBus(1) - i2csensor = bmp180.New(bus) +func readMPU6050() (float64, float64, error) {//TODO: error checking. + pitch, roll := myMPU6050.PitchAndRoll() + return pitch, roll, nil +} + +func initBMP180() error { + myBMP180 = bmp180.New(i2cbus) //TODO: error checking. + return nil +} + +func initMPU6050() error { + myMPU6050 = mpu6050.New(i2cbus) //TODO: error checking. + 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 { + mySituation.temp = temp + mySituation.pressure_alt = alt + mySituation.lastTempPressTime = time.Now() + } + } + globalStatus.RY835AI_connected = false +} + +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() + + mySituation.mu_Attitude.Lock() + + if err_mpu6050 != nil { + log.Printf("readMPU6050(): %s\n", err_mpu6050.Error()) + globalStatus.RY835AI_connected = false + break + } else { + mySituation.pitch = pitch + mySituation.roll = roll + mySituation.lastAttitudeTime = time.Now() + } + + // Send, if valid. +// if isGPSGroundTrackValid() + s := fmt.Sprintf("XATTStratux,%d,%f,%f", mySituation.trueCourse, mySituation.pitch, mySituation.roll) + + sendMsg([]byte(s), NETWORK_AHRS) + + mySituation.mu_Attitude.Unlock() + } + globalStatus.RY835AI_connected = false +} + +func isGPSValid() bool { + return time.Since(mySituation.lastFixLocalTime).Seconds() < 15 +} + +func isGPSGroundTrackValid() bool { + return time.Since(mySituation.lastGroundTrackTime).Seconds() < 15 +} + +func isAHRSValid() bool { + return time.Since(mySituation.lastAttitudeTime).Seconds() < 1 // If attitude information gets to be over 1 second old, declare invalid. +} + +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() { + timer := time.NewTicker(10 * time.Second) + for { + <-timer.C + // GPS enabled, was not connected previously? + if globalSettings.GPS_Enabled && !globalStatus.GPS_connected { + globalStatus.GPS_connected = initGPSSerial() // via USB for now. + 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{} + + go pollRY835AI() } diff --git a/sensortest.go b/sensortest.go index 7593c106..3cd63d3e 100644 --- a/sensortest.go +++ b/sensortest.go @@ -1,12 +1,12 @@ package main import ( + "./mpu6050" "fmt" "github.com/kidoman/embd" _ "github.com/kidoman/embd/host/all" - "./mpu6050" - "time" "net" + "time" ) var bus embd.I2CBus @@ -37,4 +37,4 @@ func main() { outConn.Write([]byte(s)) time.Sleep(50 * time.Millisecond) } -} \ No newline at end of file +} diff --git a/traffic.go b/traffic.go index a6671f5c..b97e84f0 100644 --- a/traffic.go +++ b/traffic.go @@ -70,8 +70,6 @@ var traffic map[uint32]TrafficInfo var trafficMutex *sync.Mutex func cleanupOldEntries() { - trafficMutex.Lock() - defer trafficMutex.Unlock() for icao_addr, ti := range traffic { if time.Since(ti.last_seen).Seconds() > float64(60) { //FIXME: 60 seconds with no update on this address - stop displaying. delete(traffic, icao_addr) @@ -148,7 +146,7 @@ func makeTrafficReport(ti TrafficInfo) { msg[18] = 0x01 // "light" - sendMsg(prepareMessage(msg)) + sendGDL90(prepareMessage(msg)) } func parseDownlinkReport(s string) {