Kenji Arai / Mbed 2 deprecated GR-PEACH_test_on_rtos_works_well

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:   L3GD20 LIS3DH TextLCD mbed-rtos mbed

kf.cpp@4:76b3113c79ff, 2014-12-14 (annotated)

Committer:
kenjiArai
Date:
Sun Dec 14 09:17:01 2014 +0000
Revision:
4:76b3113c79ff
Check rtos functions on GR-PEACH. Does NOT work on GR-PEACH but does work on LPC1768, F401RE and L152RE.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kenjiArai 4:76b3113c79ff 1 /*
kenjiArai 4:76b3113c79ff 2 * mbed Application program for the mbed ST NUCLEO F401RE Board
kenjiArai 4:76b3113c79ff 3 *
kenjiArai 4:76b3113c79ff 4 * Modified by Kenji Arai / JH1PJL
kenjiArai 4:76b3113c79ff 5 * http://www.page.sannet.ne.jp/kenjia/index.html
kenjiArai 4:76b3113c79ff 6 * http://mbed.org/users/kenjiArai/
kenjiArai 4:76b3113c79ff 7 * Revised: August 28th, 2014
kenjiArai 4:76b3113c79ff 8 */
kenjiArai 4:76b3113c79ff 9
kenjiArai 4:76b3113c79ff 10 // <Original> http://www.x-firm.com/?page_id=191
kenjiArai 4:76b3113c79ff 11
kenjiArai 4:76b3113c79ff 12 // Main module K_bot angle angles in Quids, 10 bit ADC ---------------------------------------
kenjiArai 4:76b3113c79ff 13 // 7 - Data fusing, Kalman filter
kenjiArai 4:76b3113c79ff 14
kenjiArai 4:76b3113c79ff 15 // Installation:
kenjiArai 4:76b3113c79ff 16 // Create "Kalman" and "Sensors" tabs
kenjiArai 4:76b3113c79ff 17 // Cut and paste the 2 modules in their respective tab
kenjiArai 4:76b3113c79ff 18 // Save as "Kas_bot_angle"
kenjiArai 4:76b3113c79ff 19
kenjiArai 4:76b3113c79ff 20 // --- Kalman filter module ----------------------------------------------------------------------
kenjiArai 4:76b3113c79ff 21 float Q_angle = 0.001; //0.001
kenjiArai 4:76b3113c79ff 22 float Q_gyro = 0.003; //0.003
kenjiArai 4:76b3113c79ff 23 float R_angle = 0.03; //0.03
kenjiArai 4:76b3113c79ff 24
kenjiArai 4:76b3113c79ff 25 float x_angle = 0;
kenjiArai 4:76b3113c79ff 26 float x_bias = 0;
kenjiArai 4:76b3113c79ff 27 float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
kenjiArai 4:76b3113c79ff 28 float dt, y, S;
kenjiArai 4:76b3113c79ff 29 float K_0, K_1;
kenjiArai 4:76b3113c79ff 30
kenjiArai 4:76b3113c79ff 31 float kalmanCalculate(float newAngle, float newRate, int looptime) {
kenjiArai 4:76b3113c79ff 32 dt = float(looptime)/1000;
kenjiArai 4:76b3113c79ff 33 x_angle += dt * (newRate - x_bias);
kenjiArai 4:76b3113c79ff 34 P_00 += - dt * (P_10 + P_01) + Q_angle * dt;
kenjiArai 4:76b3113c79ff 35 P_01 += - dt * P_11;
kenjiArai 4:76b3113c79ff 36 P_10 += - dt * P_11;
kenjiArai 4:76b3113c79ff 37 P_11 += + Q_gyro * dt;
kenjiArai 4:76b3113c79ff 38
kenjiArai 4:76b3113c79ff 39 y = newAngle - x_angle;
kenjiArai 4:76b3113c79ff 40 S = P_00 + R_angle;
kenjiArai 4:76b3113c79ff 41 K_0 = P_00 / S;
kenjiArai 4:76b3113c79ff 42 K_1 = P_10 / S;
kenjiArai 4:76b3113c79ff 43
kenjiArai 4:76b3113c79ff 44 x_angle += K_0 * y;
kenjiArai 4:76b3113c79ff 45 x_bias += K_1 * y;
kenjiArai 4:76b3113c79ff 46 P_00 -= K_0 * P_00;
kenjiArai 4:76b3113c79ff 47 P_01 -= K_0 * P_01;
kenjiArai 4:76b3113c79ff 48 P_10 -= K_1 * P_00;
kenjiArai 4:76b3113c79ff 49 P_11 -= K_1 * P_01;
kenjiArai 4:76b3113c79ff 50
kenjiArai 4:76b3113c79ff 51 return x_angle;
kenjiArai 4:76b3113c79ff 52 }