mirror
/
stratux
kopia lustrzana https://github.com/cyoung/stratux
1
0
Forkuj 0
Kod Zgłoszenia Projekty Wydania Wiki Aktywność

Integrating RY835AI sensors. Network handling.

pull/13/head
Christopher Young 2015-08-20 16:47:05 -04:00
rodzic 005cbea9dd
commit dcbcb1bf16
6 zmienionych plików z 295 dodań i 147 usunięć
Pokaż statystyki Ściągnij plik aktualizacji Ściągnij plik porównania Expand all files Collapse all files

71
gen_gdl90.go
Wyświetl plik

@ -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

79
mpu6050/mpu6050.go
Wyświetl plik

@ -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{}{}
}
}
}

52
network.go
Wyświetl plik

@ -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()

230
ry835ai.go
Wyświetl plik

@ -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()
}

6
sensortest.go
Wyświetl plik

@ -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)
}
}
}

4
traffic.go
Wyświetl plik

@ -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) {