kopia lustrzana https://github.com/cyoung/stratux
Separate gyro calibration and AHRS level functions.
Eric Westphal
rodzic
90c10195fb
commit
20877df847
2
goflying
2
goflying
|
|
@ -1 +1 @@
|
|||
Subproject commit 4bbb642cd04e487e65da14a159428eb943ce9290
|
||||
Subproject commit 3c8befa0067ddc9ce39b9b2c041fd6450583420a
|
||||
|
|
@ -1059,6 +1059,7 @@ type settings struct {
|
|||
AHRSLog bool
|
||||
IMUMapping [2]int // Map from aircraft axis to sensor axis: accelerometer
|
||||
SensorQuaternion [4]float64 // Quaternion mapping from sensor frame to aircraft frame
|
||||
C, D [3]float64 // IMU Accel, Gyro zero bias
|
||||
PPM int
|
||||
OwnshipModeS string
|
||||
WatchList string
|
||||
|
|
|
|||
|
|
@ -487,6 +487,21 @@ func handleCageAHRS(w http.ResponseWriter, r *http.Request) {
|
|||
}
|
||||
}
|
||||
|
||||
func handleCalibrateAHRS(w http.ResponseWriter, r *http.Request) {
|
||||
// define header in support of cross-domain AJAX
|
||||
setNoCache(w)
|
||||
w.Header().Set("Content-Type", "text/plain")
|
||||
w.Header().Set("Access-Control-Allow-Origin", "*")
|
||||
w.Header().Set("Access-Control-Allow-Method", "GET, POST, OPTIONS")
|
||||
w.Header().Set("Access-Control-Allow-Headers", "Origin, X-Requested-With, Content-Type, Accept")
|
||||
|
||||
// For an OPTION method request, we return header without processing.
|
||||
// This ensures we are recognized as supporting cross-domain AJAX REST calls.
|
||||
if r.Method == "POST" {
|
||||
CalibrateAHRS()
|
||||
}
|
||||
}
|
||||
|
||||
func handleResetGMeter(w http.ResponseWriter, r *http.Request) {
|
||||
// define header in support of cross-domain AJAX
|
||||
setNoCache(w)
|
||||
|
|
@ -799,6 +814,7 @@ func managementInterface() {
|
|||
http.HandleFunc("/roPartitionRebuild", handleroPartitionRebuild)
|
||||
http.HandleFunc("/develmodetoggle", handleDevelModeToggle)
|
||||
http.HandleFunc("/orientAHRS", handleOrientAHRS)
|
||||
http.HandleFunc("/calibrateAHRS", handleCalibrateAHRS)
|
||||
http.HandleFunc("/cageAHRS", handleCageAHRS)
|
||||
http.HandleFunc("/resetGMeter", handleResetGMeter)
|
||||
http.HandleFunc("/deletelogfile", handleDeleteLogFile)
|
||||
|
|
|
|||
|
|
@ -5,6 +5,7 @@ import (
|
|||
"log"
|
||||
"math"
|
||||
"path/filepath"
|
||||
"strings"
|
||||
"time"
|
||||
|
||||
"../goflying/ahrs"
|
||||
|
|
@ -24,9 +25,9 @@ var (
|
|||
i2cbus embd.I2CBus
|
||||
myPressureReader sensors.PressureReader
|
||||
myIMUReader sensors.IMUReader
|
||||
cal chan (bool)
|
||||
cal chan (string)
|
||||
analysisLogger *ahrs.AHRSLogger
|
||||
ahrsCalibrating, needsCage bool // Does the sensor orientation matrix need to be recalculated?
|
||||
ahrsCalibrating bool
|
||||
logMap map[string]interface{}
|
||||
)
|
||||
|
||||
|
|
@ -149,7 +150,7 @@ func sensorAttitudeSender() {
|
|||
log.Println("AHRS Info: initializing new Simple AHRS")
|
||||
s := ahrs.NewSimpleAHRS()
|
||||
m := ahrs.NewMeasurement()
|
||||
cal = make(chan (bool), 1)
|
||||
cal = make(chan (string), 1)
|
||||
|
||||
// Set up loggers for analysis
|
||||
ahrswebListener, err := ahrsweb.NewKalmanListener()
|
||||
|
|
@ -163,28 +164,36 @@ func sensorAttitudeSender() {
|
|||
// Need a sampling freq faster than 10Hz
|
||||
timer := time.NewTicker(50 * time.Millisecond) // ~20Hz update.
|
||||
for {
|
||||
// Do an initial calibration
|
||||
select { // Don't block if cal isn't receiving: only need one calibration in the queue at a time.
|
||||
case cal <- true:
|
||||
default:
|
||||
// Set sensor gyro calibrations
|
||||
if c, d := &globalSettings.C, &globalSettings.D; d[0]*d[0] + d[1]*d[1] + d[2]*d[2] > 0 {
|
||||
s.SetCalibrations(c, d)
|
||||
log.Printf("AHRS Info: IMU Calibrations read from settings: accel %6f %6f %6f; gyro %6f %6f %6f\n",
|
||||
c[0], c[1], c[2], d[0], d[1], d[2])
|
||||
} else {
|
||||
// Do an initial calibration
|
||||
select { // Don't block if cal isn't receiving: only need one calibration in the queue at a time.
|
||||
case cal <- "cal":
|
||||
default:
|
||||
}
|
||||
}
|
||||
|
||||
// Set sensor rotation matrix / quaternion
|
||||
// Set sensor quaternion
|
||||
if f := &globalSettings.SensorQuaternion; f[0]*f[0] + f[1]*f[1] + f[2]*f[2] + f[3]*f[3] > 0 {
|
||||
s.SetSensorQuaternion(f)
|
||||
needsCage = false
|
||||
} else {
|
||||
needsCage = true
|
||||
cal <- "level"
|
||||
}
|
||||
|
||||
failNum = 0
|
||||
<-timer.C
|
||||
time.Sleep(950 * time.Millisecond)
|
||||
for globalSettings.IMU_Sensor_Enabled && globalStatus.IMUConnected {
|
||||
<-timer.C
|
||||
|
||||
// Calibrate the sensors if requested.
|
||||
// Process calibration and level requests
|
||||
select {
|
||||
case <-cal:
|
||||
case action := <-cal:
|
||||
log.Printf("AHRS Info: cal received action %s\n", action)
|
||||
ahrsCalibrating = true
|
||||
myIMUReader.Read() // Clear out the averages
|
||||
var (
|
||||
|
|
@ -201,18 +210,21 @@ func sensorAttitudeSender() {
|
|||
if mpuError != nil {
|
||||
log.Printf("AHRS Info: Error reading IMU while calibrating: %s\n", mpuError)
|
||||
} else {
|
||||
s.SetCalibrations(&[3]float64{c1, c2, c3}, &[3]float64{d1, d2, d3})
|
||||
log.Printf("AHRS Info: IMU Calibration values: accel %6f %6f %6f; gyro %6f %6f %6f\n",
|
||||
c1, c2, c3, d1, d2, d3)
|
||||
}
|
||||
// Process caging if necessary.
|
||||
if needsCage {
|
||||
globalSettings.SensorQuaternion = *makeOrientationQuaternion([3]float64{c1, c2, c3})
|
||||
if strings.Contains(action, "cal") { // Calibrate gyros
|
||||
globalSettings.D = [3]float64{d1, d2, d3}
|
||||
s.SetCalibrations(nil, &globalSettings.D)
|
||||
log.Printf("AHRS Info: IMU gyro calibration: %3f %3f %3f\n", d1, d2, d3)
|
||||
}
|
||||
if strings.Contains(action, "level") { // Calibrate accel / level
|
||||
globalSettings.C = [3]float64{c1, c2, c3}
|
||||
s.SetCalibrations(&globalSettings.C, nil)
|
||||
globalSettings.SensorQuaternion = *makeOrientationQuaternion(globalSettings.C)
|
||||
s.SetSensorQuaternion(&globalSettings.SensorQuaternion)
|
||||
s.Reset()
|
||||
log.Printf("AHRS Info: IMU accel calibration: %3f %3f %3f\n", c1, c2, c3)
|
||||
log.Printf("AHRS Info: Caged to quaternion %v\n", globalSettings.SensorQuaternion)
|
||||
}
|
||||
saveSettings()
|
||||
s.SetSensorQuaternion(&globalSettings.SensorQuaternion)
|
||||
s.Reset()
|
||||
needsCage = false
|
||||
log.Println("AHRS Info: Caged")
|
||||
}
|
||||
}
|
||||
ahrsCalibrating = false
|
||||
|
|
@ -395,8 +407,12 @@ func getMinAccelDirection() (i int, err error) {
|
|||
|
||||
// CageAHRS sends a signal to the AHRSProvider that it should recalibrate and reset its level orientation.
|
||||
func CageAHRS() {
|
||||
needsCage = true
|
||||
cal <- true
|
||||
cal <- "level"
|
||||
}
|
||||
|
||||
// CageAHRS sends a signal to the AHRSProvider that it should recalibrate and reset its level orientation.
|
||||
func CalibrateAHRS() {
|
||||
cal <- "cal"
|
||||
}
|
||||
|
||||
// ResetAHRSGLoad resets the min and max to the current G load value.
|
||||
|
|
|
|||
|
|
@ -16,6 +16,7 @@ var URL_SHUTDOWN = "http://" + URL_HOST_BASE + "/shutdown";
|
|||
var URL_RESTARTAPP = "http://" + URL_HOST_BASE + "/restart";
|
||||
var URL_DEV_TOGGLE_GET = "http://" + URL_HOST_BASE + "/develmodetoggle";
|
||||
var URL_AHRS_ORIENT = "http://" + URL_HOST_BASE + "/orientAHRS";
|
||||
var URL_AHRS_CAL = "http://" + URL_HOST_BASE + "/calibrateAHRS";
|
||||
var URL_AHRS_CAGE = "http://" + URL_HOST_BASE + "/cageAHRS";
|
||||
var URL_GMETER_RESET = "http://" + URL_HOST_BASE + "/resetGMeter";
|
||||
var URL_DELETELOGFILE = "http://" + URL_HOST_BASE + "/deletelogfile";
|
||||
|
|
|
|||
|
|
@ -1,4 +1,4 @@
|
|||
angular.module('appControllers').controller('GPSCtrl', GPSCtrl); // get the main module contollers set
|
||||
angular.module('appControllers').controller('GPSCtrl', GPSCtrl); // get the main module controllers set
|
||||
GPSCtrl.$inject = ['$rootScope', '$scope', '$state', '$http', '$interval']; // Inject my dependencies
|
||||
|
||||
// create our controller function with all necessary logic
|
||||
|
|
|
|||
|
|
@ -240,4 +240,17 @@ function SettingsCtrl($rootScope, $scope, $state, $location, $window, $http) {
|
|||
$scope.Ui.turnOn("modalCalibrateFailed");
|
||||
});
|
||||
};
|
||||
|
||||
$scope.calibrateGyros = function() {
|
||||
console.log("sending calibrate message.");
|
||||
$http.post(URL_AHRS_CAL).
|
||||
then(function(response) {
|
||||
console.log("Sent calibrate message.");
|
||||
}, function(response) {
|
||||
console.log(response.data);
|
||||
$scope.Calibration_Failure_Message = response.data;
|
||||
$scope.Ui.turnOff("modalCalibrateGyros");
|
||||
$scope.Ui.turnOn("modalCalibrateGyrosFailed");
|
||||
});
|
||||
};
|
||||
}
|
||||
|
|
|
|||
|
|
@ -81,17 +81,23 @@
|
|||
<div class="panel-body">
|
||||
<div class="col-xs-12">
|
||||
<span style="position:relative; overflow: hidden;">
|
||||
<button class="btn btn-primary btn-block" ui-turn-on="modalCalibrateForward">
|
||||
Set AHRS Sensor Orientation
|
||||
</button>
|
||||
<button class="btn btn-primary btn-block" ui-turn-on="modalCalibrateForward"
|
||||
ng-disabled="!IMU_Sensor_Enabled">Set AHRS Sensor Orientation</button>
|
||||
</span>
|
||||
<div class="form-group reset-flow">
|
||||
<label class="control-label col-xs-5">G Limits</label>
|
||||
<form name="GLimitForm" ng-submit="updateGLimits()" novalidate>
|
||||
<input class="col-xs-7" type="string" required ng-model="GLimits" placeholder="Space-separated negative and positive G meter limits" ng-blur="updateGLimits()" />
|
||||
</form>
|
||||
</div>
|
||||
<div class="form-group reset-flow">
|
||||
<div class="col-xs-12">
|
||||
<button class="btn btn-primary btn-block" ui-turn-on="modalCalibrateGyros"
|
||||
ng-disabled="!IMU_Sensor_Enabled">Calibrate Gyros</button>
|
||||
</div>
|
||||
</div>
|
||||
<div class="form-group reset-flow">
|
||||
<label class="control-label col-xs-5">G Limits</label>
|
||||
<form name="GLimitForm" ng-submit="updateGLimits()" novalidate ng-disabled="!IMU_Sensor_Enabled">
|
||||
<input class="col-xs-7" type="string" required ng-model="GLimits" ng-blur="updateGLimits()"
|
||||
placeholder="Space-separated negative and positive G meter limits"/>
|
||||
</form>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
|
@ -307,7 +313,7 @@
|
|||
<div class="modal-body">
|
||||
<p>There was an error: {{Orientation_Failure_Message}}</p>
|
||||
<p><div id="orientationFailureMessage"></div></p>
|
||||
<p>The calibration failed. Please try again.</p>
|
||||
<p>The orientation failed. Please try again.</p>
|
||||
</div>
|
||||
<div class="modal-footer">
|
||||
<a ui-turn-off="modalCalibrateFailed" class="btn btn-default btn-primary">OK</a>
|
||||
|
|
@ -316,4 +322,27 @@
|
|||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div class="modal" ui-if="modalCalibrateGyros" ui-state="modalCalibrateGyros">
|
||||
<div class="modal-overlay "></div>
|
||||
<div class="vertical-alignment-helper center-block">
|
||||
<div class="modal-dialog vertical-align-center">
|
||||
<div class="modal-content">
|
||||
<div class="modal-header">
|
||||
<button class="close" ui-turn-off="modalCalibrateGyros"></button>
|
||||
<h4 class="modal-title">Calibrate MPU Gyros</h4>
|
||||
</div>
|
||||
<div class="modal-body">
|
||||
<p>Press <b>Calibrate</b> and keep the Stratux as stationary as possible for the next second.
|
||||
You should only do this in calm air as turbulence will throw off the calibrations.</p>
|
||||
</div>
|
||||
<div class="modal-footer">
|
||||
<a ui-turn-off="modalCalibrateGyros" class="btn btn-default">Cancel</a>
|
||||
<a ng-click="calibrateGyros()" ui-turn-off="modalCalibrateGyros"
|
||||
class="btn btn-default btn-primary">Calibrate</a>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue