diff --git a/main/gen_gdl90.go b/main/gen_gdl90.go index 36af3851..47f3ffbd 100644 --- a/main/gen_gdl90.go +++ b/main/gen_gdl90.go @@ -468,14 +468,19 @@ func makeStratuxStatus() []byte { } // Valid/Enabled: AHRS Enabled portion. - // msg[12] = 1 << 0 + if globalSettings.Sensors_Enabled { + msg[12] = 1 << 0 + } // Valid/Enabled: last bit unused. // Connected hardware: number of radios. msg[15] = msg[15] | (byte(globalStatus.Devices) & 0x3) - // Connected hardware. - // RY835AI: msg[15] = msg[15] | (1 << 2) + + // Connected hardware: IMU (spec really says just RY835AI, could revise for other IMUs + if globalStatus.IMUConnected { + msg[15] = msg[15] | (1 << 2) + } // Number of GPS satellites locked. msg[16] = byte(globalStatus.GPS_satellites_locked) @@ -1025,6 +1030,7 @@ type settings struct { ES_Enabled bool Ping_Enabled bool GPS_Enabled bool + Sensors_Enabled bool NetworkOutputs []networkConnection SerialOutputs map[string]serialConnection DisplayTrafficSource bool @@ -1088,6 +1094,7 @@ func defaultSettings() { globalSettings.UAT_Enabled = true globalSettings.ES_Enabled = true globalSettings.GPS_Enabled = true + globalSettings.Sensors_Enabled = true //FIXME: Need to change format below. globalSettings.NetworkOutputs = []networkConnection{ {Conn: nil, Ip: "", Port: 4000, Capability: NETWORK_GDL90_STANDARD | NETWORK_AHRS_GDL90}, @@ -1197,6 +1204,9 @@ func printStats() { log.Printf(" - Last GPS fix: %s, GPS solution type: %d using %d satellites (%d/%d seen/tracked), NACp: %d, est accuracy %.02f m\n", stratuxClock.HumanizeTime(mySituation.LastFixLocalTime), mySituation.Quality, mySituation.Satellites, mySituation.SatellitesSeen, mySituation.SatellitesTracked, mySituation.NACp, mySituation.Accuracy) log.Printf(" - GPS vertical velocity: %.02f ft/sec; GPS vertical accuracy: %v m\n", mySituation.GPSVertVel, mySituation.AccuracyVert) } + if globalSettings.Sensors_Enabled { + log.Printf(" - Last IMU read: %s, Last BMP read: %s\n", stratuxClock.HumanizeTime(mySituation.LastAttitudeTime), stratuxClock.HumanizeTime(mySituation.LastTempPressTime)) + } // Check if we're using more than 95% of the free space. If so, throw a warning (only once). if !diskUsageWarning && usage.Usage() > 95.0 { err_p := fmt.Errorf("Disk bytes used = %s (%.1f %%), Disk bytes free = %s (%.1f %%)", humanize.Bytes(usage.Used()), 100*usage.Usage(), humanize.Bytes(usage.Free()), 100*(1-usage.Usage())) @@ -1413,15 +1423,6 @@ func main() { // Start the GPS external sensor monitoring. initGPS() - // Start appropriate AHRS calc, depending on whether or not we have an IMU connected - if globalStatus.IMUConnected { - log.Println("AHRS Info: IMU connected - starting sensorAttitudeSender") - go sensorAttitudeSender() - } else { - log.Println("AHRS Info: IMU not connected - starting gpsAttitudeSender") - go gpsAttitudeSender() - } - // Start the heartbeat message loop in the background, once per second. go heartBeatSender() diff --git a/main/gps.go b/main/gps.go index 26eb4748..562c70c7 100644 --- a/main/gps.go +++ b/main/gps.go @@ -1877,7 +1877,7 @@ func gpsAttitudeSender() { for { <-timer.C myGPSPerfStats = make([]gpsPerfStats, 0) // reinitialize statistics on disconnect / reconnect - for globalSettings.GPS_Enabled && globalStatus.GPS_connected { + for !(globalSettings.Sensors_Enabled && globalStatus.IMUConnected) && (globalSettings.GPS_Enabled && globalStatus.GPS_connected) { <-timer.C if mySituation.Quality == 0 || !calcGPSAttitude() { @@ -1974,6 +1974,7 @@ func isTempPressValid() bool { func pollGPS() { readyToInitGPS = true //TODO: Implement more robust method (channel control) to kill zombie serial readers timer := time.NewTicker(4 * time.Second) + go gpsAttitudeSender() for { <-timer.C // GPS enabled, was not connected previously? diff --git a/main/managementinterface.go b/main/managementinterface.go index e9c4cbc9..0d2860f8 100644 --- a/main/managementinterface.go +++ b/main/managementinterface.go @@ -274,6 +274,8 @@ func handleSettingsSetRequest(w http.ResponseWriter, r *http.Request) { globalSettings.Ping_Enabled = val.(bool) case "GPS_Enabled": globalSettings.GPS_Enabled = val.(bool) + case "Sensors_Enabled": + globalSettings.Sensors_Enabled = val.(bool) case "DEBUG": globalSettings.DEBUG = val.(bool) case "DisplayTrafficSource": diff --git a/main/sensors.go b/main/sensors.go index eeb7cc94..6bef55e0 100644 --- a/main/sensors.go +++ b/main/sensors.go @@ -24,19 +24,24 @@ var ( func initI2CSensors() { i2cbus = embd.NewI2CBus(1) - globalStatus.PressureSensorConnected = initPressureSensor() // I2C temperature and pressure altitude. - log.Printf("AHRS Info: pressure sensor connected: %t\n", globalStatus.PressureSensorConnected) - globalStatus.IMUConnected = initIMU() // I2C accel/gyro/mag. - log.Printf("AHRS Info: IMU connected: %t\n", globalStatus.IMUConnected) + go pollSensors() + go sensorAttitudeSender() +} - if !(globalStatus.PressureSensorConnected || globalStatus.IMUConnected) { - i2cbus.Close() - log.Println("AHRS Info: I2C bus closed") - } +func pollSensors() { + timer := time.NewTicker(4 * time.Second) + for { + <-timer.C - if globalStatus.PressureSensorConnected { - go tempAndPressureSender() - log.Println("AHRS Info: monitoring pressure sensor") + // If it's not currently connected, try connecting to pressure sensor + if globalSettings.Sensors_Enabled && !globalStatus.PressureSensorConnected { + globalStatus.PressureSensorConnected = initPressureSensor() // I2C temperature and pressure altitude. + } + + // If it's not currently connected, try connecting to IMU + if globalSettings.Sensors_Enabled && !globalStatus.IMUConnected { + globalStatus.IMUConnected = initIMU() // I2C accel/gyro/mag. + } } } @@ -44,9 +49,9 @@ func initPressureSensor() (ok bool) { bmp, err := sensors.NewBMP280(&i2cbus, 100*time.Millisecond) if err == nil { myPressureReader = bmp - ok = true + go tempAndPressureSender() log.Println("AHRS Info: Successfully initialized BMP280") - return + return true } // TODO westphae: make bmp180.go to fit bmp interface @@ -63,41 +68,25 @@ func initPressureSensor() (ok bool) { //} log.Println("AHRS Info: couldn't initialize BMP280 or BMP180") - return + return false } func initIMU() (ok bool) { - var err error - - for i := 0; i < 5; i++ { - log.Printf("AHRS Info: attempt %d to connect to MPU9250\n", i) - myIMUReader, err = sensors.NewMPU9250() - if err != nil { - log.Printf("AHRS Info: attempt %d failed to connect to MPU9250\n", i) - time.Sleep(100 * time.Millisecond) - } else { - time.Sleep(time.Second) - log.Println("AHRS Info: Successfully connected MPU9250, running calibration") - myIMUReader.Calibrate(1, 5) - log.Println("AHRS Info: Successfully initialized MPU9250") - return true - } + log.Println("AHRS Info: attempting to connect to MPU9250") + imu, err := sensors.NewMPU9250() + if err == nil { + myIMUReader = imu + time.Sleep(200 * time.Millisecond) + log.Println("AHRS Info: Successfully connected MPU9250, running calibration") + myIMUReader.Calibrate(1, 5) + log.Println("AHRS Info: Successfully calibrated MPU9250") + return true } - //for i := 0; i < 5; i++ { - // myIMUReader, err = sensors.NewMPU6050() - // if err != nil { - // log.Printf("AHRS Info: attempt %d failed to connect to MPU6050\n", i) - // time.Sleep(100 * time.Millisecond) - // } else { - // ok = true - // log.Println("AHRS Info: Successfully initialized MPU6050") - // return - // } - //} + // TODO westphae: try to connect to MPU9150 - log.Println("AHRS Error: couldn't initialize MPU9250 or MPU6050") - return + log.Println("AHRS Error: couldn't initialize MPU9250 or MPU9150") + return false } func tempAndPressureSender() { @@ -113,12 +102,14 @@ func tempAndPressureSender() { // Initialize variables for rate of climb calc u := 5 / (5 + float64(dt)) // Use 5 sec decay time for rate of climb, slightly faster than typical VSI if press, err = myPressureReader.Pressure(); err != nil { - log.Printf("AHRS Error: Couldn't read temp from sensor: %s", err) + log.Printf("AHRS Error: Couldn't read pressure from sensor: %s", err) + myPressureReader.Close() + globalStatus.PressureSensorConnected = false // Try reconnecting a little later } altLast = CalcAltitude(press) timer := time.NewTicker(time.Duration(1000*dt) * time.Millisecond) - for globalStatus.PressureSensorConnected { + for globalSettings.Sensors_Enabled && globalStatus.PressureSensorConnected { <-timer.C // Read temperature and pressure altitude. @@ -129,7 +120,9 @@ func tempAndPressureSender() { press, err = myPressureReader.Pressure() if err != nil { log.Printf("AHRS Error: Couldn't read pressure from sensor: %s", err) - continue + myPressureReader.Close() + globalStatus.PressureSensorConnected = false // Try reconnecting a little later + break } // Update the Situation data. @@ -168,111 +161,113 @@ func sensorAttitudeSender() { ahrswebListener, err := ahrsweb.NewKalmanListener() if err != nil { log.Printf("AHRS Error: couldn't start ahrswebListener: %s\n", err.Error()) + } else { + defer ahrswebListener.Close() } // Need a 10Hz sampling freq timer := time.NewTicker(100 * time.Millisecond) // ~10Hz update. - for globalStatus.IMUConnected { + for { <-timer.C - t = stratuxClock.Time - m.T = float64(t.UnixNano()/1000) / 1e6 + for (globalSettings.Sensors_Enabled && globalStatus.IMUConnected) { + <-timer.C + t = stratuxClock.Time + m.T = float64(t.UnixNano() / 1000) / 1e6 - _, bx, by, bz, ax, ay, az, mx, my, mz, mpuError, magError = myIMUReader.ReadRaw() - //TODO westphae: allow user configuration of this mapping from a file, plus UI modification - //m.B1, m.B2, m.B3 = +by, -bx, +bz // This is how the RY83XAI is wired up - //m.A1, m.A2, m.A3 = -ay, +ax, -az // This is how the RY83XAI is wired up - m.B1, m.B2, m.B3 = -bx, +by, -bz // This is how the OpenFlightBox board is wired up - m.A1, m.A2, m.A3 = -ay, +ax, +az // This is how the OpenFlightBox board is wired up - m.M1, m.M2, m.M3 = +mx, +my, +mz - m.SValid = mpuError == nil - m.MValid = magError == nil - if mpuError != nil { - log.Printf("AHRS Gyro/Accel Error, not using for this run: %s\n", mpuError.Error()) - //TODO westphae: disconnect? - } - if magError != nil { - log.Printf("AHRS Magnetometer Error, not using for this run: %s\n", magError.Error()) - m.MValid = false - } - - m.WValid = t.Sub(mySituation.LastGroundTrackTime) < 500*time.Millisecond - if m.WValid { - m.W1 = mySituation.GroundSpeed * math.Sin(float64(mySituation.TrueCourse)*ahrs.Deg) - m.W2 = mySituation.GroundSpeed * math.Cos(float64(mySituation.TrueCourse)*ahrs.Deg) - if globalStatus.PressureSensorConnected { - m.W3 = mySituation.RateOfClimb * 60 / 6076.12 - } else { - m.W3 = float64(mySituation.GPSVertVel) * 3600 / 6076.12 + _, bx, by, bz, ax, ay, az, mx, my, mz, mpuError, magError = myIMUReader.ReadRaw() + //TODO westphae: allow user configuration of this mapping from a file, plus UI modification + //m.B1, m.B2, m.B3 = +by, -bx, +bz // This is how the RY83XAI is wired up + //m.A1, m.A2, m.A3 = -ay, +ax, -az // This is how the RY83XAI is wired up + m.B1, m.B2, m.B3 = -bx, +by, -bz // This is how the OpenFlightBox board is wired up + m.A1, m.A2, m.A3 = -ay, +ax, +az // This is how the OpenFlightBox board is wired up + m.M1, m.M2, m.M3 = +mx, +my, +mz + m.SValid = mpuError == nil + m.MValid = magError == nil + if mpuError != nil { + log.Printf("AHRS Gyro/Accel Error: %s\n", mpuError) + myIMUReader.Close() + globalStatus.IMUConnected = false } - } - - // Run the AHRS calcs - if s == nil { // s is nil if we should (re-)initialize the Kalman state - log.Println("AHRS Info: initializing new simple AHRS") - s = ahrs.InitializeSimple(m, "") - } - s.Compute(m) - - // Debugging server: - if ahrswebListener != nil { - ahrswebListener.Send(s.GetState(), m) - } - - // If we have valid AHRS info, then send - if s.Valid() { - mySituation.mu_Attitude.Lock() - - roll, pitch, heading = s.CalcRollPitchHeading() - mySituation.Roll = roll / ahrs.Deg - mySituation.Pitch = pitch / ahrs.Deg - mySituation.Gyro_heading = heading / ahrs.Deg - - if headingMag, errHeadingMag = myIMUReader.MagHeading(); errHeadingMag != nil { - log.Printf("AHRS MPU Error: %s\n", errHeadingMag.Error()) - } else { - mySituation.Mag_heading = headingMag + if magError != nil { + log.Printf("AHRS Magnetometer Error, not using for this run: %s\n", magError) + m.MValid = false + // Don't necessarily disconnect here, unless AHRSProvider deeply depends on magnetometer } - if slipSkid, errSlipSkid = myIMUReader.SlipSkid(); errSlipSkid != nil { - log.Printf("AHRS MPU Error: %s\n", errSlipSkid.Error()) - } else { - mySituation.SlipSkid = slipSkid + m.WValid = t.Sub(mySituation.LastGroundTrackTime) < 500*time.Millisecond + if m.WValid { + m.W1 = mySituation.GroundSpeed * math.Sin(float64(mySituation.TrueCourse) * ahrs.Deg) + m.W2 = mySituation.GroundSpeed * math.Cos(float64(mySituation.TrueCourse) * ahrs.Deg) + if globalStatus.PressureSensorConnected { + m.W3 = mySituation.RateOfClimb * 60 / 6076.12 + } else { + m.W3 = float64(mySituation.GPSVertVel) * 3600 / 6076.12 + } } - if turnRate, errTurnRate = myIMUReader.RateOfTurn(); errTurnRate != nil { - log.Printf("AHRS MPU Error: %s\n", errTurnRate.Error()) - } else { - mySituation.RateOfTurn = turnRate + // Run the AHRS calcs + if s == nil { // s is nil if we should (re-)initialize the Kalman state + log.Println("AHRS Info: initializing new simple AHRS") + s = ahrs.InitializeSimple(m, "") + } + s.Compute(m) + + // Debugging server: + if ahrswebListener != nil { + ahrswebListener.Send(s.GetState(), m) } - if gLoad, errGLoad = myIMUReader.GLoad(); errGLoad != nil { - log.Printf("AHRS MPU Error: %s\n", errGLoad.Error()) + // If we have valid AHRS info, then send + if s.Valid() { + mySituation.mu_Attitude.Lock() + + roll, pitch, heading = s.CalcRollPitchHeading() + mySituation.Roll = roll / ahrs.Deg + mySituation.Pitch = pitch / ahrs.Deg + mySituation.Gyro_heading = heading / ahrs.Deg + + if headingMag, errHeadingMag = myIMUReader.MagHeading(); errHeadingMag != nil { + log.Printf("AHRS MPU Error: %s\n", errHeadingMag.Error()) + } else { + mySituation.Mag_heading = headingMag + } + + if slipSkid, errSlipSkid = myIMUReader.SlipSkid(); errSlipSkid != nil { + log.Printf("AHRS MPU Error: %s\n", errSlipSkid.Error()) + } else { + mySituation.SlipSkid = slipSkid + } + + if turnRate, errTurnRate = myIMUReader.RateOfTurn(); errTurnRate != nil { + log.Printf("AHRS MPU Error: %s\n", errTurnRate.Error()) + } else { + mySituation.RateOfTurn = turnRate + } + + if gLoad, errGLoad = myIMUReader.GLoad(); errGLoad != nil { + log.Printf("AHRS MPU Error: %s\n", errGLoad.Error()) + } else { + mySituation.GLoad = gLoad + } + + mySituation.LastAttitudeTime = t + mySituation.mu_Attitude.Unlock() + + // 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. } else { - mySituation.GLoad = gLoad + s = nil + mySituation.LastAttitudeTime = time.Time{} } - mySituation.LastAttitudeTime = t - mySituation.mu_Attitude.Unlock() + makeAHRSGDL90Report() // Send whether or not valid - the function will invalidate the values as appropriate - // 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. - } else { - s = nil - mySituation.LastAttitudeTime = time.Time{} + logger.Log( + float64(t.UnixNano() / 1000)/1e6, + m.T, m.A1, m.A2, m.A3, m.B1, m.B2, m.B3, m.M1, m.M2, m.M3, + float64(mySituation.LastGroundTrackTime.UnixNano() / 1000)/1e6, m.W1, m.W2, m.W3, + float64(mySituation.LastTempPressTime.UnixNano() / 1000)/1e6, mySituation.Pressure_alt, + pitch/ahrs.Deg, roll/ahrs.Deg, heading/ahrs.Deg, headingMag, slipSkid, turnRate, gLoad, + float64(mySituation.LastAttitudeTime.UnixNano() / 1000)/1e6) } - - makeAHRSGDL90Report() // Send whether or not valid - the function will invalidate the values as appropriate - - logger.Log( - float64(t.UnixNano()/1000)/1e6, - m.T, m.A1, m.A2, m.A3, m.B1, m.B2, m.B3, m.M1, m.M2, m.M3, - float64(mySituation.LastGroundTrackTime.UnixNano()/1000)/1e6, m.W1, m.W2, m.W3, - float64(mySituation.LastTempPressTime.UnixNano()/1000)/1e6, mySituation.Pressure_alt, - pitch/ahrs.Deg, roll/ahrs.Deg, heading/ahrs.Deg, headingMag, slipSkid, turnRate, gLoad, - float64(mySituation.LastAttitudeTime.UnixNano()/1000)/1e6) } - log.Println("AHRS Info: Exited sensorAttitudeSender loop") - globalStatus.IMUConnected = false - ahrswebListener.Close() - myPressureReader.Close() - myIMUReader.Close() } diff --git a/web/plates/js/ahrs.js b/web/plates/js/ahrs.js index 4a5e994b..208c8065 100644 --- a/web/plates/js/ahrs.js +++ b/web/plates/js/ahrs.js @@ -9,20 +9,16 @@ function ahrsRenderer(location_id) { var ai = document.getElementById("attitude_indicator"), _this = this; - this.ps = [] - this.rs = [] - this.hs = [] - this.ss = [] + this.ps = []; + this.rs = []; + this.hs = []; + this.ss = []; ai.onload = function() { _this.ps = ai.contentDocument.getElementsByClassName("pitch"); _this.rs = ai.contentDocument.getElementsByClassName("roll"); _this.hs = ai.contentDocument.getElementsByClassName("heading"); _this.ss = ai.contentDocument.getElementsByClassName("slipSkid"); - console.log(_this.ps.length); - console.log(_this.rs.length); - console.log(_this.hs.length); - console.log(_this.ss.length); }; } @@ -35,6 +31,7 @@ ahrsRenderer.prototype = { this.pitch = 0; this.roll = 0; this.heading = 0; + this.slipSkid = 0; this.resize(); }, @@ -90,8 +87,7 @@ ahrsRenderer.prototype = { draw: function() { for (i=0; i +
+ +
+ +
+
diff --git a/web/plates/status.html b/web/plates/status.html index b56ebb2b..0093c981 100644 --- a/web/plates/status.html +++ b/web/plates/status.html @@ -57,7 +57,7 @@
{{product_rows}}
--> -
+