diff --git a/conf/firmwares/subsystems/shared/nps.makefile b/conf/firmwares/subsystems/shared/nps.makefile index 6a3179b22f..7c2d005282 100644 --- a/conf/firmwares/subsystems/shared/nps.makefile +++ b/conf/firmwares/subsystems/shared/nps.makefile @@ -49,6 +49,8 @@ nps.srcs += \ $(NPSDIR)/nps_sensor_gps.c \ $(NPSDIR)/nps_sensor_airspeed.c \ $(NPSDIR)/nps_sensor_temperature.c \ + $(NPSDIR)/nps_sensor_aoa.c \ + $(NPSDIR)/nps_sensor_sideslip.c \ $(NPSDIR)/nps_electrical.c \ $(NPSDIR)/nps_atmosphere.c \ $(NPSDIR)/nps_ivy.c \ diff --git a/conf/modules/AOA_pwm.xml b/conf/modules/AOA_pwm.xml index 63797e9011..88cc2d8004 100644 --- a/conf/modules/AOA_pwm.xml +++ b/conf/modules/AOA_pwm.xml @@ -2,7 +2,15 @@ - Angle of Attack sensor using PWM input + + Angle of Attack sensor using PWM input + + Driver for a PWM based angle of attack sensor + A second sensor can be defined for the sideslip angle + It is assumed that both sensors are the same, only sensitivity, offset and direction can differ. + + Sensor example: US DIGITAL MA3-P12-125-B + @@ -12,12 +20,19 @@ + + + + + + + @@ -35,5 +50,9 @@ + + + + diff --git a/conf/modules/nps.xml b/conf/modules/nps.xml index c445e31bd2..e7b6ca0dff 100644 --- a/conf/modules/nps.xml +++ b/conf/modules/nps.xml @@ -55,6 +55,8 @@ + + diff --git a/conf/modules/wind_estimator.xml b/conf/modules/wind_estimator.xml new file mode 100644 index 0000000000..738e1e16a4 --- /dev/null +++ b/conf/modules/wind_estimator.xml @@ -0,0 +1,41 @@ + + + + + + Wind Estimator. + Using an UKF filter generated by MATLAB running in a CHibiOS thread + Requires: + - IMU for inertial data (rates and accel) + - GPS for ground speed vector + - magnetometer for true heading + - pitot for airspeed norm + - angle of attack probe (better and faster estimate of vertical component + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + diff --git a/conf/simulator/nps/nps_sensors_params_wind_estimator.h b/conf/simulator/nps/nps_sensors_params_wind_estimator.h new file mode 100644 index 0000000000..9638a758df --- /dev/null +++ b/conf/simulator/nps/nps_sensors_params_wind_estimator.h @@ -0,0 +1,171 @@ +/* + * Copyright (C) 2016 Gautier Hattenberger + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +#ifndef NPS_SENSORS_PARAMS_H +#define NPS_SENSORS_PARAMS_H + +#include "generated/airframe.h" +#include "subsystems/imu.h" + + +#define NPS_BODY_TO_IMU_PHI IMU_BODY_TO_IMU_PHI +#define NPS_BODY_TO_IMU_THETA IMU_BODY_TO_IMU_THETA +#define NPS_BODY_TO_IMU_PSI IMU_BODY_TO_IMU_PSI + +/* + * Accelerometer + */ +/* MPU60x0 has 16bit resolution */ +#define NPS_ACCEL_MIN -32767 +#define NPS_ACCEL_MAX 32767 +/* ms-2 */ +/* aka 2^10/ACCEL_X_SENS */ +#define NPS_ACCEL_SENSITIVITY_XX (IMU_ACCEL_X_SIGN * ACCEL_BFP_OF_REAL(1./IMU_ACCEL_X_SENS)) +#define NPS_ACCEL_SENSITIVITY_YY (IMU_ACCEL_Y_SIGN * ACCEL_BFP_OF_REAL(1./IMU_ACCEL_Y_SENS)) +#define NPS_ACCEL_SENSITIVITY_ZZ (IMU_ACCEL_Z_SIGN * ACCEL_BFP_OF_REAL(1./IMU_ACCEL_Z_SENS)) + +#define NPS_ACCEL_NEUTRAL_X IMU_ACCEL_X_NEUTRAL +#define NPS_ACCEL_NEUTRAL_Y IMU_ACCEL_Y_NEUTRAL +#define NPS_ACCEL_NEUTRAL_Z IMU_ACCEL_Z_NEUTRAL +/* m2s-4 */ +#define NPS_ACCEL_NOISE_STD_DEV_X 5.e-2 +#define NPS_ACCEL_NOISE_STD_DEV_Y 5.e-2 +#define NPS_ACCEL_NOISE_STD_DEV_Z 5.e-2 +/* ms-2 */ +#define NPS_ACCEL_BIAS_X 0 +#define NPS_ACCEL_BIAS_Y 0 +#define NPS_ACCEL_BIAS_Z 0 +/* s */ +#ifndef NPS_ACCEL_DT +#define NPS_ACCEL_DT (1./125.) +#endif + + + +/* + * Gyrometer + */ +/* MPU60x0 has 16 bit resolution */ +#define NPS_GYRO_MIN -32767 +#define NPS_GYRO_MAX 32767 + +/* 2^12/GYRO_X_SENS */ +#define NPS_GYRO_SENSITIVITY_PP (IMU_GYRO_P_SIGN * RATE_BFP_OF_REAL(1./IMU_GYRO_P_SENS)) +#define NPS_GYRO_SENSITIVITY_QQ (IMU_GYRO_Q_SIGN * RATE_BFP_OF_REAL(1./IMU_GYRO_Q_SENS)) +#define NPS_GYRO_SENSITIVITY_RR (IMU_GYRO_R_SIGN * RATE_BFP_OF_REAL(1./IMU_GYRO_R_SENS)) + +#define NPS_GYRO_NEUTRAL_P IMU_GYRO_P_NEUTRAL +#define NPS_GYRO_NEUTRAL_Q IMU_GYRO_Q_NEUTRAL +#define NPS_GYRO_NEUTRAL_R IMU_GYRO_R_NEUTRAL + +#define NPS_GYRO_NOISE_STD_DEV_P RadOfDeg(0.) +#define NPS_GYRO_NOISE_STD_DEV_Q RadOfDeg(0.) +#define NPS_GYRO_NOISE_STD_DEV_R RadOfDeg(0.) + +#define NPS_GYRO_BIAS_INITIAL_P RadOfDeg( 0.0) +#define NPS_GYRO_BIAS_INITIAL_Q RadOfDeg( 0.0) +#define NPS_GYRO_BIAS_INITIAL_R RadOfDeg( 0.0) + +#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_P RadOfDeg(0.5) +#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q RadOfDeg(0.5) +#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_R RadOfDeg(0.5) +/* s */ +#ifndef NPS_GYRO_DT +#define NPS_GYRO_DT (1./125.) +#endif + + + +/* + * Magnetometer + */ + /* HMC5843 has 12 bit resolution */ +#define NPS_MAG_MIN -2047 +#define NPS_MAG_MAX 2047 + +#define NPS_MAG_IMU_TO_SENSOR_PHI 0. +#define NPS_MAG_IMU_TO_SENSOR_THETA 0. +#define NPS_MAG_IMU_TO_SENSOR_PSI 0. + +#define NPS_MAG_SENSITIVITY_XX (IMU_MAG_X_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_X_SENS)) +#define NPS_MAG_SENSITIVITY_YY (IMU_MAG_Y_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_Y_SENS)) +#define NPS_MAG_SENSITIVITY_ZZ (IMU_MAG_Z_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_Z_SENS)) + +#define NPS_MAG_NEUTRAL_X IMU_MAG_X_NEUTRAL +#define NPS_MAG_NEUTRAL_Y IMU_MAG_Y_NEUTRAL +#define NPS_MAG_NEUTRAL_Z IMU_MAG_Z_NEUTRAL + +#define NPS_MAG_NOISE_STD_DEV_X 2e-3 +#define NPS_MAG_NOISE_STD_DEV_Y 2e-3 +#define NPS_MAG_NOISE_STD_DEV_Z 2e-3 + +#define NPS_MAG_DT (1./60.) + + +/* + * Barometer + */ +/* m */ +/* aka 2^8/INS_BARO_SENS */ +#define NPS_BARO_QNH 1013.25 +#define NPS_BARO_SENSITIVITY 4.0 +#define NPS_BARO_DT (1./5.) +#define NPS_BARO_NOISE_STD_DEV 5.e-2 + +/* + * GPS + */ + +#ifndef GPS_PERFECT +#define GPS_PERFECT 1 +#endif + +#if GPS_PERFECT + +#define NPS_GPS_SPEED_NOISE_STD_DEV 0. +#define NPS_GPS_SPEED_LATENCY 0. +#define NPS_GPS_POS_NOISE_STD_DEV 0.001 +#define NPS_GPS_POS_BIAS_INITIAL_X 0. +#define NPS_GPS_POS_BIAS_INITIAL_Y 0. +#define NPS_GPS_POS_BIAS_INITIAL_Z 0. +#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X 0. +#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y 0. +#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z 0. +#define NPS_GPS_POS_LATENCY 0. + +#else + +#define NPS_GPS_SPEED_NOISE_STD_DEV 0.5 +#define NPS_GPS_SPEED_LATENCY 0.2 +#define NPS_GPS_POS_NOISE_STD_DEV 2 +#define NPS_GPS_POS_BIAS_INITIAL_X 0e-1 +#define NPS_GPS_POS_BIAS_INITIAL_Y -0e-1 +#define NPS_GPS_POS_BIAS_INITIAL_Z -0e-1 +#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X 1e-3 +#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y 1e-3 +#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z 1e-3 +#define NPS_GPS_POS_LATENCY 0.2 + +#endif /* GPS_PERFECT */ + +#define NPS_GPS_DT (1./5.) + +#endif /* NPS_SENSORS_PARAMS_H */ diff --git a/conf/telemetry/fixedwing_flight_recorder.xml b/conf/telemetry/fixedwing_flight_recorder.xml index 0f6de6cc72..cbd7da7005 100644 --- a/conf/telemetry/fixedwing_flight_recorder.xml +++ b/conf/telemetry/fixedwing_flight_recorder.xml @@ -137,6 +137,7 @@ + diff --git a/sw/airborne/boards/apogee/chibios/v1.0/board.h b/sw/airborne/boards/apogee/chibios/v1.0/board.h index 9585626f53..5814edd991 100644 --- a/sw/airborne/boards/apogee/chibios/v1.0/board.h +++ b/sw/airborne/boards/apogee/chibios/v1.0/board.h @@ -932,7 +932,7 @@ #if (USE_PWM1 && USE_PWM_INPUT2) #error "PW1 and PWM_INPUT2 are not compatible" #endif -#define PWM_INPUT2_ICU ICUD2 +#define PWM_INPUT2_ICU ICUD9 #define PWM_INPUT2_CHANNEL ICU_CHANNEL_1 #define PWM_INPUT2_GPIO_PORT GPIOA #define PWM_INPUT2_GPIO_PIN GPIO2 diff --git a/sw/airborne/boards/apogee/chibios/v1.0/mcuconf.h b/sw/airborne/boards/apogee/chibios/v1.0/mcuconf.h index d32c85afc8..a3b7ad8acb 100644 --- a/sw/airborne/boards/apogee/chibios/v1.0/mcuconf.h +++ b/sw/airborne/boards/apogee/chibios/v1.0/mcuconf.h @@ -194,11 +194,7 @@ * ICU driver system settings. */ #define STM32_ICU_USE_TIM1 TRUE -#ifdef USE_PWM_INPUT2 -#define STM32_ICU_USE_TIM2 TRUE -#else #define STM32_ICU_USE_TIM2 FALSE -#endif #define STM32_ICU_USE_TIM3 FALSE #define STM32_ICU_USE_TIM4 FALSE #define STM32_ICU_USE_TIM5 FALSE diff --git a/sw/airborne/modules/meteo/lib_ukf_wind_estimator/UKF_Wind_Estimator.c b/sw/airborne/modules/meteo/lib_ukf_wind_estimator/UKF_Wind_Estimator.c new file mode 100644 index 0000000000..ddaf173f24 --- /dev/null +++ b/sw/airborne/modules/meteo/lib_ukf_wind_estimator/UKF_Wind_Estimator.c @@ -0,0 +1,2107 @@ +/* + * File: UKF_Wind_Estimator.c + * + * Code generated for Simulink model 'UKF_Wind_Estimator'. + * + * Model version : 1.120 + * Simulink Coder version : 8.10 (R2016a) 10-Feb-2016 + * C/C++ source code generated on : Wed Nov 2 23:49:42 2016 + * + * Target selection: ert.tlc + * Embedded hardware selection: Custom Processor->Custom + * Code generation objectives: + * 1. Execution efficiency + * 2. RAM efficiency + * Validation result: Not run + */ + +#include "UKF_Wind_Estimator.h" + +/* Exported data definition */ + +/* Definition for custom storage class: Struct */ +ukf_init_type ukf_init; +ukf_params_type ukf_params; + +/* Block signals and states (auto storage) */ +DW ukf_DW; + +/* External inputs (root inport signals with auto storage) */ +ExtU ukf_U; + +/* External outputs (root outports fed by signals with auto storage) */ +ExtY ukf_Y; + +/* Forward declaration for local functions */ +static real32_T xnrm2_f(int32_T n, const real32_T x[147], int32_T ix0); +static void xgeqrf_f(real32_T A[147], real32_T tau[7]); +static void qr_e(const real32_T A[147], real32_T Q[147], real32_T R[49]); +static real32_T xnrm2(int32_T n, const real32_T x[120], int32_T ix0); +static void xgeqrf(real32_T A[120], real32_T tau[6]); +static void qr(const real32_T A[120], real32_T Q[120], real32_T R[36]); +static void mrdivide(real32_T A[42], const real32_T B_0[36]); +static void h(const real32_T x[7], const real32_T q[4], real32_T y[6]); +static void RungeKutta(const real32_T x[7], real32_T dt, const real32_T u[6], + real32_T xi[7]); + +/* Function for MATLAB Function: '/UKF_prediction' */ +static real32_T xnrm2_f(int32_T n, const real32_T x[147], int32_T ix0) +{ + real32_T y; + real32_T scale; + int32_T kend; + real32_T absxk; + real32_T t; + int32_T k; + y = 0.0F; + if (!(n < 1)) { + if (n == 1) { + y = fabsf(x[ix0 - 1]); + } else { + scale = 1.17549435E-38F; + kend = (ix0 + n) - 1; + for (k = ix0; k <= kend; k++) { + absxk = fabsf(x[k - 1]); + if (absxk > scale) { + t = scale / absxk; + y = y * t * t + 1.0F; + scale = absxk; + } else { + t = absxk / scale; + y += t * t; + } + } + + y = scale * sqrtf(y); + } + } + + return y; +} + +/* Function for MATLAB Function: '/UKF_prediction' */ +static void xgeqrf_f(real32_T A[147], real32_T tau[7]) +{ + real32_T work[7]; + int32_T i_i; + real32_T b_atmp; + real32_T xnorm; + int32_T knt; + int32_T lastv; + int32_T lastc; + int32_T ix; + real32_T b_c; + int32_T iy; + int32_T f; + int32_T g; + int32_T b_ia; + int32_T b_ix; + boolean_T exitg2; + int32_T i; + for (i = 0; i < 7; i++) { + work[i] = 0.0F; + } + + for (i = 0; i < 7; i++) { + i_i = i * 21 + i; + b_atmp = A[i_i]; + b_c = 0.0F; + xnorm = xnrm2_f(20 - i, A, i_i + 2); + if (xnorm != 0.0F) { + xnorm = hypotf(A[i_i], xnorm); + if (A[i_i] >= 0.0F) { + xnorm = -xnorm; + } + + if (fabsf(xnorm) < 9.86076132E-32F) { + knt = 0; + do { + knt++; + lastv = (i_i - i) + 21; + for (lastc = i_i + 1; lastc + 1 <= lastv; lastc++) { + A[lastc] *= 1.01412048E+31F; + } + + xnorm *= 1.01412048E+31F; + b_atmp *= 1.01412048E+31F; + } while (!(fabsf(xnorm) >= 9.86076132E-32F)); + + xnorm = hypotf(b_atmp, xnrm2_f(20 - i, A, i_i + 2)); + if (b_atmp >= 0.0F) { + xnorm = -xnorm; + } + + b_c = (xnorm - b_atmp) / xnorm; + b_atmp = 1.0F / (b_atmp - xnorm); + lastv = (i_i - i) + 21; + for (lastc = i_i + 1; lastc + 1 <= lastv; lastc++) { + A[lastc] *= b_atmp; + } + + for (lastv = 1; lastv <= knt; lastv++) { + xnorm *= 9.86076132E-32F; + } + + b_atmp = xnorm; + } else { + b_c = (xnorm - A[i_i]) / xnorm; + b_atmp = 1.0F / (A[i_i] - xnorm); + knt = (i_i - i) + 21; + for (lastv = i_i + 1; lastv + 1 <= knt; lastv++) { + A[lastv] *= b_atmp; + } + + b_atmp = xnorm; + } + } + + tau[i] = b_c; + A[i_i] = b_atmp; + if (i + 1 < 7) { + b_atmp = A[i_i]; + A[i_i] = 1.0F; + knt = (i + 1) * 21 + i; + if (tau[i] != 0.0F) { + lastv = 21 - i; + lastc = (i_i - i) + 20; + while ((lastv > 0) && (A[lastc] == 0.0F)) { + lastv--; + lastc--; + } + + lastc = 6 - i; + exitg2 = false; + while ((!exitg2) && (lastc > 0)) { + iy = ((lastc - 1) * 21 + knt) + 1; + f = iy; + do { + b_ix = 0; + if (f <= (iy + lastv) - 1) { + if (A[f - 1] != 0.0F) { + b_ix = 1; + } else { + f++; + } + } else { + lastc--; + b_ix = 2; + } + } while (b_ix == 0); + + if (b_ix == 1) { + exitg2 = true; + } + } + } else { + lastv = 0; + lastc = 0; + } + + if (lastv > 0) { + if (lastc != 0) { + for (iy = 1; iy <= lastc; iy++) { + work[iy - 1] = 0.0F; + } + + iy = 0; + f = ((lastc - 1) * 21 + knt) + 1; + for (b_ix = knt + 1; b_ix <= f; b_ix += 21) { + ix = i_i; + b_c = 0.0F; + g = (b_ix + lastv) - 1; + for (b_ia = b_ix; b_ia <= g; b_ia++) { + b_c += A[b_ia - 1] * A[ix]; + ix++; + } + + work[iy] += b_c; + iy++; + } + } + + if (!(-tau[i] == 0.0F)) { + iy = 0; + for (f = 1; f <= lastc; f++) { + if (work[iy] != 0.0F) { + b_c = work[iy] * -tau[i]; + b_ix = i_i; + ix = lastv + knt; + for (g = knt; g + 1 <= ix; g++) { + A[g] += A[b_ix] * b_c; + b_ix++; + } + } + + iy++; + knt += 21; + } + } + } + + A[i_i] = b_atmp; + } + } +} + +/* Function for MATLAB Function: '/UKF_prediction' */ +static void qr_e(const real32_T A[147], real32_T Q[147], real32_T R[49]) +{ + real32_T tau[7]; + int32_T c_i; + int32_T itau; + real32_T work[7]; + int32_T iaii; + int32_T lastv; + int32_T lastc; + int32_T ix; + real32_T c; + int32_T iy; + int32_T iac; + int32_T d; + int32_T b_ia; + int32_T jy; + boolean_T exitg2; + memcpy(&Q[0], &A[0], 147U * sizeof(real32_T)); + xgeqrf_f(Q, tau); + for (itau = 0; itau < 7; itau++) { + for (c_i = 0; c_i + 1 <= itau + 1; c_i++) { + R[c_i + 7 * itau] = Q[21 * itau + c_i]; + } + + for (c_i = itau + 1; c_i + 1 < 8; c_i++) { + R[c_i + 7 * itau] = 0.0F; + } + + work[itau] = 0.0F; + } + + itau = 6; + for (c_i = 6; c_i >= 0; c_i += -1) { + iaii = (c_i * 21 + c_i) + 1; + if (c_i + 1 < 7) { + Q[iaii - 1] = 1.0F; + if (tau[itau] != 0.0F) { + lastv = 21 - c_i; + lastc = (iaii - c_i) + 19; + while ((lastv > 0) && (Q[lastc] == 0.0F)) { + lastv--; + lastc--; + } + + lastc = 6 - c_i; + exitg2 = false; + while ((!exitg2) && (lastc > 0)) { + iy = (lastc - 1) * 21 + iaii; + jy = iy; + do { + iac = 0; + if (jy + 21 <= (iy + lastv) + 20) { + if (Q[jy + 20] != 0.0F) { + iac = 1; + } else { + jy++; + } + } else { + lastc--; + iac = 2; + } + } while (iac == 0); + + if (iac == 1) { + exitg2 = true; + } + } + } else { + lastv = 0; + lastc = 0; + } + + if (lastv > 0) { + if (lastc != 0) { + for (iy = 1; iy <= lastc; iy++) { + work[iy - 1] = 0.0F; + } + + iy = 0; + jy = ((lastc - 1) * 21 + iaii) + 21; + for (iac = iaii + 21; iac <= jy; iac += 21) { + ix = iaii; + c = 0.0F; + d = (iac + lastv) - 1; + for (b_ia = iac; b_ia <= d; b_ia++) { + c += Q[b_ia - 1] * Q[ix - 1]; + ix++; + } + + work[iy] += c; + iy++; + } + } + + if (!(-tau[itau] == 0.0F)) { + iy = iaii + 20; + jy = 0; + for (iac = 1; iac <= lastc; iac++) { + if (work[jy] != 0.0F) { + c = work[jy] * -tau[itau]; + ix = iaii; + d = lastv + iy; + for (b_ia = iy; b_ia + 1 <= d; b_ia++) { + Q[b_ia] += Q[ix - 1] * c; + ix++; + } + } + + jy++; + iy += 21; + } + } + } + } + + lastv = (iaii - c_i) + 20; + for (lastc = iaii; lastc + 1 <= lastv; lastc++) { + Q[lastc] *= -tau[itau]; + } + + Q[iaii - 1] = 1.0F - tau[itau]; + for (lastv = 1; lastv <= c_i; lastv++) { + Q[(iaii - lastv) - 1] = 0.0F; + } + + itau--; + } +} + +/* Function for MATLAB Function: '/UKF_correction' */ +static real32_T xnrm2(int32_T n, const real32_T x[120], int32_T ix0) +{ + real32_T y; + real32_T scale; + int32_T kend; + real32_T absxk; + real32_T t; + int32_T k; + y = 0.0F; + if (!(n < 1)) { + if (n == 1) { + y = fabsf(x[ix0 - 1]); + } else { + scale = 1.17549435E-38F; + kend = (ix0 + n) - 1; + for (k = ix0; k <= kend; k++) { + absxk = fabsf(x[k - 1]); + if (absxk > scale) { + t = scale / absxk; + y = y * t * t + 1.0F; + scale = absxk; + } else { + t = absxk / scale; + y += t * t; + } + } + + y = scale * sqrtf(y); + } + } + + return y; +} + +/* Function for MATLAB Function: '/UKF_correction' */ +static void xgeqrf(real32_T A[120], real32_T tau[6]) +{ + real32_T work[6]; + int32_T i_i; + real32_T b_atmp; + real32_T xnorm; + int32_T knt; + int32_T lastv; + int32_T lastc; + int32_T ix; + real32_T b_c; + int32_T iy; + int32_T f; + int32_T g; + int32_T b_ia; + int32_T b_ix; + boolean_T exitg2; + int32_T i; + for (i = 0; i < 6; i++) { + work[i] = 0.0F; + } + + for (i = 0; i < 6; i++) { + i_i = i * 20 + i; + b_atmp = A[i_i]; + b_c = 0.0F; + xnorm = xnrm2(19 - i, A, i_i + 2); + if (xnorm != 0.0F) { + xnorm = hypotf(A[i_i], xnorm); + if (A[i_i] >= 0.0F) { + xnorm = -xnorm; + } + + if (fabsf(xnorm) < 9.86076132E-32F) { + knt = 0; + do { + knt++; + lastv = (i_i - i) + 20; + for (lastc = i_i + 1; lastc + 1 <= lastv; lastc++) { + A[lastc] *= 1.01412048E+31F; + } + + xnorm *= 1.01412048E+31F; + b_atmp *= 1.01412048E+31F; + } while (!(fabsf(xnorm) >= 9.86076132E-32F)); + + xnorm = hypotf(b_atmp, xnrm2(19 - i, A, i_i + 2)); + if (b_atmp >= 0.0F) { + xnorm = -xnorm; + } + + b_c = (xnorm - b_atmp) / xnorm; + b_atmp = 1.0F / (b_atmp - xnorm); + lastv = (i_i - i) + 20; + for (lastc = i_i + 1; lastc + 1 <= lastv; lastc++) { + A[lastc] *= b_atmp; + } + + for (lastv = 1; lastv <= knt; lastv++) { + xnorm *= 9.86076132E-32F; + } + + b_atmp = xnorm; + } else { + b_c = (xnorm - A[i_i]) / xnorm; + b_atmp = 1.0F / (A[i_i] - xnorm); + knt = (i_i - i) + 20; + for (lastv = i_i + 1; lastv + 1 <= knt; lastv++) { + A[lastv] *= b_atmp; + } + + b_atmp = xnorm; + } + } + + tau[i] = b_c; + A[i_i] = b_atmp; + if (i + 1 < 6) { + b_atmp = A[i_i]; + A[i_i] = 1.0F; + knt = (i + 1) * 20 + i; + if (tau[i] != 0.0F) { + lastv = 20 - i; + lastc = (i_i - i) + 19; + while ((lastv > 0) && (A[lastc] == 0.0F)) { + lastv--; + lastc--; + } + + lastc = 5 - i; + exitg2 = false; + while ((!exitg2) && (lastc > 0)) { + iy = ((lastc - 1) * 20 + knt) + 1; + f = iy; + do { + b_ix = 0; + if (f <= (iy + lastv) - 1) { + if (A[f - 1] != 0.0F) { + b_ix = 1; + } else { + f++; + } + } else { + lastc--; + b_ix = 2; + } + } while (b_ix == 0); + + if (b_ix == 1) { + exitg2 = true; + } + } + } else { + lastv = 0; + lastc = 0; + } + + if (lastv > 0) { + if (lastc != 0) { + for (iy = 1; iy <= lastc; iy++) { + work[iy - 1] = 0.0F; + } + + iy = 0; + f = ((lastc - 1) * 20 + knt) + 1; + for (b_ix = knt + 1; b_ix <= f; b_ix += 20) { + ix = i_i; + b_c = 0.0F; + g = (b_ix + lastv) - 1; + for (b_ia = b_ix; b_ia <= g; b_ia++) { + b_c += A[b_ia - 1] * A[ix]; + ix++; + } + + work[iy] += b_c; + iy++; + } + } + + if (!(-tau[i] == 0.0F)) { + iy = 0; + for (f = 1; f <= lastc; f++) { + if (work[iy] != 0.0F) { + b_c = work[iy] * -tau[i]; + b_ix = i_i; + ix = lastv + knt; + for (g = knt; g + 1 <= ix; g++) { + A[g] += A[b_ix] * b_c; + b_ix++; + } + } + + iy++; + knt += 20; + } + } + } + + A[i_i] = b_atmp; + } + } +} + +/* Function for MATLAB Function: '/UKF_correction' */ +static void qr(const real32_T A[120], real32_T Q[120], real32_T R[36]) +{ + real32_T tau[6]; + int32_T c_i; + int32_T itau; + real32_T work[6]; + int32_T iaii; + int32_T lastv; + int32_T lastc; + int32_T ix; + real32_T c; + int32_T iy; + int32_T iac; + int32_T d; + int32_T b_ia; + int32_T jy; + boolean_T exitg2; + memcpy(&Q[0], &A[0], 120U * sizeof(real32_T)); + xgeqrf(Q, tau); + for (itau = 0; itau < 6; itau++) { + for (c_i = 0; c_i + 1 <= itau + 1; c_i++) { + R[c_i + 6 * itau] = Q[20 * itau + c_i]; + } + + for (c_i = itau + 1; c_i + 1 < 7; c_i++) { + R[c_i + 6 * itau] = 0.0F; + } + + work[itau] = 0.0F; + } + + itau = 5; + for (c_i = 5; c_i >= 0; c_i += -1) { + iaii = (c_i * 20 + c_i) + 1; + if (c_i + 1 < 6) { + Q[iaii - 1] = 1.0F; + if (tau[itau] != 0.0F) { + lastv = 20 - c_i; + lastc = (iaii - c_i) + 18; + while ((lastv > 0) && (Q[lastc] == 0.0F)) { + lastv--; + lastc--; + } + + lastc = 5 - c_i; + exitg2 = false; + while ((!exitg2) && (lastc > 0)) { + iy = (lastc - 1) * 20 + iaii; + jy = iy; + do { + iac = 0; + if (jy + 20 <= (iy + lastv) + 19) { + if (Q[jy + 19] != 0.0F) { + iac = 1; + } else { + jy++; + } + } else { + lastc--; + iac = 2; + } + } while (iac == 0); + + if (iac == 1) { + exitg2 = true; + } + } + } else { + lastv = 0; + lastc = 0; + } + + if (lastv > 0) { + if (lastc != 0) { + for (iy = 1; iy <= lastc; iy++) { + work[iy - 1] = 0.0F; + } + + iy = 0; + jy = ((lastc - 1) * 20 + iaii) + 20; + for (iac = iaii + 20; iac <= jy; iac += 20) { + ix = iaii; + c = 0.0F; + d = (iac + lastv) - 1; + for (b_ia = iac; b_ia <= d; b_ia++) { + c += Q[b_ia - 1] * Q[ix - 1]; + ix++; + } + + work[iy] += c; + iy++; + } + } + + if (!(-tau[itau] == 0.0F)) { + iy = iaii + 19; + jy = 0; + for (iac = 1; iac <= lastc; iac++) { + if (work[jy] != 0.0F) { + c = work[jy] * -tau[itau]; + ix = iaii; + d = lastv + iy; + for (b_ia = iy; b_ia + 1 <= d; b_ia++) { + Q[b_ia] += Q[ix - 1] * c; + ix++; + } + } + + jy++; + iy += 20; + } + } + } + } + + lastv = (iaii - c_i) + 19; + for (lastc = iaii; lastc + 1 <= lastv; lastc++) { + Q[lastc] *= -tau[itau]; + } + + Q[iaii - 1] = 1.0F - tau[itau]; + for (lastv = 1; lastv <= c_i; lastv++) { + Q[(iaii - lastv) - 1] = 0.0F; + } + + itau--; + } +} + +/* Function for MATLAB Function: '/main' */ +static void mrdivide(real32_T A[42], const real32_T B_0[36]) +{ + int32_T jp; + real32_T temp; + real32_T b_A[36]; + int8_T ipiv[6]; + int32_T j; + int32_T ix; + real32_T s; + int32_T c_ix; + int32_T d; + int32_T ijA; + int32_T b_jAcol; + int32_T b_kBcol; + memcpy(&b_A[0], &B_0[0], 36U * sizeof(real32_T)); + for (j = 0; j < 6; j++) { + ipiv[j] = (int8_T)(1 + j); + } + + for (j = 0; j < 5; j++) { + jp = j * 7; + b_jAcol = 0; + ix = jp; + temp = fabsf(b_A[jp]); + for (b_kBcol = 2; b_kBcol <= 6 - j; b_kBcol++) { + ix++; + s = fabsf(b_A[ix]); + if (s > temp) { + b_jAcol = b_kBcol - 1; + temp = s; + } + } + + if (b_A[jp + b_jAcol] != 0.0F) { + if (b_jAcol != 0) { + ipiv[j] = (int8_T)((j + b_jAcol) + 1); + ix = j; + b_jAcol += j; + for (b_kBcol = 0; b_kBcol < 6; b_kBcol++) { + temp = b_A[ix]; + b_A[ix] = b_A[b_jAcol]; + b_A[b_jAcol] = temp; + ix += 6; + b_jAcol += 6; + } + } + + b_jAcol = (jp - j) + 6; + for (ix = jp + 1; ix + 1 <= b_jAcol; ix++) { + b_A[ix] /= b_A[jp]; + } + } + + b_jAcol = jp; + ix = jp + 6; + for (b_kBcol = 1; b_kBcol <= 5 - j; b_kBcol++) { + temp = b_A[ix]; + if (b_A[ix] != 0.0F) { + c_ix = jp + 1; + d = (b_jAcol - j) + 12; + for (ijA = 7 + b_jAcol; ijA + 1 <= d; ijA++) { + b_A[ijA] += b_A[c_ix] * -temp; + c_ix++; + } + } + + ix += 6; + b_jAcol += 6; + } + } + + for (j = 0; j < 6; j++) { + jp = 7 * j; + b_jAcol = 6 * j; + for (ix = 1; ix <= j; ix++) { + b_kBcol = (ix - 1) * 7; + if (b_A[(ix + b_jAcol) - 1] != 0.0F) { + for (c_ix = 0; c_ix < 7; c_ix++) { + A[c_ix + jp] -= b_A[(ix + b_jAcol) - 1] * A[c_ix + b_kBcol]; + } + } + } + + temp = 1.0F / b_A[j + b_jAcol]; + for (b_jAcol = 0; b_jAcol < 7; b_jAcol++) { + A[b_jAcol + jp] *= temp; + } + } + + for (j = 5; j >= 0; j += -1) { + jp = 7 * j; + b_jAcol = 6 * j - 1; + for (ix = j + 2; ix < 7; ix++) { + b_kBcol = (ix - 1) * 7; + if (b_A[ix + b_jAcol] != 0.0F) { + for (c_ix = 0; c_ix < 7; c_ix++) { + A[c_ix + jp] -= b_A[ix + b_jAcol] * A[c_ix + b_kBcol]; + } + } + } + } + + for (j = 4; j >= 0; j += -1) { + if (j + 1 != ipiv[j]) { + jp = ipiv[j] - 1; + for (b_jAcol = 0; b_jAcol < 7; b_jAcol++) { + temp = A[7 * j + b_jAcol]; + A[b_jAcol + 7 * j] = A[7 * jp + b_jAcol]; + A[b_jAcol + 7 * jp] = temp; + } + } + } +} + +/* Function for MATLAB Function: '/UKF_correction' */ +static void h(const real32_T x[7], const real32_T q[4], real32_T y[6]) +{ + real32_T Vair; + real32_T beta; + real32_T scale; + real32_T t; + real32_T qinter_idx_0; + real32_T qinv_idx_1; + real32_T qinv_idx_2; + real32_T qinv_idx_3; + real32_T qinter_idx_2; + real32_T qinter_idx_3; + real32_T qinter_idx_1; + + /* -----------------Observation_model------------------- */ + /* description : */ + /* */ + /* Dim of the measurement vector dim(zk) = 10; */ + /* y=[y1 y2 y3 y4 y5 y6 y7 y8 y9 y10] */ + /* y=[V1 V2 V3 P1 P2 P3 B1 B2 B3 YH ] */ + /* */ + /* Inputs : */ + /* -States: x [15 x 1] */ + /* -Measures : zk [10 x 1] */ + /* Outputs : */ + /* -outputs model : y [10 x 1] */ + /* ------------------------------------------------------ */ + scale = 1.17549435E-38F; + beta = fabsf(x[0]); + if (beta > 1.17549435E-38F) { + Vair = 1.0F; + scale = beta; + } else { + t = beta / 1.17549435E-38F; + Vair = t * t; + } + + beta = fabsf(x[1]); + if (beta > scale) { + t = scale / beta; + Vair = Vair * t * t + 1.0F; + scale = beta; + } else { + t = beta / scale; + Vair += t * t; + } + + beta = fabsf(x[2]); + if (beta > scale) { + t = scale / beta; + Vair = Vair * t * t + 1.0F; + scale = beta; + } else { + t = beta / scale; + Vair += t * t; + } + + Vair = scale * sqrtf(Vair); + + /* ==> Ground speed|R0 = [u,v,w]|R0 + wind|R0 */ + /* description : */ + /* q*D*q-1 */ + scale = ((q[0] * q[0] + q[1] * q[1]) + q[2] * q[2]) + q[3] * q[3]; + t = q[0] / scale; + qinv_idx_1 = -q[1] / scale; + qinv_idx_2 = -q[2] / scale; + qinv_idx_3 = -q[3] / scale; + qinter_idx_0 = -((q[1] * x[0] + q[2] * x[1]) + q[3] * x[2]); + qinter_idx_1 = (q[2] * x[2] - q[3] * x[1]) + x[0] * q[0]; + qinter_idx_2 = (q[3] * x[0] - q[1] * x[2]) + x[1] * q[0]; + qinter_idx_3 = (q[1] * x[1] - q[2] * x[0]) + x[2] * q[0]; + + /* ==> Va = scale factor * norm([u,v,w]) */ + if (Vair > 0.0001) { + /* ==> AOA = atan(w/u) */ + scale = atan2f(x[2], x[0]); + + /* ==> Sideslip = asin(v/||Vair||) */ + beta = asinf(x[1] / Vair); + } else { + scale = 0.0F; + beta = 0.0F; + } + + y[0] = (((qinter_idx_2 * qinv_idx_3 - qinter_idx_3 * qinv_idx_2) + + qinter_idx_1 * t) + qinv_idx_1 * qinter_idx_0) + x[3]; + y[1] = (((qinter_idx_3 * qinv_idx_1 - qinter_idx_1 * qinv_idx_3) + + qinter_idx_2 * t) + qinv_idx_2 * qinter_idx_0) + x[4]; + y[2] = (((qinter_idx_1 * qinv_idx_2 - qinter_idx_2 * qinv_idx_1) + + qinter_idx_3 * t) + qinv_idx_3 * qinter_idx_0) + x[5]; + y[3] = x[6] * Vair; + y[4] = scale; + y[5] = beta; +} + +/* Function for MATLAB Function: '/UKF_prediction' */ +static void RungeKutta(const real32_T x[7], real32_T dt, const real32_T u[6], + real32_T xi[7]) +{ + real32_T k2[7]; + real32_T k3[7]; + real32_T k4[7]; + real32_T b_x[7]; + real32_T y; + int32_T i; + + /* ----------------Runge Kutta 4----------------- */ + /* description : */ + /* dt is an integration step (tk+1-tk) */ + /* h_2 is define as step/2 */ + /* */ + /* Non linear function f(x,u), results : X(:,k)=[Xk+1(0) ... Xk+1(2n+1)] */ + /* =RungeKutta(X(:,k),dt,omega,a); */ + /* -------------------Process_model--------------------- */ + /* description : */ + /* */ + /* Dim of the state vector : dim(x) = 6; */ + /* x=[x1 x2 x3 x4 x5 x6] */ + /* x=[uk vk wk uw0 vw0 ww0] */ + /* */ + /* dx=f(x,u) */ + /* dx1=f(x1,u) */ + /* dx2=f(x2,u) */ + /* ... */ + /* dx=[dx1; dx2; ...] */ + /* Inputs : */ + /* -States: x */ + /* -Command inputs : omega,a */ + /* Outputs : */ + /* -state dot : dx */ + /* ------------------------------------------------------ */ + /* dot_uvw, dot_wind_bias, dot_airspeed_scale */ + for (i = 0; i < 7; i++) { + xi[i] = 0.0F; + } + + /* a = single(rep(q, u(4:6))); */ + /* ==> dot_UVW */ + xi[0] = (u[2] * x[1] + u[3]) - u[1] * x[2]; + xi[1] = (u[0] * x[2] + u[4]) - u[2] * x[0]; + xi[2] = (u[1] * x[0] + u[5]) - u[0] * x[1]; + + /* phi = single(atan2(2*(q(1)*q(2)+q(3)*q(4)), 1-2*(q(2)^2+q(3)^2))); */ + /* theta = single(asin(2*(q(1)*q(3)-q(2)*q(4)))); */ + /* dx(1)=a(1)-9.81*sin(theta)+ omega(3)* x(2)-omega(2)*x(3); */ + /* dx(2)=a(2)+9.81*cos(theta)*sin(phi) + omega(1)* x(3)-omega(3)*x(1); */ + /* dx(3)=a(3)+9.81*cos(theta)*cos(phi) + omega(2)* x(1)-omega(1)*x(2); */ + /* ==> dot_biais */ + /* already at zero */ + /* -------------------Process_model--------------------- */ + /* description : */ + /* */ + /* Dim of the state vector : dim(x) = 6; */ + /* x=[x1 x2 x3 x4 x5 x6] */ + /* x=[uk vk wk uw0 vw0 ww0] */ + /* */ + /* dx=f(x,u) */ + /* dx1=f(x1,u) */ + /* dx2=f(x2,u) */ + /* ... */ + /* dx=[dx1; dx2; ...] */ + /* Inputs : */ + /* -States: x */ + /* -Command inputs : omega,a */ + /* Outputs : */ + /* -state dot : dx */ + /* ------------------------------------------------------ */ + /* dot_uvw, dot_wind_bias, dot_airspeed_scale */ + for (i = 0; i < 7; i++) { + b_x[i] = xi[i] / 2.0F * dt + x[i]; + k2[i] = 0.0F; + } + + /* a = single(rep(q, u(4:6))); */ + /* ==> dot_UVW */ + k2[0] = (u[2] * b_x[1] + u[3]) - u[1] * b_x[2]; + k2[1] = (u[0] * b_x[2] + u[4]) - u[2] * b_x[0]; + k2[2] = (u[1] * b_x[0] + u[5]) - u[0] * b_x[1]; + + /* phi = single(atan2(2*(q(1)*q(2)+q(3)*q(4)), 1-2*(q(2)^2+q(3)^2))); */ + /* theta = single(asin(2*(q(1)*q(3)-q(2)*q(4)))); */ + /* dx(1)=a(1)-9.81*sin(theta)+ omega(3)* x(2)-omega(2)*x(3); */ + /* dx(2)=a(2)+9.81*cos(theta)*sin(phi) + omega(1)* x(3)-omega(3)*x(1); */ + /* dx(3)=a(3)+9.81*cos(theta)*cos(phi) + omega(2)* x(1)-omega(1)*x(2); */ + /* ==> dot_biais */ + /* already at zero */ + /* -------------------Process_model--------------------- */ + /* description : */ + /* */ + /* Dim of the state vector : dim(x) = 6; */ + /* x=[x1 x2 x3 x4 x5 x6] */ + /* x=[uk vk wk uw0 vw0 ww0] */ + /* */ + /* dx=f(x,u) */ + /* dx1=f(x1,u) */ + /* dx2=f(x2,u) */ + /* ... */ + /* dx=[dx1; dx2; ...] */ + /* Inputs : */ + /* -States: x */ + /* -Command inputs : omega,a */ + /* Outputs : */ + /* -state dot : dx */ + /* ------------------------------------------------------ */ + /* dot_uvw, dot_wind_bias, dot_airspeed_scale */ + for (i = 0; i < 7; i++) { + b_x[i] = k2[i] / 2.0F * dt + x[i]; + k3[i] = 0.0F; + } + + /* a = single(rep(q, u(4:6))); */ + /* ==> dot_UVW */ + k3[0] = (u[2] * b_x[1] + u[3]) - u[1] * b_x[2]; + k3[1] = (u[0] * b_x[2] + u[4]) - u[2] * b_x[0]; + k3[2] = (u[1] * b_x[0] + u[5]) - u[0] * b_x[1]; + + /* phi = single(atan2(2*(q(1)*q(2)+q(3)*q(4)), 1-2*(q(2)^2+q(3)^2))); */ + /* theta = single(asin(2*(q(1)*q(3)-q(2)*q(4)))); */ + /* dx(1)=a(1)-9.81*sin(theta)+ omega(3)* x(2)-omega(2)*x(3); */ + /* dx(2)=a(2)+9.81*cos(theta)*sin(phi) + omega(1)* x(3)-omega(3)*x(1); */ + /* dx(3)=a(3)+9.81*cos(theta)*cos(phi) + omega(2)* x(1)-omega(1)*x(2); */ + /* ==> dot_biais */ + /* already at zero */ + /* -------------------Process_model--------------------- */ + /* description : */ + /* */ + /* Dim of the state vector : dim(x) = 6; */ + /* x=[x1 x2 x3 x4 x5 x6] */ + /* x=[uk vk wk uw0 vw0 ww0] */ + /* */ + /* dx=f(x,u) */ + /* dx1=f(x1,u) */ + /* dx2=f(x2,u) */ + /* ... */ + /* dx=[dx1; dx2; ...] */ + /* Inputs : */ + /* -States: x */ + /* -Command inputs : omega,a */ + /* Outputs : */ + /* -state dot : dx */ + /* ------------------------------------------------------ */ + /* dot_uvw, dot_wind_bias, dot_airspeed_scale */ + for (i = 0; i < 7; i++) { + b_x[i] = dt * k3[i] + x[i]; + k4[i] = 0.0F; + } + + /* a = single(rep(q, u(4:6))); */ + /* ==> dot_UVW */ + k4[0] = (u[2] * b_x[1] + u[3]) - u[1] * b_x[2]; + k4[1] = (u[0] * b_x[2] + u[4]) - u[2] * b_x[0]; + k4[2] = (u[1] * b_x[0] + u[5]) - u[0] * b_x[1]; + + /* phi = single(atan2(2*(q(1)*q(2)+q(3)*q(4)), 1-2*(q(2)^2+q(3)^2))); */ + /* theta = single(asin(2*(q(1)*q(3)-q(2)*q(4)))); */ + /* dx(1)=a(1)-9.81*sin(theta)+ omega(3)* x(2)-omega(2)*x(3); */ + /* dx(2)=a(2)+9.81*cos(theta)*sin(phi) + omega(1)* x(3)-omega(3)*x(1); */ + /* dx(3)=a(3)+9.81*cos(theta)*cos(phi) + omega(2)* x(1)-omega(1)*x(2); */ + /* ==> dot_biais */ + /* already at zero */ + y = dt / 6.0F; + for (i = 0; i < 7; i++) { + xi[i] = (((k2[i] + k3[i]) * 2.0F + xi[i]) + k4[i]) * y + x[i]; + } +} + +/* Model step function */ +void UKF_Wind_Estimator_step(void) +{ + real32_T lambda; + int32_T jmax; + int32_T colj; + int32_T b_j; + boolean_T exitg1; + real32_T Y[49]; + real32_T u[6]; + real32_T SQ[49]; + real32_T p[7]; + real32_T unusedU0[147]; + int32_T jj; + real32_T ajj; + int32_T ix; + int32_T iy; + int32_T b_ix; + int32_T e; + int32_T ia; + real32_T SR[36]; + real32_T unusedU0_0[120]; + real32_T K[42]; + real32_T U[42]; + real32_T d[225]; + real32_T rtb_x1[7]; + real32_T rtb_Wc[15]; + real32_T rtb_Wm[15]; + real32_T rtb_P1[49]; + real32_T rtb_X[105]; + real32_T rtb_Y[105]; + real32_T rtb_Z[90]; + real32_T rtb_z[6]; + int32_T i; + real32_T lambda_0[120]; + real32_T lambda_1[147]; + int32_T b_iy; + real32_T lambda_2[49]; + real32_T SR_0[36]; + real32_T lambda_3[36]; + real32_T rtb_Z_0[90]; + real32_T tmp[6]; + + /* MATLAB Function: '/initialization' incorporates: + * Inport: '/P0' + * Inport: '/alpha' + * Inport: '/beta' + * Inport: '/ki' + * Inport: '/x0' + * UnitDelay: '/ Delay' + * UnitDelay: '/ Delay1' + */ + /* MATLAB Function 'initialization': ':1' */ + /* ----------------Initialisation--------------------- */ + /* P=0*eye(11); */ + /* x=(linspace(0,0,11))'; */ + /* persistent Q */ + /* persistent R */ + if (!ukf_DW.P_not_empty) { + /* ':1:11' */ + /* ':1:12' */ + memcpy(&SQ[0], &(ukf_init.P0[0]), 49U * sizeof(real32_T)); + jmax = 0; + colj = 0; + b_j = 1; + exitg1 = false; + while ((!exitg1) && (b_j < 8)) { + jj = (colj + b_j) - 1; + lambda = 0.0F; + if (!(b_j - 1 < 1)) { + ix = colj; + iy = colj; + for (i = 1; i < b_j; i++) { + lambda += SQ[ix] * SQ[iy]; + ix++; + iy++; + } + } + + ajj = SQ[jj] - lambda; + if (ajj > 0.0F) { + ajj = sqrtf(ajj); + SQ[jj] = ajj; + if (b_j < 7) { + if (b_j - 1 != 0) { + b_iy = jj + 7; + ix = ((6 - b_j) * 7 + colj) + 8; + for (iy = colj + 8; iy <= ix; iy += 7) { + b_ix = colj; + lambda = 0.0F; + e = (iy + b_j) - 2; + for (ia = iy; ia <= e; ia++) { + lambda += SQ[ia - 1] * SQ[b_ix]; + b_ix++; + } + + SQ[b_iy] += -lambda; + b_iy += 7; + } + } + + lambda = 1.0F / ajj; + ix = ((6 - b_j) * 7 + jj) + 8; + for (b_iy = jj + 7; b_iy + 1 <= ix; b_iy += 7) { + SQ[b_iy] *= lambda; + } + + colj += 7; + } + + b_j++; + } else { + SQ[jj] = ajj; + jmax = b_j; + exitg1 = true; + } + } + + if (jmax == 0) { + jmax = 7; + } else { + jmax--; + } + + for (b_j = 0; b_j + 1 <= jmax; b_j++) { + for (colj = b_j + 1; colj + 1 <= jmax; colj++) { + SQ[colj + 7 * b_j] = 0.0F; + } + } + + for (jmax = 0; jmax < 7; jmax++) { + for (b_j = 0; b_j < 7; b_j++) { + rtb_P1[b_j + 7 * jmax] = SQ[7 * b_j + jmax]; + } + } + + ukf_DW.P_not_empty = true; + } else { + /* ':1:14' */ + memcpy(&rtb_P1[0], &ukf_DW.Delay_DSTATE[0], 49U * sizeof(real32_T)); + } + + if (!ukf_DW.x_not_empty) { + /* ':1:17' */ + /* ':1:18' */ + for (i = 0; i < 7; i++) { + rtb_x1[i] = ukf_init.x0[i]; + } + + ukf_DW.x_not_empty = true; + } else { + /* ':1:20' */ + for (i = 0; i < 7; i++) { + rtb_x1[i] = ukf_DW.Delay1_DSTATE[i]; + } + } + + /* %%%%%%%%%%%%%%%--PROGRAMME PRINCIPAL--%%%%%%%%%%%%%%%% */ + /* alpha=0.01; %10^-1==0 : garantis que la covariance soit semi-definit positive. */ + /* Default k=0; */ + /* beta=2; %informations sur les moments d'ordre 3 */ + /* Default beta=2 choix optimal quand la distrib est sup. gaussienne. */ + /* dt=0.02; %dt pour le Runge-Kutta */ + /* --------Calcul du lambda----------------- */ + /* ':1:40' */ + /* dimension de l'etat */ + /* ':1:42' */ + lambda = ukf_init.alpha * ukf_init.alpha * (7.0F + ukf_init.ki) - 7.0F; + + /* ':1:43' */ + /* ----------------Calcul des ponderations Wm et Wc-------------------- */ + /* ':1:47' */ + ajj = 0.5F / (7.0F + lambda); + rtb_Wm[0] = lambda / (7.0F + lambda); + for (i = 0; i < 14; i++) { + rtb_Wm[i + 1] = ajj; + } + + /* ':1:48' */ + for (i = 0; i < 15; i++) { + rtb_Wc[i] = rtb_Wm[i]; + } + + /* ':1:49' */ + rtb_Wc[0] = ((1.0F - ukf_init.alpha * ukf_init.alpha) + ukf_init.beta) + + rtb_Wm[0]; + + /* ':1:50' */ + /* ':1:51' */ + /* ':1:52' */ + lambda = sqrtf(7.0F + lambda); + + /* MATLAB Function: '/sigmas' */ + /* MATLAB Function 'sigmas': ':1' */ + /* ----------------Sigma-point--------------------------- */ + /* sigmas points et concatenation : n*(2n+1) */ + /* Inputs : */ + /* -States: x */ + /* -Covariance on state : P */ + /* -Weighting squate-root */ + /* -sigmas-point (Wm,Wc) c */ + /* Outputs : */ + /* -Sigmas-points (state) : X */ + /* ---------------------------------------------------- */ + /* Covariance with ponderation: A = c * P */ + /* ':1:15' */ + for (jmax = 0; jmax < 49; jmax++) { + SQ[jmax] = lambda * rtb_P1[jmax]; + } + + /* dimension modification on state */ + /* ':1:18' */ + /* concatenation : X=[x Y+A Y-A] */ + /* ':1:21' */ + for (jmax = 0; jmax < 7; jmax++) { + for (b_j = 0; b_j < 7; b_j++) { + Y[b_j + 7 * jmax] = rtb_x1[b_j]; + } + + rtb_X[jmax] = rtb_x1[jmax]; + } + + for (jmax = 0; jmax < 49; jmax++) { + rtb_X[jmax + 7] = Y[jmax] + SQ[jmax]; + rtb_X[jmax + 56] = Y[jmax] - SQ[jmax]; + } + + /* End of MATLAB Function: '/sigmas' */ + + /* MATLAB Function: '/UKF_prediction' incorporates: + * Inport: '/Q' + * Inport: '/accel' + * Inport: '/dt' + * Inport: '/rates' + * MATLAB Function: '/initialization' + */ + /* MATLAB Function 'UKF_prediction': ':1' */ + /* ':1:38' */ + /* ':1:31' */ + /* ----------------Prediction Step------------------------- */ + /* Sigma point and model fonction f(x,u) */ + /* Inputs : */ + /* -Sigmas-points (state) : X */ + /* -Weighting sigmas-points : Wm, Wc */ + /* -Command input : omega, a */ + /* -Dimension vector state: n */ + /* -Integration step : dt */ + /* -Dimension 2n+1 : L */ + /* -Weighting state (Wk) : Q */ + /* Outputs : */ + /* -Predicted state: y */ + /* -Sigmas_state : Y */ + /* -Covariance on state : H */ + /* ---------------------------------------------------- */ + /* ----------------Prediction------------------------- */ + /* description : */ + /* Output : SigmaX(k+1) et xmoy(k+1) */ + /* */ + /* input state size */ + /* sigma state size */ + /* ':1:26' */ + for (i = 0; i < 7; i++) { + rtb_x1[i] = 0.0F; + } + + /* ':1:27' */ + memset(&rtb_Y[0], 0, 105U * sizeof(real32_T)); + + /* ':1:29' */ + u[0] = ukf_U.rates[0]; + u[3] = ukf_U.accel[0]; + u[1] = ukf_U.rates[1]; + u[4] = ukf_U.accel[1]; + u[2] = ukf_U.rates[2]; + u[5] = ukf_U.accel[2]; + + /* ':1:30' */ + for (i = 0; i < 15; i++) { + /* ':1:30' */ + /* ':1:31' */ + RungeKutta(&rtb_X[7 * i], ukf_params.dt, u, &rtb_Y[7 * i]); + + /* non linear function */ + /* ':1:32' */ + for (jmax = 0; jmax < 7; jmax++) { + rtb_x1[jmax] += rtb_Y[7 * i + jmax] * rtb_Wm[i]; + } + + /* predicted vector on the state */ + /* y = Y(:,1); */ + /* ':1:30' */ + } + + /* ':1:36' */ + for (b_iy = 0; b_iy < 49; b_iy++) { + SQ[b_iy] = sqrtf(ukf_params.Q[b_iy]); + } + + /* square-root of R */ + /* ':1:38' */ + lambda = sqrtf(rtb_Wm[1]); + + /* ':1:40' */ + /* 33*11=>[11*22 11*11] */ + /* ':1:42' */ + for (jmax = 0; jmax < 7; jmax++) { + for (b_j = 0; b_j < 14; b_j++) { + lambda_1[b_j + 21 * jmax] = (rtb_Y[(1 + b_j) * 7 + jmax] - rtb_x1[jmax]) * + lambda; + } + } + + for (jmax = 0; jmax < 7; jmax++) { + for (b_j = 0; b_j < 7; b_j++) { + lambda_1[(b_j + 21 * jmax) + 14] = SQ[7 * b_j + jmax]; + } + } + + qr_e(lambda_1, unusedU0, SQ); + + /* ':1:44' */ + for (jmax = 0; jmax < 7; jmax++) { + p[jmax] = rtb_Y[jmax] - rtb_x1[jmax]; + } + + if (rtb_Wc[0] < 0.0F) { + /* ':1:46' */ + /* ':1:47' */ + lambda = powf(-rtb_Wc[0], 0.25F); + ajj = powf(-rtb_Wc[0], 0.25F); + for (jmax = 0; jmax < 7; jmax++) { + for (b_j = 0; b_j < 7; b_j++) { + Y[jmax + 7 * b_j] = 0.0F; + for (colj = 0; colj < 7; colj++) { + Y[jmax + 7 * b_j] += SQ[7 * jmax + colj] * SQ[7 * b_j + colj]; + } + + lambda_2[jmax + 7 * b_j] = lambda * p[jmax] * (ajj * p[b_j]); + } + } + + for (jmax = 0; jmax < 7; jmax++) { + for (b_j = 0; b_j < 7; b_j++) { + rtb_P1[b_j + 7 * jmax] = Y[7 * jmax + b_j] - lambda_2[7 * jmax + b_j]; + } + } + + b_j = 0; + i = 0; + jmax = 1; + exitg1 = false; + while ((!exitg1) && (jmax < 8)) { + colj = (i + jmax) - 1; + lambda = 0.0F; + if (!(jmax - 1 < 1)) { + jj = i; + b_iy = i; + for (ix = 1; ix < jmax; ix++) { + lambda += rtb_P1[jj] * rtb_P1[b_iy]; + jj++; + b_iy++; + } + } + + lambda = rtb_P1[colj] - lambda; + if (lambda > 0.0F) { + lambda = sqrtf(lambda); + rtb_P1[colj] = lambda; + if (jmax < 7) { + if (jmax - 1 != 0) { + jj = colj + 7; + b_iy = ((6 - jmax) * 7 + i) + 8; + for (ix = i + 8; ix <= b_iy; ix += 7) { + iy = i; + ajj = 0.0F; + b_ix = (ix + jmax) - 2; + for (e = ix; e <= b_ix; e++) { + ajj += rtb_P1[e - 1] * rtb_P1[iy]; + iy++; + } + + rtb_P1[jj] += -ajj; + jj += 7; + } + } + + lambda = 1.0F / lambda; + jj = ((6 - jmax) * 7 + colj) + 8; + for (colj += 7; colj + 1 <= jj; colj += 7) { + rtb_P1[colj] *= lambda; + } + + i += 7; + } + + jmax++; + } else { + rtb_P1[colj] = lambda; + b_j = jmax; + exitg1 = true; + } + } + + if (b_j == 0) { + i = 7; + } else { + i = b_j - 1; + } + + for (b_j = 0; b_j + 1 <= i; b_j++) { + for (jmax = b_j + 1; jmax + 1 <= i; jmax++) { + rtb_P1[jmax + 7 * b_j] = 0.0F; + } + } + } else { + /* ':1:49' */ + lambda = powf(rtb_Wc[0], 0.25F); + ajj = powf(rtb_Wc[0], 0.25F); + for (jmax = 0; jmax < 7; jmax++) { + for (b_j = 0; b_j < 7; b_j++) { + Y[jmax + 7 * b_j] = 0.0F; + for (colj = 0; colj < 7; colj++) { + Y[jmax + 7 * b_j] += SQ[7 * jmax + colj] * SQ[7 * b_j + colj]; + } + + lambda_2[jmax + 7 * b_j] = lambda * p[jmax] * (ajj * p[b_j]); + } + } + + for (jmax = 0; jmax < 7; jmax++) { + for (b_j = 0; b_j < 7; b_j++) { + rtb_P1[b_j + 7 * jmax] = Y[7 * jmax + b_j] + lambda_2[7 * jmax + b_j]; + } + } + + jmax = 0; + colj = 0; + b_j = 1; + exitg1 = false; + while ((!exitg1) && (b_j < 8)) { + jj = (colj + b_j) - 1; + lambda = 0.0F; + if (!(b_j - 1 < 1)) { + ix = colj; + iy = colj; + for (b_iy = 1; b_iy < b_j; b_iy++) { + lambda += rtb_P1[ix] * rtb_P1[iy]; + ix++; + iy++; + } + } + + ajj = rtb_P1[jj] - lambda; + if (ajj > 0.0F) { + ajj = sqrtf(ajj); + rtb_P1[jj] = ajj; + if (b_j < 7) { + if (b_j - 1 != 0) { + b_iy = jj + 7; + ix = ((6 - b_j) * 7 + colj) + 8; + for (iy = colj + 8; iy <= ix; iy += 7) { + b_ix = colj; + lambda = 0.0F; + e = (iy + b_j) - 2; + for (ia = iy; ia <= e; ia++) { + lambda += rtb_P1[ia - 1] * rtb_P1[b_ix]; + b_ix++; + } + + rtb_P1[b_iy] += -lambda; + b_iy += 7; + } + } + + lambda = 1.0F / ajj; + ix = ((6 - b_j) * 7 + jj) + 8; + for (i = jj + 7; i + 1 <= ix; i += 7) { + rtb_P1[i] *= lambda; + } + + colj += 7; + } + + b_j++; + } else { + rtb_P1[jj] = ajj; + jmax = b_j; + exitg1 = true; + } + } + + if (jmax == 0) { + jmax = 7; + } else { + jmax--; + } + + for (b_j = 0; b_j + 1 <= jmax; b_j++) { + for (colj = b_j + 1; colj + 1 <= jmax; colj++) { + rtb_P1[colj + 7 * b_j] = 0.0F; + } + } + } + + /* End of MATLAB Function: '/UKF_prediction' */ + + /* MATLAB Function: '/UKF_correction' incorporates: + * Inport: '/R' + * Inport: '/q' + * MATLAB Function: '/initialization' + */ + /* MATLAB Function 'UKF_correction': ':1' */ + /* ':1:49' */ + /* ':1:41' */ + /* ':1:38' */ + /* ----------------Correction Step------------------------- */ + /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */ + /* By Condomines Jean_Philippe - 1 may 2015- */ + /* jp.condomines@gmail.com */ + /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */ + /* Sigma point and output fonction h(x_k) */ + /* Inputs : */ + /* -Sigmas-points (state) : X */ + /* -Weighting sigmas-points : Wm, Wc */ + /* -Dimension of the measurement vector: n */ + /* -Dimension 2n+1 : L */ + /* -Weighting measures (Vk+1): R */ + /* -Measures : zk */ + /* Outputs : */ + /* -Sigmas_measures : Z */ + /* -Predicted measurement : z */ + /* -Covariance on measures : J */ + /* ---------------------------------------------------- */ + /* measurement input size */ + /* input state size */ + /* sigma state size */ + /* Zeros vector for predicted measures */ + /* ':1:29' */ + for (i = 0; i < 6; i++) { + rtb_z[i] = 0.0F; + } + + /* Zeros vector for predicted Sigmas_measures */ + /* ':1:32' */ + /* "for" loop for "measures predicted" and "sigmas_measures" predicted */ + /* ':1:35' */ + for (i = 0; i < 15; i++) { + /* ':1:35' */ + /* non linear function h(x), results : Z(:,k)=[zk+1(0) ... zk+1(2n+1)] */ + /* ':1:38' */ + h(&rtb_Y[7 * i], ukf_U.q, u); + for (jmax = 0; jmax < 6; jmax++) { + rtb_Z[jmax + 6 * i] = u[jmax]; + } + + /* Predicted measurement */ + /* ':1:41' */ + for (jmax = 0; jmax < 6; jmax++) { + rtb_z[jmax] += rtb_Z[6 * i + jmax] * rtb_Wm[i]; + } + + /* z = Z(:,1); */ + /* ':1:35' */ + } + + /* Square-root of R */ + /* ':1:46' */ + for (b_iy = 0; b_iy < 36; b_iy++) { + SR[b_iy] = sqrtf(ukf_params.R[b_iy]); + } + + /* Subtracting of "predicted Sigma measures" et "predicted measurement" with ponderation "Wc" */ + /* ':1:49' */ + lambda = sqrtf(rtb_Wm[1]); + + /* Concatenation with the square root of the matrix "R" */ + /* ':1:52' */ + /* factorization qr, result use in "J" and rest in "Temp" */ + /* Sz,k+1 = qr{[sqrt(Wc(1)) * [Zk+1(:,2:2n+1)-zk+1] sqrt(R)] */ + /* ':1:56' */ + for (jmax = 0; jmax < 6; jmax++) { + for (b_j = 0; b_j < 14; b_j++) { + lambda_0[b_j + 20 * jmax] = (rtb_Z[(1 + b_j) * 6 + jmax] - rtb_z[jmax]) * + lambda; + } + } + + for (jmax = 0; jmax < 6; jmax++) { + for (b_j = 0; b_j < 6; b_j++) { + lambda_0[(b_j + 20 * jmax) + 14] = SR[6 * b_j + jmax]; + } + } + + qr(lambda_0, unusedU0_0, SR); + + /* Subtracting the first column "zk+1(0)" with all the measurements zk+1 predicted */ + /* ':1:59' */ + for (jmax = 0; jmax < 6; jmax++) { + u[jmax] = rtb_Z[jmax] - rtb_z[jmax]; + } + + /* Test on Wc(1) for cholesky factorization : Cholupdate{Sz,k+1,p,Wc(0)} */ + if (rtb_Wc[0] < 0.0F) { + /* ':1:62' */ + /* ':1:63' */ + lambda = powf(-rtb_Wc[0], 0.25F); + ajj = powf(-rtb_Wc[0], 0.25F); + for (jmax = 0; jmax < 6; jmax++) { + for (b_j = 0; b_j < 6; b_j++) { + SR_0[jmax + 6 * b_j] = 0.0F; + for (colj = 0; colj < 6; colj++) { + SR_0[jmax + 6 * b_j] += SR[6 * jmax + colj] * SR[6 * b_j + colj]; + } + + lambda_3[jmax + 6 * b_j] = lambda * u[jmax] * (ajj * u[b_j]); + } + } + + for (jmax = 0; jmax < 6; jmax++) { + for (b_j = 0; b_j < 6; b_j++) { + SR[b_j + 6 * jmax] = SR_0[6 * jmax + b_j] - lambda_3[6 * jmax + b_j]; + } + } + + b_j = 0; + i = 0; + jmax = 1; + exitg1 = false; + while ((!exitg1) && (jmax < 7)) { + colj = (i + jmax) - 1; + lambda = 0.0F; + if (!(jmax - 1 < 1)) { + jj = i; + b_iy = i; + for (ix = 1; ix < jmax; ix++) { + lambda += SR[jj] * SR[b_iy]; + jj++; + b_iy++; + } + } + + lambda = SR[colj] - lambda; + if (lambda > 0.0F) { + lambda = sqrtf(lambda); + SR[colj] = lambda; + if (jmax < 6) { + if (jmax - 1 != 0) { + jj = colj + 6; + b_iy = ((5 - jmax) * 6 + i) + 7; + for (ix = i + 7; ix <= b_iy; ix += 6) { + iy = i; + ajj = 0.0F; + b_ix = (ix + jmax) - 2; + for (e = ix; e <= b_ix; e++) { + ajj += SR[e - 1] * SR[iy]; + iy++; + } + + SR[jj] += -ajj; + jj += 6; + } + } + + lambda = 1.0F / lambda; + jj = ((5 - jmax) * 6 + colj) + 7; + for (colj += 6; colj + 1 <= jj; colj += 6) { + SR[colj] *= lambda; + } + + i += 6; + } + + jmax++; + } else { + SR[colj] = lambda; + b_j = jmax; + exitg1 = true; + } + } + + if (b_j == 0) { + i = 6; + } else { + i = b_j - 1; + } + + for (b_j = 0; b_j + 1 <= i; b_j++) { + for (jmax = b_j + 1; jmax + 1 <= i; jmax++) { + SR[jmax + 6 * b_j] = 0.0F; + } + } + } else { + /* ':1:65' */ + lambda = powf(rtb_Wc[0], 0.25F); + ajj = powf(rtb_Wc[0], 0.25F); + for (jmax = 0; jmax < 6; jmax++) { + for (b_j = 0; b_j < 6; b_j++) { + SR_0[jmax + 6 * b_j] = 0.0F; + for (colj = 0; colj < 6; colj++) { + SR_0[jmax + 6 * b_j] += SR[6 * jmax + colj] * SR[6 * b_j + colj]; + } + + lambda_3[jmax + 6 * b_j] = lambda * u[jmax] * (ajj * u[b_j]); + } + } + + for (jmax = 0; jmax < 6; jmax++) { + for (b_j = 0; b_j < 6; b_j++) { + SR[b_j + 6 * jmax] = SR_0[6 * jmax + b_j] + lambda_3[6 * jmax + b_j]; + } + } + + jmax = 0; + colj = 0; + b_j = 1; + exitg1 = false; + while ((!exitg1) && (b_j < 7)) { + jj = (colj + b_j) - 1; + lambda = 0.0F; + if (!(b_j - 1 < 1)) { + ix = colj; + iy = colj; + for (b_iy = 1; b_iy < b_j; b_iy++) { + lambda += SR[ix] * SR[iy]; + ix++; + iy++; + } + } + + ajj = SR[jj] - lambda; + if (ajj > 0.0F) { + ajj = sqrtf(ajj); + SR[jj] = ajj; + if (b_j < 6) { + if (b_j - 1 != 0) { + b_iy = jj + 6; + ix = ((5 - b_j) * 6 + colj) + 7; + for (iy = colj + 7; iy <= ix; iy += 6) { + b_ix = colj; + lambda = 0.0F; + e = (iy + b_j) - 2; + for (ia = iy; ia <= e; ia++) { + lambda += SR[ia - 1] * SR[b_ix]; + b_ix++; + } + + SR[b_iy] += -lambda; + b_iy += 6; + } + } + + lambda = 1.0F / ajj; + ix = ((5 - b_j) * 6 + jj) + 7; + for (i = jj + 6; i + 1 <= ix; i += 6) { + SR[i] *= lambda; + } + + colj += 6; + } + + b_j++; + } else { + SR[jj] = ajj; + jmax = b_j; + exitg1 = true; + } + } + + if (jmax == 0) { + jmax = 6; + } else { + jmax--; + } + + for (b_j = 0; b_j + 1 <= jmax; b_j++) { + for (colj = b_j + 1; colj + 1 <= jmax; colj++) { + SR[colj + 6 * b_j] = 0.0F; + } + } + } + + /* Transposition to have a lower triangular matrix */ + /* ':1:69' */ + for (jmax = 0; jmax < 6; jmax++) { + for (b_j = 0; b_j < 6; b_j++) { + SR_0[b_j + 6 * jmax] = SR[6 * b_j + jmax]; + } + } + + for (jmax = 0; jmax < 6; jmax++) { + for (b_j = 0; b_j < 6; b_j++) { + SR[b_j + 6 * jmax] = SR_0[6 * jmax + b_j]; + } + } + + /* End of MATLAB Function: '/UKF_correction' */ + + /* MATLAB Function: '/main' incorporates: + * Inport: '/aoa' + * Inport: '/sideslip' + * Inport: '/va ' + * Inport: '/vk' + */ + memcpy(&Y[0], &rtb_P1[0], 49U * sizeof(real32_T)); + + /* MATLAB Function 'main': ':1' */ + /* ':1:9' */ + /* measurement input size */ + /* input state size */ + /* sigma state size */ + /* ':1:7' */ + /* ':1:9' */ + memset(&d[0], 0, 225U * sizeof(real32_T)); + + /* Kalman gain : (Pxz/Sz')/Sz */ + /* Caution "/" is least squares root for "Ax = b" */ + /* ':1:13' */ + for (b_j = 0; b_j < 15; b_j++) { + d[b_j + 15 * b_j] = rtb_Wc[b_j]; + for (jmax = 0; jmax < 7; jmax++) { + rtb_X[jmax + 7 * b_j] = rtb_Y[7 * b_j + jmax] - rtb_x1[jmax]; + } + } + + for (jmax = 0; jmax < 7; jmax++) { + for (b_j = 0; b_j < 15; b_j++) { + rtb_Y[jmax + 7 * b_j] = 0.0F; + for (colj = 0; colj < 15; colj++) { + rtb_Y[jmax + 7 * b_j] += rtb_X[7 * colj + jmax] * d[15 * b_j + colj]; + } + } + } + + for (jmax = 0; jmax < 6; jmax++) { + for (b_j = 0; b_j < 15; b_j++) { + rtb_Z_0[b_j + 15 * jmax] = rtb_Z[6 * b_j + jmax] - rtb_z[jmax]; + } + } + + for (jmax = 0; jmax < 7; jmax++) { + for (b_j = 0; b_j < 6; b_j++) { + K[jmax + 7 * b_j] = 0.0F; + for (colj = 0; colj < 15; colj++) { + K[jmax + 7 * b_j] += rtb_Y[7 * colj + jmax] * rtb_Z_0[15 * b_j + colj]; + } + } + } + + for (jmax = 0; jmax < 6; jmax++) { + for (b_j = 0; b_j < 6; b_j++) { + SR_0[b_j + 6 * jmax] = SR[6 * b_j + jmax]; + } + } + + mrdivide(K, SR_0); + mrdivide(K, SR); + + /* Predicted state with Kalman gain "K" : x = xpred + K * (zk - z) */ + /* ':1:16' */ + /* Quaternion normalisation */ + /* x(1:4)=normalQ(x(1:4)'); */ + /* Cholesky factorization : U = K * Sz */ + /* ':1:22' */ + for (jmax = 0; jmax < 7; jmax++) { + for (b_j = 0; b_j < 6; b_j++) { + U[jmax + 7 * b_j] = 0.0F; + for (colj = 0; colj < 6; colj++) { + U[jmax + 7 * b_j] += K[7 * colj + jmax] * SR[6 * b_j + colj]; + } + } + } + + /* "for" loop for cholesky factorization */ + /* ':1:25' */ + /* "Sk" in "P" and Transpose of "P" */ + /* ':1:30' */ + /* SqrtP = sqrt(diag(Sk*Sk')); */ + tmp[0] = ukf_U.vk[0]; + tmp[1] = ukf_U.vk[1]; + tmp[2] = ukf_U.vk[2]; + tmp[3] = ukf_U.va; + tmp[4] = ukf_U.aoa; + tmp[5] = ukf_U.sideslip; + for (i = 0; i < 6; i++) { + /* ':1:25' */ + /* ':1:26' */ + for (jmax = 0; jmax < 7; jmax++) { + for (b_j = 0; b_j < 7; b_j++) { + rtb_P1[jmax + 7 * b_j] = 0.0F; + for (colj = 0; colj < 7; colj++) { + rtb_P1[jmax + 7 * b_j] += Y[7 * jmax + colj] * Y[7 * b_j + colj]; + } + + lambda_2[jmax + 7 * b_j] = U[7 * i + jmax] * U[7 * i + b_j]; + } + } + + for (jmax = 0; jmax < 7; jmax++) { + for (b_j = 0; b_j < 7; b_j++) { + SQ[b_j + 7 * jmax] = rtb_P1[7 * jmax + b_j] - lambda_2[7 * jmax + b_j]; + } + } + + jmax = 0; + colj = 0; + b_j = 1; + exitg1 = false; + while ((!exitg1) && (b_j < 8)) { + jj = (colj + b_j) - 1; + lambda = 0.0F; + if (!(b_j - 1 < 1)) { + ix = colj; + iy = colj; + for (b_iy = 1; b_iy < b_j; b_iy++) { + lambda += SQ[ix] * SQ[iy]; + ix++; + iy++; + } + } + + ajj = SQ[jj] - lambda; + if (ajj > 0.0F) { + ajj = sqrtf(ajj); + SQ[jj] = ajj; + if (b_j < 7) { + if (b_j - 1 != 0) { + b_iy = jj + 7; + ix = ((6 - b_j) * 7 + colj) + 8; + for (iy = colj + 8; iy <= ix; iy += 7) { + b_ix = colj; + lambda = 0.0F; + e = (iy + b_j) - 2; + for (ia = iy; ia <= e; ia++) { + lambda += SQ[ia - 1] * SQ[b_ix]; + b_ix++; + } + + SQ[b_iy] += -lambda; + b_iy += 7; + } + } + + lambda = 1.0F / ajj; + ix = ((6 - b_j) * 7 + jj) + 8; + for (b_iy = jj + 7; b_iy + 1 <= ix; b_iy += 7) { + SQ[b_iy] *= lambda; + } + + colj += 7; + } + + b_j++; + } else { + SQ[jj] = ajj; + jmax = b_j; + exitg1 = true; + } + } + + if (jmax == 0) { + jmax = 7; + } else { + jmax--; + } + + for (b_j = 0; b_j + 1 <= jmax; b_j++) { + for (colj = b_j + 1; colj + 1 <= jmax; colj++) { + SQ[colj + 7 * b_j] = 0.0F; + } + } + + memcpy(&Y[0], &SQ[0], 49U * sizeof(real32_T)); + + /* ':1:25' */ + u[i] = tmp[i] - rtb_z[i]; + } + + for (i = 0; i < 7; i++) { + lambda = 0.0F; + for (jmax = 0; jmax < 6; jmax++) { + lambda += K[7 * jmax + i] * u[jmax]; + } + + p[i] = rtb_x1[i] + lambda; + for (jmax = 0; jmax < 7; jmax++) { + rtb_P1[jmax + 7 * i] = Y[7 * jmax + i]; + } + + /* Outport: '/xout' */ + ukf_Y.xout[i] = p[i]; + } + + /* End of MATLAB Function: '/main' */ + + /* Outport: '/Pout' */ + memcpy(&ukf_Y.Pout[0], &rtb_P1[0], 49U * sizeof(real32_T)); + + /* Update for UnitDelay: '/ Delay1' */ + for (i = 0; i < 7; i++) { + ukf_DW.Delay1_DSTATE[i] = p[i]; + } + + /* End of Update for UnitDelay: '/ Delay1' */ + + /* Update for UnitDelay: '/ Delay' */ + memcpy(&ukf_DW.Delay_DSTATE[0], &rtb_P1[0], 49U * sizeof(real32_T)); +} + +/* Model initialize function */ +void UKF_Wind_Estimator_initialize(void) +{ + /* (no initialization code required) */ +} + +/* + * File trailer for generated code. + * + * [EOF] + */ diff --git a/sw/airborne/modules/meteo/lib_ukf_wind_estimator/UKF_Wind_Estimator.h b/sw/airborne/modules/meteo/lib_ukf_wind_estimator/UKF_Wind_Estimator.h new file mode 100644 index 0000000000..0246a03507 --- /dev/null +++ b/sw/airborne/modules/meteo/lib_ukf_wind_estimator/UKF_Wind_Estimator.h @@ -0,0 +1,115 @@ +/* + * File: UKF_Wind_Estimator.h + * + * Code generated for Simulink model 'UKF_Wind_Estimator'. + * + * Model version : 1.120 + * Simulink Coder version : 8.10 (R2016a) 10-Feb-2016 + * C/C++ source code generated on : Wed Nov 2 23:49:42 2016 + * + * Target selection: ert.tlc + * Embedded hardware selection: Custom Processor->Custom + * Code generation objectives: + * 1. Execution efficiency + * 2. RAM efficiency + * Validation result: Not run + */ + +#ifndef RTW_HEADER_UKF_Wind_Estimator_h_ +#define RTW_HEADER_UKF_Wind_Estimator_h_ +#include +#include +#ifndef UKF_Wind_Estimator_COMMON_INCLUDES_ +# define UKF_Wind_Estimator_COMMON_INCLUDES_ +#include "rtwtypes.h" +#endif /* UKF_Wind_Estimator_COMMON_INCLUDES_ */ + +/* Macros for accessing real-time model data structure */ + +/* Block signals and states (auto storage) for system '' */ +typedef struct { + real32_T Delay1_DSTATE[7]; /* '/ Delay1' */ + real32_T Delay_DSTATE[49]; /* '/ Delay' */ + boolean_T x_not_empty; /* '/initialization' */ + boolean_T P_not_empty; /* '/initialization' */ +} DW; + +/* External inputs (root inport signals with auto storage) */ +typedef struct { + real32_T rates[3]; /* '/rates' */ + real32_T accel[3]; /* '/accel' */ + real32_T q[4]; /* '/q' */ + real32_T vk[3]; /* '/vk' */ + real32_T va; /* '/va ' */ + real32_T aoa; /* '/aoa' */ + real32_T sideslip; /* '/sideslip' */ +} ExtU; + +/* External outputs (root outports fed by signals with auto storage) */ +typedef struct { + real32_T xout[7]; /* '/xout' */ + real32_T Pout[49]; /* '/Pout' */ +} ExtY; + +/* Type definition for custom storage class: Struct */ +typedef struct ukf_init_tag { + real32_T x0[7]; + real32_T P0[49]; + real32_T ki; + real32_T alpha; + real32_T beta; +} ukf_init_type; + +typedef struct ukf_params_tag { + real32_T Q[49]; + real32_T R[36]; + real32_T dt; +} ukf_params_type; + +/* Block signals and states (auto storage) */ +extern DW ukf_DW; + +/* External inputs (root inport signals with auto storage) */ +extern ExtU ukf_U; + +/* External outputs (root outports fed by signals with auto storage) */ +extern ExtY ukf_Y; + +/* Model entry point functions */ +extern void UKF_Wind_Estimator_initialize(void); +extern void UKF_Wind_Estimator_step(void); + +/* Exported data declaration */ + +/* Declaration for custom storage class: Struct */ +extern ukf_init_type ukf_init; +extern ukf_params_type ukf_params; + +/*- + * The generated code includes comments that allow you to trace directly + * back to the appropriate location in the model. The basic format + * is /block_name, where system is the system number (uniquely + * assigned by Simulink) and block_name is the name of the block. + * + * Use the MATLAB hilite_system command to trace the generated code back + * to the model. For example, + * + * hilite_system('') - opens system 3 + * hilite_system('/Kp') - opens and selects block Kp which resides in S3 + * + * Here is the system hierarchy for this model + * + * '' : 'UKF_Wind_Estimator' + * '' : 'UKF_Wind_Estimator/UKF_correction' + * '' : 'UKF_Wind_Estimator/UKF_prediction' + * '' : 'UKF_Wind_Estimator/initialization' + * '' : 'UKF_Wind_Estimator/main' + * '' : 'UKF_Wind_Estimator/sigmas' + */ +#endif /* RTW_HEADER_UKF_Wind_Estimator_h_ */ + +/* + * File trailer for generated code. + * + * [EOF] + */ diff --git a/sw/airborne/modules/meteo/lib_ukf_wind_estimator/rtwtypes.h b/sw/airborne/modules/meteo/lib_ukf_wind_estimator/rtwtypes.h new file mode 100644 index 0000000000..a018b996e8 --- /dev/null +++ b/sw/airborne/modules/meteo/lib_ukf_wind_estimator/rtwtypes.h @@ -0,0 +1,96 @@ +/* + * File: rtwtypes.h + * + * Code generated for Simulink model 'UKF_Wind_Estimator'. + * + * Model version : 1.120 + * Simulink Coder version : 8.10 (R2016a) 10-Feb-2016 + * C/C++ source code generated on : Wed Nov 2 23:49:42 2016 + * + * Target selection: ert.tlc + * Embedded hardware selection: Custom Processor->Custom + * Code generation objectives: + * 1. Execution efficiency + * 2. RAM efficiency + * Validation result: Not run + */ + +#ifndef RTWTYPES_H +#define RTWTYPES_H + +/* Logical type definitions */ +#if (!defined(__cplusplus)) +# ifndef false +# define false (0U) +# endif + +# ifndef true +# define true (1U) +# endif +#endif + +/*=======================================================================* + * Target hardware information + * Device type: Custom Processor->Custom + * Number of bits: char: 8 short: 16 int: 32 + * long: 32 + * native word size: 32 + * Byte ordering: LittleEndian + * Signed integer division rounds to: Zero + * Shift right on a signed integer as arithmetic shift: on + *=======================================================================*/ + +/*=======================================================================* + * Fixed width word size data types: * + * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * + * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * + * real32_T, real64_T - 32 and 64 bit floating point numbers * + *=======================================================================*/ +typedef signed char int8_T; +typedef unsigned char uint8_T; +typedef short int16_T; +typedef unsigned short uint16_T; +typedef int int32_T; +typedef unsigned int uint32_T; +typedef float real32_T; +typedef double real64_T; + +/*===========================================================================* + * Generic type definitions: boolean_T, char_T, byte_T, int_T, uint_T, * + * real_T, time_T, ulong_T. * + *===========================================================================*/ +typedef double real_T; +typedef double time_T; +typedef unsigned char boolean_T; +typedef int int_T; +typedef unsigned int uint_T; +typedef unsigned long ulong_T; +typedef char char_T; +typedef unsigned char uchar_T; +typedef char_T byte_T; + +/*=======================================================================* + * Min and Max: * + * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * + * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * + *=======================================================================*/ +#define MAX_int8_T ((int8_T)(127)) +#define MIN_int8_T ((int8_T)(-128)) +#define MAX_uint8_T ((uint8_T)(255U)) +#define MAX_int16_T ((int16_T)(32767)) +#define MIN_int16_T ((int16_T)(-32768)) +#define MAX_uint16_T ((uint16_T)(65535U)) +#define MAX_int32_T ((int32_T)(2147483647)) +#define MIN_int32_T ((int32_T)(-2147483647-1)) +#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) + +/* Block D-Work pointer type */ +typedef void * pointer_T; + +#endif /* RTWTYPES_H */ + +/* + * File trailer for generated code. + * + * [EOF] + */ diff --git a/sw/airborne/modules/meteo/wind_estimator.c b/sw/airborne/modules/meteo/wind_estimator.c new file mode 100644 index 0000000000..238a8ad68d --- /dev/null +++ b/sw/airborne/modules/meteo/wind_estimator.c @@ -0,0 +1,551 @@ +/* + * Copyright (C) 2016 Johan Maurin, Gautier Hattenberger + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file "modules/meteo/wind_estimator.c" + * @author Maurin + * + * Wind Estimator based on generated library from Matlab + */ + +#include "modules/meteo/wind_estimator.h" +#include "modules/meteo/lib_ukf_wind_estimator/UKF_Wind_Estimator.h" +#include "mcu_periph/sys_time.h" +#include "math/pprz_algebra_float.h" +#include "math/pprz_geodetic_float.h" +#include "generated/modules.h" +#include "state.h" +#include +#ifndef SITL // no chibios threads in sim +#if !USE_CHIBIOS_RTOS +#error Only Chibios is supported +#endif +#include +#include +#endif + +/** + * Default parameters + */ +#ifndef WE_UKF_KI +#define WE_UKF_KI 0.f // >= 0 to ensure that covariance is positive semi-definite +#endif +#ifndef WE_UKF_ALPHA +#define WE_UKF_ALPHA 0.5f // sigma point dispersion +#endif +#ifndef WE_UKF_BETA +#define WE_UKF_BETA 2.f // 2 is the best choice when distribution is gaussian +#endif +#ifndef WE_UKF_P0 +#define WE_UKF_P0 0.2f // initial covariance diagonal element +#endif +#ifndef WE_UKF_R_GS +#define WE_UKF_R_GS 0.5f // ground speed measurement confidence +#endif +#ifndef WE_UKF_R_VA +#define WE_UKF_R_VA 0.5f // airspeed measurement confidence +#endif +#ifndef WE_UKF_R_AOA +#define WE_UKF_R_AOA 0.002f // angle of attack measurement confidence +#endif +#ifndef WE_UKF_R_SSA +#define WE_UKF_R_SSA 0.002f // sideslip angle measurement confidence +#endif +#ifndef WE_UKF_Q_VA +#define WE_UKF_Q_VA 0.1f // airspeed model confidence +#endif +#ifndef WE_UKF_Q_VA_SCALE +#define WE_UKF_Q_VA_SCALE 0.0001f // airspeed scale factor model confidence +#endif +#ifndef WE_UKF_Q_WIND +#define WE_UKF_Q_WIND 0.001f // wind model confidence +#endif + +#ifndef SEND_WIND_ESTIMATOR +#define SEND_WIND_ESTIMATOR TRUE +#endif + +#include "subsystems/datalink/downlink.h" + +static void send_wind_estimator(struct transport_tx *trans, struct link_device *dev) +{ + uint8_t flags = 7; // send all data + float upwind = -wind_estimator.wind.z; + float airspeed = float_vect3_norm(&wind_estimator.airspeed); + pprz_msg_send_WIND_INFO_RET(trans, dev, AC_ID, + &flags, + &wind_estimator.wind.y, // east + &wind_estimator.wind.x, // north + &upwind, + &airspeed); +} + +#if PERIODIC_TELEMETRY +#include "subsystems/datalink/telemetry.h" +#endif + +#ifndef LOG_WIND_ESTIMATOR +#define LOG_WIND_ESTIMATOR FALSE +#endif + +#if LOG_WIND_ESTIMATOR +#ifndef SITL +#include "modules/loggers/sdlog_chibios.h" +#define PrintLog sdLogWriteLog +#define LogFileIsOpen() (pprzLogFile != -1) +#else // SITL: print in a file +#include +#define PrintLog fprintf +#define LogFileIsOpen() (pprzLogFile != NULL) +static FILE* pprzLogFile = NULL; +#endif +static bool log_we_started; +#endif + +// matrix element +#define MAT_EL(_m, _l, _c, _n) _m[_l + _c * _n] + +// wind estimator public structure +struct WindEstimator wind_estimator; + +// local variables +static uint32_t time_step_before; // last periodic time + +/* Thread declaration + * MATLAB UKF is using at least 6.6KB of stack + */ +#ifndef SITL +static THD_WORKING_AREA(wa_thd_windestimation, 8 * 1024); +static __attribute__((noreturn)) void thd_windestimate(void *arg); + +static MUTEX_DECL(we_ukf_mtx); // mutex for data acces protection +static SEMAPHORE_DECL(we_ukf_sem, 0); // semaaphore for sending signal +#endif + +/*----------------init_calculator---------------------*/ +/* Init fonction to init different type of variable */ +/* for the calculator before to calculate */ +/*----------------------------------------------------*/ +void init_calculator(void) +{ + UKF_Wind_Estimator_initialize(); + + // FIXME would be better to force Matlab to do this in initialize function + // zero input vector + memset(&ukf_U, 0, sizeof(ExtU)); + // zero output vector + memset(&ukf_Y, 0, sizeof(ExtY)); + // zero internal structure + memset(&ukf_DW, 0, sizeof(DW)); + // zero init structure + memset(&ukf_init, 0, sizeof(ukf_init_type)); + // zero params structure + memset(&ukf_params, 0, sizeof(ukf_params_type)); + + ukf_init.x0[6] = 1.0f; // initial airspeed scale factor + + MAT_EL(ukf_init.P0, 0, 0, 7) = WE_UKF_P0; + MAT_EL(ukf_init.P0, 1, 1, 7) = WE_UKF_P0; + MAT_EL(ukf_init.P0, 2, 2, 7) = WE_UKF_P0; + MAT_EL(ukf_init.P0, 3, 3, 7) = WE_UKF_P0; + MAT_EL(ukf_init.P0, 4, 4, 7) = WE_UKF_P0; + MAT_EL(ukf_init.P0, 5, 5, 7) = WE_UKF_P0; + MAT_EL(ukf_init.P0, 6, 6, 7) = WE_UKF_P0; + + MAT_EL(ukf_params.R, 0, 0, 6) = powf(WE_UKF_R_GS, 2); + MAT_EL(ukf_params.R, 1, 1, 6) = powf(WE_UKF_R_GS, 2); + MAT_EL(ukf_params.R, 2, 2, 6) = powf(WE_UKF_R_GS, 2); + MAT_EL(ukf_params.R, 3, 3, 6) = powf(WE_UKF_R_VA, 2); + MAT_EL(ukf_params.R, 4, 4, 6) = powf(WE_UKF_R_AOA, 2); + MAT_EL(ukf_params.R, 5, 5, 6) = powf(WE_UKF_R_SSA, 2); + + MAT_EL(ukf_params.Q, 0, 0, 7) = powf(WE_UKF_Q_VA, 2); + MAT_EL(ukf_params.Q, 1, 1, 7) = powf(WE_UKF_Q_VA, 2); + MAT_EL(ukf_params.Q, 2, 2, 7) = powf(WE_UKF_Q_VA, 2); + MAT_EL(ukf_params.Q, 3, 3, 7) = powf(WE_UKF_Q_WIND, 2); + MAT_EL(ukf_params.Q, 4, 4, 7) = powf(WE_UKF_Q_WIND, 2); + MAT_EL(ukf_params.Q, 5, 5, 7) = powf(WE_UKF_Q_WIND, 2); + MAT_EL(ukf_params.Q, 6, 6, 7) = powf(WE_UKF_Q_VA_SCALE, 2); + + ukf_init.ki = WE_UKF_KI; + ukf_init.alpha = WE_UKF_ALPHA; + ukf_init.beta = WE_UKF_BETA; + ukf_params.dt = WIND_ESTIMATOR_PERIODIC_PERIOD; // actually measured later + + wind_estimator.data_available = false; + wind_estimator.reset = false; + + wind_estimator.r_gs = WE_UKF_R_GS; + wind_estimator.r_va = WE_UKF_R_VA; + wind_estimator.r_aoa = WE_UKF_R_AOA; + wind_estimator.r_ssa = WE_UKF_R_SSA; + wind_estimator.q_va = WE_UKF_Q_VA; + wind_estimator.q_wind = WE_UKF_Q_WIND; + wind_estimator.q_va_scale = WE_UKF_Q_VA_SCALE; + + time_step_before = 0; +} + +// Run one step and save result +static inline void wind_estimator_step(void) +{ +#if LOG_WIND_ESTIMATOR + if (LogFileIsOpen()) { + if (!log_we_started) { + // print header with initial parameters + int i; + PrintLog(pprzLogFile, "# Wind Estimator\n#\n"); + PrintLog(pprzLogFile, "# Q = diag( "); + for (i = 0; i < 7; i++) + PrintLog(pprzLogFile, "%.8f ", MAT_EL(ukf_params.Q, i, i, 7)); + PrintLog(pprzLogFile, ")\n"); + PrintLog(pprzLogFile, "# R = diag( "); + for (i = 0; i < 5; i++) + PrintLog(pprzLogFile, "%.8f ", MAT_EL(ukf_params.R, i, i, 5)); + PrintLog(pprzLogFile, ")\n"); + PrintLog(pprzLogFile, "# ki = %.5f\n", ukf_init.ki); + PrintLog(pprzLogFile, "# alpha = %.5f\n", ukf_init.alpha); + PrintLog(pprzLogFile, "# beta = %.5f\n", ukf_init.beta); + PrintLog(pprzLogFile, "#\n"); + PrintLog(pprzLogFile, "p q r ax ay az q1 q2 q3 q4 vkx vky vkz va aoa ssa u v w wx wy wz vas t\n"); + log_we_started = true; + } + PrintLog(pprzLogFile, "%.5f %.5f %.5f %.3f %.3f %.3f %.4f %.4f %.4f %.4f %.5f %.5f %.5f %.5f %.5f %.5f ", + ukf_U.rates[0], + ukf_U.rates[1], + ukf_U.rates[2], + ukf_U.accel[0], + ukf_U.accel[1], + ukf_U.accel[2], + ukf_U.q[0], + ukf_U.q[1], + ukf_U.q[2], + ukf_U.q[3], + ukf_U.vk[0], + ukf_U.vk[1], + ukf_U.vk[2], + ukf_U.va, + ukf_U.aoa, + ukf_U.sideslip + ); + } +#endif + + // estimate wind if airspeed is high enough + if (ukf_U.va > 5.0f) { + // run estimation + UKF_Wind_Estimator_step(); + // update output structure + wind_estimator.airspeed.x = ukf_Y.xout[0]; + wind_estimator.airspeed.y = ukf_Y.xout[1]; + wind_estimator.airspeed.z = ukf_Y.xout[2]; + wind_estimator.wind.x = ukf_Y.xout[3]; + wind_estimator.wind.y = ukf_Y.xout[4]; + wind_estimator.wind.z = ukf_Y.xout[5]; + // set ready flag + wind_estimator.data_available = true; + } else { + wind_estimator.airspeed.x = 0.f; + wind_estimator.airspeed.y = 0.f; + wind_estimator.airspeed.z = 0.f; + wind_estimator.wind.x = 0.f; + wind_estimator.wind.y = 0.f; + wind_estimator.wind.z = 0.f; + } + +#if LOG_WIND_ESTIMATOR + if (log_we_started) { + PrintLog(pprzLogFile, "%.3f %.3f %.3f %.3f %.3f %.3f %.3f %d\n", + wind_estimator.airspeed.x, + wind_estimator.airspeed.y, + wind_estimator.airspeed.z, + wind_estimator.wind.x, + wind_estimator.wind.y, + wind_estimator.wind.z, + ukf_Y.xout[6], + time_step_before + ); + } +#endif +} + +#include "subsystems/imu.h" +/*----------------wind_estimator_periodic-------------*/ +/* Put Data from State in struct use by the Thread */ +/*----------------------------------------------------*/ +void wind_estimator_periodic(void) +{ + // try to lock mutex without blocking + // if it fails, it means that previous computation took too long +#ifndef SITL + if (chMtxTryLock(&we_ukf_mtx)) { +#endif + if (wind_estimator.reset) { + init_calculator(); + } + // update input vector from state interface + ukf_U.rates[0] = stateGetBodyRates_f()->p; // rad/s + ukf_U.rates[1] = stateGetBodyRates_f()->q; // rad/s + ukf_U.rates[2] = stateGetBodyRates_f()->r; // rad/s + // transform data in body frame + struct FloatVect3 accel_ned = { + stateGetAccelNed_f()->x, + stateGetAccelNed_f()->y, + stateGetAccelNed_f()->z + }; +// struct FloatRMat *ned_to_body = stateGetNedToBodyRMat_f(); +// struct FloatVect3 accel_body; +// float_rmat_vmult(&accel_body, ned_to_body, &accel_ned); + + ///// IMU test + struct FloatVect3 accel_body = { + .x = ACCEL_FLOAT_OF_BFP(imu.accel.x), + .y = ACCEL_FLOAT_OF_BFP(imu.accel.y), + .z = ACCEL_FLOAT_OF_BFP(imu.accel.z) + }; + struct FloatVect3 tmp = accel_body; + //struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&body_to_imu); + //int32_rmat_transp_vmult(&accel_body, body_to_imu_rmat, accel); + // tilt and remove gravity + struct FloatVect3 gravity_ned = { 0.f, 0.f, 9.81f }; + struct FloatVect3 gravity_body = { 0.f, 0.f, 0.f }; +#if 0 + struct FloatRMat *ned_to_body_rmat = stateGetNedToBodyRMat_f(); + float_rmat_vmult(&gravity_body, ned_to_body_rmat, &gravity_ned); + VECT3_ADD(accel_body, gravity_body); +#else + struct FloatEulers *b2i_e = stateGetNedToBodyEulers_f(); + accel_body.x += -9.81*sinf(b2i_e->theta); + accel_body.y += 9.81*cosf(b2i_e->theta)*sinf(b2i_e->phi); + accel_body.z += 9.81*cosf(b2i_e->theta)*cosf(b2i_e->phi); +#endif + ///// End test + + ukf_U.accel[0] = accel_body.x; // m/s^2 + ukf_U.accel[1] = accel_body.y; // m/s^2 + ukf_U.accel[2] = accel_body.z; // m/s^2 + ukf_U.q[0] = stateGetNedToBodyQuat_f()->qi; + ukf_U.q[1] = stateGetNedToBodyQuat_f()->qx; + ukf_U.q[2] = stateGetNedToBodyQuat_f()->qy; + ukf_U.q[3] = stateGetNedToBodyQuat_f()->qz; + ukf_U.vk[0] = stateGetSpeedNed_f()->x; // m/s + ukf_U.vk[1] = stateGetSpeedNed_f()->y; // m/s + ukf_U.vk[2] = stateGetSpeedNed_f()->z; // m/s + ukf_U.va = stateGetAirspeed_f(); // m/s + ukf_U.aoa = stateGetAngleOfAttack_f(); // rad. + ukf_U.sideslip = stateGetSideslip_f(); // rad. + + ///// TEST + // update input vector from state interface + //ukf_U.rates[0] = 0.; + //ukf_U.rates[1] = 0.; + //ukf_U.rates[2] = 0.; + //ukf_U.accel[0] = 0.; + //ukf_U.accel[1] = 0.; + //ukf_U.accel[2] = 0.; + //ukf_U.q[0] = 1.0; + //ukf_U.q[1] = 0.; + //ukf_U.q[2] = 0.; + //ukf_U.q[3] = 0.; + //ukf_U.vk[0] = 15.; + //ukf_U.vk[1] = 0.; + //ukf_U.vk[2] = 0.; + //ukf_U.va = 15.; + //ukf_U.aoa = 0.; + //ukf_U.sideslip = 0.; + + //DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, 16, (float *)(&ukf_U)); + + float msg[] = { + tmp.x, + tmp.y, + tmp.z, + gravity_body.x, + gravity_body.y, + gravity_body.z, + accel_body.x, + accel_body.y, + accel_body.z, + accel_ned.x, + accel_ned.y, + accel_ned.z + }; + DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, 12, msg); + + // compute DT and set input vector + if (time_step_before == 0) { + ukf_params.dt = WIND_ESTIMATOR_PERIODIC_PERIOD; + time_step_before = get_sys_time_msec(); + } else { + ukf_params.dt = (get_sys_time_msec() - time_step_before) / 1000.f; + time_step_before = get_sys_time_msec(); + } +#ifndef SITL + chMtxUnlock(&we_ukf_mtx); + // send signal to computation thread + chSemSignal(&we_ukf_sem); + } +#else + wind_estimator_step(); +#endif +} + +/*----------------- wind_estimator_event -------------*/ +/* Fonction put data in State */ +/*----------------------------------------------------*/ +void wind_estimator_event(void) +{ + if (wind_estimator.data_available) { +#ifndef SITL + if (chMtxTryLock(&we_ukf_mtx)) { +#endif + struct FloatVect2 wind_ne = { + wind_estimator.wind.x, + wind_estimator.wind.y + }; + stateSetHorizontalWindspeed_f(&wind_ne); //NEED CHECK + stateSetVerticalWindspeed_f(wind_estimator.wind.z); //NEED CHECK + + // TODO do something with corrected airspeed norm + +#if SEND_WIND_ESTIMATOR + // send over telemetry + send_wind_estimator(&(DefaultChannel).trans_tx, &(DefaultDevice).device); +#endif + + wind_estimator.data_available = false; +#ifndef SITL + chMtxUnlock(&we_ukf_mtx); + } +#endif + } +} + +/*------------------thd_windestimate------------------*/ +/* Thread fonction */ +/*----------------------------------------------------*/ +#ifndef SITL +static void thd_windestimate(void *arg) +{ + (void) arg; + chRegSetThreadName("wind estimation"); + + while (true) { + // wait for incoming request signal + chSemWait(&we_ukf_sem); + // lock state + chMtxLock(&we_ukf_mtx); + // run estimation step + wind_estimator_step(); + // unlock + chMtxUnlock(&we_ukf_mtx); + } +} +#endif + +/*-----------------wind_estimator_init----------------*/ +/* Init the Thread and the calculator */ +/*----------------------------------------------------*/ +void wind_estimator_init(void) +{ + init_calculator(); + +#if LOG_WIND_ESTIMATOR + log_we_started = false; +#if SITL + // open log file for writing + // path should be specified in airframe file + uint32_t counter = 0; + char filename[512]; + snprintf(filename, 512, "%s/we_ukf_%05d.csv", STRINGIFY(WE_LOG_PATH), counter); + // check availale name + while ((pprzLogFile = fopen(filename, "r"))) { + fclose(pprzLogFile); + snprintf(filename, 512, "%s/we_ukf_%05d.csv", STRINGIFY(WE_LOG_PATH), ++counter); + } + pprzLogFile = fopen(filename, "w"); + if (pprzLogFile == NULL) { + printf("Failed to open WE log file '%s'\n",filename); + } else { + printf("Opening WE log file '%s'\n",filename); + } +#endif +#endif + +#ifndef SITL + // Start wind estimation thread + chThdCreateStatic(wa_thd_windestimation, sizeof(wa_thd_windestimation), + NORMALPRIO + 2, thd_windestimate, NULL); +#endif + +#if PERIODIC_TELEMETRY + // register for periodic telemetry (for logging with flight recorder) + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_WIND_INFO_RET, send_wind_estimator); +#endif +} + +void wind_estimator_Set_R_GS(float _v) +{ + wind_estimator.r_gs = _v; + MAT_EL(ukf_params.R, 0, 0, 6) = powf(_v, 2); + MAT_EL(ukf_params.R, 1, 1, 6) = powf(_v, 2); + MAT_EL(ukf_params.R, 2, 2, 6) = powf(_v, 2); +} + +void wind_estimator_Set_R_VA(float _v) +{ + wind_estimator.r_va = _v; + MAT_EL(ukf_params.R, 3, 3, 6) = powf(_v, 2); +} + +void wind_estimator_Set_R_AOA(float _v) +{ + wind_estimator.r_aoa = _v; + MAT_EL(ukf_params.R, 4, 4, 6) = powf(_v, 2); +} + +void wind_estimator_Set_R_SSA(float _v) +{ + wind_estimator.r_ssa = _v; + MAT_EL(ukf_params.R, 5, 5, 6) = powf(_v, 2); +} + +void wind_estimator_Set_Q_VA(float _v) +{ + wind_estimator.q_va = _v; + MAT_EL(ukf_params.Q, 0, 0, 7) = powf(_v, 2); + MAT_EL(ukf_params.Q, 1, 1, 7) = powf(_v, 2); + MAT_EL(ukf_params.Q, 2, 2, 7) = powf(_v, 2); +} + +void wind_estimator_Set_Q_WIND(float _v) +{ + wind_estimator.q_wind = _v; + MAT_EL(ukf_params.Q, 3, 3, 7) = powf(_v, 2); + MAT_EL(ukf_params.Q, 4, 4, 7) = powf(_v, 2); + MAT_EL(ukf_params.Q, 5, 5, 7) = powf(_v, 2); +} + +void wind_estimator_Set_Q_VA_SCALE(float _v) +{ + wind_estimator.q_va_scale = _v; + MAT_EL(ukf_params.Q, 6, 6, 7) = powf(_v, 2); +} + diff --git a/sw/airborne/modules/meteo/wind_estimator.h b/sw/airborne/modules/meteo/wind_estimator.h new file mode 100644 index 0000000000..16be21b3c4 --- /dev/null +++ b/sw/airborne/modules/meteo/wind_estimator.h @@ -0,0 +1,66 @@ +/* + * Copyright (C) 2016 Johan Maurin, Gautier Hattenberger + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file "modules/meteo/wind_estimator.h" + * @author Maurin + * + * Wind Estimator based on generated library from Matlab + */ + +#ifndef WIND_ESTIMATOR_H +#define WIND_ESTIMATOR_H + +#include "std.h" +#include "math/pprz_algebra_float.h" + +struct WindEstimator { + struct FloatVect3 airspeed; ///< airspeed vector in body frame + struct FloatVect3 wind; ///< wind vector in NED frame + bool data_available; ///< new data available + bool reset; ///< reset filter flag + + // filter parameters + float r_gs; ///< noise associated to ground speed measurement + float r_va; ///< noise associated to airspeed norm measurement + float r_aoa; ///< noise associated to angle of attack measurement + float r_ssa; ///< noise associated to sideslip angle measurement + float q_va; ///< noise associated to airspeed vector model + float q_wind; ///< noise associated to wind vector model + float q_va_scale; ///< noise associated to airspeed scale factor model +}; + +extern struct WindEstimator wind_estimator; + +extern void wind_estimator_init(void); +extern void wind_estimator_periodic(void); +extern void wind_estimator_event(void); + +// paramenters settings handler +extern void wind_estimator_Set_R_GS(float _v); +extern void wind_estimator_Set_R_VA(float _v); +extern void wind_estimator_Set_R_AOA(float _v); +extern void wind_estimator_Set_R_SSA(float _v); +extern void wind_estimator_Set_Q_VA(float _v); +extern void wind_estimator_Set_Q_WIND(float _v); +extern void wind_estimator_Set_Q_VA_SCALE(float _v); + +#endif + diff --git a/sw/airborne/modules/sensors/aoa_pwm.c b/sw/airborne/modules/sensors/aoa_pwm.c index da05135060..b6897e9308 100644 --- a/sw/airborne/modules/sensors/aoa_pwm.c +++ b/sw/airborne/modules/sensors/aoa_pwm.c @@ -23,6 +23,11 @@ * @author Jean-François Erdelyi * @brief Angle of Attack sensor on PWM * +* Driver for a PWM based angle of attack sensor +* A second sensor can be defined for the sideslip angle +* It is assumed that both sensors are the same, only +* sensitivity, offset and direction can differ. +* * SENSOR, exemple : US DIGITAL MA3-P12-125-B * @see http://www.usdigital.com/products/encoders/absolute/rotary/shaft/ma3 */ @@ -38,6 +43,8 @@ bool log_started; #endif +/* Config parameters for angle of attack sensor (mandatory) */ + #ifndef AOA_PWM_CHANNEL #error "AOA_PWM_CHANNEL needs to be defined to use AOA_pwm module" #endif @@ -83,16 +90,57 @@ bool log_started; struct Aoa_Pwm aoa_pwm; + +/* Config parameters for sideslip angle sensor (optional) */ + +#if defined USE_SIDESLIP && !(defined SSA_PWM_CHANNEL) +#error "SSA_PWM_CHANNEL needs to be defined to use sideslip sensor" +#endif + +// Default extra offset that can be ajusted from settings +#ifndef SSA_OFFSET +#define SSA_OFFSET 0.0f +#endif +// Default filter value +#ifndef SSA_FILTER +#define SSA_FILTER 0.0f +#endif +// Default sensitivity (2*pi on a PWM of period AOA_PWM_PERIOD) +#ifndef SSA_SENS +#define SSA_SENS ((2.0f*M_PI)/AOA_PWM_PERIOD) +#endif +// Set SSA_REVERSE to TRUE to change rotation direction +#if SSA_REVERSE +#define SSA_SIGN -1 +#else +#define SSA_SIGN 1 +#endif +struct Aoa_Pwm ssa_pwm; + + +/* telemetry */ +enum Aoa_Type aoa_send_type; + #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" static void send_aoa(struct transport_tx *trans, struct link_device *dev) { - pprz_msg_send_AOA(trans, dev, AC_ID, &aoa_pwm.raw, &aoa_pwm.angle); + // FIXME use a second message, more fields or add a sensor ID to send sideslip + switch (aoa_send_type) { + case SEND_TYPE_SIDESLIP: + pprz_msg_send_AOA(trans, dev, AC_ID, &ssa_pwm.raw, &ssa_pwm.angle); + break; + case SEND_TYPE_AOA: + default: + pprz_msg_send_AOA(trans, dev, AC_ID, &aoa_pwm.raw, &aoa_pwm.angle); + break; + } } #endif +/* init */ void aoa_pwm_init(void) { aoa_pwm.offset = AOA_OFFSET; @@ -100,22 +148,28 @@ void aoa_pwm_init(void) aoa_pwm.sens = AOA_SENS; aoa_pwm.angle = 0.0f; aoa_pwm.raw = 0.0f; + ssa_pwm.offset = SSA_OFFSET; + ssa_pwm.filter = SSA_FILTER; + ssa_pwm.sens = SSA_SENS; + ssa_pwm.angle = 0.0f; + ssa_pwm.raw = 0.0f; #if LOG_AOA log_started = false; #endif + aoa_send_type = SEND_TYPE_AOA; #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AOA, send_aoa); #endif } +/* update, log and send */ void aoa_pwm_update(void) { static float prev_aoa = 0.0f; - // raw duty cycle in usec - uint32_t duty_raw = get_pwm_input_duty_in_usec(AOA_PWM_CHANNEL); + uint32_t aoa_duty_raw = get_pwm_input_duty_in_usec(AOA_PWM_CHANNEL); // remove some offset if needed - aoa_pwm.raw = duty_raw - AOA_PWM_OFFSET; + aoa_pwm.raw = aoa_duty_raw - AOA_PWM_OFFSET; // FIXME for some reason, the last value of the MA3 encoder is not 4096 but 4097 // this case is not handled since we don't care about angles close to +- 180 deg aoa_pwm.angle = AOA_SIGN * (((float)aoa_pwm.raw * aoa_pwm.sens) - aoa_pwm.offset - AOA_ANGLE_OFFSET); @@ -127,18 +181,36 @@ void aoa_pwm_update(void) { stateSetAngleOfAttack_f(aoa_pwm.angle); #endif +#if USE_SIDESLIP + static float prev_ssa = 0.0f; + // raw duty cycle in usec + uint32_t ssa_duty_raw = get_pwm_input_duty_in_usec(SSA_PWM_CHANNEL); + + // remove some offset if needed + ssa_pwm.raw = ssa_duty_raw - AOA_PWM_OFFSET; + // FIXME for some reason, the last value of the MA3 encoder is not 4096 but 4097 + // this case is not handled since we don't care about angles close to +- 180 deg + ssa_pwm.angle = SSA_SIGN * (((float)ssa_pwm.raw * ssa_pwm.sens) - ssa_pwm.offset - AOA_ANGLE_OFFSET); + // filter angle + ssa_pwm.angle = ssa_pwm.filter * prev_ssa + (1.0f - ssa_pwm.filter) * ssa_pwm.angle; + prev_ssa = ssa_pwm.angle; + + stateSetSideslip_f(ssa_pwm.angle); +#endif + #if SEND_SYNC_AOA - RunOnceEvery(10, DOWNLINK_SEND_AOA(DefaultChannel, DefaultDevice, &aoa_pwm.raw, &aoa_pwm.angle)); + RunOnceEvery(10, send_aoa(&(DefaultChannel).trans_tx, &(DefaultDevice).device)); #endif #if LOG_AOA if(pprzLogFile != -1) { if (!log_started) { - sdLogWriteLog(pprzLogFile, "AOA_PWM: ANGLE(deg) RAW(int16)\n"); + sdLogWriteLog(pprzLogFile, "AOA_PWM: AOA_ANGLE(deg) AOA_RAW(int16) SSA_ANGLE(deg) SSA_RAW(int16)\n"); log_started = true; } else { - float angle = DegOfRad(aoa_pwm.angle); - sdLogWriteLog(pprzLogFile, "AOA_PWM: %.3f %d\n", angle, aoa_pwm.raw); + float aoa_angle = DegOfRad(aoa_pwm.angle); + float ssa_angle = DegOfRad(ssa_pwm.angle); + sdLogWriteLog(pprzLogFile, "AOA_PWM: %.3f %d %.3f %d\n", aoa_angle, aoa_pwm.raw, ssa_angle, ssa_pwm.raw); } } #endif diff --git a/sw/airborne/modules/sensors/aoa_pwm.h b/sw/airborne/modules/sensors/aoa_pwm.h index 7834e2e2fc..7aa986b982 100644 --- a/sw/airborne/modules/sensors/aoa_pwm.h +++ b/sw/airborne/modules/sensors/aoa_pwm.h @@ -23,6 +23,11 @@ * @author Jean-François Erdelyi * @brief Angle of Attack sensor on PWM * +* Driver for a PWM based angle of attack sensor +* A second sensor can be defined for the sideslip angle +* It is assumed that both sensors are the same, only +* sensitivity, offset and direction can differ. +* * SENSOR, example : US DIGITAL MA3-P12-125-B */ @@ -44,7 +49,16 @@ struct Aoa_Pwm { float filter; }; -extern struct Aoa_Pwm aoa_pwm; +extern struct Aoa_Pwm aoa_pwm; // angle of attack sensor +extern struct Aoa_Pwm ssa_pwm; // sideslip angle sensor + +/** Selection of sensor type to be send over telemetry + */ +enum Aoa_Type { + SEND_TYPE_AOA, + SEND_TYPE_SIDESLIP +}; +extern enum Aoa_Type aoa_send_type; extern void aoa_pwm_init(void); extern void aoa_pwm_update(void); diff --git a/sw/simulator/mesonh/__init__.py b/sw/simulator/mesonh/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sw/simulator/mesonh/lauch_test b/sw/simulator/mesonh/lauch_test new file mode 100755 index 0000000000..c6abc15cee --- /dev/null +++ b/sw/simulator/mesonh/lauch_test @@ -0,0 +1,2 @@ +python mesonh.py -t 3600 -f ARMHR.1.sc464.001diaKCL.nc ARMHR.1.sc464.002diaKCL.nc ARMHR.1.sc464.003diaKCL.nc ARMHR.1.sc464.004diaKCL.nc ARMHR.1.sc464.005diaKCL.nc ARMHR.1.sc464.006diaKCL.nc ARMHR.1.sc464.007diaKCL.nc ARMHR.1.sc464.008diaKCL.nc -x 2.2 -y 2.2 + diff --git a/sw/simulator/mesonh/mesonh.py b/sw/simulator/mesonh/mesonh.py new file mode 100755 index 0000000000..3f9dd008c9 --- /dev/null +++ b/sw/simulator/mesonh/mesonh.py @@ -0,0 +1,131 @@ +from __future__ import absolute_import, print_function, division +from mesonh_atmosphere import MesoNHAtmosphere +import os +import sys +import signal +import time +import socket +import struct +import cmath +import numpy as np +from os import getenv +# if PAPARAZZI_SRC not set, then assume the tree containing this +# file is a reasonable substitute +PPRZ_SRC = getenv("PAPARAZZI_SRC", os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), '../../../'))) +sys.path.append(PPRZ_SRC + "/sw/ext/pprzlink/lib/v1.0/python") + +from pprzlink.ivy import IvyMessagesInterface +from pprzlink.message import PprzMessage + +M_IN_KM = 1000. + +atm = None +origin = np.array([0, 0, 0, 0]) +scale = np.array([1., 1/M_IN_KM, 1/M_IN_KM, 1/M_IN_KM]) + +start_time = time.time() + + +def get_wind(east, north, up): + t = time.time() - start_time + print("east :",east) + print("north :",north) + print("up :",up) + loc = np.array([t, up, east, north]) + loc = loc*scale + origin + print("loc:",loc) + weast, wnorth, wup = atm.get_wind(loc) + return weast, wnorth, wup + + +def ivy_request_callback(sender, msg, resp, *args, **kwargs): + """ + Ivy Callback for Paparazzi Requests + """ + + if msg.msg_class == "ground" and msg.name == "WORLD_ENV_REQ": + return worldenv_cb(msg, resp) + else: + return None + + +#def worldenv_cb(m, r): +def worldenv_cb(ac_id, msg): + """ + Callback for paparazzi WORLD_ENV requests + """ + # request location (in meters) + east, north, up = float(msg.get_field(3)),\ + float(msg.get_field(4)),\ + float(msg.get_field(5)) + up *= -1 + # convert in km + translation with mesoNH origin + weast, wnorth, wup = get_wind(east, north, up) + print("wind_est:") + print(weast) + print(wnorth) + print(wup) + msg_back=PprzMessage("ground", "WORLD_ENV") + msg_back.set_value_by_name("wind_east",weast) + msg_back.set_value_by_name("wind_north",wnorth) + msg_back.set_value_by_name("wind_up",wup) + msg_back.set_value_by_name("ir_contrast",400) + msg_back.set_value_by_name("time_scale",1) + msg_back.set_value_by_name("gps_availability",1) + ivy.send(msg_back,None) + + +def signal_handler(signal, frame): + print('\nShutting down IVY...') + ivy.shutdown() + print("Done.") + + +def main(): + # parse arguments + import argparse as ap + + argp = ap.ArgumentParser(description="Environment variables provider " + "for Paparazzi simulation from MesoNH data") + + argp.add_argument("-t", "--time-step", required=True, type=int, + help="Duration of a time step between MesoNH Files.") + argp.add_argument("-f", "--files", required=True, nargs='+', + help="MesoNH netCDF files, in temporal ordering") + argp.add_argument("-x", "--origin-x", required=False, type=float, + default=0., + help="Origin translation x.") + argp.add_argument("-y", "--origin-y", required=False, type=float, + default=0., + help="Origin translation y.") + argp.add_argument("-z", "--origin-z", required=False, type=float, + default=0., + help="Origin translation z.") + arguments = argp.parse_args() + + print(arguments) + + # register signal handler for ctrl+c to stop the program + signal.signal(signal.SIGINT, signal_handler) + + # origin for translation from paparazzi to mesoNH frame + global origin + origin = np.array([0, arguments.origin_z, arguments.origin_x, arguments.origin_y]) + + # build atmosphere simulation source + global atm + atm = MesoNHAtmosphere(arguments.files, arguments.time_step, tinit=0) + + # init ivy and register callback for WORLD_ENV_REQ and NPS_SPEED_POS + global ivy + ivy = IvyMessagesInterface("MesoNH"); + ivy.subscribe(worldenv_cb,'(.* WORLD_ENV_REQ .*)') + + # wait for ivy to stop + from ivy.std_api import IvyMainLoop + + signal.pause() + + +if __name__ == '__main__': + main() diff --git a/sw/simulator/mesonh/mesonh_atmosphere.py b/sw/simulator/mesonh/mesonh_atmosphere.py new file mode 100644 index 0000000000..ed07a7a022 --- /dev/null +++ b/sw/simulator/mesonh/mesonh_atmosphere.py @@ -0,0 +1,147 @@ +""" + Reading mesoNH netCDF files and providing an interface to read chunks +of data and interpolate points. + + TODO : test with large netCDF files, add mapping of variables as an argument. +""" + +from __future__ import division, print_function, absolute_import +from netCDF4 import MFDataset, Dataset +import numpy as np +from scipy.interpolate import RegularGridInterpolator + + +def find_lt(a, x): + 'Find rightmost value less than x' + return np.searchsorted(a, x, side='left')-1 + + +def find_le(a, x): + 'Find rightmost value less than or equal to x' + return np.searchsorted(a, x, side='right')-1 + + +def find_gt(a, x): + 'Find leftmost value greater than x' + return np.searchsorted(a, x, side='right') + + +def find_ge(a, x): + 'Find leftmost item greater than or equal to x' + return np.searchsorted(a, x, side='left') + +WX = 'UT' # Wind x,y and z composants +WY = 'VT' +WZ = 'WT' +PTEMP = 'THT' # potential temperature +RVAP = 'RVT' # vapor mixing ratio +LWATER = 'RCT' # liquid water +ZSCALE = 'VLEV' # vertical level (height) scale +XSCALE = 'S_N_direction' # south -> north axis scale +YSCALE = 'W_E_direction' # west -> east axis scale +TIME = 'time' # time dimension +X = 2 +Y = 3 +Z = 1 +T = 0 + + +class MesoNHAtmosphere: + """ + Interpolation in space-time from mesoNH data over a grid. + + The functions valued on the grid are assumed to be periodic in the + x and y axis (south->north and west->east) : + f(x)=f(x+xmax-xmin) where [xmin,xmax] are the bounds on x + of the mesoNH grid. + + Clipping is done on the z (height) and time axis : + f(z)=f(min(max(z,zmin),zmax)) where [zmin,zmax] are the bound on z + of the mesoNH grid. + + Two interpolation methods are currently supported : nearest neighbour + and linear. + + Uses RegularGridInterpolator from scipy package. + + """ + data = [] + + interpolator = None + boundsmax = None + boundsmin = None + + def __init__(self, files, tstep, tinit=0): + if np.isscalar(files): + self.data = Dataset(files) + else: + self.data = MFDataset(files) + pz = self.data.variables[ZSCALE][:, 0, 0] + px = self.data.variables[XSCALE] + py = self.data.variables[YSCALE] + pt = np.array([tinit + i * tstep for i in + range(self.data.variables[TIME].shape[0])]) + self.boundsmax = [pt[-1], pz[-1], px[-1], py[-1]] + self.boundsmin = [pt[0], pz[0], px[0], py[0]] + + self.grid_coordinates = (pt, pz, px, py) + self.grid_shape = (len(pt), len(pz), len(px), len(py)) + + def get_points(self, points, var, method='nearest'): + """ Get value of variable on points + + Arguments: + points: a ndarray containing the point coordinates on the last + dimension + var: the name of the variable in the mesoNH file(s) + method: 'nearest' and 'linear' interpolation are currently supported + """ + points = np.array(points) + p = self._apply_bounds(points) + caxes = tuple(range(p.ndim-1)) + bounds = zip(p.min(axis=caxes), p.max(axis=caxes)) + interpolator = self._get_interpolator(bounds, var, method) + return interpolator(p, method).squeeze() + + def _get_interpolator(self, bounds, var, method="nearest"): + slice_indexes, coordinates = self._slicyfy(bounds) + values = self._get_var_values(var, slice_indexes) + ip = RegularGridInterpolator(coordinates, values, method) + return ip + + def _slicyfy(self, bounds): + slice_indexes = () + coordinates = () + for d, b in enumerate(bounds): + dslice = slice(find_le(self.grid_coordinates[d], b[0]), + find_gt(self.grid_coordinates[d], b[1])+1) + slice_indexes += dslice, + coordinates += self.grid_coordinates[d][dslice], + + return slice_indexes, coordinates + + def _get_var_values(self, var, idx=Ellipsis): + return self.data[var][idx] + + def _apply_bounds(self, point): + x = point[..., X] + y = point[..., Y] + z = point[..., Z] + t = point[..., T] + + p = np.ndarray(point.shape) + p[..., T] = np.clip(t, self.boundsmin[T], self.boundsmax[T]) + p[..., Z] = np.clip(z, self.boundsmin[Z], self.boundsmax[Z]) + p[..., X] = (x-self.boundsmin[X]) % (self.boundsmax[X]-self.boundsmin[X])\ + + self.boundsmin[X] + p[..., Y] = (y-self.boundsmin[Y]) % (self.boundsmax[Y]-self.boundsmin[Y])\ + + self.boundsmin[Y] + + return p + + def get_wind(self, points, method='nearest'): + """Convenience method for getting 3D wind. See get_points.""" + return np.array((self.get_points(points, WX, method), + self.get_points(points, WY, method), + self.get_points(points, WZ, method) + )) diff --git a/sw/simulator/nps/nps_autopilot_fixedwing.c b/sw/simulator/nps/nps_autopilot_fixedwing.c index 8309fe3fe3..f02a2d0175 100644 --- a/sw/simulator/nps/nps_autopilot_fixedwing.c +++ b/sw/simulator/nps/nps_autopilot_fixedwing.c @@ -138,7 +138,7 @@ void nps_autopilot_run_step(double time) AbiSendMsgTEMPERATURE(BARO_SIM_SENDER_ID, (float)sensors.temp.value); } -#if USE_AIRSPEED +#if USE_AIRSPEED || USE_NPS_AIRSPEED if (nps_sensors_airspeed_available()) { stateSetAirspeed_f((float)sensors.airspeed.value); Fbw(event_task); @@ -165,6 +165,22 @@ void nps_autopilot_run_step(double time) } #endif +#if USE_NPS_AOA + if (nps_sensors_aoa_available()) { + stateSetAngleOfAttack_f((float)sensors.aoa.value); + Fbw(event_task); + Ap(event_task); + } +#endif + +#if USE_NPS_SIDESLIP + if (nps_sensors_sideslip_available()) { + stateSetSideslip_f((float)sensors.sideslip.value); + Fbw(event_task); + Ap(event_task); + } +#endif + if (nps_bypass_ahrs) { sim_overwrite_ahrs(); } diff --git a/sw/simulator/nps/nps_fdm.h b/sw/simulator/nps/nps_fdm.h index f76b249450..fbd4e218b6 100644 --- a/sw/simulator/nps/nps_fdm.h +++ b/sw/simulator/nps/nps_fdm.h @@ -112,6 +112,8 @@ struct NpsFdm { double dynamic_pressure; ///< dynamic pressure in Pascal double temperature; ///< current temperature in degrees Celcius double pressure_sl; ///< pressure at sea level in Pascal + double aoa; ///< angle of attack in rad + double sideslip; ///< sideslip angle in rad // Control surface positions (normalized values) float elevator; diff --git a/sw/simulator/nps/nps_fdm_jsbsim.cpp b/sw/simulator/nps/nps_fdm_jsbsim.cpp index 5de4c08905..2760222c64 100644 --- a/sw/simulator/nps/nps_fdm_jsbsim.cpp +++ b/sw/simulator/nps/nps_fdm_jsbsim.cpp @@ -42,6 +42,7 @@ #include #include #include +#include #include #include @@ -463,6 +464,12 @@ static void fetch_state(void) fdm.dynamic_pressure = PascalOfPsf(FDMExec->GetAuxiliary()->Getqbar()); fdm.temperature = CelsiusOfRankine(FDMExec->GetAtmosphere()->GetTemperature()); + /* + * angle of attack and SlideSlip. + */ + fdm.aoa= FDMExec->GetPropertyManager()->GetNode("aero/alpha-rad")->getDoubleValue(); + fdm.sideslip = FDMExec->GetPropertyManager()->GetNode("aero/beta-rad")->getDoubleValue(); + /* * Control surface positions * diff --git a/sw/simulator/nps/nps_sensor_aoa.c b/sw/simulator/nps/nps_sensor_aoa.c new file mode 100644 index 0000000000..5524cb69e6 --- /dev/null +++ b/sw/simulator/nps/nps_sensor_aoa.c @@ -0,0 +1,77 @@ +/* + * Copyright (C) 2016 Johan Maurin, Gautier Hattenberger + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file nps_sensor_aoa.c + * + * Simulated Angle of Attack of the Wind for NPS simulator. + * + */ + +#include "nps_sensor_aoa.h" + +#include "generated/airframe.h" + +#include "std.h" +#include "nps_fdm.h" +#include "nps_random.h" +#include NPS_SENSORS_PARAMS + +/// 10Hz default +#ifndef NPS_AOA_DT +#define NPS_AOA_DT 0.01 +#endif + +/// standard deviation in radian (default 0.001 rad) +#ifndef NPS_AOA_NOISE_STD_DEV +#define NPS_AOA_NOISE_STD_DEV 0.001 +#endif + +#ifndef NPS_AOA_OFFSET +#define NPS_AOA_OFFSET 0 +#endif + + +void nps_sensor_aoa_init(struct NpsSensorAngleOfAttack *aoa, double time) +{ + aoa->value = 0.; + aoa->offset = NPS_AOA_OFFSET; + aoa->noise_std_dev = NPS_AOA_NOISE_STD_DEV; + aoa->next_update = time; + aoa->data_available = FALSE; +} + + +void nps_sensor_aoa_run_step(struct NpsSensorAngleOfAttack *aoa, double time) +{ + + if (time < aoa->next_update) { + return; + } + + /* equivalent airspeed + sensor offset */ + aoa->value = fdm.aoa + aoa->offset; + /* add noise with std dev rad */ + aoa->value += get_gaussian_noise() * aoa->noise_std_dev; + + aoa->next_update += NPS_AOA_DT; + aoa->data_available = TRUE; +} + diff --git a/sw/simulator/nps/nps_sensor_aoa.h b/sw/simulator/nps/nps_sensor_aoa.h new file mode 100644 index 0000000000..c6be3f9645 --- /dev/null +++ b/sw/simulator/nps/nps_sensor_aoa.h @@ -0,0 +1,48 @@ +/* + * Copyright (C) 2016 Johan Maurin, Gautier Hattenberger + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file nps_sensor_aoa.h + * + * Simulated Angle Of Attack of the Wind for NPS simulator. + * + */ + +#ifndef NPS_SENSOR_AOA_H +#define NPS_SENSOR_AOA_H + +#include "math/pprz_algebra.h" +#include "math/pprz_algebra_double.h" +#include "math/pprz_algebra_float.h" +#include "std.h" + +struct NpsSensorAngleOfAttack { + double value; ///< angle of attack reading in radian + double offset; ///< offset in meters/second + double noise_std_dev; ///< noise standard deviation + double next_update; + bool data_available; +}; + + +extern void nps_sensor_aoa_init(struct NpsSensorAngleOfAttack *aoa, double time); +extern void nps_sensor_aoa_run_step(struct NpsSensorAngleOfAttack *aoa, double time); + +#endif /* NPS_SENSOR_AOA_H */ diff --git a/sw/simulator/nps/nps_sensor_sideslip.c b/sw/simulator/nps/nps_sensor_sideslip.c new file mode 100644 index 0000000000..f2fbab3c43 --- /dev/null +++ b/sw/simulator/nps/nps_sensor_sideslip.c @@ -0,0 +1,78 @@ +/* + * Copyright (C) 2016 Johan Maurin, Gautier Hattenberger + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file nps_sensor_sideslip.c + * + * Simulated Angle of Attack of the Wind for NPS simulator. + * + */ + +#include "nps_sensor_sideslip.h" + +#include "generated/airframe.h" +#include + +#include "std.h" +#include "nps_fdm.h" +#include "nps_random.h" +#include NPS_SENSORS_PARAMS + +/// 10Hz default +#ifndef NPS_SIDESLIP_DT +#define NPS_SIDESLIP_DT 0.01 +#endif + +/// standard deviation in radian (default 0.001 rad) +#ifndef NPS_SIDESLIP_NOISE_STD_DEV +#define NPS_SIDESLIP_NOISE_STD_DEV 0.001 +#endif + +#ifndef NPS_SIDESLIP_OFFSET +#define NPS_SIDESLIP_OFFSET 0 +#endif + + +void nps_sensor_sideslip_init(struct NpsSensorSideSlip *sideslip, double time) +{ + sideslip->value = 0.; + sideslip->offset = NPS_SIDESLIP_OFFSET; + sideslip->noise_std_dev = NPS_SIDESLIP_NOISE_STD_DEV; + sideslip->next_update = time; + sideslip->data_available = FALSE; +} + + +void nps_sensor_sideslip_run_step(struct NpsSensorSideSlip *sideslip, double time) +{ + + if (time < sideslip->next_update) { + return; + } + + /* equivalent airspeed + sensor offset */ + sideslip->value = fdm.sideslip + sideslip->offset; + /* add noise with std dev rad */ + sideslip->value += get_gaussian_noise() * sideslip->noise_std_dev; + + sideslip->next_update += NPS_SIDESLIP_DT; + sideslip->data_available = TRUE; +} + diff --git a/sw/simulator/nps/nps_sensor_sideslip.h b/sw/simulator/nps/nps_sensor_sideslip.h new file mode 100644 index 0000000000..c91f9fef30 --- /dev/null +++ b/sw/simulator/nps/nps_sensor_sideslip.h @@ -0,0 +1,49 @@ +/* + * Copyright (C) 2016 Johan Maurin, Gautier Hattenberger + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file nps_sensor_sideslip.h + * + * Simulated Angle Of Attack of the Wind for NPS simulator. + * + */ + +#ifndef NPS_SENSOR_SIDESLIP_H +#define NPS_SENSOR_SIDESLIP_H + +#include "math/pprz_algebra.h" +#include "math/pprz_algebra_double.h" +#include "math/pprz_algebra_float.h" +#include "std.h" + +struct NpsSensorSideSlip{ + double value; ///< sideslip reading in radian + double offset; ///< offset in meters/second + double noise_std_dev; ///< noise standard deviation + double next_update; + bool data_available; +}; + + +extern void nps_sensor_sideslip_init(struct NpsSensorSideSlip *sideslip, double time); +extern void nps_sensor_sideslip_run_step(struct NpsSensorSideSlip *sideslip, double time); + +#endif /* NPS_SENSOR_SIDESLIP_H */ + diff --git a/sw/simulator/nps/nps_sensors.c b/sw/simulator/nps/nps_sensors.c index 0b8744d586..11744c8e7d 100644 --- a/sw/simulator/nps/nps_sensors.c +++ b/sw/simulator/nps/nps_sensors.c @@ -20,7 +20,8 @@ void nps_sensors_init(double time) nps_sensor_sonar_init(&sensors.sonar, time); nps_sensor_airspeed_init(&sensors.airspeed, time); nps_sensor_temperature_init(&sensors.temp, time); - + nps_sensor_aoa_init(&sensors.aoa, time); + nps_sensor_sideslip_init(&sensors.sideslip,time); } @@ -34,6 +35,8 @@ void nps_sensors_run_step(double time) nps_sensor_sonar_run_step(&sensors.sonar, time); nps_sensor_airspeed_run_step(&sensors.airspeed, time); nps_sensor_temperature_run_step(&sensors.temp, time); + nps_sensor_aoa_run_step(&sensors.aoa, time); + nps_sensor_sideslip_run_step(&sensors.sideslip,time); } @@ -99,3 +102,21 @@ bool nps_sensors_temperature_available(void) } return FALSE; } + +bool nps_sensors_aoa_available(void) +{ + if (sensors.aoa.data_available) { + sensors.aoa.data_available = FALSE; + return TRUE; + } + return FALSE; +} + +bool nps_sensors_sideslip_available(void) +{ + if (sensors.sideslip.data_available) { + sensors.sideslip.data_available = FALSE; + return TRUE; + } + return FALSE; +} diff --git a/sw/simulator/nps/nps_sensors.h b/sw/simulator/nps/nps_sensors.h index 4f67a864c6..19f6d1bc18 100644 --- a/sw/simulator/nps/nps_sensors.h +++ b/sw/simulator/nps/nps_sensors.h @@ -10,6 +10,8 @@ #include "nps_sensor_sonar.h" #include "nps_sensor_airspeed.h" #include "nps_sensor_temperature.h" +#include "nps_sensor_aoa.h" +#include "nps_sensor_sideslip.h" struct NpsSensors { struct DoubleRMat body_to_imu_rmat; @@ -21,6 +23,8 @@ struct NpsSensors { struct NpsSensorSonar sonar; struct NpsSensorAirspeed airspeed; struct NpsSensorTemperature temp; + struct NpsSensorAngleOfAttack aoa; + struct NpsSensorSideSlip sideslip; }; extern struct NpsSensors sensors; @@ -35,6 +39,7 @@ extern bool nps_sensors_gps_available(); extern bool nps_sensors_sonar_available(); extern bool nps_sensors_airspeed_available(); extern bool nps_sensors_temperature_available(); - +extern bool nps_sensors_aoa_available(); +extern bool nps_sensors_sideslip_available(); #endif /* NPS_SENSORS_H */