Freescale Quadcopter with Freedom K64F Board
Dependencies: FXAS21000 FXLS8471Q FXOS8700Q MAG3110 MMA8652 MPL3115A2 mbed kalman mbed-dsp
Fork of Freescale_Multi-Sensor_Shield by
Quadcopter based on Freescale FRDM-K64F, Freescale FRDM-FXS-9AXIS, Hobbypower X525 V3 Quadcopter Foldable Kit with 1000KV Motors and SimonK 30A ESC
main.cpp@4:1bc3ca07a412, 2015-09-01 (annotated)
- Committer:
- julioefajardo
- Date:
- Tue Sep 01 18:21:17 2015 +0000
- Revision:
- 4:1bc3ca07a412
- Parent:
- 3:58ebc00f1a68
- Child:
- 5:74ca8be12359
Version 0.2; Kalman Filter and CMSIS-DSP
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
screamer | 0:bfb567985c64 | 1 | /* Copyright (c) 2010-2011 mbed.org, MIT License |
screamer | 0:bfb567985c64 | 2 | * |
screamer | 0:bfb567985c64 | 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software |
screamer | 0:bfb567985c64 | 4 | * and associated documentation files (the "Software"), to deal in the Software without |
screamer | 0:bfb567985c64 | 5 | * restriction, including without limitation the rights to use, copy, modify, merge, publish, |
screamer | 0:bfb567985c64 | 6 | * distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the |
screamer | 0:bfb567985c64 | 7 | * Software is furnished to do so, subject to the following conditions: |
screamer | 0:bfb567985c64 | 8 | * |
screamer | 0:bfb567985c64 | 9 | * The above copyright notice and this permission notice shall be included in all copies or |
screamer | 0:bfb567985c64 | 10 | * substantial portions of the Software. |
screamer | 0:bfb567985c64 | 11 | * |
screamer | 0:bfb567985c64 | 12 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING |
screamer | 0:bfb567985c64 | 13 | * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND |
screamer | 0:bfb567985c64 | 14 | * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, |
screamer | 0:bfb567985c64 | 15 | * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
screamer | 0:bfb567985c64 | 16 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. |
screamer | 0:bfb567985c64 | 17 | */ |
screamer | 0:bfb567985c64 | 18 | |
screamer | 0:bfb567985c64 | 19 | #include "mbed.h" |
screamer | 0:bfb567985c64 | 20 | #include "FXOS8700Q.h" |
screamer | 0:bfb567985c64 | 21 | #include "FXAS21000.h" |
julioefajardo | 3:58ebc00f1a68 | 22 | #include "kalman.c" |
screamer | 0:bfb567985c64 | 23 | |
julioefajardo | 3:58ebc00f1a68 | 24 | #define PI 3.1415926535897932384626433832795 |
julioefajardo | 3:58ebc00f1a68 | 25 | #define Rad2Dree 57.295779513082320876798154814105 |
julioefajardo | 3:58ebc00f1a68 | 26 | |
julioefajardo | 2:4bc4e25328cc | 27 | FXOS8700Q_acc combo_acc(A5, A4, FXOS8700CQ_SLAVE_ADDR0); |
julioefajardo | 2:4bc4e25328cc | 28 | FXOS8700Q_mag combo_mag(A5, A4, FXOS8700CQ_SLAVE_ADDR0); |
julioefajardo | 2:4bc4e25328cc | 29 | FXAS21000 gyro(A5, A4); |
screamer | 0:bfb567985c64 | 30 | |
julioefajardo | 3:58ebc00f1a68 | 31 | Timer GlobalTime; |
julioefajardo | 3:58ebc00f1a68 | 32 | Timer ProgramTimer; |
julioefajardo | 3:58ebc00f1a68 | 33 | |
julioefajardo | 2:4bc4e25328cc | 34 | //PwmOut M1(PTB18); |
julioefajardo | 2:4bc4e25328cc | 35 | PwmOut M1(D13); |
julioefajardo | 2:4bc4e25328cc | 36 | PwmOut M2(D12); |
julioefajardo | 2:4bc4e25328cc | 37 | PwmOut M3(D11); |
julioefajardo | 2:4bc4e25328cc | 38 | PwmOut M4(D10); |
screamer | 0:bfb567985c64 | 39 | |
screamer | 0:bfb567985c64 | 40 | Serial pc(USBTX, USBRX); |
screamer | 0:bfb567985c64 | 41 | |
julioefajardo | 3:58ebc00f1a68 | 42 | kalman filter_pitch; |
julioefajardo | 3:58ebc00f1a68 | 43 | kalman filter_roll; |
julioefajardo | 3:58ebc00f1a68 | 44 | |
julioefajardo | 3:58ebc00f1a68 | 45 | float R; |
julioefajardo | 3:58ebc00f1a68 | 46 | double angle[3]; |
julioefajardo | 3:58ebc00f1a68 | 47 | unsigned long timer; |
julioefajardo | 3:58ebc00f1a68 | 48 | long loopStartTime; |
julioefajardo | 3:58ebc00f1a68 | 49 | |
screamer | 0:bfb567985c64 | 50 | int main() |
screamer | 0:bfb567985c64 | 51 | { |
julioefajardo | 2:4bc4e25328cc | 52 | float gyro_data[3]; |
screamer | 0:bfb567985c64 | 53 | MotionSensorDataUnits adata; |
screamer | 0:bfb567985c64 | 54 | MotionSensorDataUnits mdata; |
julioefajardo | 2:4bc4e25328cc | 55 | |
screamer | 0:bfb567985c64 | 56 | printf("\r\nStarting\r\n\r\n"); |
julioefajardo | 2:4bc4e25328cc | 57 | |
julioefajardo | 3:58ebc00f1a68 | 58 | GlobalTime.start(); |
julioefajardo | 3:58ebc00f1a68 | 59 | |
julioefajardo | 2:4bc4e25328cc | 60 | M1.period(0.02f); //Comparten el mismo timer |
julioefajardo | 2:4bc4e25328cc | 61 | M1.pulsewidth(0.0006f); |
julioefajardo | 2:4bc4e25328cc | 62 | M2.pulsewidth(0.0006f); |
julioefajardo | 2:4bc4e25328cc | 63 | M3.pulsewidth(0.0006f); |
julioefajardo | 2:4bc4e25328cc | 64 | M4.pulsewidth(0.0006f); |
julioefajardo | 2:4bc4e25328cc | 65 | |
screamer | 0:bfb567985c64 | 66 | combo_acc.enable(); |
screamer | 0:bfb567985c64 | 67 | combo_mag.enable(); |
screamer | 0:bfb567985c64 | 68 | printf("FXOS8700 Combo = %X\r\n", combo_acc.whoAmI()); |
screamer | 0:bfb567985c64 | 69 | printf("FXAS21000 Gyro = %X\r\n", gyro.getWhoAmI()); |
screamer | 0:bfb567985c64 | 70 | |
julioefajardo | 3:58ebc00f1a68 | 71 | kalman_init(&filter_pitch, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); |
julioefajardo | 3:58ebc00f1a68 | 72 | kalman_init(&filter_roll, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); |
julioefajardo | 3:58ebc00f1a68 | 73 | |
screamer | 0:bfb567985c64 | 74 | wait(3); |
screamer | 0:bfb567985c64 | 75 | |
julioefajardo | 4:1bc3ca07a412 | 76 | |
julioefajardo | 3:58ebc00f1a68 | 77 | ProgramTimer.start(); |
julioefajardo | 3:58ebc00f1a68 | 78 | loopStartTime = ProgramTimer.read_us(); |
julioefajardo | 3:58ebc00f1a68 | 79 | timer = loopStartTime; |
julioefajardo | 3:58ebc00f1a68 | 80 | |
screamer | 0:bfb567985c64 | 81 | while(1) { |
screamer | 0:bfb567985c64 | 82 | combo_acc.getAxis(adata); |
julioefajardo | 3:58ebc00f1a68 | 83 | combo_mag.getAxis(mdata); |
julioefajardo | 3:58ebc00f1a68 | 84 | gyro.ReadXYZ(gyro_data); |
julioefajardo | 3:58ebc00f1a68 | 85 | |
julioefajardo | 3:58ebc00f1a68 | 86 | R = sqrt(std::pow(adata.x, 2) + std::pow(adata.y, 2) + std::pow(adata.z, 2)); |
screamer | 0:bfb567985c64 | 87 | |
julioefajardo | 3:58ebc00f1a68 | 88 | kalman_predict(&filter_pitch, gyro_data[0], (ProgramTimer.read_us() - timer)); |
julioefajardo | 3:58ebc00f1a68 | 89 | kalman_update(&filter_pitch, acos(adata.x/R)); |
julioefajardo | 3:58ebc00f1a68 | 90 | kalman_predict(&filter_roll, gyro_data[1], (ProgramTimer.read_us() - timer)); |
julioefajardo | 3:58ebc00f1a68 | 91 | kalman_update(&filter_roll, acos(adata.y/R)); |
julioefajardo | 3:58ebc00f1a68 | 92 | |
julioefajardo | 3:58ebc00f1a68 | 93 | angle[0] = kalman_get_angle(&filter_pitch); |
julioefajardo | 3:58ebc00f1a68 | 94 | angle[1] = kalman_get_angle(&filter_roll); |
julioefajardo | 3:58ebc00f1a68 | 95 | |
julioefajardo | 3:58ebc00f1a68 | 96 | timer = ProgramTimer.read_us(); |
julioefajardo | 3:58ebc00f1a68 | 97 | |
julioefajardo | 3:58ebc00f1a68 | 98 | printf("FXOS8700 Acc: X:%6.3f Y:%6.3f Z:%6.3f\r\n", adata.x, adata.y, adata.z); |
screamer | 0:bfb567985c64 | 99 | printf("FXOS8700 Mag: X:%6.2f Y:%6.2f Z:%6.2f\r\n", mdata.x, mdata.y, mdata.z); |
screamer | 0:bfb567985c64 | 100 | printf("FXAS21000 Gyro: X:%6.2f Y:%6.2f Z:%6.2f\r\n", gyro_data[0], gyro_data[1], gyro_data[2]); |
julioefajardo | 3:58ebc00f1a68 | 101 | printf("Roll Angle X: %.6f Pitch Angle Y: %.6f \n\r", Rad2Dree * angle[1], Rad2Dree * angle[0]); |
screamer | 0:bfb567985c64 | 102 | printf("\r\n"); |
screamer | 0:bfb567985c64 | 103 | |
julioefajardo | 2:4bc4e25328cc | 104 | wait(0.5); |
screamer | 0:bfb567985c64 | 105 | } |
screamer | 0:bfb567985c64 | 106 | } |