diff --git a/Makefile b/Makefile index cfa218fb..2ec2e2e8 100644 --- a/Makefile +++ b/Makefile @@ -9,11 +9,10 @@ endif all: make xdump978 make xdump1090 - make xlinux-mpu9150 make xgen_gdl90 xgen_gdl90: - go get -t -d -v ./main ./test ./linux-mpu9150/mpu ./godump978 ./mpu6050 ./uatparse + go get -t -d -v ./main ./test ./godump978 ./mpu6050 ./uatparse go build $(BUILDINFO) -p 4 main/gen_gdl90.go main/traffic.go main/ry835ai.go main/network.go main/managementinterface.go main/sdr.go main/ping.go main/uibroadcast.go main/monotonic.go main/datalog.go main/equations.go xdump1090: @@ -24,10 +23,6 @@ xdump978: cd dump978 && make lib sudo cp -f ./libdump978.so /usr/lib/libdump978.so -xlinux-mpu9150: - git submodule update --init - cd linux-mpu9150 && make -f Makefile-native-shared - .PHONY: test test: make -C test @@ -53,4 +48,3 @@ clean: rm -f gen_gdl90 libdump978.so cd dump1090 && make clean cd dump978 && make clean - rm -f linux-mpu9150/*.o linux-mpu9150/*.so diff --git a/main/gen_gdl90.go b/main/gen_gdl90.go index a9cac821..9f5c140a 100755 --- a/main/gen_gdl90.go +++ b/main/gen_gdl90.go @@ -438,10 +438,8 @@ func makeStratuxStatus() []byte { // Connected hardware: number of radios. msg[15] = msg[15] | (byte(globalStatus.Devices) & 0x3) - // Connected hardware: RY835AI. - if globalStatus.RY835AI_connected { - msg[15] = msg[15] | (1 << 2) - } + // Connected hardware. + // RY835AI: msg[15] = msg[15] | (1 << 2) // Number of GPS satellites locked. msg[16] = byte(globalStatus.GPS_satellites_locked) @@ -1024,7 +1022,6 @@ type status struct { GPS_connected bool GPS_solution string GPS_detected_type uint - RY835AI_connected bool Uptime int64 UptimeClock time.Time CPUTemp float32 diff --git a/main/ry835ai.go b/main/ry835ai.go index 19f21fee..04de5ae0 100644 --- a/main/ry835ai.go +++ b/main/ry835ai.go @@ -27,8 +27,6 @@ import ( "os" "os/exec" - - "../mpu6050" ) const ( @@ -88,7 +86,7 @@ type SituationData struct { Pressure_alt float64 LastTempPressTime time.Time - // From MPU6050 accel/gyro. + // From AHRS source. Pitch float64 Roll float64 Gyro_heading float64 @@ -464,10 +462,11 @@ func validateNMEAChecksum(s string) (string, bool) { //TODO: Some more robust checking above current and last speed. //TODO: Dynamic adjust for gain based on groundspeed func setTrueCourse(groundSpeed uint16, trueCourse float64) { - if myMPU6050 != nil && globalStatus.RY835AI_connected && globalSettings.AHRS_Enabled { - if mySituation.GroundSpeed >= 7 && groundSpeed >= 7 { - myMPU6050.ResetHeading(trueCourse, 0.10) - } + if mySituation.GroundSpeed >= 7 && groundSpeed >= 7 { + // This was previously used to filter small ground speed spikes caused by GPS position drift. + // It was passed to the previous AHRS heading calculator. Currently unused, maybe in the future it will be. + _ = trueCourse + _ = groundSpeed } } @@ -1844,7 +1843,6 @@ func gpsSerialReader() { var i2cbus embd.I2CBus var myBMP180 *bmp180.BMP180 -var myMPU6050 *mpu6050.MPU6050 func readBMP180() (float64, float64, error) { // ºCelsius, Meters temp, err := myBMP180.Temperature() @@ -1859,21 +1857,11 @@ func readBMP180() (float64, float64, error) { // ºCelsius, Meters return temp, altitude, nil } -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() //TODO: error checking. - return nil -} - func initI2C() error { i2cbus = embd.NewI2CBus(1) //TODO: error checking. return nil @@ -1975,37 +1963,6 @@ func gpsAttitudeSender() { } } -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 - } - - mySituation.mu_Attitude.Lock() - - mySituation.Pitch = pitch - mySituation.Roll = roll - mySituation.Gyro_heading = myMPU6050.Heading() //FIXME. Experimental. - mySituation.LastAttitudeTime = stratuxClock.Time - - // Send, if valid. - // if isGPSGroundTrackValid(), etc. - - // 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. - makeAHRSGDL90Report() - - mySituation.mu_Attitude.Unlock() - } - globalStatus.RY835AI_connected = false -} - /* updateConstellation(): Periodic cleanup and statistics calculation for 'Satellites' data structure. Calling functions must protect this in a satelliteMutex. @@ -2081,18 +2038,12 @@ func initAHRS() error { if err := initI2C(); err != nil { // I2C bus. return err } - if err := initBMP180(); err != nil { // I2C temperature and pressure altitude. - i2cbus.Close() + + if err := initBMP180(); err == nil { // I2C temperature and pressure altitude. + go tempAndPressureReader() + } else { 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 } diff --git a/mpu6050/mpu6050.go b/mpu6050/mpu6050.go deleted file mode 100644 index 1661899b..00000000 --- a/mpu6050/mpu6050.go +++ /dev/null @@ -1,183 +0,0 @@ -// 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 ( - "../linux-mpu9150/mpu" - "log" - "math" - "time" -) - -//https://www.olimex.com/Products/Modules/Sensors/MOD-MPU6050/resources/RM-MPU-60xxA_rev_4.pdf -const ( - pollDelay = 98 * time.Millisecond // ~10Hz -) - -// MPU6050 represents a InvenSense MPU6050 sensor. -type MPU6050 struct { - Poll time.Duration - - started bool - - pitch float64 - roll float64 - - // Calibration variables. - calibrated bool - pitch_history []float64 - roll_history []float64 - pitch_resting float64 - roll_resting float64 - - // For tracking heading (mixing GPS track and the gyro output). - heading float64 // Current heading. - gps_track float64 // Last reading directly from the gyro for comparison with current heading. - gps_track_valid bool - heading_correction float64 - - quit chan struct{} -} - -// New returns a handle to a MPU6050 sensor. -func New() *MPU6050 { - n := &MPU6050{Poll: pollDelay} - n.StartUp() - return n -} - -func (d *MPU6050) StartUp() error { - mpu_sample_rate := 10 // 10 Hz read rate of hardware IMU - yaw_mix_factor := 0 // must be zero if no magnetometer - mpu.InitMPU(mpu_sample_rate, yaw_mix_factor) - - d.pitch_history = make([]float64, 0) - d.roll_history = make([]float64, 0) - - d.started = true - d.Run() - - 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. - pitch_adjust := float64(0) - for _, v := range d.pitch_history { - pitch_adjust = pitch_adjust + v - } - pitch_adjust = pitch_adjust / float64(len(d.pitch_history)) - d.pitch_resting = pitch_adjust - - roll_adjust := float64(0) - for _, v := range d.roll_history { - roll_adjust = roll_adjust + v - } - roll_adjust = roll_adjust / float64(len(d.roll_history)) - d.roll_resting = roll_adjust - log.Printf("calibrate: pitch %f, roll %f\n", pitch_adjust, roll_adjust) - d.calibrated = true -} - -*/ - -func normalizeHeading(h float64) float64 { - for h < float64(0.0) { - h = h + float64(360.0) - } - for h >= float64(360.0) { - h = h - float64(360.0) - } - return h -} - -func (d *MPU6050) getMPUData() { - pr, rr, hr, err := mpu.ReadMPU() - - // Convert from radians to degrees. - pitch := float64(pr) * (float64(180.0) / math.Pi) - roll := float64(-rr) * (float64(180.0) / math.Pi) - heading := float64(hr) * (float64(180.0) / math.Pi) - if heading < float64(0.0) { - heading = float64(360.0) + heading - } - - if err == nil { - d.pitch = pitch - d.roll = roll - - // Heading is raw value off the IMU. Without mag compass fusion, need to apply correction bias. - // Amount of correction is set by ResetHeading() -- doesn't necessarily have to be based off GPS. - d.heading = normalizeHeading((heading - d.heading_correction)) - - } else { - // log.Printf("mpu6050.calculatePitchAndRoll(): mpu.ReadMPU() err: %s\n", err.Error()) - } -} - -// Temperature returns the current temperature reading. -func (d *MPU6050) PitchAndRoll() (float64, float64) { - return (d.pitch - d.pitch_resting), (d.roll - d.roll_resting) -} - -func (d *MPU6050) Heading() float64 { - return d.heading -} - -func (d *MPU6050) Run() { - time.Sleep(d.Poll) - go func() { - d.quit = make(chan struct{}) - timer := time.NewTicker(d.Poll) - // calibrateTimer := time.NewTicker(1 * time.Minute) - for { - select { - case <-timer.C: - d.getMPUData() - // case <-calibrateTimer.C: - // d.calibrate() - // calibrateTimer.Stop() - case <-d.quit: - mpu.CloseMPU() - return - } - } - }() - return -} - -// Set heading from a known value (usually GPS true heading). -func (d *MPU6050) ResetHeading(newHeading float64, gain float64) { - if gain < 0.001 { // sanitize our inputs! - gain = 0.001 - } else if gain > 1 { - gain = 1 - } - - old_hdg := d.heading // only used for debug log report - //newHeading = float64(30*time.Now().Minute()) // demo input for testing - newHeading = normalizeHeading(newHeading) // sanitize the inputs - - // By applying gain factor, this becomes a 1st order function that slowly converges on solution. - // Time constant is poll rate over gain. With gain of 0.1, convergence to +/-2 deg on a 180 correction difference is about 4 sec; 0.01 converges in 45 sec. - - hdg_corr_bias := float64(d.heading - newHeading) // desired adjustment to heading_correction - if hdg_corr_bias > 180 { - hdg_corr_bias = hdg_corr_bias - 360 - } else if hdg_corr_bias < -180 { - hdg_corr_bias = hdg_corr_bias + 360 - } - hdg_corr_bias = hdg_corr_bias * gain - d.heading_correction = normalizeHeading(d.heading_correction + hdg_corr_bias) - log.Printf("Adjusted heading. Old: %f Desired: %f Adjustment: %f New: %f\n", old_hdg, newHeading, hdg_corr_bias, d.heading-hdg_corr_bias) -} - -// Close. -func (d *MPU6050) Close() { - if d.quit != nil { - d.quit <- struct{}{} - } -} diff --git a/notes/app-vendor-integration.md b/notes/app-vendor-integration.md index a0a4ce9e..67c52eb2 100644 --- a/notes/app-vendor-integration.md +++ b/notes/app-vendor-integration.md @@ -103,7 +103,6 @@ Stratux makes available a webserver to retrieve statistics which may be useful t "GPS_satellites_locked": 0, // Number of GPS satellites used in last GPS lock. "GPS_connected": true, // GPS unit connected and functioning. "GPS_solution": "", // "DGPS (WAAS)", "3D GPS", "N/A", or "" when GPS not connected/enabled. - "RY835AI_connected": false, // GPS/AHRS unit - use only for debugging (this will be removed). "Uptime": 227068, // Device uptime (in milliseconds). "CPUTemp": 42.236 // CPU temperature (in ºC). } diff --git a/test/sensortest.go b/test/sensortest.go deleted file mode 100644 index 11aff6c5..00000000 --- a/test/sensortest.go +++ /dev/null @@ -1,36 +0,0 @@ -package main - -import ( - "../mpu6050" - "fmt" - "net" - "time" -) - -var attSensor *mpu6050.MPU6050 - -func readMPU6050() (float64, float64) { - pitch, roll := attSensor.PitchAndRoll() - - return pitch, roll -} - -func initMPU6050() { - attSensor = mpu6050.New() -} - -func main() { - initMPU6050() - addr, err := net.ResolveUDPAddr("udp", "192.168.1.255:49002") - if err != nil { - panic(err) - } - outConn, err := net.DialUDP("udp", nil, addr) - for { - pitch, roll := readMPU6050() - s := fmt.Sprintf("XATTMy Sim,%f,%f,%f", attSensor.Heading(), pitch, roll) - fmt.Printf("%f, %f\n", pitch, roll) - outConn.Write([]byte(s)) - time.Sleep(50 * time.Millisecond) - } -}