diff --git a/main/ry835ai.go b/main/ry83Xai.go similarity index 98% rename from main/ry835ai.go rename to main/ry83Xai.go index 4cd01edc..faa2e6ac 100644 --- a/main/ry835ai.go +++ b/main/ry83Xai.go @@ -86,10 +86,14 @@ type SituationData struct { Pressure_alt float64 LastTempPressTime time.Time - // From MPU6050 accel/gyro. + // From MPU6050 or MPU9250 accel/gyro. Pitch float64 Roll float64 Gyro_heading float64 + Mag_heading float64 + SlipSkid float64 + RateOfTurn float64 + GLoad float64 LastAttitudeTime time.Time } @@ -428,10 +432,11 @@ func validateNMEAChecksum(s string) (string, bool) { // changes while on the ground and "movement" is really only changes in GPS fix as it settles down. //TODO: Some more robust checking above current and last speed. //TODO: Dynamic adjust for gain based on groundspeed +//westphae: Do I need to do anything here? func setTrueCourse(groundSpeed uint16, trueCourse float64) { - if myMPU6050 != nil && globalStatus.RY835AI_connected && globalSettings.AHRS_Enabled { + if myMPU != nil && globalStatus.RY835AI_connected && globalSettings.AHRS_Enabled { if mySituation.GroundSpeed >= 7 && groundSpeed >= 7 { - myMPU6050.ResetHeading(trueCourse, 0.10) + myMPU.ResetHeading(trueCourse, 0.10) } } } @@ -1338,7 +1343,7 @@ func gpsSerialReader() { var i2cbus embd.I2CBus var myBMP180 *bmp180.BMP180 -var myMPU6050 *mpu6050.MPU6050 +var myMPU mpu.MPU func readBMP180() (float64, float64, error) { // ÂșCelsius, Meters temp, err := myBMP180.Temperature() @@ -1358,8 +1363,9 @@ func initBMP180() error { return nil } -func initMPU6050() error { - myMPU6050 = mpu6050.New() //TODO: error checking. +//TODO westphae: set up myMPU as MPU6050 or MPU9250, depending on which exists +func initMPU() error { + myMPU = mpu.NewMPU6050() //TODO: error checking. return nil } @@ -1404,9 +1410,9 @@ func makeAHRSGDL90Report() { pitch := int16(float64(mySituation.Pitch) * float64(10.0)) roll := int16(float64(mySituation.Roll) * float64(10.0)) hdg := uint16(float64(mySituation.Gyro_heading) * float64(10.0)) - slip_skid := int16(float64(0) * float64(10.0)) - yaw_rate := int16(float64(0) * float64(10.0)) - g := int16(float64(1.0) * float64(10.0)) + slip_skid := int16(float64(mySituation.SlipSkid) * float64(10.0)) + turn_rate := int16(float64(mySituation.RateOfTurn) * float64(10.0)) + g := int16(float64(mySituation.GLoad) * float64(10.0)) // Roll. msg[4] = byte((roll >> 8) & 0xFF) @@ -1425,8 +1431,8 @@ func makeAHRSGDL90Report() { msg[11] = byte(slip_skid & 0xFF) // Yaw rate. - msg[12] = byte((yaw_rate >> 8) & 0xFF) - msg[13] = byte(yaw_rate & 0xFF) + msg[12] = byte((turn_rate >> 8) & 0xFF) + msg[13] = byte(turn_rate & 0xFF) // "G". msg[14] = byte((g >> 8) & 0xFF) @@ -1440,14 +1446,14 @@ func attitudeReaderSender() { for globalStatus.RY835AI_connected && globalSettings.AHRS_Enabled { <-timer.C // Read pitch and roll. - pitch, err_pitch := myMPU6050.Pitch() + pitch, err_pitch := myMPU.Pitch() if err_pitch != nil { log.Printf("readMPU6050(): %s\n", err_pitch.Error()) globalStatus.RY835AI_connected = false break } - roll, err_roll := myMPU6050.Roll() + roll, err_roll := myMPU.Roll() if err_roll != nil { log.Printf("readMPU6050(): %s\n", err_roll.Error()) @@ -1455,7 +1461,7 @@ func attitudeReaderSender() { break } - heading, err_heading := myMPU6050.Heading() //FIXME. Experimental. + heading, err_heading := myMPU.Heading() //FIXME. Experimental. if err_heading != nil { log.Printf("readMPU6050(): %s\n", err_heading.Error()) globalStatus.RY835AI_connected = false @@ -1559,7 +1565,8 @@ func initAHRS() error { i2cbus.Close() return err } - if err := initMPU6050(); err != nil { // I2C accel/gyro. + // TODO westphae: Initialize MPU9250 here + if err := initMPU(); err != nil { // I2C accel/gyro. i2cbus.Close() myBMP180.Close() return err diff --git a/mpu/mpu.go b/mpu/mpu.go new file mode 100644 index 00000000..a9135d80 --- /dev/null +++ b/mpu/mpu.go @@ -0,0 +1,13 @@ +package mpu + +type MPU interface { + Close() + ResetHeading(float64, float64) + Pitch() (float64, error) + Roll() (float64, error) + Heading() (float64, error) + MagHeading() (float64, error) + SlipSkid() (float64, error) + RateOfTurn() (float64, error) + GLoad() (float64, error) +} \ No newline at end of file diff --git a/mpu6050/mpu6050.go b/mpu/mpu6050.go similarity index 94% rename from mpu6050/mpu6050.go rename to mpu/mpu6050.go index 9d3cd5cc..c7d6dc44 100644 --- a/mpu6050/mpu6050.go +++ b/mpu/mpu6050.go @@ -1,6 +1,6 @@ // Package mpu6050 allows interfacing with InvenSense mpu6050 barometric pressure sensor. This sensor // has the ability to provided compensated temperature and pressure readings. -package mpu6050 +package mpu import ( "../linux-mpu9150/mpu" @@ -40,7 +40,7 @@ type MPU6050 struct { } // New returns a handle to a MPU6050 sensor. -func New() *MPU6050 { +func NewMPU6050() *MPU6050 { n := &MPU6050{poll: pollDelay} n.startUp() return n @@ -185,3 +185,8 @@ func (d *MPU6050) Close() { d.quit <- struct{}{} } } + +func (d *MPU6050) MagHeading() (float64, error) {return 0, nil} +func (d *MPU6050) SlipSkid() (float64, error) {return 0, nil} +func (d *MPU6050) RateOfTurn() (float64, error) {return 0, nil} +func (d *MPU6050) GLoad() (float64, error) {return 0, nil} diff --git a/test/sensortest.go b/test/sensortest.go index 5ad168dd..0574b2e4 100644 --- a/test/sensortest.go +++ b/test/sensortest.go @@ -7,7 +7,7 @@ import ( "time" ) -var attSensor *mpu6050.MPU6050 +var attSensor mpu.MPU func readMPU6050() (float64, float64) { pitch, _ := attSensor.Pitch() @@ -17,7 +17,7 @@ func readMPU6050() (float64, float64) { } func initMPU6050() { - attSensor = mpu6050.New() + attSensor = mpu.NewMPU6050() } func main() {