kopia lustrzana https://github.com/cyoung/stratux
Add Enable_Sensors to settings UI
rodzic
601efa9023
commit
a6642353d7
|
@ -468,14 +468,19 @@ func makeStratuxStatus() []byte {
|
||||||
}
|
}
|
||||||
|
|
||||||
// Valid/Enabled: AHRS Enabled portion.
|
// Valid/Enabled: AHRS Enabled portion.
|
||||||
// msg[12] = 1 << 0
|
if globalSettings.Sensors_Enabled {
|
||||||
|
msg[12] = 1 << 0
|
||||||
|
}
|
||||||
|
|
||||||
// Valid/Enabled: last bit unused.
|
// Valid/Enabled: last bit unused.
|
||||||
|
|
||||||
// Connected hardware: number of radios.
|
// Connected hardware: number of radios.
|
||||||
msg[15] = msg[15] | (byte(globalStatus.Devices) & 0x3)
|
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.
|
// Number of GPS satellites locked.
|
||||||
msg[16] = byte(globalStatus.GPS_satellites_locked)
|
msg[16] = byte(globalStatus.GPS_satellites_locked)
|
||||||
|
@ -1025,6 +1030,7 @@ type settings struct {
|
||||||
ES_Enabled bool
|
ES_Enabled bool
|
||||||
Ping_Enabled bool
|
Ping_Enabled bool
|
||||||
GPS_Enabled bool
|
GPS_Enabled bool
|
||||||
|
Sensors_Enabled bool
|
||||||
NetworkOutputs []networkConnection
|
NetworkOutputs []networkConnection
|
||||||
SerialOutputs map[string]serialConnection
|
SerialOutputs map[string]serialConnection
|
||||||
DisplayTrafficSource bool
|
DisplayTrafficSource bool
|
||||||
|
@ -1088,6 +1094,7 @@ func defaultSettings() {
|
||||||
globalSettings.UAT_Enabled = true
|
globalSettings.UAT_Enabled = true
|
||||||
globalSettings.ES_Enabled = true
|
globalSettings.ES_Enabled = true
|
||||||
globalSettings.GPS_Enabled = true
|
globalSettings.GPS_Enabled = true
|
||||||
|
globalSettings.Sensors_Enabled = true
|
||||||
//FIXME: Need to change format below.
|
//FIXME: Need to change format below.
|
||||||
globalSettings.NetworkOutputs = []networkConnection{
|
globalSettings.NetworkOutputs = []networkConnection{
|
||||||
{Conn: nil, Ip: "", Port: 4000, Capability: NETWORK_GDL90_STANDARD | NETWORK_AHRS_GDL90},
|
{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(" - 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)
|
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).
|
// 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 {
|
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()))
|
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.
|
// Start the GPS external sensor monitoring.
|
||||||
initGPS()
|
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.
|
// Start the heartbeat message loop in the background, once per second.
|
||||||
go heartBeatSender()
|
go heartBeatSender()
|
||||||
|
|
||||||
|
|
|
@ -1877,7 +1877,7 @@ func gpsAttitudeSender() {
|
||||||
for {
|
for {
|
||||||
<-timer.C
|
<-timer.C
|
||||||
myGPSPerfStats = make([]gpsPerfStats, 0) // reinitialize statistics on disconnect / reconnect
|
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
|
<-timer.C
|
||||||
|
|
||||||
if mySituation.Quality == 0 || !calcGPSAttitude() {
|
if mySituation.Quality == 0 || !calcGPSAttitude() {
|
||||||
|
@ -1974,6 +1974,7 @@ func isTempPressValid() bool {
|
||||||
func pollGPS() {
|
func pollGPS() {
|
||||||
readyToInitGPS = true //TODO: Implement more robust method (channel control) to kill zombie serial readers
|
readyToInitGPS = true //TODO: Implement more robust method (channel control) to kill zombie serial readers
|
||||||
timer := time.NewTicker(4 * time.Second)
|
timer := time.NewTicker(4 * time.Second)
|
||||||
|
go gpsAttitudeSender()
|
||||||
for {
|
for {
|
||||||
<-timer.C
|
<-timer.C
|
||||||
// GPS enabled, was not connected previously?
|
// GPS enabled, was not connected previously?
|
||||||
|
|
|
@ -274,6 +274,8 @@ func handleSettingsSetRequest(w http.ResponseWriter, r *http.Request) {
|
||||||
globalSettings.Ping_Enabled = val.(bool)
|
globalSettings.Ping_Enabled = val.(bool)
|
||||||
case "GPS_Enabled":
|
case "GPS_Enabled":
|
||||||
globalSettings.GPS_Enabled = val.(bool)
|
globalSettings.GPS_Enabled = val.(bool)
|
||||||
|
case "Sensors_Enabled":
|
||||||
|
globalSettings.Sensors_Enabled = val.(bool)
|
||||||
case "DEBUG":
|
case "DEBUG":
|
||||||
globalSettings.DEBUG = val.(bool)
|
globalSettings.DEBUG = val.(bool)
|
||||||
case "DisplayTrafficSource":
|
case "DisplayTrafficSource":
|
||||||
|
|
261
main/sensors.go
261
main/sensors.go
|
@ -24,19 +24,24 @@ var (
|
||||||
func initI2CSensors() {
|
func initI2CSensors() {
|
||||||
i2cbus = embd.NewI2CBus(1)
|
i2cbus = embd.NewI2CBus(1)
|
||||||
|
|
||||||
globalStatus.PressureSensorConnected = initPressureSensor() // I2C temperature and pressure altitude.
|
go pollSensors()
|
||||||
log.Printf("AHRS Info: pressure sensor connected: %t\n", globalStatus.PressureSensorConnected)
|
go sensorAttitudeSender()
|
||||||
globalStatus.IMUConnected = initIMU() // I2C accel/gyro/mag.
|
}
|
||||||
log.Printf("AHRS Info: IMU connected: %t\n", globalStatus.IMUConnected)
|
|
||||||
|
|
||||||
if !(globalStatus.PressureSensorConnected || globalStatus.IMUConnected) {
|
func pollSensors() {
|
||||||
i2cbus.Close()
|
timer := time.NewTicker(4 * time.Second)
|
||||||
log.Println("AHRS Info: I2C bus closed")
|
for {
|
||||||
}
|
<-timer.C
|
||||||
|
|
||||||
if globalStatus.PressureSensorConnected {
|
// If it's not currently connected, try connecting to pressure sensor
|
||||||
go tempAndPressureSender()
|
if globalSettings.Sensors_Enabled && !globalStatus.PressureSensorConnected {
|
||||||
log.Println("AHRS Info: monitoring pressure sensor")
|
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)
|
bmp, err := sensors.NewBMP280(&i2cbus, 100*time.Millisecond)
|
||||||
if err == nil {
|
if err == nil {
|
||||||
myPressureReader = bmp
|
myPressureReader = bmp
|
||||||
ok = true
|
go tempAndPressureSender()
|
||||||
log.Println("AHRS Info: Successfully initialized BMP280")
|
log.Println("AHRS Info: Successfully initialized BMP280")
|
||||||
return
|
return true
|
||||||
}
|
}
|
||||||
|
|
||||||
// TODO westphae: make bmp180.go to fit bmp interface
|
// 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")
|
log.Println("AHRS Info: couldn't initialize BMP280 or BMP180")
|
||||||
return
|
return false
|
||||||
}
|
}
|
||||||
|
|
||||||
func initIMU() (ok bool) {
|
func initIMU() (ok bool) {
|
||||||
var err error
|
log.Println("AHRS Info: attempting to connect to MPU9250")
|
||||||
|
imu, err := sensors.NewMPU9250()
|
||||||
for i := 0; i < 5; i++ {
|
if err == nil {
|
||||||
log.Printf("AHRS Info: attempt %d to connect to MPU9250\n", i)
|
myIMUReader = imu
|
||||||
myIMUReader, err = sensors.NewMPU9250()
|
time.Sleep(200 * time.Millisecond)
|
||||||
if err != nil {
|
log.Println("AHRS Info: Successfully connected MPU9250, running calibration")
|
||||||
log.Printf("AHRS Info: attempt %d failed to connect to MPU9250\n", i)
|
myIMUReader.Calibrate(1, 5)
|
||||||
time.Sleep(100 * time.Millisecond)
|
log.Println("AHRS Info: Successfully calibrated MPU9250")
|
||||||
} else {
|
return true
|
||||||
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
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
//for i := 0; i < 5; i++ {
|
// TODO westphae: try to connect to MPU9150
|
||||||
// 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
|
|
||||||
// }
|
|
||||||
//}
|
|
||||||
|
|
||||||
log.Println("AHRS Error: couldn't initialize MPU9250 or MPU6050")
|
log.Println("AHRS Error: couldn't initialize MPU9250 or MPU9150")
|
||||||
return
|
return false
|
||||||
}
|
}
|
||||||
|
|
||||||
func tempAndPressureSender() {
|
func tempAndPressureSender() {
|
||||||
|
@ -113,12 +102,14 @@ func tempAndPressureSender() {
|
||||||
// Initialize variables for rate of climb calc
|
// 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
|
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 {
|
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)
|
altLast = CalcAltitude(press)
|
||||||
|
|
||||||
timer := time.NewTicker(time.Duration(1000*dt) * time.Millisecond)
|
timer := time.NewTicker(time.Duration(1000*dt) * time.Millisecond)
|
||||||
for globalStatus.PressureSensorConnected {
|
for globalSettings.Sensors_Enabled && globalStatus.PressureSensorConnected {
|
||||||
<-timer.C
|
<-timer.C
|
||||||
|
|
||||||
// Read temperature and pressure altitude.
|
// Read temperature and pressure altitude.
|
||||||
|
@ -129,7 +120,9 @@ func tempAndPressureSender() {
|
||||||
press, err = myPressureReader.Pressure()
|
press, err = myPressureReader.Pressure()
|
||||||
if err != nil {
|
if err != nil {
|
||||||
log.Printf("AHRS Error: Couldn't read pressure from sensor: %s", err)
|
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.
|
// Update the Situation data.
|
||||||
|
@ -168,111 +161,113 @@ func sensorAttitudeSender() {
|
||||||
ahrswebListener, err := ahrsweb.NewKalmanListener()
|
ahrswebListener, err := ahrsweb.NewKalmanListener()
|
||||||
if err != nil {
|
if err != nil {
|
||||||
log.Printf("AHRS Error: couldn't start ahrswebListener: %s\n", err.Error())
|
log.Printf("AHRS Error: couldn't start ahrswebListener: %s\n", err.Error())
|
||||||
|
} else {
|
||||||
|
defer ahrswebListener.Close()
|
||||||
}
|
}
|
||||||
|
|
||||||
// Need a 10Hz sampling freq
|
// Need a 10Hz sampling freq
|
||||||
timer := time.NewTicker(100 * time.Millisecond) // ~10Hz update.
|
timer := time.NewTicker(100 * time.Millisecond) // ~10Hz update.
|
||||||
for globalStatus.IMUConnected {
|
for {
|
||||||
<-timer.C
|
<-timer.C
|
||||||
t = stratuxClock.Time
|
for (globalSettings.Sensors_Enabled && globalStatus.IMUConnected) {
|
||||||
m.T = float64(t.UnixNano()/1000) / 1e6
|
<-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()
|
_, 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
|
//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.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.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.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.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.M1, m.M2, m.M3 = +mx, +my, +mz
|
||||||
m.SValid = mpuError == nil
|
m.SValid = mpuError == nil
|
||||||
m.MValid = magError == nil
|
m.MValid = magError == nil
|
||||||
if mpuError != nil {
|
if mpuError != nil {
|
||||||
log.Printf("AHRS Gyro/Accel Error, not using for this run: %s\n", mpuError.Error())
|
log.Printf("AHRS Gyro/Accel Error: %s\n", mpuError)
|
||||||
//TODO westphae: disconnect?
|
myIMUReader.Close()
|
||||||
}
|
globalStatus.IMUConnected = false
|
||||||
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
|
|
||||||
}
|
}
|
||||||
}
|
if magError != nil {
|
||||||
|
log.Printf("AHRS Magnetometer Error, not using for this run: %s\n", magError)
|
||||||
// Run the AHRS calcs
|
m.MValid = false
|
||||||
if s == nil { // s is nil if we should (re-)initialize the Kalman state
|
// Don't necessarily disconnect here, unless AHRSProvider deeply depends on magnetometer
|
||||||
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 slipSkid, errSlipSkid = myIMUReader.SlipSkid(); errSlipSkid != nil {
|
m.WValid = t.Sub(mySituation.LastGroundTrackTime) < 500*time.Millisecond
|
||||||
log.Printf("AHRS MPU Error: %s\n", errSlipSkid.Error())
|
if m.WValid {
|
||||||
} else {
|
m.W1 = mySituation.GroundSpeed * math.Sin(float64(mySituation.TrueCourse) * ahrs.Deg)
|
||||||
mySituation.SlipSkid = slipSkid
|
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 {
|
// Run the AHRS calcs
|
||||||
log.Printf("AHRS MPU Error: %s\n", errTurnRate.Error())
|
if s == nil { // s is nil if we should (re-)initialize the Kalman state
|
||||||
} else {
|
log.Println("AHRS Info: initializing new simple AHRS")
|
||||||
mySituation.RateOfTurn = turnRate
|
s = ahrs.InitializeSimple(m, "")
|
||||||
|
}
|
||||||
|
s.Compute(m)
|
||||||
|
|
||||||
|
// Debugging server:
|
||||||
|
if ahrswebListener != nil {
|
||||||
|
ahrswebListener.Send(s.GetState(), m)
|
||||||
}
|
}
|
||||||
|
|
||||||
if gLoad, errGLoad = myIMUReader.GLoad(); errGLoad != nil {
|
// If we have valid AHRS info, then send
|
||||||
log.Printf("AHRS MPU Error: %s\n", errGLoad.Error())
|
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 {
|
} else {
|
||||||
mySituation.GLoad = gLoad
|
s = nil
|
||||||
|
mySituation.LastAttitudeTime = time.Time{}
|
||||||
}
|
}
|
||||||
|
|
||||||
mySituation.LastAttitudeTime = t
|
makeAHRSGDL90Report() // Send whether or not valid - the function will invalidate the values as appropriate
|
||||||
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.
|
logger.Log(
|
||||||
} else {
|
float64(t.UnixNano() / 1000)/1e6,
|
||||||
s = nil
|
m.T, m.A1, m.A2, m.A3, m.B1, m.B2, m.B3, m.M1, m.M2, m.M3,
|
||||||
mySituation.LastAttitudeTime = time.Time{}
|
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()
|
|
||||||
}
|
}
|
||||||
|
|
|
@ -9,20 +9,16 @@ function ahrsRenderer(location_id) {
|
||||||
var ai = document.getElementById("attitude_indicator"),
|
var ai = document.getElementById("attitude_indicator"),
|
||||||
_this = this;
|
_this = this;
|
||||||
|
|
||||||
this.ps = []
|
this.ps = [];
|
||||||
this.rs = []
|
this.rs = [];
|
||||||
this.hs = []
|
this.hs = [];
|
||||||
this.ss = []
|
this.ss = [];
|
||||||
|
|
||||||
ai.onload = function() {
|
ai.onload = function() {
|
||||||
_this.ps = ai.contentDocument.getElementsByClassName("pitch");
|
_this.ps = ai.contentDocument.getElementsByClassName("pitch");
|
||||||
_this.rs = ai.contentDocument.getElementsByClassName("roll");
|
_this.rs = ai.contentDocument.getElementsByClassName("roll");
|
||||||
_this.hs = ai.contentDocument.getElementsByClassName("heading");
|
_this.hs = ai.contentDocument.getElementsByClassName("heading");
|
||||||
_this.ss = ai.contentDocument.getElementsByClassName("slipSkid");
|
_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.pitch = 0;
|
||||||
this.roll = 0;
|
this.roll = 0;
|
||||||
this.heading = 0;
|
this.heading = 0;
|
||||||
|
this.slipSkid = 0;
|
||||||
|
|
||||||
this.resize();
|
this.resize();
|
||||||
},
|
},
|
||||||
|
@ -90,8 +87,7 @@ ahrsRenderer.prototype = {
|
||||||
|
|
||||||
draw: function() {
|
draw: function() {
|
||||||
for (i=0; i<this.ps.length; i++) {
|
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++) {
|
for (i=0; i<this.rs.length; i++) {
|
||||||
|
@ -99,11 +95,15 @@ ahrsRenderer.prototype = {
|
||||||
}
|
}
|
||||||
|
|
||||||
for (i=0; i<this.hs.length; i++) {
|
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++) {
|
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)")
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
|
@ -6,7 +6,7 @@ function SettingsCtrl($rootScope, $scope, $state, $location, $window, $http) {
|
||||||
|
|
||||||
$scope.$parent.helppage = 'plates/settings-help.html';
|
$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 = {};
|
var settings = {};
|
||||||
for (i = 0; i < toggles.length; i++) {
|
for (i = 0; i < toggles.length; i++) {
|
||||||
settings[toggles[i]] = undefined;
|
settings[toggles[i]] = undefined;
|
||||||
|
@ -26,6 +26,7 @@ function SettingsCtrl($rootScope, $scope, $state, $location, $window, $http) {
|
||||||
$scope.ES_Enabled = settings.ES_Enabled;
|
$scope.ES_Enabled = settings.ES_Enabled;
|
||||||
$scope.Ping_Enabled = settings.Ping_Enabled;
|
$scope.Ping_Enabled = settings.Ping_Enabled;
|
||||||
$scope.GPS_Enabled = settings.GPS_Enabled;
|
$scope.GPS_Enabled = settings.GPS_Enabled;
|
||||||
|
$scope.Sensors_Enabled = settings.Sensors_Enabled;
|
||||||
$scope.DisplayTrafficSource = settings.DisplayTrafficSource;
|
$scope.DisplayTrafficSource = settings.DisplayTrafficSource;
|
||||||
$scope.DEBUG = settings.DEBUG;
|
$scope.DEBUG = settings.DEBUG;
|
||||||
$scope.ReplayLog = settings.ReplayLog;
|
$scope.ReplayLog = settings.ReplayLog;
|
||||||
|
|
|
@ -94,7 +94,6 @@ function StatusCtrl($rootScope, $scope, $state, $http, $interval) {
|
||||||
$scope.visible_uat = true;
|
$scope.visible_uat = true;
|
||||||
$scope.visible_es = true;
|
$scope.visible_es = true;
|
||||||
$scope.visible_gps = true;
|
$scope.visible_gps = true;
|
||||||
$scope.visible_ahrs = true;
|
|
||||||
|
|
||||||
// Simple GET request example (note: responce is asynchronous)
|
// Simple GET request example (note: responce is asynchronous)
|
||||||
$http.get(URL_SETTINGS_GET).
|
$http.get(URL_SETTINGS_GET).
|
||||||
|
|
|
@ -28,6 +28,12 @@
|
||||||
<ui-switch ng-model='GPS_Enabled' settings-change></ui-switch>
|
<ui-switch ng-model='GPS_Enabled' settings-change></ui-switch>
|
||||||
</div>
|
</div>
|
||||||
</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>
|
</div>
|
||||||
</div>
|
</div>
|
||||||
|
|
|
@ -57,7 +57,7 @@
|
||||||
<div>{{product_rows}}</div>
|
<div>{{product_rows}}</div>
|
||||||
</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"> </span>
|
<span class="col-xs-1"> </span>
|
||||||
</div>
|
</div>
|
||||||
<div class="row" ng-class="{'section_invisible': !visible_uat}">
|
<div class="row" ng-class="{'section_invisible': !visible_uat}">
|
||||||
|
|
Ładowanie…
Reference in New Issue