kopia lustrzana https://github.com/cyoung/stratux
Remove mpu6050 references.
Christopher Young
rodzic
a47b1483c7
commit
bc830b0a4e
8
Makefile
8
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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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{}{}
|
||||
}
|
||||
}
|
||||
|
|
@ -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).
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
}
|
||||
}
|
||||
Ładowanie…
Reference in New Issue