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

Add Enable_Sensors to settings UI

pull/578/head
Eric Westphal 2017-01-07 08:05:37 -05:00
rodzic 601efa9023
commit a6642353d7
9 zmienionych plików z 166 dodań i 161 usunięć
Pokaż statystyki Ściągnij plik aktualizacji Ściągnij plik porównania Expand all files Collapse all files

25
main/gen_gdl90.go
Wyświetl plik

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

3
main/gps.go
Wyświetl plik

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

2
main/managementinterface.go
Wyświetl plik

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

261
main/sensors.go
Wyświetl plik

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

24
web/plates/js/ahrs.js
Wyświetl plik

@ -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<this.ps.length; i++) {
console.log("pitch " + i);
this.ps[i].setAttribute("transform", "translate(0,"+this.pitch*10+")")
this.ps[i].setAttribute("transform", "translate(0,"+this.pitch * 10+")")
}
for (i=0; i<this.rs.length; i++) {
@ -99,11 +95,15 @@ ahrsRenderer.prototype = {
}
for (i=0; i<this.hs.length; i++) {
this.hs[i].setAttribute("transform", "translate("+(-this.heading*2)+",0)")
var h = this.heading;
while (h < 0) {
h += 360
}
this.hs[i].setAttribute("transform", "translate("+(-(this.heading % 360) * 2)+",0)")
}
for (i=0; i<this.ss.length; i++) {
this.ss[i].setAttribute("transform", "translate("+(-this.slipSkid*2)+",0)")
this.ss[i].setAttribute("transform", "translate("+(-this.slipSkid * 2)+",0)")
}
}
};

3
web/plates/js/settings.js
Wyświetl plik

@ -6,7 +6,7 @@ function SettingsCtrl($rootScope, $scope, $state, $location, $window, $http) {
$scope.$parent.helppage = 'plates/settings-help.html';
var toggles = ['UAT_Enabled', 'ES_Enabled', 'Ping_Enabled', 'GPS_Enabled', 'DisplayTrafficSource', 'DEBUG', 'ReplayLog'];
var toggles = ['UAT_Enabled', 'ES_Enabled', 'Ping_Enabled', 'GPS_Enabled', 'Sensors_Enabled', 'DisplayTrafficSource', 'DEBUG', 'ReplayLog'];
var settings = {};
for (i = 0; i < toggles.length; i++) {
settings[toggles[i]] = undefined;
@ -26,6 +26,7 @@ function SettingsCtrl($rootScope, $scope, $state, $location, $window, $http) {
$scope.ES_Enabled = settings.ES_Enabled;
$scope.Ping_Enabled = settings.Ping_Enabled;
$scope.GPS_Enabled = settings.GPS_Enabled;
$scope.Sensors_Enabled = settings.Sensors_Enabled;
$scope.DisplayTrafficSource = settings.DisplayTrafficSource;
$scope.DEBUG = settings.DEBUG;
$scope.ReplayLog = settings.ReplayLog;

1
web/plates/js/status.js
Wyświetl plik

@ -94,7 +94,6 @@ function StatusCtrl($rootScope, $scope, $state, $http, $interval) {
$scope.visible_uat = true;
$scope.visible_es = true;
$scope.visible_gps = true;
$scope.visible_ahrs = true;
// Simple GET request example (note: responce is asynchronous)
$http.get(URL_SETTINGS_GET).

6
web/plates/settings.html
Wyświetl plik

@ -28,6 +28,12 @@
<ui-switch ng-model='GPS_Enabled' settings-change></ui-switch>
</div>
</div>
<div class="form-group">
<label class="control-label col-xs-7">Sensors</label>
<div class="col-xs-5">
<ui-switch ng-model='Sensors_Enabled' settings-change></ui-switch>
</div>
</div>
</div>
</div>
</div>

2
web/plates/status.html
Wyświetl plik

@ -57,7 +57,7 @@
<div>{{product_rows}}</div>
</div>
-->
<div class="row" ng-class="{ 'section_invisible': (!visible_gps && !visible_ahrs && !visible_uat)}">
<div class="row" ng-class="{ 'section_invisible': (!visible_gps && !visible_uat)}">
<span class="col-xs-1">&nbsp;</span>
</div>
<div class="row" ng-class="{'section_invisible': !visible_uat}">