Hybrid sEMG + IMU activated controller for Galileo Bionic Hand Prosthesis
Dependencies: FXAS21000 FXOS8700Q kalman mbed-dsp mbed-rtos mbed
main.cpp@1:84347af5a1f2, 2015-11-01 (annotated)
- 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?
User | Revision | Line number | New 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 | } |