Hybrid sEMG + IMU activated controller for Galileo Bionic Hand Prosthesis

Dependencies:   FXAS21000 FXOS8700Q kalman mbed-dsp mbed-rtos mbed

Committer:
julioefajardo
Date:
Sun Nov 01 18:38:43 2015 +0000
Revision:
1:84347af5a1f2
Parent:
0:2245e978868c
Version 1.5 - RTOS; Threads; Mutex; EMG sampler; IMU sampler; Kalman filter; Serial Communication; Feature Extraction; Neural Network Classifier

Who changed what in which revision?

UserRevisionLine numberNew contents of line
julioefajardo 0:2245e978868c 1 #include "mbed.h"
julioefajardo 0:2245e978868c 2 #include "FXOS8700Q.h"
julioefajardo 0:2245e978868c 3 #include "FXAS21000.h"
julioefajardo 0:2245e978868c 4 #include "rtos.h"
julioefajardo 0:2245e978868c 5 #include "kalman.c"
julioefajardo 0:2245e978868c 6 #include "arm_math.h"
julioefajardo 1:84347af5a1f2 7 #include "rtwtypes.h"
julioefajardo 0:2245e978868c 8
julioefajardo 0:2245e978868c 9 #define TRUE 1
julioefajardo 0:2245e978868c 10 #define FALSE 0
julioefajardo 0:2245e978868c 11 #define RAW 0
julioefajardo 0:2245e978868c 12 #define RECTIFIED 1
julioefajardo 0:2245e978868c 13 #define SMOOTH 2
julioefajardo 0:2245e978868c 14 #define THRESHOLDF 0.25f
julioefajardo 0:2245e978868c 15 #define THRESHOLDE 0.25f
julioefajardo 0:2245e978868c 16 #define FLEXION2 0.0f
julioefajardo 0:2245e978868c 17 #define Rad2Dree 57.295779513082320876798154814105f
julioefajardo 0:2245e978868c 18
julioefajardo 0:2245e978868c 19 Serial pc(USBTX, USBRX);
julioefajardo 0:2245e978868c 20 AnalogIn Ref(A0);
julioefajardo 0:2245e978868c 21 AnalogIn E1(A1);
julioefajardo 0:2245e978868c 22 AnalogIn E2(A2);
julioefajardo 0:2245e978868c 23 AnalogIn E3(A3);
julioefajardo 0:2245e978868c 24 FXOS8700Q_acc combo_acc(D14, D15, FXOS8700CQ_SLAVE_ADDR1);
julioefajardo 0:2245e978868c 25 FXOS8700Q_mag combo_mag(D14, D15, FXOS8700CQ_SLAVE_ADDR1);
julioefajardo 0:2245e978868c 26 FXAS21000 gyro(D14, D15);
julioefajardo 0:2245e978868c 27 DigitalOut led1(LED1);
julioefajardo 1:84347af5a1f2 28 DigitalOut led2(LED2);
julioefajardo 0:2245e978868c 29 InterruptIn sw2(SW2);
julioefajardo 0:2245e978868c 30 Timer GlobalTime;
julioefajardo 0:2245e978868c 31 Timer ProgramTimer;
julioefajardo 0:2245e978868c 32
julioefajardo 0:2245e978868c 33 //Threadas
julioefajardo 0:2245e978868c 34 uint32_t button_pressed;
julioefajardo 0:2245e978868c 35 Thread *thread2;
julioefajardo 0:2245e978868c 36 Thread *thread3;
julioefajardo 0:2245e978868c 37 Thread *thread4;
julioefajardo 0:2245e978868c 38
julioefajardo 0:2245e978868c 39 //EMG samples and DSP variables
julioefajardo 0:2245e978868c 40 float32_t EMG1, EMG2, EMG3;
julioefajardo 0:2245e978868c 41 float32_t samples[25];
julioefajardo 0:2245e978868c 42 float32_t samples2[25];
julioefajardo 0:2245e978868c 43 float32_t samples3[25];
julioefajardo 0:2245e978868c 44 float32_t abs_output[25];
julioefajardo 0:2245e978868c 45 float32_t abs_output2[25];
julioefajardo 0:2245e978868c 46 float32_t abs_output3[25];
julioefajardo 1:84347af5a1f2 47 float32_t MAV1 = 0.0f;
julioefajardo 1:84347af5a1f2 48 float32_t MAV2 = 0.0f;
julioefajardo 1:84347af5a1f2 49 float32_t MAV3 = 0.0f;
julioefajardo 1:84347af5a1f2 50
julioefajardo 1:84347af5a1f2 51 typedef struct {
julioefajardo 1:84347af5a1f2 52 float32_t iemg;
julioefajardo 1:84347af5a1f2 53 float32_t wl;
julioefajardo 1:84347af5a1f2 54 float32_t var;
julioefajardo 1:84347af5a1f2 55 int32_t wamp;
julioefajardo 1:84347af5a1f2 56 int32_t zc;
julioefajardo 1:84347af5a1f2 57 int32_t ssc;
julioefajardo 1:84347af5a1f2 58 } emg_features;
julioefajardo 1:84347af5a1f2 59
julioefajardo 1:84347af5a1f2 60 emg_features feat1 = {0.0f,0.0f,0.0f,0,0,0};
julioefajardo 1:84347af5a1f2 61 emg_features feat2 = {0.0f,0.0f,0.0f,0,0,0};
julioefajardo 0:2245e978868c 62
julioefajardo 0:2245e978868c 63 //IMU
julioefajardo 1:84347af5a1f2 64 float32_t gyro_data[3];
julioefajardo 0:2245e978868c 65 MotionSensorDataUnits adata;
julioefajardo 0:2245e978868c 66 MotionSensorDataUnits mdata;
julioefajardo 1:84347af5a1f2 67 float32_t R;
julioefajardo 1:84347af5a1f2 68 float32_t R_XY;
julioefajardo 1:84347af5a1f2 69 float32_t R_mean;
julioefajardo 1:84347af5a1f2 70 float32_t angle[3];
julioefajardo 1:84347af5a1f2 71 float32_t roll;
julioefajardo 1:84347af5a1f2 72 float32_t pitch;
julioefajardo 0:2245e978868c 73 kalman filter_pitch;
julioefajardo 0:2245e978868c 74 kalman filter_roll;
julioefajardo 1:84347af5a1f2 75 kalman filter_yaw;
julioefajardo 1:84347af5a1f2 76 float32_t imu_ones[25];
julioefajardo 1:84347af5a1f2 77 float32_t accx_samples[25];
julioefajardo 1:84347af5a1f2 78 float32_t accy_samples[25];
julioefajardo 1:84347af5a1f2 79 float32_t accz_samples[25];
julioefajardo 1:84347af5a1f2 80 float32_t roll_samples[25];
julioefajardo 1:84347af5a1f2 81 float32_t pitch_samples[25];
julioefajardo 1:84347af5a1f2 82 float32_t R_samples[25];
julioefajardo 1:84347af5a1f2 83 float32_t imu_abs_samples[25];
julioefajardo 1:84347af5a1f2 84
julioefajardo 1:84347af5a1f2 85
julioefajardo 1:84347af5a1f2 86 typedef struct {
julioefajardo 1:84347af5a1f2 87 float32_t l1;
julioefajardo 1:84347af5a1f2 88 float32_t mean;
julioefajardo 1:84347af5a1f2 89 float32_t var;
julioefajardo 1:84347af5a1f2 90 } imu_features;
julioefajardo 1:84347af5a1f2 91
julioefajardo 1:84347af5a1f2 92 imu_features ax_feat = {0.0f,0.0f,0.0f};
julioefajardo 1:84347af5a1f2 93 imu_features ay_feat = {0.0f,0.0f,0.0f};
julioefajardo 1:84347af5a1f2 94 imu_features az_feat = {0.0f,0.0f,0.0f};
julioefajardo 1:84347af5a1f2 95 imu_features pitch_feat = {0.0f,0.0f,0.0f};
julioefajardo 1:84347af5a1f2 96 imu_features roll_feat = {0.0f,0.0f,0.0f};
julioefajardo 1:84347af5a1f2 97 imu_features R_feat = {0.0f,0.0f,0.0f};
julioefajardo 1:84347af5a1f2 98
julioefajardo 1:84347af5a1f2 99 float32_t imu_y[4] = {0.0f,0.0f,0.0f,0.0f}; //neural network output
julioefajardo 1:84347af5a1f2 100 float32_t imu_x[18] = {0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f}; //imu neural network input
julioefajardo 1:84347af5a1f2 101 float32_t imu_max = 0.0f; //max accuracy
julioefajardo 1:84347af5a1f2 102 uint32_t imu_class = 0; //class
julioefajardo 0:2245e978868c 103
julioefajardo 0:2245e978868c 104 //OS
julioefajardo 1:84347af5a1f2 105 Mutex imu_mutex;
julioefajardo 1:84347af5a1f2 106 Mutex emg_mutex;
julioefajardo 0:2245e978868c 107
julioefajardo 0:2245e978868c 108 //time
julioefajardo 0:2245e978868c 109 unsigned long timer;
julioefajardo 0:2245e978868c 110 long loopStartTime;
julioefajardo 0:2245e978868c 111
julioefajardo 0:2245e978868c 112 void sw2_press(void);
julioefajardo 0:2245e978868c 113 void button_thread(void const *argument);
julioefajardo 0:2245e978868c 114 void imu_thread(void const *argument);
julioefajardo 0:2245e978868c 115 void sampler_thread(void const *argument);
julioefajardo 1:84347af5a1f2 116 void imu_proc_thread(void const *argument);
julioefajardo 0:2245e978868c 117
julioefajardo 1:84347af5a1f2 118 imu_features imuGetFeatures(float32_t samples[50]);
julioefajardo 0:2245e978868c 119 void Serial_Oscilloscope(uint8_t activation, uint8_t type);
julioefajardo 1:84347af5a1f2 120 void IMU_Logger(void);
julioefajardo 1:84347af5a1f2 121 void IMU_Classifier(const float x1[18], float b_y1[4]);
julioefajardo 1:84347af5a1f2 122 boolean_T rtIsNaNF(real32_T value);
julioefajardo 0:2245e978868c 123
julioefajardo 0:2245e978868c 124 int main(void)
julioefajardo 0:2245e978868c 125 {
julioefajardo 0:2245e978868c 126 pc.baud(115200); //Serial com at 115200 bauds
julioefajardo 1:84347af5a1f2 127 led1 = 1; led2 = 1;
julioefajardo 0:2245e978868c 128 Thread thread(imu_thread);
julioefajardo 0:2245e978868c 129 thread2 = new Thread(button_thread);
julioefajardo 0:2245e978868c 130 thread3 = new Thread(sampler_thread);
julioefajardo 1:84347af5a1f2 131 thread4 = new Thread(imu_proc_thread);
julioefajardo 1:84347af5a1f2 132
julioefajardo 1:84347af5a1f2 133 arm_fill_f32(1.0f, imu_ones, 50);
julioefajardo 1:84347af5a1f2 134
julioefajardo 0:2245e978868c 135 GlobalTime.start();
julioefajardo 0:2245e978868c 136
julioefajardo 0:2245e978868c 137 combo_acc.enable();
julioefajardo 0:2245e978868c 138 combo_mag.enable();
julioefajardo 1:84347af5a1f2 139 //pc.printf("FXOS8700 Combo = %X\r\n", combo_acc.whoAmI());
julioefajardo 1:84347af5a1f2 140 //pc.printf("FXAS21000 Gyro = %X\r\n", gyro.getWhoAmI());
julioefajardo 0:2245e978868c 141
julioefajardo 0:2245e978868c 142 kalman_init(&filter_pitch, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
julioefajardo 0:2245e978868c 143 kalman_init(&filter_roll, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
julioefajardo 0:2245e978868c 144
julioefajardo 0:2245e978868c 145 button_pressed = 0;
julioefajardo 0:2245e978868c 146 sw2.fall(&sw2_press);
julioefajardo 0:2245e978868c 147
julioefajardo 0:2245e978868c 148 ProgramTimer.start();
julioefajardo 0:2245e978868c 149 loopStartTime = ProgramTimer.read_us();
julioefajardo 0:2245e978868c 150 timer = loopStartTime;
julioefajardo 0:2245e978868c 151
julioefajardo 0:2245e978868c 152 while (true) {
julioefajardo 0:2245e978868c 153 led1 = !led1;
julioefajardo 1:84347af5a1f2 154 //pc.printf("SW2 was pressed (last 5 seconds): %d \n\r", button_pressed);
julioefajardo 1:84347af5a1f2 155 //pc.printf("Acc Vector: %.6f XY Acc Vector: %.6f Roll Angle X: %.6f Pitch Angle Y: %.6f \r\n", R, R_XY, Rad2Dree * angle[1], Rad2Dree * angle[0]);
julioefajardo 1:84347af5a1f2 156 //fflush(stdout);
julioefajardo 1:84347af5a1f2 157 //pc.printf("%.6f,%.6f,%.6f,%.6f,%.6f,%.6f\r",adata.x,adata.y,adata.z,R_XY,angle[0],angle[1]);
julioefajardo 1:84347af5a1f2 158
julioefajardo 0:2245e978868c 159 button_pressed = 0;
julioefajardo 1:84347af5a1f2 160 Thread::wait(100);
julioefajardo 1:84347af5a1f2 161
julioefajardo 0:2245e978868c 162 }
julioefajardo 0:2245e978868c 163 }
julioefajardo 0:2245e978868c 164
julioefajardo 0:2245e978868c 165 void sw2_press(void){
julioefajardo 0:2245e978868c 166 thread2->signal_set(0x1);
julioefajardo 0:2245e978868c 167 }
julioefajardo 0:2245e978868c 168
julioefajardo 0:2245e978868c 169 void button_thread(void const *argument){
julioefajardo 0:2245e978868c 170 while (true) {
julioefajardo 0:2245e978868c 171 Thread::signal_wait(0x1);
julioefajardo 0:2245e978868c 172 button_pressed++;
julioefajardo 0:2245e978868c 173 }
julioefajardo 0:2245e978868c 174 }
julioefajardo 0:2245e978868c 175
julioefajardo 0:2245e978868c 176 void imu_thread(void const *argument){
julioefajardo 0:2245e978868c 177 while (true) {
julioefajardo 0:2245e978868c 178
julioefajardo 1:84347af5a1f2 179 combo_acc.getAxis(adata);
julioefajardo 1:84347af5a1f2 180 gyro.ReadXYZ(gyro_data);
julioefajardo 1:84347af5a1f2 181
julioefajardo 0:2245e978868c 182 R = sqrt(std::pow(adata.x, 2) + std::pow(adata.y, 2) + std::pow(adata.z, 2));
julioefajardo 1:84347af5a1f2 183 R_XY = sqrt(std::pow(adata.x, 2) + std::pow(adata.y, 2));
julioefajardo 0:2245e978868c 184
julioefajardo 0:2245e978868c 185 kalman_predict(&filter_pitch, gyro_data[0], (ProgramTimer.read_us() - timer));
julioefajardo 0:2245e978868c 186 kalman_update(&filter_pitch, acos(adata.x/R));
julioefajardo 0:2245e978868c 187 kalman_predict(&filter_roll, gyro_data[1], (ProgramTimer.read_us() - timer));
julioefajardo 0:2245e978868c 188 kalman_update(&filter_roll, acos(adata.y/R));
julioefajardo 0:2245e978868c 189
julioefajardo 0:2245e978868c 190 angle[0] = kalman_get_angle(&filter_pitch);
julioefajardo 0:2245e978868c 191 angle[1] = kalman_get_angle(&filter_roll);
julioefajardo 1:84347af5a1f2 192
julioefajardo 1:84347af5a1f2 193 imu_mutex.lock();
julioefajardo 1:84347af5a1f2 194 for(int j=25-1;j>0;j--) {
julioefajardo 1:84347af5a1f2 195 accx_samples[j]=accx_samples[j-1];
julioefajardo 1:84347af5a1f2 196 accy_samples[j]=accy_samples[j-1];
julioefajardo 1:84347af5a1f2 197 accz_samples[j]=accz_samples[j-1];
julioefajardo 1:84347af5a1f2 198 roll_samples[j]=roll_samples[j-1];
julioefajardo 1:84347af5a1f2 199 pitch_samples[j]=pitch_samples[j-1];
julioefajardo 1:84347af5a1f2 200 R_samples[j]=R_samples[j-1];
julioefajardo 1:84347af5a1f2 201 }
julioefajardo 1:84347af5a1f2 202 accx_samples[0]=adata.x;
julioefajardo 1:84347af5a1f2 203 accy_samples[0]=adata.y;
julioefajardo 1:84347af5a1f2 204 accz_samples[0]=adata.z;
julioefajardo 1:84347af5a1f2 205 roll_samples[0]=angle[1];
julioefajardo 1:84347af5a1f2 206 pitch_samples[0]=angle[0];
julioefajardo 1:84347af5a1f2 207 R_samples[0]=R_XY;
julioefajardo 1:84347af5a1f2 208 imu_mutex.unlock();
julioefajardo 1:84347af5a1f2 209
julioefajardo 1:84347af5a1f2 210 thread4->signal_set(0x1);
julioefajardo 0:2245e978868c 211 Thread::wait(10);
julioefajardo 0:2245e978868c 212 }
julioefajardo 0:2245e978868c 213 }
julioefajardo 0:2245e978868c 214
julioefajardo 0:2245e978868c 215 void sampler_thread(void const *argument){
julioefajardo 0:2245e978868c 216 while (true) {
julioefajardo 0:2245e978868c 217 EMG1 = (E1.read()-Ref.read())*3.3f;
julioefajardo 0:2245e978868c 218 EMG2 = (E2.read()-Ref.read())*3.3f;
julioefajardo 0:2245e978868c 219 EMG3 = (E3.read()-Ref.read())*3.3f;
julioefajardo 0:2245e978868c 220 for(int j=24;j>0;j--) {
julioefajardo 0:2245e978868c 221 samples[j]=samples[j-1]; //Fill Array
julioefajardo 0:2245e978868c 222 samples2[j]=samples2[j-1]; //Fill Array
julioefajardo 0:2245e978868c 223 samples3[j]=samples3[j-1]; //Fill Array
julioefajardo 0:2245e978868c 224 }
julioefajardo 0:2245e978868c 225 samples[0]=EMG1;
julioefajardo 0:2245e978868c 226 samples2[0]=EMG2;
julioefajardo 0:2245e978868c 227 samples3[0]=EMG3;
julioefajardo 0:2245e978868c 228 Thread::wait(1);
julioefajardo 0:2245e978868c 229 }
julioefajardo 0:2245e978868c 230 }
julioefajardo 0:2245e978868c 231
julioefajardo 0:2245e978868c 232
julioefajardo 1:84347af5a1f2 233 void imu_proc_thread(void const *argument){
julioefajardo 0:2245e978868c 234 while (true) {
julioefajardo 1:84347af5a1f2 235 //int i;
julioefajardo 1:84347af5a1f2 236 Thread::signal_wait(0x1);
julioefajardo 1:84347af5a1f2 237 imu_mutex.lock();
julioefajardo 1:84347af5a1f2 238 arm_mean_f32(R_samples, 25, &R_mean);
julioefajardo 1:84347af5a1f2 239 if(R_mean>=1.125f){
julioefajardo 1:84347af5a1f2 240 //for(i=0;i<25;i++) pc.printf("%.6f,",accx_samples[i]);
julioefajardo 1:84347af5a1f2 241 ax_feat = imuGetFeatures(accx_samples);
julioefajardo 1:84347af5a1f2 242 ay_feat = imuGetFeatures(accy_samples);
julioefajardo 1:84347af5a1f2 243 az_feat = imuGetFeatures(accz_samples);
julioefajardo 1:84347af5a1f2 244 R_feat = imuGetFeatures(R_samples);
julioefajardo 1:84347af5a1f2 245 pitch_feat = imuGetFeatures(pitch_samples);
julioefajardo 1:84347af5a1f2 246 roll_feat = imuGetFeatures(roll_samples);
julioefajardo 1:84347af5a1f2 247 //IMU_Logger();
julioefajardo 1:84347af5a1f2 248 imu_x[0] = ax_feat.l1;
julioefajardo 1:84347af5a1f2 249 imu_x[1] = ax_feat.mean;
julioefajardo 1:84347af5a1f2 250 imu_x[2] = ax_feat.var;
julioefajardo 1:84347af5a1f2 251 imu_x[3] = ay_feat.l1;
julioefajardo 1:84347af5a1f2 252 imu_x[4] = ay_feat.mean;
julioefajardo 1:84347af5a1f2 253 imu_x[5] = ay_feat.var;
julioefajardo 1:84347af5a1f2 254 imu_x[6] = az_feat.l1;
julioefajardo 1:84347af5a1f2 255 imu_x[7] = az_feat.mean;
julioefajardo 1:84347af5a1f2 256 imu_x[8] = az_feat.var;
julioefajardo 1:84347af5a1f2 257 imu_x[9] = R_feat.l1;
julioefajardo 1:84347af5a1f2 258 imu_x[10] = R_feat.mean;
julioefajardo 1:84347af5a1f2 259 imu_x[11] = R_feat.var;
julioefajardo 1:84347af5a1f2 260 imu_x[12] = pitch_feat.l1;
julioefajardo 1:84347af5a1f2 261 imu_x[13] = pitch_feat.mean;
julioefajardo 1:84347af5a1f2 262 imu_x[14] = pitch_feat.var;
julioefajardo 1:84347af5a1f2 263 imu_x[15] = roll_feat.l1;
julioefajardo 1:84347af5a1f2 264 imu_x[16] = roll_feat.mean;
julioefajardo 1:84347af5a1f2 265 imu_x[17] = roll_feat.var;
julioefajardo 1:84347af5a1f2 266 IMU_Classifier(imu_x,imu_y);
julioefajardo 1:84347af5a1f2 267 arm_max_f32(imu_y,4,&imu_max,&imu_class);
julioefajardo 1:84347af5a1f2 268 pc.printf("%d ",imu_class);
julioefajardo 1:84347af5a1f2 269 switch(imu_class){
julioefajardo 1:84347af5a1f2 270 case 0: pc.printf("Up\n\r"); break;
julioefajardo 1:84347af5a1f2 271 case 1: pc.printf("Down\n\r"); break;
julioefajardo 1:84347af5a1f2 272 case 2: pc.printf("Left\n\r"); break;
julioefajardo 1:84347af5a1f2 273 case 3: pc.printf("Right\n\r"); break;
julioefajardo 1:84347af5a1f2 274 }
julioefajardo 1:84347af5a1f2 275 }
julioefajardo 1:84347af5a1f2 276 imu_mutex.unlock();
julioefajardo 1:84347af5a1f2 277 Thread::wait(500);
julioefajardo 0:2245e978868c 278 }
julioefajardo 0:2245e978868c 279 }
julioefajardo 0:2245e978868c 280
julioefajardo 0:2245e978868c 281 void Serial_Oscilloscope(uint8_t activation, uint8_t type){
julioefajardo 0:2245e978868c 282 if (activation){
julioefajardo 0:2245e978868c 283 switch(type){
julioefajardo 0:2245e978868c 284 case RAW: pc.printf("%.10f,%.10f,%.10f\n\r",EMG1,EMG2,EMG3); break;
julioefajardo 0:2245e978868c 285 case RECTIFIED: pc.printf("%.10f,%.10f,%.10f\n\r",abs_output[0],abs_output2[0],abs_output3[0]);break;
julioefajardo 1:84347af5a1f2 286 case SMOOTH: pc.printf("%.10f,%.10f,%.10f\n\r",MAV1,MAV2,MAV3); break;
julioefajardo 0:2245e978868c 287 default: pc.printf("%.10f,%.10f,%.10f\n\r",EMG1,EMG2,EMG3);
julioefajardo 0:2245e978868c 288 }
julioefajardo 0:2245e978868c 289 }
julioefajardo 0:2245e978868c 290 }
julioefajardo 1:84347af5a1f2 291
julioefajardo 1:84347af5a1f2 292 void IMU_Logger(void){
julioefajardo 1:84347af5a1f2 293 pc.printf("%.10f,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f\r",ax_feat.l1,ax_feat.mean,ax_feat.var,ay_feat.l1,ay_feat.mean,ay_feat.var,az_feat.l1,az_feat.mean,az_feat.var);
julioefajardo 1:84347af5a1f2 294 pc.printf("%.10f,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f\r",R_feat.l1,R_feat.mean,R_feat.var,pitch_feat.l1,pitch_feat.mean,pitch_feat.var,roll_feat.l1,roll_feat.mean,roll_feat.var);
julioefajardo 1:84347af5a1f2 295 }
julioefajardo 1:84347af5a1f2 296
julioefajardo 1:84347af5a1f2 297 imu_features imuGetFeatures(float32_t samples[25]){
julioefajardo 1:84347af5a1f2 298 imu_features feat;
julioefajardo 1:84347af5a1f2 299 float32_t l1, mean, var;
julioefajardo 1:84347af5a1f2 300
julioefajardo 1:84347af5a1f2 301 /*L1*/
julioefajardo 1:84347af5a1f2 302 arm_abs_f32(samples, imu_abs_samples, 25);
julioefajardo 1:84347af5a1f2 303 arm_dot_prod_f32(imu_abs_samples, imu_ones, 25, &l1);
julioefajardo 1:84347af5a1f2 304
julioefajardo 1:84347af5a1f2 305 /*MAV*/
julioefajardo 1:84347af5a1f2 306 arm_mean_f32(imu_abs_samples, 25, &mean);
julioefajardo 1:84347af5a1f2 307 /*variance*/
julioefajardo 1:84347af5a1f2 308 arm_var_f32(samples, 25, &var);
julioefajardo 1:84347af5a1f2 309
julioefajardo 1:84347af5a1f2 310 feat.l1 = l1;
julioefajardo 1:84347af5a1f2 311 feat.mean = mean;
julioefajardo 1:84347af5a1f2 312 feat.var = var;
julioefajardo 1:84347af5a1f2 313
julioefajardo 1:84347af5a1f2 314 return feat;
julioefajardo 1:84347af5a1f2 315 }
julioefajardo 1:84347af5a1f2 316
julioefajardo 1:84347af5a1f2 317 void IMU_Classifier(const float x1[18], float b_y1[4]){
julioefajardo 1:84347af5a1f2 318 float xp1[18];
julioefajardo 1:84347af5a1f2 319 int ixstart;
julioefajardo 1:84347af5a1f2 320 static const double settings_gain[18] = { 0.0615861197203374, 1.53965323006161,
julioefajardo 1:84347af5a1f2 321 0.857469392512132, 0.0600806679095755, 1.50201694590539, 1.23879030268757,
julioefajardo 1:84347af5a1f2 322 0.0663679885953248, 1.65919957723595, 0.599294154820359, 0.113465456292823,
julioefajardo 1:84347af5a1f2 323 2.83663399337079, 2.21904272848878, 0.053290743371231, 1.33226929425699,
julioefajardo 1:84347af5a1f2 324 2.74309649260372, 0.0572933421985231, 1.43233365754206, 2.73777852926756 };
julioefajardo 1:84347af5a1f2 325
julioefajardo 1:84347af5a1f2 326 static const double settings_xoffset[18] = { 3.13208, 0.1252832, 0.01800423,
julioefajardo 1:84347af5a1f2 327 9.610352, 0.3844141, 0.06231475, 16.18701, 0.6474805, 0.008212686, 28.19626,
julioefajardo 1:84347af5a1f2 328 1.12785, 0.06004226, 23.18947, 0.927579, 0.006299257, 12.31171, 0.4924683,
julioefajardo 1:84347af5a1f2 329 0.02629572 };
julioefajardo 1:84347af5a1f2 330
julioefajardo 1:84347af5a1f2 331 double b[45];
julioefajardo 1:84347af5a1f2 332 static const double a[45] = { -1.7580289769551161, -1.3423592326953466,
julioefajardo 1:84347af5a1f2 333 1.5454917034757418, 1.6192469035519192, 1.1088397665812808,
julioefajardo 1:84347af5a1f2 334 -1.3294886162934598, -1.5402696585790496, -1.0670236222833622,
julioefajardo 1:84347af5a1f2 335 0.79657239712499928, -0.67248449362717955, -0.93265917892314076,
julioefajardo 1:84347af5a1f2 336 0.51780467293444077, 0.75531292620200852, 0.68952452013799781,
julioefajardo 1:84347af5a1f2 337 -0.9167788050264305, 0.40679456848166357, -0.78466204076547852,
julioefajardo 1:84347af5a1f2 338 0.0746048711915274, -0.352749928885278, 0.017737649172833974,
julioefajardo 1:84347af5a1f2 339 -0.038510072629115363, -0.20772701105519994, 0.45325062847723907,
julioefajardo 1:84347af5a1f2 340 0.26044952985819775, 0.39677487553689345, -0.078712365946250917,
julioefajardo 1:84347af5a1f2 341 -0.0825777988627018, -0.4727716865854944, 0.527910227195504,
julioefajardo 1:84347af5a1f2 342 -0.74851602503119774, 0.63024557363169653, -0.58582825900364521,
julioefajardo 1:84347af5a1f2 343 0.68849439469421991, -0.84750206038432641, -1.2000641131247758,
julioefajardo 1:84347af5a1f2 344 -0.832240518854391, 1.0889450399496252, -1.3059529805809877,
julioefajardo 1:84347af5a1f2 345 -1.4651416897453897, 1.3742855905702005, 1.2927860034088354,
julioefajardo 1:84347af5a1f2 346 1.60109353014912, 1.4478939752003774, -1.6843870868642861,
julioefajardo 1:84347af5a1f2 347 1.8617437390920597 };
julioefajardo 1:84347af5a1f2 348
julioefajardo 1:84347af5a1f2 349 float b_b[45];
julioefajardo 1:84347af5a1f2 350 int ix;
julioefajardo 1:84347af5a1f2 351 float denom;
julioefajardo 1:84347af5a1f2 352 static const float fv0[810] = { 0.459632903F, 0.0611374F, -0.421426982F,
julioefajardo 1:84347af5a1f2 353 0.148853645F, -0.177980483F, 0.247128889F, 0.214431226F, 0.451934367F,
julioefajardo 1:84347af5a1f2 354 -0.532601058F, 0.0238353293F, 0.497370124F, -0.575586F, -0.499140948F,
julioefajardo 1:84347af5a1f2 355 -0.0641773343F, 0.53166759F, 0.0778795481F, 0.225708947F, -0.498524398F,
julioefajardo 1:84347af5a1f2 356 0.694334269F, -0.60842818F, 0.325595617F, 0.252601981F, 0.365996331F,
julioefajardo 1:84347af5a1f2 357 0.384501338F, 0.00875493512F, -0.4245179F, -0.668958485F, -0.683882117F,
julioefajardo 1:84347af5a1f2 358 0.181472883F, -0.426831633F, 0.297112852F, -0.583310187F, 0.456262171F,
julioefajardo 1:84347af5a1f2 359 -0.459338129F, -0.641833425F, -0.459789753F, 0.204319239F, -0.186706707F,
julioefajardo 1:84347af5a1f2 360 -0.285366446F, 0.640228748F, 0.0229709782F, -0.0642111823F, 0.553962111F,
julioefajardo 1:84347af5a1f2 361 -0.33109349F, 0.247776791F, 0.443146557F, -0.542193532F, 0.119367629F,
julioefajardo 1:84347af5a1f2 362 -0.451677382F, -0.786408F, 0.0928186F, 0.316778958F, 0.707230628F,
julioefajardo 1:84347af5a1f2 363 -0.746820033F, -0.147853762F, 0.219747201F, -0.79193157F, -0.194468275F,
julioefajardo 1:84347af5a1f2 364 -0.499102324F, 0.274141163F, 0.022611469F, 0.128222346F, -0.607686698F,
julioefajardo 1:84347af5a1f2 365 0.48310256F, -0.593217313F, 0.536703944F, 0.2398123F, 0.150423944F,
julioefajardo 1:84347af5a1f2 366 -0.336881399F, -0.769578874F, -0.308003962F, 0.29553327F, -0.83044821F,
julioefajardo 1:84347af5a1f2 367 0.161142096F, -0.0343337208F, 0.581449211F, -0.873495698F, -0.28423211F,
julioefajardo 1:84347af5a1f2 368 -0.78573F, 0.383646518F, -0.753481686F, 0.452582896F, 0.226794824F,
julioefajardo 1:84347af5a1f2 369 0.375832409F, -0.498845309F, -0.258810967F, -0.499766558F, 0.404769868F,
julioefajardo 1:84347af5a1f2 370 -0.0505502634F, 0.461976469F, -0.389700621F, -0.713240385F, -0.481977165F,
julioefajardo 1:84347af5a1f2 371 -0.441101372F, 0.769881368F, 0.228649467F, 0.113842353F, -0.513090611F,
julioefajardo 1:84347af5a1f2 372 0.57655704F, 0.00201666565F, -0.0780342743F, -0.0782211572F, -0.263174862F,
julioefajardo 1:84347af5a1f2 373 -0.454693228F, -0.401941508F, -0.28711313F, -0.51956F, -0.413541466F,
julioefajardo 1:84347af5a1f2 374 -0.0222236887F, -0.825855196F, 0.663189113F, 0.648147404F, -0.120558769F,
julioefajardo 1:84347af5a1f2 375 -0.216713682F, -0.42545253F, 0.63155061F, -0.410026342F, -0.737376034F,
julioefajardo 1:84347af5a1f2 376 0.199930415F, -0.214675143F, -0.65693754F, -0.323603958F, -0.831128061F,
julioefajardo 1:84347af5a1f2 377 -0.548523843F, 0.398182899F, 0.475979269F, -0.111701392F, -0.558011353F,
julioefajardo 1:84347af5a1f2 378 -0.246838376F, -0.176599175F, 0.494953543F, -0.930735171F, -0.485517114F,
julioefajardo 1:84347af5a1f2 379 -0.660200715F, -0.151493624F, 0.239350051F, 0.50181222F, -0.0828042775F,
julioefajardo 1:84347af5a1f2 380 -0.0873984F, -0.215416417F, 0.192907035F, -0.3374F, 0.213343263F,
julioefajardo 1:84347af5a1f2 381 -0.400835454F, -0.206182152F, -0.0448735021F, 0.67194277F, -0.42755118F,
julioefajardo 1:84347af5a1f2 382 1.01745749F, 0.661765158F, -0.494972438F, 0.0940658301F, 0.00391307333F,
julioefajardo 1:84347af5a1f2 383 -0.407710791F, 0.106923632F, -0.206497744F, 0.754692852F, 0.176430732F,
julioefajardo 1:84347af5a1f2 384 0.385327101F, 0.0846962854F, 0.323739618F, -0.151615232F, 0.174422458F,
julioefajardo 1:84347af5a1f2 385 0.740742445F, 0.591877937F, 0.223434791F, 0.334855318F, 0.282185555F,
julioefajardo 1:84347af5a1f2 386 -0.214522555F, -0.0105024902F, 0.273585677F, 0.114028879F, 0.543323517F,
julioefajardo 1:84347af5a1f2 387 -0.291319072F, -0.432583898F, -0.821406F, -0.251906455F, -0.0459434502F,
julioefajardo 1:84347af5a1f2 388 -0.455817074F, 0.483560562F, -0.182349116F, -0.171598256F, 0.561632752F,
julioefajardo 1:84347af5a1f2 389 0.540451586F, 0.0060411403F, -0.653444648F, -0.236067981F, -0.182513028F,
julioefajardo 1:84347af5a1f2 390 0.202278212F, -0.370738387F, -0.0789217F, 0.572875738F, -0.231134444F,
julioefajardo 1:84347af5a1f2 391 -0.0330054797F, -0.0838245675F, -0.813296914F, 0.0739755705F, 0.118539378F,
julioefajardo 1:84347af5a1f2 392 -0.72018075F, -0.368218392F, 0.207165778F, -0.267799765F, 0.378820896F,
julioefajardo 1:84347af5a1f2 393 0.507289112F, 0.269455492F, 0.0232036524F, -0.524693549F, 0.325159252F,
julioefajardo 1:84347af5a1f2 394 0.773608327F, 0.137095451F, 0.178058892F, -0.351133883F, 0.132823557F,
julioefajardo 1:84347af5a1f2 395 0.34390527F, 0.495856106F, 0.183722347F, 0.328040898F, 0.716017127F,
julioefajardo 1:84347af5a1f2 396 -0.492127508F, 0.573495567F, 0.306710184F, 0.602395654F, -0.475191712F,
julioefajardo 1:84347af5a1f2 397 -0.550692558F, -0.337273896F, 0.232871071F, -0.414121956F, 0.338607788F,
julioefajardo 1:84347af5a1f2 398 -0.689573646F, 0.449012369F, 0.100024827F, 0.379222423F, 0.189025134F,
julioefajardo 1:84347af5a1f2 399 0.843987286F, 0.693846166F, -0.185747191F, 0.0946606621F, -0.453255206F,
julioefajardo 1:84347af5a1f2 400 -1.33637404F, 0.19069238F, 0.126079515F, -0.0326298513F, 0.788161218F,
julioefajardo 1:84347af5a1f2 401 0.135455295F, 0.336440802F, 0.425412089F, 0.102825224F, 0.63407582F,
julioefajardo 1:84347af5a1f2 402 -0.270895481F, -0.720058799F, 0.695959747F, -0.023164358F, -0.0242635924F,
julioefajardo 1:84347af5a1f2 403 -0.671196F, 0.554490209F, 0.190925688F, 0.272486627F, -0.0107945595F,
julioefajardo 1:84347af5a1f2 404 -0.742727757F, 0.13697502F, -0.775040329F, -0.71145767F, 0.26798436F,
julioefajardo 1:84347af5a1f2 405 -0.377285868F, 0.252919018F, 1.06671643F, -0.13240622F, -0.494904369F,
julioefajardo 1:84347af5a1f2 406 0.453439087F, 0.132181391F, 0.668464959F, 0.501227736F, 0.424547732F,
julioefajardo 1:84347af5a1f2 407 -0.280139863F, -0.019443851F, 0.542961895F, -0.512611568F, -0.559954345F,
julioefajardo 1:84347af5a1f2 408 -0.339779437F, 0.0962056518F, 0.247770652F, 0.970269084F, -0.098800607F,
julioefajardo 1:84347af5a1f2 409 0.0772119F, 0.165748924F, -0.425046772F, 0.372346252F, 0.561903596F,
julioefajardo 1:84347af5a1f2 410 -0.465825468F, -0.00290585565F, -0.882256508F, 1.38634479F, -0.292974889F,
julioefajardo 1:84347af5a1f2 411 -0.342612416F, 0.0741659775F, -0.297257066F, 0.423533231F, 0.0936212242F,
julioefajardo 1:84347af5a1f2 412 0.603145778F, 0.517324328F, -0.739661038F, 0.644301593F, -0.291442573F,
julioefajardo 1:84347af5a1f2 413 0.0685671866F, 0.0421827547F, 0.463616759F, 0.574609816F, 0.732411F,
julioefajardo 1:84347af5a1f2 414 -0.103880793F, 0.14776741F, 0.350388736F, 0.034289483F, 0.228362814F,
julioefajardo 1:84347af5a1f2 415 0.623231232F, -0.854294896F, -0.513297081F, 1.01049197F, -0.468677133F,
julioefajardo 1:84347af5a1f2 416 -0.89124316F, 0.180666521F, 0.618507206F, 0.34617874F, -0.478432477F,
julioefajardo 1:84347af5a1f2 417 -0.0531243756F, 0.204267785F, 0.472398907F, 0.0415514335F, 1.01171207F,
julioefajardo 1:84347af5a1f2 418 0.394830078F, -0.0686556473F, -0.852999389F, -0.0204684064F, 0.2879785F,
julioefajardo 1:84347af5a1f2 419 -0.70830971F, 0.301956505F, -0.581959546F, 0.608920038F, 0.334958166F,
julioefajardo 1:84347af5a1f2 420 -0.136173159F, -0.063511923F, 0.243027285F, 0.140252739F, 0.769615531F,
julioefajardo 1:84347af5a1f2 421 0.645489156F, 0.254678458F, -0.284347504F, 0.374404907F, -0.536988378F,
julioefajardo 1:84347af5a1f2 422 0.717072845F, 0.486760736F, 0.31468308F, 0.329237908F, 0.265202075F,
julioefajardo 1:84347af5a1f2 423 -0.460599929F, -0.273192674F, -0.18682602F, -0.533184588F, -0.433163792F,
julioefajardo 1:84347af5a1f2 424 0.153697655F, -0.971417725F, 0.0716149211F, 0.388068318F, 0.304798931F,
julioefajardo 1:84347af5a1f2 425 0.0493364856F, 0.525654316F, -0.734183729F, -0.418183059F, -0.391056359F,
julioefajardo 1:84347af5a1f2 426 0.349859178F, 0.295310169F, -0.162787989F, 0.986191332F, 0.893915057F,
julioefajardo 1:84347af5a1f2 427 0.468418807F, 0.227301195F, -0.893956F, -0.0392929763F, 0.547721744F,
julioefajardo 1:84347af5a1f2 428 0.277446F, 0.0684096217F, -0.817378163F, 0.294014931F, -0.789526939F,
julioefajardo 1:84347af5a1f2 429 -0.101080045F, 0.1247008F, 0.390003234F, 0.123767406F, 0.819431722F,
julioefajardo 1:84347af5a1f2 430 -0.075544F, 0.692092419F, -0.023473734F, 1.12595487F, -0.0727373213F,
julioefajardo 1:84347af5a1f2 431 0.185063079F, -0.127668485F, 0.0907674953F, 1.01107F, 0.103436872F,
julioefajardo 1:84347af5a1f2 432 -0.104357071F, -0.612592816F, 0.118782118F, 0.294045866F, -0.182440594F,
julioefajardo 1:84347af5a1f2 433 0.632002592F, 0.443847477F, -0.648681045F, 0.0449045971F, -0.132676274F,
julioefajardo 1:84347af5a1f2 434 0.650167644F, -0.520409822F, -0.00697870459F, -0.0560558848F, -0.0724578276F,
julioefajardo 1:84347af5a1f2 435 0.488136113F, -0.304092556F, 0.362224847F, 0.15520215F, -0.527926445F,
julioefajardo 1:84347af5a1f2 436 -0.353179634F, 0.369286567F, -0.05207555F, -0.510196149F, -0.129422769F,
julioefajardo 1:84347af5a1f2 437 0.190302268F, -0.544416964F, 0.247901723F, -0.173602447F, 0.65681529F,
julioefajardo 1:84347af5a1f2 438 -0.372931182F, 0.239432991F, -0.3728562F, -0.289556086F, -0.465267807F,
julioefajardo 1:84347af5a1f2 439 -0.0267323405F, 0.211196959F, -0.0809709206F, -0.211201906F, 0.0164073836F,
julioefajardo 1:84347af5a1f2 440 0.211801484F, 0.28273347F, 0.24367331F, 0.712016702F, -0.232072771F,
julioefajardo 1:84347af5a1f2 441 0.352156729F, -0.413937926F, -0.67757833F, -0.0740010217F, 0.0034886F,
julioefajardo 1:84347af5a1f2 442 0.10386055F, 0.491702259F, 0.419249F, -0.321603328F, 0.258051783F,
julioefajardo 1:84347af5a1f2 443 -0.0750256628F, 0.785783052F, 0.487906694F, -0.302251101F, 0.144713297F,
julioefajardo 1:84347af5a1f2 444 0.212649152F, 0.0338810794F, 0.489597201F, -0.141343772F, -0.133599311F,
julioefajardo 1:84347af5a1f2 445 -0.426484972F, 0.708699524F, 0.27392745F, 0.05124259F, 0.430232316F,
julioefajardo 1:84347af5a1f2 446 0.101622045F, 0.335402131F, -0.0293640438F, 0.446573496F, 0.0604294352F,
julioefajardo 1:84347af5a1f2 447 0.654577732F, -0.67903477F, -0.479712456F, -0.514000893F, -0.50363785F,
julioefajardo 1:84347af5a1f2 448 -0.260914654F, -0.113499187F, -0.363629967F, 0.381589234F, -0.00877389684F,
julioefajardo 1:84347af5a1f2 449 0.378615469F, 0.622865498F, 0.64521867F, -0.464941829F, -0.316285223F,
julioefajardo 1:84347af5a1f2 450 0.33551228F, -0.631272078F, -0.0607743897F, -0.189647585F, 0.527016699F,
julioefajardo 1:84347af5a1f2 451 0.154212683F, 0.116914578F, 0.183713183F, 0.346713036F, 0.0502914973F,
julioefajardo 1:84347af5a1f2 452 -0.170981154F, -0.47873193F, -0.103974499F, -0.296331912F, -0.682696223F,
julioefajardo 1:84347af5a1f2 453 0.418706745F, -0.702610731F, 0.113047369F, 0.11163982F, -0.219719663F,
julioefajardo 1:84347af5a1f2 454 -0.265369058F, -0.304860383F, 0.247510567F, 0.605953753F, -0.221037015F,
julioefajardo 1:84347af5a1f2 455 -0.479578376F, -0.364096701F, 0.232404232F, -0.923531771F, -0.607895613F,
julioefajardo 1:84347af5a1f2 456 0.379731268F, -0.63160032F, 0.704049F, -0.623289347F, 0.211085469F,
julioefajardo 1:84347af5a1f2 457 -0.201172546F, 0.496017128F, 0.111459091F, -0.216925532F, -0.456628412F,
julioefajardo 1:84347af5a1f2 458 0.264619768F, 0.478987068F, -0.304841787F, 0.143561065F, 0.12293458F,
julioefajardo 1:84347af5a1f2 459 0.41051206F, 0.242302909F, -0.755550683F, 0.268951803F, 0.669802606F,
julioefajardo 1:84347af5a1f2 460 -0.444868356F, 1.0796355F, 0.0833751F, -0.494852066F, -0.434321523F,
julioefajardo 1:84347af5a1f2 461 -0.0662497878F, 0.282435954F, 0.518994629F, 0.440311342F, -0.314596683F,
julioefajardo 1:84347af5a1f2 462 -0.00901326071F, -0.16180931F, -0.645285F, -0.405436337F, 0.182191074F,
julioefajardo 1:84347af5a1f2 463 0.295300335F, -0.322578758F, 0.196225166F, -0.502243817F, -0.786416531F,
julioefajardo 1:84347af5a1f2 464 0.523073316F, 0.118213534F, 0.408718884F, -0.063704811F, 0.104132973F,
julioefajardo 1:84347af5a1f2 465 0.582743F, 0.927500665F, 0.20945847F, -0.929123461F, 0.632320702F,
julioefajardo 1:84347af5a1f2 466 0.172647551F, 0.653148234F, -0.099445492F, -0.0184663199F, 0.563922763F,
julioefajardo 1:84347af5a1f2 467 -0.247307703F, 0.08971598F, 0.270614028F, 0.235931277F, 0.593309343F,
julioefajardo 1:84347af5a1f2 468 0.293943584F, 0.189385429F, -0.65996927F, -0.21901688F, -0.583070755F,
julioefajardo 1:84347af5a1f2 469 0.341855168F, 1.1340152F, 0.281593F, 0.575785697F, 0.676429689F, 0.459074F,
julioefajardo 1:84347af5a1f2 470 0.0225899909F, 0.656224668F, 0.132585585F, -0.743888676F, -0.614794552F,
julioefajardo 1:84347af5a1f2 471 1.03475749F, -0.0882811099F, 0.611714F, -0.473244578F, 0.369189858F,
julioefajardo 1:84347af5a1f2 472 0.362521559F, -0.681019485F, 0.300381541F, -0.209409952F, -0.610560894F,
julioefajardo 1:84347af5a1f2 473 -0.0188860334F, -0.641817153F, -0.807590604F, -0.465627879F, -0.365699679F,
julioefajardo 1:84347af5a1f2 474 -0.231352642F, -1.01078808F, -0.127937913F, -0.181259409F, 0.0717183352F,
julioefajardo 1:84347af5a1f2 475 0.24582836F, -0.144107684F, 0.0802998692F, 0.0219049808F, -0.386688113F,
julioefajardo 1:84347af5a1f2 476 0.0793484673F, 0.213585958F, 0.703600287F, -0.0976952165F, -0.3840639F,
julioefajardo 1:84347af5a1f2 477 0.157325551F, -0.0620508F, -0.593766451F, -0.427041501F, 0.962344646F,
julioefajardo 1:84347af5a1f2 478 0.325952917F, -0.481346279F, -0.883274317F, -0.448941231F, 0.281245708F,
julioefajardo 1:84347af5a1f2 479 0.416657F, -0.52693814F, 0.689424396F, 0.296314508F, 0.432173222F,
julioefajardo 1:84347af5a1f2 480 -0.828454316F, 0.726714194F, 0.523919344F, 0.516204655F, 0.442432255F,
julioefajardo 1:84347af5a1f2 481 0.45585233F, 0.376238436F, -0.461082667F, -0.151951969F, -0.449687511F,
julioefajardo 1:84347af5a1f2 482 -0.836503446F, 0.659184694F, -0.29375577F, -0.535523951F, -0.468601644F,
julioefajardo 1:84347af5a1f2 483 -0.293337F, 0.365438163F, -0.737874627F, 0.190893024F, -0.0833086F,
julioefajardo 1:84347af5a1f2 484 0.492872238F, -0.434281528F, -0.282535583F, -0.805321336F, -0.503223121F,
julioefajardo 1:84347af5a1f2 485 0.0819781274F, -0.249333F, -0.0285112094F, -0.352597207F, 0.724770725F,
julioefajardo 1:84347af5a1f2 486 0.15054968F, 0.319918931F, 0.62603879F, -0.160482243F, -0.16062668F,
julioefajardo 1:84347af5a1f2 487 -0.27161333F, 0.346072078F, 0.537718773F, -0.552904546F, -0.494646609F,
julioefajardo 1:84347af5a1f2 488 -0.336943179F, -0.669887364F, -0.00652140286F, -0.222761601F, -0.604594052F,
julioefajardo 1:84347af5a1f2 489 -0.723421872F, 0.733910799F, 0.463824034F, 0.26406005F, 0.419488341F,
julioefajardo 1:84347af5a1f2 490 -0.69037348F, 0.441993326F, 0.737772286F, 0.243240789F, -0.262164325F,
julioefajardo 1:84347af5a1f2 491 0.143315554F, -0.142399684F, -0.079456754F, -0.746082723F, 0.56054467F,
julioefajardo 1:84347af5a1f2 492 -0.576788604F, -0.162682116F, -0.661055088F, -0.0100884913F, -0.84846276F,
julioefajardo 1:84347af5a1f2 493 0.502910495F, -0.556128383F, -0.381545156F, -0.226393521F, -0.290395468F,
julioefajardo 1:84347af5a1f2 494 -0.638160288F, 0.405303419F, 0.45708254F, -0.231018662F, -0.600171506F,
julioefajardo 1:84347af5a1f2 495 -0.317634284F, -0.796222687F, -0.0962729752F, 0.302519232F, 0.227096081F,
julioefajardo 1:84347af5a1f2 496 -0.697582F, -0.609304726F, 0.478130311F, 0.589749038F, 0.0213635098F,
julioefajardo 1:84347af5a1f2 497 -0.726078153F, 0.357511342F, -0.27600044F, -0.282378614F, 0.232002884F,
julioefajardo 1:84347af5a1f2 498 -1.03095603F, -0.546820879F, 0.453093469F, 0.452624887F, -0.0799274892F,
julioefajardo 1:84347af5a1f2 499 0.28348428F, 0.36944288F, 0.815283835F, -0.270792842F, 0.45711121F,
julioefajardo 1:84347af5a1f2 500 0.0650191829F, -0.00474550668F, -0.173524231F, 0.111019306F, -0.22976172F,
julioefajardo 1:84347af5a1f2 501 0.324191928F, 0.480501801F, -0.596600354F, -0.44761616F, -0.116759337F,
julioefajardo 1:84347af5a1f2 502 -0.0663144886F, 0.528708041F, 0.85675323F, 0.423383653F, -0.833274245F,
julioefajardo 1:84347af5a1f2 503 -0.676730037F, -0.675398231F, 1.18571389F, 0.545059323F, 0.194837719F,
julioefajardo 1:84347af5a1f2 504 -0.571213663F, 0.231315166F, -0.689943135F, -0.517713F, 0.245689139F,
julioefajardo 1:84347af5a1f2 505 -0.36406526F, 0.159462124F, 0.377790302F, 0.15022406F, 0.203641087F,
julioefajardo 1:84347af5a1f2 506 -0.231097415F, 0.311318755F, 0.771822393F, 0.402989596F, -0.383682787F,
julioefajardo 1:84347af5a1f2 507 -0.434375495F, -0.156969324F, 0.271495759F, 0.100708269F, 0.0158944763F,
julioefajardo 1:84347af5a1f2 508 -0.182064518F, -0.46497798F, -0.53833F, 0.636102617F, -0.416234672F,
julioefajardo 1:84347af5a1f2 509 -0.0785247087F, -0.0448715724F, -0.431249201F, 0.146780565F, 0.031037325F,
julioefajardo 1:84347af5a1f2 510 -0.75488F, 0.38843447F, -0.295456231F, -0.127952173F, -0.481856853F,
julioefajardo 1:84347af5a1f2 511 0.130730167F, -0.694406748F, 0.0459047593F, -0.53592F, -0.676697969F,
julioefajardo 1:84347af5a1f2 512 0.458953559F, -0.456679523F, 0.348396122F, 0.657496393F, 0.0483298F,
julioefajardo 1:84347af5a1f2 513 0.143341869F, 0.715741456F, -0.366335869F, 0.218585283F, -0.794284F,
julioefajardo 1:84347af5a1f2 514 0.618138731F };
julioefajardo 1:84347af5a1f2 515
julioefajardo 1:84347af5a1f2 516 float x[45];
julioefajardo 1:84347af5a1f2 517 double c_b[4];
julioefajardo 1:84347af5a1f2 518 static const double b_a[4] = { 0.843573525672487, 0.52887395783527291,
julioefajardo 1:84347af5a1f2 519 0.67012100803296748, 0.49760939396991682 };
julioefajardo 1:84347af5a1f2 520
julioefajardo 1:84347af5a1f2 521 float numer[4];
julioefajardo 1:84347af5a1f2 522 static const float fv1[180] = { -0.576368749F, -0.461289287F, 0.359091967F,
julioefajardo 1:84347af5a1f2 523 0.156713441F, 1.09660339F, -0.57649368F, -0.868483543F, -0.701349378F,
julioefajardo 1:84347af5a1f2 524 0.437389016F, -0.0926702321F, 0.103165969F, -0.118973181F, -0.839078665F,
julioefajardo 1:84347af5a1f2 525 -0.364395678F, 0.698953509F, 0.193260536F, -0.757779062F, -0.571485519F,
julioefajardo 1:84347af5a1f2 526 -0.66037178F, -1.29905105F, -0.201243833F, 0.485816419F, 0.798221F,
julioefajardo 1:84347af5a1f2 527 -0.417164803F, -0.831456721F, 0.145439208F, -0.624888301F, -0.686319172F,
julioefajardo 1:84347af5a1f2 528 -1.01671267F, -0.952691317F, -0.346321493F, -0.474031448F, -0.466582507F,
julioefajardo 1:84347af5a1f2 529 -0.434489608F, 0.0789648071F, -0.972864866F, 0.628803194F, 0.82423836F,
julioefajardo 1:84347af5a1f2 530 0.392369598F, -1.1727767F, -0.51170367F, -0.79655838F, 0.487405777F,
julioefajardo 1:84347af5a1f2 531 0.640642822F, 1.03092039F, -0.718461812F, -0.919755638F, -0.072865434F,
julioefajardo 1:84347af5a1f2 532 1.09984982F, -0.727400839F, -0.270503879F, -0.476028502F, -0.577538967F,
julioefajardo 1:84347af5a1f2 533 0.310665369F, -0.786185086F, 1.33968735F, -0.261592031F, 0.159126773F,
julioefajardo 1:84347af5a1f2 534 -0.615301311F, -0.165091559F, -0.919524F, 0.589730442F, 0.887396574F,
julioefajardo 1:84347af5a1f2 535 -0.275334418F, 0.369588107F, -0.644339323F, 0.131432697F, 0.828792572F,
julioefajardo 1:84347af5a1f2 536 0.634907126F, -0.218690842F, -0.779631317F, -0.26734677F, -0.51404953F,
julioefajardo 1:84347af5a1f2 537 -0.538019836F, 0.493474483F, 0.724827945F, -0.0821629688F, 0.126185417F,
julioefajardo 1:84347af5a1f2 538 -0.86919862F, -1.1688292F, -0.788445175F, -0.743515372F, 1.08614969F,
julioefajardo 1:84347af5a1f2 539 0.882474661F, 0.943030596F, 0.0328377932F, -0.917530715F, 0.737649441F,
julioefajardo 1:84347af5a1f2 540 0.37147674F, 0.572349489F, 0.534060121F, -0.778717935F, 0.111810587F,
julioefajardo 1:84347af5a1f2 541 -0.219216034F, -0.667796552F, -0.313792109F, 0.438846558F, -0.316282123F,
julioefajardo 1:84347af5a1f2 542 -1.13036656F, 0.664084792F, 1.1458832F, -0.0636171848F, 0.464452535F,
julioefajardo 1:84347af5a1f2 543 -1.01223803F, 0.634164453F, 0.317785263F, 0.492384404F, -1.02305794F,
julioefajardo 1:84347af5a1f2 544 1.06032813F, 0.77377373F, -1.05769241F, -0.370299608F, -0.465102851F,
julioefajardo 1:84347af5a1f2 545 0.62622422F, -0.640314877F, -0.148292065F, -0.208885193F, -0.709490895F,
julioefajardo 1:84347af5a1f2 546 -1.15019488F, 0.829242766F, 0.120800421F, 0.694289267F, -0.382296205F,
julioefajardo 1:84347af5a1f2 547 0.90905267F, 1.55740833F, -0.13578631F, -0.692770541F, -0.589448631F,
julioefajardo 1:84347af5a1f2 548 0.397961974F, -0.523246586F, -0.814948142F, 0.564497709F, 0.0221325234F,
julioefajardo 1:84347af5a1f2 549 1.3606391F, -0.130946055F, -0.356777936F, -0.273988575F, 0.733114958F,
julioefajardo 1:84347af5a1f2 550 -0.371666F, 0.590279281F, 1.07341897F, 0.694256663F, -0.389202297F,
julioefajardo 1:84347af5a1f2 551 0.186797455F, 1.00938177F, -0.705201328F, -0.808631361F, 1.14770544F,
julioefajardo 1:84347af5a1f2 552 0.814412832F, -0.400106907F, 0.402769178F, -0.402200758F, 0.122468233F,
julioefajardo 1:84347af5a1f2 553 0.0504386574F, -0.452429116F, -0.146729171F, -0.967114568F, -0.130777434F,
julioefajardo 1:84347af5a1f2 554 -0.37407282F, 0.184210435F, -0.377413243F, 0.806694806F, 1.11887777F,
julioefajardo 1:84347af5a1f2 555 -0.513802588F, -0.530572236F, 0.836555302F, -0.410351485F, 0.752652407F,
julioefajardo 1:84347af5a1f2 556 -0.644252837F, -0.311244279F, -0.628207624F, 0.233606949F, -0.431879401F,
julioefajardo 1:84347af5a1f2 557 -0.998878F, 0.732248843F, -0.708933234F, 0.0570094101F, -0.292068034F,
julioefajardo 1:84347af5a1f2 558 1.09782875F, -1.47580659F };
julioefajardo 1:84347af5a1f2 559
julioefajardo 1:84347af5a1f2 560 boolean_T exitg1;
julioefajardo 1:84347af5a1f2 561 float fv2[1];
julioefajardo 1:84347af5a1f2 562 boolean_T b_x;
julioefajardo 1:84347af5a1f2 563
julioefajardo 1:84347af5a1f2 564 /* MYNEURALNETWORKFUNCTION neural network simulation function. */
julioefajardo 1:84347af5a1f2 565 /* */
julioefajardo 1:84347af5a1f2 566 /* Generated by Neural Network Toolbox function genFunction, 30-Oct-2015 21:46:01. */
julioefajardo 1:84347af5a1f2 567 /* */
julioefajardo 1:84347af5a1f2 568 /* [y1] = myNeuralNetworkFunction(x1) takes these arguments: */
julioefajardo 1:84347af5a1f2 569 /* x = 18xQ matrix, input #1 */
julioefajardo 1:84347af5a1f2 570 /* and returns: */
julioefajardo 1:84347af5a1f2 571 /* y = 4xQ matrix, output #1 */
julioefajardo 1:84347af5a1f2 572 /* where Q is the number of samples. */
julioefajardo 1:84347af5a1f2 573 /* ===== NEURAL NETWORK CONSTANTS ===== */
julioefajardo 1:84347af5a1f2 574 /* Input 1 */
julioefajardo 1:84347af5a1f2 575 /* Layer 1 */
julioefajardo 1:84347af5a1f2 576 /* Layer 2 */
julioefajardo 1:84347af5a1f2 577 /* ===== SIMULATION ======== */
julioefajardo 1:84347af5a1f2 578 /* Dimensions */
julioefajardo 1:84347af5a1f2 579 /* samples */
julioefajardo 1:84347af5a1f2 580 /* Input 1 */
julioefajardo 1:84347af5a1f2 581 /* ===== MODULE FUNCTIONS ======== */
julioefajardo 1:84347af5a1f2 582 /* Map Minimum and Maximum Input Processing Function */
julioefajardo 1:84347af5a1f2 583 for (ixstart = 0; ixstart < 18; ixstart++) {
julioefajardo 1:84347af5a1f2 584 xp1[ixstart] = (x1[ixstart] - (float)settings_xoffset[ixstart]) * (float)
julioefajardo 1:84347af5a1f2 585 settings_gain[ixstart] + -1.0F;
julioefajardo 1:84347af5a1f2 586 }
julioefajardo 1:84347af5a1f2 587
julioefajardo 1:84347af5a1f2 588 /* Layer 1 */
julioefajardo 1:84347af5a1f2 589 memcpy(&b[0], &a[0], 45U * sizeof(double));
julioefajardo 1:84347af5a1f2 590
julioefajardo 1:84347af5a1f2 591 /* Sigmoid Symmetric Transfer Function */
julioefajardo 1:84347af5a1f2 592 for (ix = 0; ix < 45; ix++) {
julioefajardo 1:84347af5a1f2 593 denom = 0.0F;
julioefajardo 1:84347af5a1f2 594 for (ixstart = 0; ixstart < 18; ixstart++) {
julioefajardo 1:84347af5a1f2 595 denom += fv0[ix + 45 * ixstart] * xp1[ixstart];
julioefajardo 1:84347af5a1f2 596 }
julioefajardo 1:84347af5a1f2 597
julioefajardo 1:84347af5a1f2 598 b_b[ix] = (float)b[ix] + denom;
julioefajardo 1:84347af5a1f2 599 }
julioefajardo 1:84347af5a1f2 600
julioefajardo 1:84347af5a1f2 601 for (ix = 0; ix < 45; ix++) {
julioefajardo 1:84347af5a1f2 602 x[ix] = (real32_T)exp(-2.0F * b_b[ix]);
julioefajardo 1:84347af5a1f2 603 }
julioefajardo 1:84347af5a1f2 604
julioefajardo 1:84347af5a1f2 605 /* Layer 2 */
julioefajardo 1:84347af5a1f2 606 for (ixstart = 0; ixstart < 4; ixstart++) {
julioefajardo 1:84347af5a1f2 607 c_b[ixstart] = b_a[ixstart];
julioefajardo 1:84347af5a1f2 608 }
julioefajardo 1:84347af5a1f2 609
julioefajardo 1:84347af5a1f2 610 for (ix = 0; ix < 45; ix++) {
julioefajardo 1:84347af5a1f2 611 b_b[ix] = 2.0F / (1.0F + x[ix]) - 1.0F;
julioefajardo 1:84347af5a1f2 612 }
julioefajardo 1:84347af5a1f2 613
julioefajardo 1:84347af5a1f2 614 for (ix = 0; ix < 4; ix++) {
julioefajardo 1:84347af5a1f2 615 denom = 0.0F;
julioefajardo 1:84347af5a1f2 616 for (ixstart = 0; ixstart < 45; ixstart++) {
julioefajardo 1:84347af5a1f2 617 denom += fv1[ix + (ixstart << 2)] * b_b[ixstart];
julioefajardo 1:84347af5a1f2 618 }
julioefajardo 1:84347af5a1f2 619
julioefajardo 1:84347af5a1f2 620 numer[ix] = (float)c_b[ix] + denom;
julioefajardo 1:84347af5a1f2 621 }
julioefajardo 1:84347af5a1f2 622
julioefajardo 1:84347af5a1f2 623 /* Competitive Soft Transfer Function */
julioefajardo 1:84347af5a1f2 624 ixstart = 1;
julioefajardo 1:84347af5a1f2 625 denom = numer[0];
julioefajardo 1:84347af5a1f2 626 if (rtIsNaNF(numer[0])) {
julioefajardo 1:84347af5a1f2 627 ix = 2;
julioefajardo 1:84347af5a1f2 628 exitg1 = false;
julioefajardo 1:84347af5a1f2 629 while ((!exitg1) && (ix < 5)) {
julioefajardo 1:84347af5a1f2 630 ixstart = ix;
julioefajardo 1:84347af5a1f2 631 if (!rtIsNaNF(numer[ix - 1])) {
julioefajardo 1:84347af5a1f2 632 denom = numer[ix - 1];
julioefajardo 1:84347af5a1f2 633 exitg1 = true;
julioefajardo 1:84347af5a1f2 634 } else {
julioefajardo 1:84347af5a1f2 635 ix++;
julioefajardo 1:84347af5a1f2 636 }
julioefajardo 1:84347af5a1f2 637 }
julioefajardo 1:84347af5a1f2 638 }
julioefajardo 1:84347af5a1f2 639
julioefajardo 1:84347af5a1f2 640 if (ixstart < 4) {
julioefajardo 1:84347af5a1f2 641 while (ixstart + 1 < 5) {
julioefajardo 1:84347af5a1f2 642 if (numer[ixstart] > denom) {
julioefajardo 1:84347af5a1f2 643 denom = numer[ixstart];
julioefajardo 1:84347af5a1f2 644 }
julioefajardo 1:84347af5a1f2 645
julioefajardo 1:84347af5a1f2 646 ixstart++;
julioefajardo 1:84347af5a1f2 647 }
julioefajardo 1:84347af5a1f2 648 }
julioefajardo 1:84347af5a1f2 649
julioefajardo 1:84347af5a1f2 650 for (ixstart = 0; ixstart < 4; ixstart++) {
julioefajardo 1:84347af5a1f2 651 numer[ixstart] = (real32_T)exp(numer[ixstart] - denom);
julioefajardo 1:84347af5a1f2 652 }
julioefajardo 1:84347af5a1f2 653
julioefajardo 1:84347af5a1f2 654 denom = numer[0];
julioefajardo 1:84347af5a1f2 655 for (ixstart = 0; ixstart < 3; ixstart++) {
julioefajardo 1:84347af5a1f2 656 denom += numer[ixstart + 1];
julioefajardo 1:84347af5a1f2 657 }
julioefajardo 1:84347af5a1f2 658
julioefajardo 1:84347af5a1f2 659 fv2[0] = denom;
julioefajardo 1:84347af5a1f2 660 b_x = (denom == 0.0F);
julioefajardo 1:84347af5a1f2 661 ixstart = 0;
julioefajardo 1:84347af5a1f2 662 if (b_x) {
julioefajardo 1:84347af5a1f2 663 ixstart = 1;
julioefajardo 1:84347af5a1f2 664 }
julioefajardo 1:84347af5a1f2 665
julioefajardo 1:84347af5a1f2 666 ix = 0;
julioefajardo 1:84347af5a1f2 667 while (ix <= ixstart - 1) {
julioefajardo 1:84347af5a1f2 668 fv2[0] = 1.0F;
julioefajardo 1:84347af5a1f2 669 ix = 1;
julioefajardo 1:84347af5a1f2 670 }
julioefajardo 1:84347af5a1f2 671
julioefajardo 1:84347af5a1f2 672 for (ixstart = 0; ixstart < 4; ixstart++) {
julioefajardo 1:84347af5a1f2 673 b_y1[ixstart] = numer[ixstart] / fv2[0];
julioefajardo 1:84347af5a1f2 674 }
julioefajardo 1:84347af5a1f2 675
julioefajardo 1:84347af5a1f2 676 /* Output 1 */
julioefajardo 1:84347af5a1f2 677 }
julioefajardo 1:84347af5a1f2 678
julioefajardo 1:84347af5a1f2 679 boolean_T rtIsNaNF(real32_T value){
julioefajardo 1:84347af5a1f2 680 #if defined(_MSC_VER) && (_MSC_VER <= 1200)
julioefajardo 1:84347af5a1f2 681 return _isnan((real_T)value)? true:false;
julioefajardo 1:84347af5a1f2 682 #else
julioefajardo 1:84347af5a1f2 683 return (value!=value)? 1U:0U;
julioefajardo 1:84347af5a1f2 684 #endif
julioefajardo 1:84347af5a1f2 685 }