Team_temp / VT2_domc_copy

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed-dev

Fork of Adafruit9-DOf_AHRS_Regler_Discrete by RT-Labor IMS

Source/main.cpp@3:cc828af4a4c6, 2018-09-20 (annotated)

Committer:
rtlabor
Date:
Thu Sep 20 07:43:09 2018 +0000
Revision:
3:cc828af4a4c6
Parent:
1:8c4f93e10af3 
Madgwick Filter mit Quaternionen Ausgabe, Achtung, teilweise muss neu verbunden werden

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bmanga95 0:772bf4786416 1 #include "mbed.h"
bmanga95 0:772bf4786416 2 #define _MBED_
bmanga95 0:772bf4786416 3 #include "Adafruit_9DOF.h"
bmanga95 0:772bf4786416 4 #include "Serial_base.h"
domanpie 1:8c4f93e10af3 5 #include "MadgwickAHRS.h"
bmanga95 0:772bf4786416 6
domanpie 1:8c4f93e10af3 7 /**
domanpie 1:8c4f93e10af3 8 * Defines
domanpie 1:8c4f93e10af3 9 */
domanpie 1:8c4f93e10af3 10 #define PI 3.1415926536
domanpie 1:8c4f93e10af3 11
bmanga95 0:772bf4786416 12
bmanga95 0:772bf4786416 13 Adafruit_9DOF dof = Adafruit_9DOF();
bmanga95 0:772bf4786416 14
bmanga95 0:772bf4786416 15 Adafruit_LSM303_Accel_Unified accel = Adafruit_LSM303_Accel_Unified(30301);
bmanga95 0:772bf4786416 16
bmanga95 0:772bf4786416 17 Adafruit_LSM303_Mag_Unified mag = Adafruit_LSM303_Mag_Unified(30302);
bmanga95 0:772bf4786416 18
domanpie 1:8c4f93e10af3 19 Adafruit_L3GD20_Unified gyro = Adafruit_L3GD20_Unified(30303);
bmanga95 0:772bf4786416 20
domanpie 1:8c4f93e10af3 21 //Debug/Communication with PC through
rtlabor 3:cc828af4a4c6 22 Serial pc(SERIAL_TX, SERIAL_RX,115200); // serial connection via USB - programmer
domanpie 1:8c4f93e10af3 23
domanpie 1:8c4f93e10af3 24 // Magnetometer correction
domanpie 1:8c4f93e10af3 25 // mounted in gimbal
domanpie 1:8c4f93e10af3 26
domanpie 1:8c4f93e10af3 27 //Cal for Nickachse
domanpie 1:8c4f93e10af3 28 //double RotMag [3][3] = {{0.947568, -0.0262851, 0.0355492},
domanpie 1:8c4f93e10af3 29 // {-0.0262851, 0.98413, 0.014105},
domanpie 1:8c4f93e10af3 30 // {0.0355492, 0.014105, 0.970769}};
domanpie 1:8c4f93e10af3 31
domanpie 1:8c4f93e10af3 32 //double offsetsMag [3] = { -22.1205, -13.9738, -2.55159 };
domanpie 1:8c4f93e10af3 33
domanpie 1:8c4f93e10af3 34 double offsetsMag[3] = {23.5297, -7.51693, 17.5943};
domanpie 1:8c4f93e10af3 35 double RotMag[3][3] = {{0.678312, -0.0371453, 0.0139236}, {-0.0371453, 0.993299, 0.0229827}, {0.0139236, 0.0229827, 0.809984}};
domanpie 1:8c4f93e10af3 36
domanpie 1:8c4f93e10af3 37 //double gainsMag [3] = { 39.1659, 38.1534, 35.7245 };
domanpie 1:8c4f93e10af3 38 //double refMag = 47.9824; //nT
domanpie 1:8c4f93e10af3 39
domanpie 1:8c4f93e10af3 40 //Accelerometer correction
domanpie 1:8c4f93e10af3 41
domanpie 1:8c4f93e10af3 42 double RotAccel [3][3] = { {-0.1251, -0.7810, -0.6118},
domanpie 1:8c4f93e10af3 43 { 0.0409, 0.6121, -0.7879},
domanpie 1:8c4f93e10af3 44 { 0.9913, -0.1238, -0.0466} };
domanpie 1:8c4f93e10af3 45
domanpie 1:8c4f93e10af3 46
domanpie 1:8c4f93e10af3 47 double offsetsAccel [3] = { -0.215747, 0.509946, -0.235360 };
domanpie 1:8c4f93e10af3 48 double gainsAccel [3] = { 0.978548, 0.976637, 0.939921 };
domanpie 1:8c4f93e10af3 49
domanpie 1:8c4f93e10af3 50
domanpie 1:8c4f93e10af3 51 //Gyrometer correction
domanpie 1:8c4f93e10af3 52 double offsetsGyro [3] = { -0.0107709, 0.00729576, 0.0225833 };
bmanga95 0:772bf4786416 53
domanpie 1:8c4f93e10af3 54 //Filter for orientation estimation
domanpie 1:8c4f93e10af3 55 Madgwick filter;
domanpie 1:8c4f93e10af3 56
domanpie 1:8c4f93e10af3 57 DigitalIn CPUEnable(D11);
domanpie 1:8c4f93e10af3 58 DigitalOut motorEnable(D12, 0);
domanpie 1:8c4f93e10af3 59 PwmOut SollStrom(PB_3); //in [A]
domanpie 1:8c4f93e10af3 60
domanpie 1:8c4f93e10af3 61 //Timer for controller interrupt
domanpie 1:8c4f93e10af3 62 // the controll loop will be atached to the timer interrupt
domanpie 1:8c4f93e10af3 63 Ticker ControllerLoopTimer;
rtlabor 3:cc828af4a4c6 64 void updateAHRS(void);
domanpie 1:8c4f93e10af3 65
domanpie 1:8c4f93e10af3 66 //Sensor events, contains the last read sensor values
domanpie 1:8c4f93e10af3 67 sensors_event_t mag_event;
domanpie 1:8c4f93e10af3 68 sensors_event_t accel_event;
domanpie 1:8c4f93e10af3 69 sensors_event_t gyro_event;
bmanga95 0:772bf4786416 70
domanpie 1:8c4f93e10af3 71 //Sensor values after the calibration/correction is applied, (x,y,z)
domanpie 1:8c4f93e10af3 72 double CorrectedMagnetometer[3];
domanpie 1:8c4f93e10af3 73 double CorrectedAccelerometer[3];
domanpie 1:8c4f93e10af3 74 double CorrectedGyrometer[3];
domanpie 1:8c4f93e10af3 75
domanpie 1:8c4f93e10af3 76 //"User Interface", Start/Stop controller
domanpie 1:8c4f93e10af3 77 DigitalIn SystemEnable(USER_BUTTON);
domanpie 1:8c4f93e10af3 78 //Status LED, on when controller on
domanpie 1:8c4f93e10af3 79 DigitalOut SystemEnableLED(LED3, 0);
domanpie 1:8c4f93e10af3 80
domanpie 1:8c4f93e10af3 81 //variables for loop time estimation
domanpie 1:8c4f93e10af3 82 uint32_t LoopCounter = 0;
domanpie 1:8c4f93e10af3 83 Timer t;
domanpie 1:8c4f93e10af3 84 float TotalTime;
domanpie 1:8c4f93e10af3 85
domanpie 1:8c4f93e10af3 86 //maximaler Fehler
domanpie 1:8c4f93e10af3 87 float eMaxPos = 0;
domanpie 1:8c4f93e10af3 88 float eMaxNeg = 0;
bmanga95 0:772bf4786416 89
bmanga95 0:772bf4786416 90 /**************************************************************************
bmanga95 0:772bf4786416 91 /
bmanga95 0:772bf4786416 92 /
bmanga95 0:772bf4786416 93 @brief Initialises all the sensors used by this example
bmanga95 0:772bf4786416 94
bmanga95 0:772bf4786416 95 /
bmanga95 0:772bf4786416 96 **************************************************************************/
bmanga95 0:772bf4786416 97
bmanga95 0:772bf4786416 98 void initSensors()
bmanga95 0:772bf4786416 99 {
rtlabor 3:cc828af4a4c6 100 pc.printf(("Init...\r\n"));
bmanga95 0:772bf4786416 101 if(!accel.begin())
bmanga95 0:772bf4786416 102 {
bmanga95 0:772bf4786416 103
bmanga95 0:772bf4786416 104 /* There was a problem detecting the LSM303 ... check your connections */
bmanga95 0:772bf4786416 105
rtlabor 3:cc828af4a4c6 106 pc.printf(("Ooops, no LSM303 detected ... Check your wiring!\r\n"));
bmanga95 0:772bf4786416 107
bmanga95 0:772bf4786416 108 while(1);
bmanga95 0:772bf4786416 109
bmanga95 0:772bf4786416 110 }
bmanga95 0:772bf4786416 111 if(!mag.begin())
bmanga95 0:772bf4786416 112 {
bmanga95 0:772bf4786416 113
bmanga95 0:772bf4786416 114 /* There was a problem detecting the LSM303 ... check your connections */
bmanga95 0:772bf4786416 115
rtlabor 3:cc828af4a4c6 116 pc.printf("Ooops, no LSM303 detected ... Check your wiring!\r\n");
domanpie 1:8c4f93e10af3 117
domanpie 1:8c4f93e10af3 118 while(1);
domanpie 1:8c4f93e10af3 119
domanpie 1:8c4f93e10af3 120 }
domanpie 1:8c4f93e10af3 121
domanpie 1:8c4f93e10af3 122 gyroRange_t rng = GYRO_RANGE_2000DPS; //set gyro range to +/-2000dps
domanpie 1:8c4f93e10af3 123
domanpie 1:8c4f93e10af3 124 if(!gyro.begin(rng))
domanpie 1:8c4f93e10af3 125 {
domanpie 1:8c4f93e10af3 126
domanpie 1:8c4f93e10af3 127 /* There was a problem detecting the LSM303 ... check your connections */
domanpie 1:8c4f93e10af3 128
rtlabor 3:cc828af4a4c6 129 pc.printf("Ooops, no L3GD20 detected ... Check your wiring!\r\n");
bmanga95 0:772bf4786416 130
bmanga95 0:772bf4786416 131 while(1);
bmanga95 0:772bf4786416 132
bmanga95 0:772bf4786416 133 }
bmanga95 0:772bf4786416 134
bmanga95 0:772bf4786416 135 }
domanpie 1:8c4f93e10af3 136 /**************************************************************************
bmanga95 0:772bf4786416 137
domanpie 1:8c4f93e10af3 138 @brief system initialisation
bmanga95 0:772bf4786416 139
bmanga95 0:772bf4786416 140
domanpie 1:8c4f93e10af3 141 **************************************************************************/
bmanga95 0:772bf4786416 142
domanpie 1:8c4f93e10af3 143 void setup(void)
domanpie 1:8c4f93e10af3 144 {
domanpie 1:8c4f93e10af3 145 pc.printf(("Adafruit 9 DOF Pitch/Roll/Heading Example \n"));
domanpie 1:8c4f93e10af3 146 pc.printf("\n");
bmanga95 0:772bf4786416 147
bmanga95 0:772bf4786416 148 /* Initialise the sensors */
bmanga95 0:772bf4786416 149 initSensors();
bmanga95 0:772bf4786416 150
bmanga95 0:772bf4786416 151 }
bmanga95 0:772bf4786416 152
bmanga95 0:772bf4786416 153
domanpie 1:8c4f93e10af3 154 int main()
domanpie 1:8c4f93e10af3 155 {
domanpie 1:8c4f93e10af3 156 //init system
domanpie 1:8c4f93e10af3 157 setup();
rtlabor 3:cc828af4a4c6 158
domanpie 1:8c4f93e10af3 159 //set PWM frequncy to 1 kHZ
rtlabor 3:cc828af4a4c6 160 wait_ms(500);
rtlabor 3:cc828af4a4c6 161 pc.printf("Started! \r\n");
rtlabor 3:cc828af4a4c6 162 //ControllerLoopTimer.attach(&updateControllers, 1.0f); //Assume Ts = 1/100;
domanpie 1:8c4f93e10af3 163 //controller on -> LED on
domanpie 1:8c4f93e10af3 164 t.start();
domanpie 1:8c4f93e10af3 165 //do as long user button d'ont get pressed
rtlabor 3:cc828af4a4c6 166 while ( 1 )// schafft etwa 400Hz
domanpie 1:8c4f93e10af3 167 {
rtlabor 3:cc828af4a4c6 168 LoopCounter++;
rtlabor 3:cc828af4a4c6 169 updateAHRS();
rtlabor 3:cc828af4a4c6 170 if(LoopCounter == 100)
rtlabor 3:cc828af4a4c6 171 {
rtlabor 3:cc828af4a4c6 172 LoopCounter = 0;
rtlabor 3:cc828af4a4c6 173 //pc.printf("R %3.2f P %3.2f Y %3.2f \r\n",filter.getRoll(),filter.getPitch(),filter.getYaw());
rtlabor 3:cc828af4a4c6 174 //pc.printf("R %2.4f P %2.4f Y %2.4f \r\n",gyro_event.gyro.x,gyro_event.gyro.y,gyro_event.gyro.z);
rtlabor 3:cc828af4a4c6 175 filter.getRoll();
rtlabor 3:cc828af4a4c6 176 pc.printf("A %f B %f C %f D %f\r\n",filter.Q[0],filter.Q[1],filter.Q[2],filter.Q[3]);
rtlabor 3:cc828af4a4c6 177 }
rtlabor 3:cc828af4a4c6 178 //wait_ms(100);
rtlabor 3:cc828af4a4c6 179
rtlabor 3:cc828af4a4c6 180 }
rtlabor 3:cc828af4a4c6 181 }
rtlabor 3:cc828af4a4c6 182
rtlabor 3:cc828af4a4c6 183
rtlabor 3:cc828af4a4c6 184 void updateAHRS(void)
rtlabor 3:cc828af4a4c6 185 {
rtlabor 3:cc828af4a4c6 186
domanpie 1:8c4f93e10af3 187 mag.getEvent(&mag_event);
domanpie 1:8c4f93e10af3 188 accel.getEvent(&accel_event);
domanpie 1:8c4f93e10af3 189 gyro.getEvent(&gyro_event);
domanpie 1:8c4f93e10af3 190
domanpie 1:8c4f93e10af3 191 //Magnetometer correction
domanpie 1:8c4f93e10af3 192 mag_event.magnetic.x = ((double)mag_event.magnetic.x) - offsetsMag[0];
domanpie 1:8c4f93e10af3 193 mag_event.magnetic.y = ((double)mag_event.magnetic.y) - offsetsMag[1];
domanpie 1:8c4f93e10af3 194 mag_event.magnetic.z = ((double)mag_event.magnetic.z) - offsetsMag[2];
domanpie 1:8c4f93e10af3 195
domanpie 1:8c4f93e10af3 196 CorrectedMagnetometer[0] = ((double)mag_event.magnetic.x)*RotMag[0][0] + ((double)mag_event.magnetic.y)*RotMag[0][1] + ((double)mag_event.magnetic.z)*RotMag[0][2];
domanpie 1:8c4f93e10af3 197 CorrectedMagnetometer[1] = ((double)mag_event.magnetic.x)*RotMag[1][0] + ((double)mag_event.magnetic.y)*RotMag[1][1] + ((double)mag_event.magnetic.z)*RotMag[1][2];
domanpie 1:8c4f93e10af3 198 CorrectedMagnetometer[2] = ((double)mag_event.magnetic.x)*RotMag[2][0] + ((double)mag_event.magnetic.y)*RotMag[2][1] + ((double)mag_event.magnetic.z)*RotMag[2][2];
domanpie 1:8c4f93e10af3 199
domanpie 1:8c4f93e10af3 200
domanpie 1:8c4f93e10af3 201 //Accelerometer correction
domanpie 1:8c4f93e10af3 202 CorrectedAccelerometer[0] = ((double)accel_event.acceleration.x) - offsetsAccel[0];
domanpie 1:8c4f93e10af3 203 CorrectedAccelerometer[1] = ((double)accel_event.acceleration.y) - offsetsAccel[1];
domanpie 1:8c4f93e10af3 204 CorrectedAccelerometer[2] = ((double)accel_event.acceleration.z) - offsetsAccel[2];
domanpie 1:8c4f93e10af3 205
domanpie 1:8c4f93e10af3 206 CorrectedAccelerometer[0] = CorrectedAccelerometer[0] * gainsAccel[0];
domanpie 1:8c4f93e10af3 207 CorrectedAccelerometer[1] = CorrectedAccelerometer[1] * gainsAccel[1];
domanpie 1:8c4f93e10af3 208 CorrectedAccelerometer[2] = CorrectedAccelerometer[2] * gainsAccel[2];
domanpie 1:8c4f93e10af3 209
domanpie 1:8c4f93e10af3 210
domanpie 1:8c4f93e10af3 211 //Gyrometer correction
domanpie 1:8c4f93e10af3 212 CorrectedGyrometer[0] = ((double)gyro_event.gyro.x) - offsetsGyro[0];
domanpie 1:8c4f93e10af3 213 CorrectedGyrometer[1] = ((double)gyro_event.gyro.y) - offsetsGyro[1];
domanpie 1:8c4f93e10af3 214 CorrectedGyrometer[2] = ((double)gyro_event.gyro.z) - offsetsGyro[2];
domanpie 1:8c4f93e10af3 215
domanpie 1:8c4f93e10af3 216 //Perform Madgwick-filter update
domanpie 1:8c4f93e10af3 217 filter.update( CorrectedGyrometer[0]/ PI * 180, -CorrectedGyrometer[1]/ PI * 180, -CorrectedGyrometer[2]/ PI * 180 ,
domanpie 1:8c4f93e10af3 218 -CorrectedAccelerometer[0] , CorrectedAccelerometer[1] , CorrectedAccelerometer[2] ,
rtlabor 3:cc828af4a4c6 219 CorrectedMagnetometer[0] , -CorrectedMagnetometer[1], -CorrectedMagnetometer[2]);
rtlabor 3:cc828af4a4c6 220
bmanga95 0:772bf4786416 221 }