From 22f1b784525622ac39c9db50675c3937c7aeecf0 Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Fri, 17 Oct 2014 21:07:34 +0400 Subject: [PATCH 001/269] Adding vertical (Z) velocity to inav estimator --- .../position_estimator_inav_main.c | 3 +++ .../position_estimator_inav_params.c | 12 ++++++++++++ .../position_estimator_inav_params.h | 2 ++ 3 files changed, 17 insertions(+) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 81bbaa6587..a1bfa2dfc1 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -846,6 +846,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float w_xy_gps_p = params.w_xy_gps_p * w_gps_xy; float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy; float w_z_gps_p = params.w_z_gps_p * w_gps_z; + float w_z_gps_v = params.w_z_gps_v * w_gps_z; float w_xy_vision_p = params.w_xy_vision_p; float w_xy_vision_v = params.w_xy_vision_v; @@ -876,6 +877,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (use_gps_z) { accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p; + accel_bias_corr[2] -= corr_gps[2][1] * w_z_gps_v; } /* transform error vector from NED frame to body frame */ @@ -960,6 +962,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) epv = fminf(epv, gps.epv); inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); + inertial_filter_correct(corr_gps[2][1], dt, z_est, 1, w_z_gps_v); } if (use_vision_z) { diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index cc0fb4155e..1a1b3d3102 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -63,6 +63,17 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f); */ PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f); +/** + * Z velocity weight for GPS + * + * Weight (cutoff frequency) for GPS altitude velocity measurements. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ +PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_V, 1.0f); + /** * Z axis weight for vision * @@ -281,6 +292,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h) { h->w_z_baro = param_find("INAV_W_Z_BARO"); h->w_z_gps_p = param_find("INAV_W_Z_GPS_P"); + h->w_z_gps_v = param_find("INAV_W_Z_GPS_V"); h->w_z_vision_p = param_find("INAV_W_Z_VIS_P"); h->w_z_sonar = param_find("INAV_W_Z_SONAR"); h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P"); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index d0a65e42ea..51bbda412a 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -44,6 +44,7 @@ struct position_estimator_inav_params { float w_z_baro; float w_z_gps_p; + float w_z_gps_v; float w_z_vision_p; float w_z_sonar; float w_xy_gps_p; @@ -68,6 +69,7 @@ struct position_estimator_inav_params { struct position_estimator_inav_param_handles { param_t w_z_baro; param_t w_z_gps_p; + param_t w_z_gps_v; param_t w_z_vision_p; param_t w_z_sonar; param_t w_xy_gps_p; From c82996850be2df523394c2ac49db55258819a8a3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Dec 2014 12:20:23 +0100 Subject: [PATCH 002/269] Update EKF estimator to most recent version from Paul Riseborough --- .../ekf_att_pos_estimator_main.cpp | 19 +- .../estimator_21states.cpp | 2142 ----------------- .../estimator_21states.h | 247 -- ...or_23states.cpp => estimator_22states.cpp} | 921 ++++--- ...imator_23states.h => estimator_22states.h} | 42 +- src/modules/ekf_att_pos_estimator/module.mk | 2 +- 6 files changed, 487 insertions(+), 2886 deletions(-) delete mode 100644 src/modules/ekf_att_pos_estimator/estimator_21states.cpp delete mode 100644 src/modules/ekf_att_pos_estimator/estimator_21states.h rename src/modules/ekf_att_pos_estimator/{estimator_23states.cpp => estimator_22states.cpp} (79%) rename src/modules/ekf_att_pos_estimator/{estimator_23states.h => estimator_22states.h} (91%) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index e7805daa91..9e32045ac3 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -84,7 +84,7 @@ #include #include -#include "estimator_23states.h" +#include "estimator_22states.h" @@ -1371,10 +1371,15 @@ FixedwingEstimator::task_main() } if (newRangeData) { - _ekf->fuseRngData = true; - _ekf->useRangeFinder = true; - _ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f)); - _ekf->GroundEKF(); + + if (_ekf->Tnb.z.z > 0.9f) { + // _ekf->rngMea is set in sensor readout already + _ekf->fuseRngData = true; + _ekf->fuseOptFlowData = false; + _ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f)); + _ekf->OpticalFlowEKF(); + _ekf->fuseRngData = false; + } } @@ -1514,8 +1519,8 @@ FixedwingEstimator::task_main() if (hrt_elapsed_time(&_wind.timestamp) > 99000) { _wind.timestamp = _global_pos.timestamp; - _wind.windspeed_north = _ekf->windSpdFiltNorth; - _wind.windspeed_east = _ekf->windSpdFiltEast; + _wind.windspeed_north = _ekf->states[14]; + _wind.windspeed_east = _ekf->states[15]; // XXX we need to do something smart about the covariance here // but we default to the estimate covariance for now _wind.covariance_north = _ekf->P[14][14]; diff --git a/src/modules/ekf_att_pos_estimator/estimator_21states.cpp b/src/modules/ekf_att_pos_estimator/estimator_21states.cpp deleted file mode 100644 index 67bfec4ea3..0000000000 --- a/src/modules/ekf_att_pos_estimator/estimator_21states.cpp +++ /dev/null @@ -1,2142 +0,0 @@ -#include "estimator_21states.h" - -#include - -AttPosEKF::AttPosEKF() : - fusionModeGPS(0), - covSkipCount(0), - EAS2TAS(1.0f), - statesInitialised(false), - fuseVelData(false), - fusePosData(false), - fuseHgtData(false), - fuseMagData(false), - fuseVtasData(false), - onGround(true), - staticMode(true), - useAirspeed(true), - useCompass(true), - numericalProtection(true), - storeIndex(0), - magDeclination(0.0f) -{ - InitialiseParameters(); -} - -AttPosEKF::~AttPosEKF() -{ -} - -void AttPosEKF::UpdateStrapdownEquationsNED() -{ - Vector3f delVelNav; - float q00; - float q11; - float q22; - float q33; - float q01; - float q02; - float q03; - float q12; - float q13; - float q23; - Mat3f Tbn; - Mat3f Tnb; - float rotationMag; - float qUpdated[4]; - float quatMag; - double deltaQuat[4]; - const Vector3f gravityNED = {0.0,0.0,GRAVITY_MSS}; - -// Remove sensor bias errors - correctedDelAng.x = dAngIMU.x - states[10]; - correctedDelAng.y = dAngIMU.y - states[11]; - correctedDelAng.z = dAngIMU.z - states[12]; - dVelIMU.x = dVelIMU.x; - dVelIMU.y = dVelIMU.y; - dVelIMU.z = dVelIMU.z; - -// Save current measurements - Vector3f prevDelAng = correctedDelAng; - -// Apply corrections for earths rotation rate and coning errors -// * and + operators have been overloaded - correctedDelAng = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng); -// Convert the rotation vector to its equivalent quaternion - rotationMag = correctedDelAng.length(); - if (rotationMag < 1e-12f) - { - deltaQuat[0] = 1.0; - deltaQuat[1] = 0.0; - deltaQuat[2] = 0.0; - deltaQuat[3] = 0.0; - } - else - { - deltaQuat[0] = cos(0.5f*rotationMag); - double rotScaler = (sin(0.5f*rotationMag))/rotationMag; - deltaQuat[1] = correctedDelAng.x*rotScaler; - deltaQuat[2] = correctedDelAng.y*rotScaler; - deltaQuat[3] = correctedDelAng.z*rotScaler; - } - -// Update the quaternions by rotating from the previous attitude through -// the delta angle rotation quaternion - qUpdated[0] = states[0]*deltaQuat[0] - states[1]*deltaQuat[1] - states[2]*deltaQuat[2] - states[3]*deltaQuat[3]; - qUpdated[1] = states[0]*deltaQuat[1] + states[1]*deltaQuat[0] + states[2]*deltaQuat[3] - states[3]*deltaQuat[2]; - qUpdated[2] = states[0]*deltaQuat[2] + states[2]*deltaQuat[0] + states[3]*deltaQuat[1] - states[1]*deltaQuat[3]; - qUpdated[3] = states[0]*deltaQuat[3] + states[3]*deltaQuat[0] + states[1]*deltaQuat[2] - states[2]*deltaQuat[1]; - -// Normalise the quaternions and update the quaternion states - quatMag = sqrtf(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3])); - if (quatMag > 1e-16f) - { - float quatMagInv = 1.0f/quatMag; - states[0] = quatMagInv*qUpdated[0]; - states[1] = quatMagInv*qUpdated[1]; - states[2] = quatMagInv*qUpdated[2]; - states[3] = quatMagInv*qUpdated[3]; - } - -// Calculate the body to nav cosine matrix - q00 = sq(states[0]); - q11 = sq(states[1]); - q22 = sq(states[2]); - q33 = sq(states[3]); - q01 = states[0]*states[1]; - q02 = states[0]*states[2]; - q03 = states[0]*states[3]; - q12 = states[1]*states[2]; - q13 = states[1]*states[3]; - q23 = states[2]*states[3]; - - Tbn.x.x = q00 + q11 - q22 - q33; - Tbn.y.y = q00 - q11 + q22 - q33; - Tbn.z.z = q00 - q11 - q22 + q33; - Tbn.x.y = 2*(q12 - q03); - Tbn.x.z = 2*(q13 + q02); - Tbn.y.x = 2*(q12 + q03); - Tbn.y.z = 2*(q23 - q01); - Tbn.z.x = 2*(q13 - q02); - Tbn.z.y = 2*(q23 + q01); - - Tnb = Tbn.transpose(); - -// transform body delta velocities to delta velocities in the nav frame -// * and + operators have been overloaded - //delVelNav = Tbn*dVelIMU + gravityNED*dtIMU; - delVelNav.x = Tbn.x.x*dVelIMU.x + Tbn.x.y*dVelIMU.y + Tbn.x.z*dVelIMU.z + gravityNED.x*dtIMU; - delVelNav.y = Tbn.y.x*dVelIMU.x + Tbn.y.y*dVelIMU.y + Tbn.y.z*dVelIMU.z + gravityNED.y*dtIMU; - delVelNav.z = Tbn.z.x*dVelIMU.x + Tbn.z.y*dVelIMU.y + Tbn.z.z*dVelIMU.z + gravityNED.z*dtIMU; - -// calculate the magnitude of the nav acceleration (required for GPS -// variance estimation) - accNavMag = delVelNav.length()/dtIMU; - -// If calculating position save previous velocity - float lastVelocity[3]; - lastVelocity[0] = states[4]; - lastVelocity[1] = states[5]; - lastVelocity[2] = states[6]; - -// Sum delta velocities to get velocity - states[4] = states[4] + delVelNav.x; - states[5] = states[5] + delVelNav.y; - states[6] = states[6] + delVelNav.z; - -// If calculating postions, do a trapezoidal integration for position - states[7] = states[7] + 0.5f*(states[4] + lastVelocity[0])*dtIMU; - states[8] = states[8] + 0.5f*(states[5] + lastVelocity[1])*dtIMU; - states[9] = states[9] + 0.5f*(states[6] + lastVelocity[2])*dtIMU; - - // Constrain states (to protect against filter divergence) - //ConstrainStates(); -} - -void AttPosEKF::CovariancePrediction(float dt) -{ - // scalars - float daxCov; - float dayCov; - float dazCov; - float dvxCov; - float dvyCov; - float dvzCov; - float dvx; - float dvy; - float dvz; - float dax; - float day; - float daz; - float q0; - float q1; - float q2; - float q3; - float dax_b; - float day_b; - float daz_b; - - // arrays - float processNoise[21]; - float SF[14]; - float SG[8]; - float SQ[11]; - float SPP[13] = {0}; - float nextP[21][21]; - - // calculate covariance prediction process noise - windVelSigma = dt*0.1f; - dAngBiasSigma = dt*5.0e-7f; - magEarthSigma = dt*3.0e-4f; - magBodySigma = dt*3.0e-4f; - for (uint8_t i= 0; i<=9; i++) processNoise[i] = 1.0e-9f; - for (uint8_t i=10; i<=12; i++) processNoise[i] = dAngBiasSigma; - if (onGround) processNoise[12] = dAngBiasSigma * yawVarScale; - for (uint8_t i=13; i<=14; i++) processNoise[i] = windVelSigma; - for (uint8_t i=15; i<=17; i++) processNoise[i] = magEarthSigma; - for (uint8_t i=18; i<=20; i++) processNoise[i] = magBodySigma; - for (uint8_t i= 0; i<=20; i++) processNoise[i] = sq(processNoise[i]); - - // set variables used to calculate covariance growth - dvx = summedDelVel.x; - dvy = summedDelVel.y; - dvz = summedDelVel.z; - dax = summedDelAng.x; - day = summedDelAng.y; - daz = summedDelAng.z; - q0 = states[0]; - q1 = states[1]; - q2 = states[2]; - q3 = states[3]; - dax_b = states[10]; - day_b = states[11]; - daz_b = states[12]; - daxCov = sq(dt*1.4544411e-2f); - dayCov = sq(dt*1.4544411e-2f); - dazCov = sq(dt*1.4544411e-2f); - if (onGround) dazCov = dazCov * sq(yawVarScale); - dvxCov = sq(dt*0.5f); - dvyCov = sq(dt*0.5f); - dvzCov = sq(dt*0.5f); - - // Predicted covariance calculation - SF[0] = 2*dvx*q1 + 2*dvy*q2 + 2*dvz*q3; - SF[1] = 2*dvx*q3 + 2*dvy*q0 - 2*dvz*q1; - SF[2] = 2*dvx*q0 - 2*dvy*q3 + 2*dvz*q2; - SF[3] = day/2 - day_b/2; - SF[4] = daz/2 - daz_b/2; - SF[5] = dax/2 - dax_b/2; - SF[6] = dax_b/2 - dax/2; - SF[7] = daz_b/2 - daz/2; - SF[8] = day_b/2 - day/2; - SF[9] = q1/2; - SF[10] = q2/2; - SF[11] = q3/2; - SF[12] = 2*dvz*q0; - SF[13] = 2*dvy*q1; - - SG[0] = q0/2; - SG[1] = sq(q3); - SG[2] = sq(q2); - SG[3] = sq(q1); - SG[4] = sq(q0); - SG[5] = 2*q2*q3; - SG[6] = 2*q1*q3; - SG[7] = 2*q1*q2; - - SQ[0] = dvzCov*(SG[5] - 2*q0*q1)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyCov*(SG[5] + 2*q0*q1)*(SG[1] - SG[2] + SG[3] - SG[4]) + dvxCov*(SG[6] - 2*q0*q2)*(SG[7] + 2*q0*q3); - SQ[1] = dvzCov*(SG[6] + 2*q0*q2)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxCov*(SG[6] - 2*q0*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvyCov*(SG[5] + 2*q0*q1)*(SG[7] - 2*q0*q3); - SQ[2] = dvzCov*(SG[5] - 2*q0*q1)*(SG[6] + 2*q0*q2) - dvyCov*(SG[7] - 2*q0*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxCov*(SG[7] + 2*q0*q3)*(SG[1] + SG[2] - SG[3] - SG[4]); - SQ[3] = (dayCov*q1*SG[0])/2 - (dazCov*q1*SG[0])/2 - (daxCov*q2*q3)/4; - SQ[4] = (dazCov*q2*SG[0])/2 - (daxCov*q2*SG[0])/2 - (dayCov*q1*q3)/4; - SQ[5] = (daxCov*q3*SG[0])/2 - (dayCov*q3*SG[0])/2 - (dazCov*q1*q2)/4; - SQ[6] = (daxCov*q1*q2)/4 - (dazCov*q3*SG[0])/2 - (dayCov*q1*q2)/4; - SQ[7] = (dazCov*q1*q3)/4 - (daxCov*q1*q3)/4 - (dayCov*q2*SG[0])/2; - SQ[8] = (dayCov*q2*q3)/4 - (daxCov*q1*SG[0])/2 - (dazCov*q2*q3)/4; - SQ[9] = sq(SG[0]); - SQ[10] = sq(q1); - - SPP[0] = SF[12] + SF[13] - 2*dvx*q2; - SPP[1] = 2*dvx*q0 - 2*dvy*q3 + 2*dvz*q2; - SPP[2] = 2*dvx*q3 + 2*dvy*q0 - 2*dvz*q1; - SPP[3] = SF[11]; - SPP[4] = SF[10]; - SPP[5] = SF[9]; - SPP[6] = SF[7]; - SPP[7] = SF[8]; - - nextP[0][0] = P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3] + (daxCov*SQ[10])/4 + SF[6]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SPP[7]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[6]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[5]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) + SPP[4]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) + SPP[3]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) + (dayCov*sq(q2))/4 + (dazCov*sq(q3))/4; - nextP[0][1] = P[0][1] + SQ[8] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3] + SF[5]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[4]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[7]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[3]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) - SPP[4]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) - (q0*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]))/2; - nextP[0][2] = P[0][2] + SQ[7] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3] + SF[3]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[5]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[6]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) - SPP[3]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) + SPP[5]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) - (q0*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]))/2; - nextP[0][3] = P[0][3] + SQ[6] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3] + SF[4]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[3]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SF[6]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[4]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) - SPP[5]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) - (q0*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]))/2; - nextP[0][4] = P[0][4] + P[1][4]*SF[6] + P[2][4]*SPP[7] + P[3][4]*SPP[6] + P[10][4]*SPP[5] + P[11][4]*SPP[4] + P[12][4]*SPP[3] + SF[2]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[0]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SPP[0]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) - SPP[2]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]); - nextP[0][5] = P[0][5] + P[1][5]*SF[6] + P[2][5]*SPP[7] + P[3][5]*SPP[6] + P[10][5]*SPP[5] + P[11][5]*SPP[4] + P[12][5]*SPP[3] + SF[1]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[0]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SF[2]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) - SPP[0]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]); - nextP[0][6] = P[0][6] + P[1][6]*SF[6] + P[2][6]*SPP[7] + P[3][6]*SPP[6] + P[10][6]*SPP[5] + P[11][6]*SPP[4] + P[12][6]*SPP[3] + SF[1]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SF[0]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[0]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) - SPP[1]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]); - nextP[0][7] = P[0][7] + P[1][7]*SF[6] + P[2][7]*SPP[7] + P[3][7]*SPP[6] + P[10][7]*SPP[5] + P[11][7]*SPP[4] + P[12][7]*SPP[3] + dt*(P[0][4] + P[1][4]*SF[6] + P[2][4]*SPP[7] + P[3][4]*SPP[6] + P[10][4]*SPP[5] + P[11][4]*SPP[4] + P[12][4]*SPP[3]); - nextP[0][8] = P[0][8] + P[1][8]*SF[6] + P[2][8]*SPP[7] + P[3][8]*SPP[6] + P[10][8]*SPP[5] + P[11][8]*SPP[4] + P[12][8]*SPP[3] + dt*(P[0][5] + P[1][5]*SF[6] + P[2][5]*SPP[7] + P[3][5]*SPP[6] + P[10][5]*SPP[5] + P[11][5]*SPP[4] + P[12][5]*SPP[3]); - nextP[0][9] = P[0][9] + P[1][9]*SF[6] + P[2][9]*SPP[7] + P[3][9]*SPP[6] + P[10][9]*SPP[5] + P[11][9]*SPP[4] + P[12][9]*SPP[3] + dt*(P[0][6] + P[1][6]*SF[6] + P[2][6]*SPP[7] + P[3][6]*SPP[6] + P[10][6]*SPP[5] + P[11][6]*SPP[4] + P[12][6]*SPP[3]); - nextP[0][10] = P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]; - nextP[0][11] = P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]; - nextP[0][12] = P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]; - nextP[0][13] = P[0][13] + P[1][13]*SF[6] + P[2][13]*SPP[7] + P[3][13]*SPP[6] + P[10][13]*SPP[5] + P[11][13]*SPP[4] + P[12][13]*SPP[3]; - nextP[0][14] = P[0][14] + P[1][14]*SF[6] + P[2][14]*SPP[7] + P[3][14]*SPP[6] + P[10][14]*SPP[5] + P[11][14]*SPP[4] + P[12][14]*SPP[3]; - nextP[0][15] = P[0][15] + P[1][15]*SF[6] + P[2][15]*SPP[7] + P[3][15]*SPP[6] + P[10][15]*SPP[5] + P[11][15]*SPP[4] + P[12][15]*SPP[3]; - nextP[0][16] = P[0][16] + P[1][16]*SF[6] + P[2][16]*SPP[7] + P[3][16]*SPP[6] + P[10][16]*SPP[5] + P[11][16]*SPP[4] + P[12][16]*SPP[3]; - nextP[0][17] = P[0][17] + P[1][17]*SF[6] + P[2][17]*SPP[7] + P[3][17]*SPP[6] + P[10][17]*SPP[5] + P[11][17]*SPP[4] + P[12][17]*SPP[3]; - nextP[0][18] = P[0][18] + P[1][18]*SF[6] + P[2][18]*SPP[7] + P[3][18]*SPP[6] + P[10][18]*SPP[5] + P[11][18]*SPP[4] + P[12][18]*SPP[3]; - nextP[0][19] = P[0][19] + P[1][19]*SF[6] + P[2][19]*SPP[7] + P[3][19]*SPP[6] + P[10][19]*SPP[5] + P[11][19]*SPP[4] + P[12][19]*SPP[3]; - nextP[0][20] = P[0][20] + P[1][20]*SF[6] + P[2][20]*SPP[7] + P[3][20]*SPP[6] + P[10][20]*SPP[5] + P[11][20]*SPP[4] + P[12][20]*SPP[3]; - nextP[1][0] = P[1][0] + SQ[8] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2 + SF[6]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SPP[7]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[6]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[5]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) + SPP[4]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) + SPP[3]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2); - nextP[1][1] = P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] + daxCov*SQ[9] - (P[10][1]*q0)/2 + SF[5]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[4]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[7]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[3]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) - SPP[4]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2) + (dayCov*sq(q3))/4 + (dazCov*sq(q2))/4 - (q0*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2))/2; - nextP[1][2] = P[1][2] + SQ[5] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2 + SF[3]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[5]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[6]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) - SPP[3]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) + SPP[5]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2) - (q0*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2))/2; - nextP[1][3] = P[1][3] + SQ[4] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2 + SF[4]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[3]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SF[6]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[4]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) - SPP[5]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) - (q0*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2))/2; - nextP[1][4] = P[1][4] + P[0][4]*SF[5] + P[2][4]*SF[4] + P[3][4]*SPP[7] + P[11][4]*SPP[3] - P[12][4]*SPP[4] - (P[10][4]*q0)/2 + SF[2]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[0]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SPP[0]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) - SPP[2]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2); - nextP[1][5] = P[1][5] + P[0][5]*SF[5] + P[2][5]*SF[4] + P[3][5]*SPP[7] + P[11][5]*SPP[3] - P[12][5]*SPP[4] - (P[10][5]*q0)/2 + SF[1]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[0]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SF[2]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) - SPP[0]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2); - nextP[1][6] = P[1][6] + P[0][6]*SF[5] + P[2][6]*SF[4] + P[3][6]*SPP[7] + P[11][6]*SPP[3] - P[12][6]*SPP[4] - (P[10][6]*q0)/2 + SF[1]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SF[0]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[0]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) - SPP[1]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2); - nextP[1][7] = P[1][7] + P[0][7]*SF[5] + P[2][7]*SF[4] + P[3][7]*SPP[7] + P[11][7]*SPP[3] - P[12][7]*SPP[4] - (P[10][7]*q0)/2 + dt*(P[1][4] + P[0][4]*SF[5] + P[2][4]*SF[4] + P[3][4]*SPP[7] + P[11][4]*SPP[3] - P[12][4]*SPP[4] - (P[10][4]*q0)/2); - nextP[1][8] = P[1][8] + P[0][8]*SF[5] + P[2][8]*SF[4] + P[3][8]*SPP[7] + P[11][8]*SPP[3] - P[12][8]*SPP[4] - (P[10][8]*q0)/2 + dt*(P[1][5] + P[0][5]*SF[5] + P[2][5]*SF[4] + P[3][5]*SPP[7] + P[11][5]*SPP[3] - P[12][5]*SPP[4] - (P[10][5]*q0)/2); - nextP[1][9] = P[1][9] + P[0][9]*SF[5] + P[2][9]*SF[4] + P[3][9]*SPP[7] + P[11][9]*SPP[3] - P[12][9]*SPP[4] - (P[10][9]*q0)/2 + dt*(P[1][6] + P[0][6]*SF[5] + P[2][6]*SF[4] + P[3][6]*SPP[7] + P[11][6]*SPP[3] - P[12][6]*SPP[4] - (P[10][6]*q0)/2); - nextP[1][10] = P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2; - nextP[1][11] = P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2; - nextP[1][12] = P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2; - nextP[1][13] = P[1][13] + P[0][13]*SF[5] + P[2][13]*SF[4] + P[3][13]*SPP[7] + P[11][13]*SPP[3] - P[12][13]*SPP[4] - (P[10][13]*q0)/2; - nextP[1][14] = P[1][14] + P[0][14]*SF[5] + P[2][14]*SF[4] + P[3][14]*SPP[7] + P[11][14]*SPP[3] - P[12][14]*SPP[4] - (P[10][14]*q0)/2; - nextP[1][15] = P[1][15] + P[0][15]*SF[5] + P[2][15]*SF[4] + P[3][15]*SPP[7] + P[11][15]*SPP[3] - P[12][15]*SPP[4] - (P[10][15]*q0)/2; - nextP[1][16] = P[1][16] + P[0][16]*SF[5] + P[2][16]*SF[4] + P[3][16]*SPP[7] + P[11][16]*SPP[3] - P[12][16]*SPP[4] - (P[10][16]*q0)/2; - nextP[1][17] = P[1][17] + P[0][17]*SF[5] + P[2][17]*SF[4] + P[3][17]*SPP[7] + P[11][17]*SPP[3] - P[12][17]*SPP[4] - (P[10][17]*q0)/2; - nextP[1][18] = P[1][18] + P[0][18]*SF[5] + P[2][18]*SF[4] + P[3][18]*SPP[7] + P[11][18]*SPP[3] - P[12][18]*SPP[4] - (P[10][18]*q0)/2; - nextP[1][19] = P[1][19] + P[0][19]*SF[5] + P[2][19]*SF[4] + P[3][19]*SPP[7] + P[11][19]*SPP[3] - P[12][19]*SPP[4] - (P[10][19]*q0)/2; - nextP[1][20] = P[1][20] + P[0][20]*SF[5] + P[2][20]*SF[4] + P[3][20]*SPP[7] + P[11][20]*SPP[3] - P[12][20]*SPP[4] - (P[10][20]*q0)/2; - nextP[2][0] = P[2][0] + SQ[7] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2 + SF[6]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SPP[7]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[6]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[5]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) + SPP[4]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) + SPP[3]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2); - nextP[2][1] = P[2][1] + SQ[5] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2 + SF[5]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[4]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[7]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[3]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) - SPP[4]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2) - (q0*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2))/2; - nextP[2][2] = P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] + dayCov*SQ[9] + (dazCov*SQ[10])/4 - (P[11][2]*q0)/2 + SF[3]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[5]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[6]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) - SPP[3]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) + SPP[5]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2) + (daxCov*sq(q3))/4 - (q0*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2))/2; - nextP[2][3] = P[2][3] + SQ[3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2 + SF[4]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[3]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SF[6]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[4]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) - SPP[5]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) - (q0*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2))/2; - nextP[2][4] = P[2][4] + P[0][4]*SF[3] + P[3][4]*SF[5] + P[1][4]*SPP[6] - P[10][4]*SPP[3] + P[12][4]*SPP[5] - (P[11][4]*q0)/2 + SF[2]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[0]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SPP[0]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) - SPP[2]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2); - nextP[2][5] = P[2][5] + P[0][5]*SF[3] + P[3][5]*SF[5] + P[1][5]*SPP[6] - P[10][5]*SPP[3] + P[12][5]*SPP[5] - (P[11][5]*q0)/2 + SF[1]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[0]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SF[2]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) - SPP[0]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2); - nextP[2][6] = P[2][6] + P[0][6]*SF[3] + P[3][6]*SF[5] + P[1][6]*SPP[6] - P[10][6]*SPP[3] + P[12][6]*SPP[5] - (P[11][6]*q0)/2 + SF[1]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SF[0]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[0]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) - SPP[1]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2); - nextP[2][7] = P[2][7] + P[0][7]*SF[3] + P[3][7]*SF[5] + P[1][7]*SPP[6] - P[10][7]*SPP[3] + P[12][7]*SPP[5] - (P[11][7]*q0)/2 + dt*(P[2][4] + P[0][4]*SF[3] + P[3][4]*SF[5] + P[1][4]*SPP[6] - P[10][4]*SPP[3] + P[12][4]*SPP[5] - (P[11][4]*q0)/2); - nextP[2][8] = P[2][8] + P[0][8]*SF[3] + P[3][8]*SF[5] + P[1][8]*SPP[6] - P[10][8]*SPP[3] + P[12][8]*SPP[5] - (P[11][8]*q0)/2 + dt*(P[2][5] + P[0][5]*SF[3] + P[3][5]*SF[5] + P[1][5]*SPP[6] - P[10][5]*SPP[3] + P[12][5]*SPP[5] - (P[11][5]*q0)/2); - nextP[2][9] = P[2][9] + P[0][9]*SF[3] + P[3][9]*SF[5] + P[1][9]*SPP[6] - P[10][9]*SPP[3] + P[12][9]*SPP[5] - (P[11][9]*q0)/2 + dt*(P[2][6] + P[0][6]*SF[3] + P[3][6]*SF[5] + P[1][6]*SPP[6] - P[10][6]*SPP[3] + P[12][6]*SPP[5] - (P[11][6]*q0)/2); - nextP[2][10] = P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2; - nextP[2][11] = P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2; - nextP[2][12] = P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2; - nextP[2][13] = P[2][13] + P[0][13]*SF[3] + P[3][13]*SF[5] + P[1][13]*SPP[6] - P[10][13]*SPP[3] + P[12][13]*SPP[5] - (P[11][13]*q0)/2; - nextP[2][14] = P[2][14] + P[0][14]*SF[3] + P[3][14]*SF[5] + P[1][14]*SPP[6] - P[10][14]*SPP[3] + P[12][14]*SPP[5] - (P[11][14]*q0)/2; - nextP[2][15] = P[2][15] + P[0][15]*SF[3] + P[3][15]*SF[5] + P[1][15]*SPP[6] - P[10][15]*SPP[3] + P[12][15]*SPP[5] - (P[11][15]*q0)/2; - nextP[2][16] = P[2][16] + P[0][16]*SF[3] + P[3][16]*SF[5] + P[1][16]*SPP[6] - P[10][16]*SPP[3] + P[12][16]*SPP[5] - (P[11][16]*q0)/2; - nextP[2][17] = P[2][17] + P[0][17]*SF[3] + P[3][17]*SF[5] + P[1][17]*SPP[6] - P[10][17]*SPP[3] + P[12][17]*SPP[5] - (P[11][17]*q0)/2; - nextP[2][18] = P[2][18] + P[0][18]*SF[3] + P[3][18]*SF[5] + P[1][18]*SPP[6] - P[10][18]*SPP[3] + P[12][18]*SPP[5] - (P[11][18]*q0)/2; - nextP[2][19] = P[2][19] + P[0][19]*SF[3] + P[3][19]*SF[5] + P[1][19]*SPP[6] - P[10][19]*SPP[3] + P[12][19]*SPP[5] - (P[11][19]*q0)/2; - nextP[2][20] = P[2][20] + P[0][20]*SF[3] + P[3][20]*SF[5] + P[1][20]*SPP[6] - P[10][20]*SPP[3] + P[12][20]*SPP[5] - (P[11][20]*q0)/2; - nextP[3][0] = P[3][0] + SQ[6] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2 + SF[6]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SPP[7]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[6]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[5]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) + SPP[4]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) + SPP[3]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2); - nextP[3][1] = P[3][1] + SQ[4] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2 + SF[5]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[4]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[7]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[3]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) - SPP[4]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2) - (q0*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2))/2; - nextP[3][2] = P[3][2] + SQ[3] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2 + SF[3]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[5]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[6]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) - SPP[3]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) + SPP[5]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2) - (q0*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2))/2; - nextP[3][3] = P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] + (dayCov*SQ[10])/4 + dazCov*SQ[9] - (P[12][3]*q0)/2 + SF[4]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[3]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SF[6]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[4]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) - SPP[5]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) + (daxCov*sq(q2))/4 - (q0*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2))/2; - nextP[3][4] = P[3][4] + P[0][4]*SF[4] + P[1][4]*SF[3] + P[2][4]*SF[6] + P[10][4]*SPP[4] - P[11][4]*SPP[5] - (P[12][4]*q0)/2 + SF[2]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[0]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SPP[0]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) - SPP[2]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2); - nextP[3][5] = P[3][5] + P[0][5]*SF[4] + P[1][5]*SF[3] + P[2][5]*SF[6] + P[10][5]*SPP[4] - P[11][5]*SPP[5] - (P[12][5]*q0)/2 + SF[1]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[0]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SF[2]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) - SPP[0]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2); - nextP[3][6] = P[3][6] + P[0][6]*SF[4] + P[1][6]*SF[3] + P[2][6]*SF[6] + P[10][6]*SPP[4] - P[11][6]*SPP[5] - (P[12][6]*q0)/2 + SF[1]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SF[0]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[0]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) - SPP[1]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2); - nextP[3][7] = P[3][7] + P[0][7]*SF[4] + P[1][7]*SF[3] + P[2][7]*SF[6] + P[10][7]*SPP[4] - P[11][7]*SPP[5] - (P[12][7]*q0)/2 + dt*(P[3][4] + P[0][4]*SF[4] + P[1][4]*SF[3] + P[2][4]*SF[6] + P[10][4]*SPP[4] - P[11][4]*SPP[5] - (P[12][4]*q0)/2); - nextP[3][8] = P[3][8] + P[0][8]*SF[4] + P[1][8]*SF[3] + P[2][8]*SF[6] + P[10][8]*SPP[4] - P[11][8]*SPP[5] - (P[12][8]*q0)/2 + dt*(P[3][5] + P[0][5]*SF[4] + P[1][5]*SF[3] + P[2][5]*SF[6] + P[10][5]*SPP[4] - P[11][5]*SPP[5] - (P[12][5]*q0)/2); - nextP[3][9] = P[3][9] + P[0][9]*SF[4] + P[1][9]*SF[3] + P[2][9]*SF[6] + P[10][9]*SPP[4] - P[11][9]*SPP[5] - (P[12][9]*q0)/2 + dt*(P[3][6] + P[0][6]*SF[4] + P[1][6]*SF[3] + P[2][6]*SF[6] + P[10][6]*SPP[4] - P[11][6]*SPP[5] - (P[12][6]*q0)/2); - nextP[3][10] = P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2; - nextP[3][11] = P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2; - nextP[3][12] = P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2; - nextP[3][13] = P[3][13] + P[0][13]*SF[4] + P[1][13]*SF[3] + P[2][13]*SF[6] + P[10][13]*SPP[4] - P[11][13]*SPP[5] - (P[12][13]*q0)/2; - nextP[3][14] = P[3][14] + P[0][14]*SF[4] + P[1][14]*SF[3] + P[2][14]*SF[6] + P[10][14]*SPP[4] - P[11][14]*SPP[5] - (P[12][14]*q0)/2; - nextP[3][15] = P[3][15] + P[0][15]*SF[4] + P[1][15]*SF[3] + P[2][15]*SF[6] + P[10][15]*SPP[4] - P[11][15]*SPP[5] - (P[12][15]*q0)/2; - nextP[3][16] = P[3][16] + P[0][16]*SF[4] + P[1][16]*SF[3] + P[2][16]*SF[6] + P[10][16]*SPP[4] - P[11][16]*SPP[5] - (P[12][16]*q0)/2; - nextP[3][17] = P[3][17] + P[0][17]*SF[4] + P[1][17]*SF[3] + P[2][17]*SF[6] + P[10][17]*SPP[4] - P[11][17]*SPP[5] - (P[12][17]*q0)/2; - nextP[3][18] = P[3][18] + P[0][18]*SF[4] + P[1][18]*SF[3] + P[2][18]*SF[6] + P[10][18]*SPP[4] - P[11][18]*SPP[5] - (P[12][18]*q0)/2; - nextP[3][19] = P[3][19] + P[0][19]*SF[4] + P[1][19]*SF[3] + P[2][19]*SF[6] + P[10][19]*SPP[4] - P[11][19]*SPP[5] - (P[12][19]*q0)/2; - nextP[3][20] = P[3][20] + P[0][20]*SF[4] + P[1][20]*SF[3] + P[2][20]*SF[6] + P[10][20]*SPP[4] - P[11][20]*SPP[5] - (P[12][20]*q0)/2; - nextP[4][0] = P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2] + SF[6]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SPP[7]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[6]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[5]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) + SPP[4]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) + SPP[3]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]); - nextP[4][1] = P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2] + SF[5]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[4]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[7]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[3]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) - SPP[4]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]) - (q0*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]))/2; - nextP[4][2] = P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2] + SF[3]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[5]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[6]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) - SPP[3]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) + SPP[5]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]) - (q0*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]))/2; - nextP[4][3] = P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2] + SF[4]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[3]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SF[6]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[4]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) - SPP[5]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) - (q0*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]))/2; - nextP[4][4] = P[4][4] + P[0][4]*SF[2] + P[1][4]*SF[0] + P[2][4]*SPP[0] - P[3][4]*SPP[2] + dvyCov*sq(SG[7] - 2*q0*q3) + dvzCov*sq(SG[6] + 2*q0*q2) + SF[2]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[0]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SPP[0]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) - SPP[2]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + dvxCov*sq(SG[1] + SG[2] - SG[3] - SG[4]); - nextP[4][5] = P[4][5] + SQ[2] + P[0][5]*SF[2] + P[1][5]*SF[0] + P[2][5]*SPP[0] - P[3][5]*SPP[2] + SF[1]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[0]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SF[2]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) - SPP[0]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]); - nextP[4][6] = P[4][6] + SQ[1] + P[0][6]*SF[2] + P[1][6]*SF[0] + P[2][6]*SPP[0] - P[3][6]*SPP[2] + SF[1]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SF[0]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[0]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) - SPP[1]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]); - nextP[4][7] = P[4][7] + P[0][7]*SF[2] + P[1][7]*SF[0] + P[2][7]*SPP[0] - P[3][7]*SPP[2] + dt*(P[4][4] + P[0][4]*SF[2] + P[1][4]*SF[0] + P[2][4]*SPP[0] - P[3][4]*SPP[2]); - nextP[4][8] = P[4][8] + P[0][8]*SF[2] + P[1][8]*SF[0] + P[2][8]*SPP[0] - P[3][8]*SPP[2] + dt*(P[4][5] + P[0][5]*SF[2] + P[1][5]*SF[0] + P[2][5]*SPP[0] - P[3][5]*SPP[2]); - nextP[4][9] = P[4][9] + P[0][9]*SF[2] + P[1][9]*SF[0] + P[2][9]*SPP[0] - P[3][9]*SPP[2] + dt*(P[4][6] + P[0][6]*SF[2] + P[1][6]*SF[0] + P[2][6]*SPP[0] - P[3][6]*SPP[2]); - nextP[4][10] = P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]; - nextP[4][11] = P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]; - nextP[4][12] = P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]; - nextP[4][13] = P[4][13] + P[0][13]*SF[2] + P[1][13]*SF[0] + P[2][13]*SPP[0] - P[3][13]*SPP[2]; - nextP[4][14] = P[4][14] + P[0][14]*SF[2] + P[1][14]*SF[0] + P[2][14]*SPP[0] - P[3][14]*SPP[2]; - nextP[4][15] = P[4][15] + P[0][15]*SF[2] + P[1][15]*SF[0] + P[2][15]*SPP[0] - P[3][15]*SPP[2]; - nextP[4][16] = P[4][16] + P[0][16]*SF[2] + P[1][16]*SF[0] + P[2][16]*SPP[0] - P[3][16]*SPP[2]; - nextP[4][17] = P[4][17] + P[0][17]*SF[2] + P[1][17]*SF[0] + P[2][17]*SPP[0] - P[3][17]*SPP[2]; - nextP[4][18] = P[4][18] + P[0][18]*SF[2] + P[1][18]*SF[0] + P[2][18]*SPP[0] - P[3][18]*SPP[2]; - nextP[4][19] = P[4][19] + P[0][19]*SF[2] + P[1][19]*SF[0] + P[2][19]*SPP[0] - P[3][19]*SPP[2]; - nextP[4][20] = P[4][20] + P[0][20]*SF[2] + P[1][20]*SF[0] + P[2][20]*SPP[0] - P[3][20]*SPP[2]; - nextP[5][0] = P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0] + SF[6]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SPP[7]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[6]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[5]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) + SPP[4]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) + SPP[3]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]); - nextP[5][1] = P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0] + SF[5]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[4]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[7]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[3]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) - SPP[4]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]) - (q0*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]))/2; - nextP[5][2] = P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0] + SF[3]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[5]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[6]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) - SPP[3]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) + SPP[5]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]) - (q0*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]))/2; - nextP[5][3] = P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0] + SF[4]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[3]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SF[6]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[4]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) - SPP[5]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) - (q0*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]))/2; - nextP[5][4] = P[5][4] + SQ[2] + P[0][4]*SF[1] + P[2][4]*SF[0] + P[3][4]*SF[2] - P[1][4]*SPP[0] + SF[2]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[0]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SPP[0]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) - SPP[2]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]); - nextP[5][5] = P[5][5] + P[0][5]*SF[1] + P[2][5]*SF[0] + P[3][5]*SF[2] - P[1][5]*SPP[0] + dvxCov*sq(SG[7] + 2*q0*q3) + dvzCov*sq(SG[5] - 2*q0*q1) + SF[1]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[0]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SF[2]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) - SPP[0]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + dvyCov*sq(SG[1] - SG[2] + SG[3] - SG[4]); - nextP[5][6] = P[5][6] + SQ[0] + P[0][6]*SF[1] + P[2][6]*SF[0] + P[3][6]*SF[2] - P[1][6]*SPP[0] + SF[1]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SF[0]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[0]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) - SPP[1]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]); - nextP[5][7] = P[5][7] + P[0][7]*SF[1] + P[2][7]*SF[0] + P[3][7]*SF[2] - P[1][7]*SPP[0] + dt*(P[5][4] + P[0][4]*SF[1] + P[2][4]*SF[0] + P[3][4]*SF[2] - P[1][4]*SPP[0]); - nextP[5][8] = P[5][8] + P[0][8]*SF[1] + P[2][8]*SF[0] + P[3][8]*SF[2] - P[1][8]*SPP[0] + dt*(P[5][5] + P[0][5]*SF[1] + P[2][5]*SF[0] + P[3][5]*SF[2] - P[1][5]*SPP[0]); - nextP[5][9] = P[5][9] + P[0][9]*SF[1] + P[2][9]*SF[0] + P[3][9]*SF[2] - P[1][9]*SPP[0] + dt*(P[5][6] + P[0][6]*SF[1] + P[2][6]*SF[0] + P[3][6]*SF[2] - P[1][6]*SPP[0]); - nextP[5][10] = P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]; - nextP[5][11] = P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]; - nextP[5][12] = P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]; - nextP[5][13] = P[5][13] + P[0][13]*SF[1] + P[2][13]*SF[0] + P[3][13]*SF[2] - P[1][13]*SPP[0]; - nextP[5][14] = P[5][14] + P[0][14]*SF[1] + P[2][14]*SF[0] + P[3][14]*SF[2] - P[1][14]*SPP[0]; - nextP[5][15] = P[5][15] + P[0][15]*SF[1] + P[2][15]*SF[0] + P[3][15]*SF[2] - P[1][15]*SPP[0]; - nextP[5][16] = P[5][16] + P[0][16]*SF[1] + P[2][16]*SF[0] + P[3][16]*SF[2] - P[1][16]*SPP[0]; - nextP[5][17] = P[5][17] + P[0][17]*SF[1] + P[2][17]*SF[0] + P[3][17]*SF[2] - P[1][17]*SPP[0]; - nextP[5][18] = P[5][18] + P[0][18]*SF[1] + P[2][18]*SF[0] + P[3][18]*SF[2] - P[1][18]*SPP[0]; - nextP[5][19] = P[5][19] + P[0][19]*SF[1] + P[2][19]*SF[0] + P[3][19]*SF[2] - P[1][19]*SPP[0]; - nextP[5][20] = P[5][20] + P[0][20]*SF[1] + P[2][20]*SF[0] + P[3][20]*SF[2] - P[1][20]*SPP[0]; - nextP[6][0] = P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1] + SF[6]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SPP[7]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[6]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[5]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) + SPP[4]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) + SPP[3]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]); - nextP[6][1] = P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1] + SF[5]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[4]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[7]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[3]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) - SPP[4]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]) - (q0*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]))/2; - nextP[6][2] = P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1] + SF[3]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[5]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[6]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) - SPP[3]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) + SPP[5]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]) - (q0*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]))/2; - nextP[6][3] = P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1] + SF[4]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[3]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SF[6]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[4]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) - SPP[5]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) - (q0*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]))/2; - nextP[6][4] = P[6][4] + SQ[1] + P[1][4]*SF[1] + P[3][4]*SF[0] + P[0][4]*SPP[0] - P[2][4]*SPP[1] + SF[2]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[0]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SPP[0]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) - SPP[2]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]); - nextP[6][5] = P[6][5] + SQ[0] + P[1][5]*SF[1] + P[3][5]*SF[0] + P[0][5]*SPP[0] - P[2][5]*SPP[1] + SF[1]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[0]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SF[2]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) - SPP[0]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]); - nextP[6][6] = P[6][6] + P[1][6]*SF[1] + P[3][6]*SF[0] + P[0][6]*SPP[0] - P[2][6]*SPP[1] + dvxCov*sq(SG[6] - 2*q0*q2) + dvyCov*sq(SG[5] + 2*q0*q1) + SF[1]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SF[0]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[0]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) - SPP[1]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + dvzCov*sq(SG[1] - SG[2] - SG[3] + SG[4]); - nextP[6][7] = P[6][7] + P[1][7]*SF[1] + P[3][7]*SF[0] + P[0][7]*SPP[0] - P[2][7]*SPP[1] + dt*(P[6][4] + P[1][4]*SF[1] + P[3][4]*SF[0] + P[0][4]*SPP[0] - P[2][4]*SPP[1]); - nextP[6][8] = P[6][8] + P[1][8]*SF[1] + P[3][8]*SF[0] + P[0][8]*SPP[0] - P[2][8]*SPP[1] + dt*(P[6][5] + P[1][5]*SF[1] + P[3][5]*SF[0] + P[0][5]*SPP[0] - P[2][5]*SPP[1]); - nextP[6][9] = P[6][9] + P[1][9]*SF[1] + P[3][9]*SF[0] + P[0][9]*SPP[0] - P[2][9]*SPP[1] + dt*(P[6][6] + P[1][6]*SF[1] + P[3][6]*SF[0] + P[0][6]*SPP[0] - P[2][6]*SPP[1]); - nextP[6][10] = P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]; - nextP[6][11] = P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]; - nextP[6][12] = P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]; - nextP[6][13] = P[6][13] + P[1][13]*SF[1] + P[3][13]*SF[0] + P[0][13]*SPP[0] - P[2][13]*SPP[1]; - nextP[6][14] = P[6][14] + P[1][14]*SF[1] + P[3][14]*SF[0] + P[0][14]*SPP[0] - P[2][14]*SPP[1]; - nextP[6][15] = P[6][15] + P[1][15]*SF[1] + P[3][15]*SF[0] + P[0][15]*SPP[0] - P[2][15]*SPP[1]; - nextP[6][16] = P[6][16] + P[1][16]*SF[1] + P[3][16]*SF[0] + P[0][16]*SPP[0] - P[2][16]*SPP[1]; - nextP[6][17] = P[6][17] + P[1][17]*SF[1] + P[3][17]*SF[0] + P[0][17]*SPP[0] - P[2][17]*SPP[1]; - nextP[6][18] = P[6][18] + P[1][18]*SF[1] + P[3][18]*SF[0] + P[0][18]*SPP[0] - P[2][18]*SPP[1]; - nextP[6][19] = P[6][19] + P[1][19]*SF[1] + P[3][19]*SF[0] + P[0][19]*SPP[0] - P[2][19]*SPP[1]; - nextP[6][20] = P[6][20] + P[1][20]*SF[1] + P[3][20]*SF[0] + P[0][20]*SPP[0] - P[2][20]*SPP[1]; - nextP[7][0] = P[7][0] + P[4][0]*dt + SF[6]*(P[7][1] + P[4][1]*dt) + SPP[7]*(P[7][2] + P[4][2]*dt) + SPP[6]*(P[7][3] + P[4][3]*dt) + SPP[5]*(P[7][10] + P[4][10]*dt) + SPP[4]*(P[7][11] + P[4][11]*dt) + SPP[3]*(P[7][12] + P[4][12]*dt); - nextP[7][1] = P[7][1] + P[4][1]*dt + SF[5]*(P[7][0] + P[4][0]*dt) + SF[4]*(P[7][2] + P[4][2]*dt) + SPP[7]*(P[7][3] + P[4][3]*dt) + SPP[3]*(P[7][11] + P[4][11]*dt) - SPP[4]*(P[7][12] + P[4][12]*dt) - (q0*(P[7][10] + P[4][10]*dt))/2; - nextP[7][2] = P[7][2] + P[4][2]*dt + SF[3]*(P[7][0] + P[4][0]*dt) + SF[5]*(P[7][3] + P[4][3]*dt) + SPP[6]*(P[7][1] + P[4][1]*dt) - SPP[3]*(P[7][10] + P[4][10]*dt) + SPP[5]*(P[7][12] + P[4][12]*dt) - (q0*(P[7][11] + P[4][11]*dt))/2; - nextP[7][3] = P[7][3] + P[4][3]*dt + SF[4]*(P[7][0] + P[4][0]*dt) + SF[3]*(P[7][1] + P[4][1]*dt) + SF[6]*(P[7][2] + P[4][2]*dt) + SPP[4]*(P[7][10] + P[4][10]*dt) - SPP[5]*(P[7][11] + P[4][11]*dt) - (q0*(P[7][12] + P[4][12]*dt))/2; - nextP[7][4] = P[7][4] + P[4][4]*dt + SF[0]*(P[7][1] + P[4][1]*dt) + SF[2]*(P[7][0] + P[4][0]*dt) + SPP[0]*(P[7][2] + P[4][2]*dt) - SPP[2]*(P[7][3] + P[4][3]*dt); - nextP[7][5] = P[7][5] + P[4][5]*dt + SF[1]*(P[7][0] + P[4][0]*dt) + SF[0]*(P[7][2] + P[4][2]*dt) + SF[2]*(P[7][3] + P[4][3]*dt) - SPP[0]*(P[7][1] + P[4][1]*dt); - nextP[7][6] = P[7][6] + P[4][6]*dt + SF[1]*(P[7][1] + P[4][1]*dt) + SF[0]*(P[7][3] + P[4][3]*dt) + SPP[0]*(P[7][0] + P[4][0]*dt) - SPP[1]*(P[7][2] + P[4][2]*dt); - nextP[7][7] = P[7][7] + P[4][7]*dt + dt*(P[7][4] + P[4][4]*dt); - nextP[7][8] = P[7][8] + P[4][8]*dt + dt*(P[7][5] + P[4][5]*dt); - nextP[7][9] = P[7][9] + P[4][9]*dt + dt*(P[7][6] + P[4][6]*dt); - nextP[7][10] = P[7][10] + P[4][10]*dt; - nextP[7][11] = P[7][11] + P[4][11]*dt; - nextP[7][12] = P[7][12] + P[4][12]*dt; - nextP[7][13] = P[7][13] + P[4][13]*dt; - nextP[7][14] = P[7][14] + P[4][14]*dt; - nextP[7][15] = P[7][15] + P[4][15]*dt; - nextP[7][16] = P[7][16] + P[4][16]*dt; - nextP[7][17] = P[7][17] + P[4][17]*dt; - nextP[7][18] = P[7][18] + P[4][18]*dt; - nextP[7][19] = P[7][19] + P[4][19]*dt; - nextP[7][20] = P[7][20] + P[4][20]*dt; - nextP[8][0] = P[8][0] + P[5][0]*dt + SF[6]*(P[8][1] + P[5][1]*dt) + SPP[7]*(P[8][2] + P[5][2]*dt) + SPP[6]*(P[8][3] + P[5][3]*dt) + SPP[5]*(P[8][10] + P[5][10]*dt) + SPP[4]*(P[8][11] + P[5][11]*dt) + SPP[3]*(P[8][12] + P[5][12]*dt); - nextP[8][1] = P[8][1] + P[5][1]*dt + SF[5]*(P[8][0] + P[5][0]*dt) + SF[4]*(P[8][2] + P[5][2]*dt) + SPP[7]*(P[8][3] + P[5][3]*dt) + SPP[3]*(P[8][11] + P[5][11]*dt) - SPP[4]*(P[8][12] + P[5][12]*dt) - (q0*(P[8][10] + P[5][10]*dt))/2; - nextP[8][2] = P[8][2] + P[5][2]*dt + SF[3]*(P[8][0] + P[5][0]*dt) + SF[5]*(P[8][3] + P[5][3]*dt) + SPP[6]*(P[8][1] + P[5][1]*dt) - SPP[3]*(P[8][10] + P[5][10]*dt) + SPP[5]*(P[8][12] + P[5][12]*dt) - (q0*(P[8][11] + P[5][11]*dt))/2; - nextP[8][3] = P[8][3] + P[5][3]*dt + SF[4]*(P[8][0] + P[5][0]*dt) + SF[3]*(P[8][1] + P[5][1]*dt) + SF[6]*(P[8][2] + P[5][2]*dt) + SPP[4]*(P[8][10] + P[5][10]*dt) - SPP[5]*(P[8][11] + P[5][11]*dt) - (q0*(P[8][12] + P[5][12]*dt))/2; - nextP[8][4] = P[8][4] + P[5][4]*dt + SF[0]*(P[8][1] + P[5][1]*dt) + SF[2]*(P[8][0] + P[5][0]*dt) + SPP[0]*(P[8][2] + P[5][2]*dt) - SPP[2]*(P[8][3] + P[5][3]*dt); - nextP[8][5] = P[8][5] + P[5][5]*dt + SF[1]*(P[8][0] + P[5][0]*dt) + SF[0]*(P[8][2] + P[5][2]*dt) + SF[2]*(P[8][3] + P[5][3]*dt) - SPP[0]*(P[8][1] + P[5][1]*dt); - nextP[8][6] = P[8][6] + P[5][6]*dt + SF[1]*(P[8][1] + P[5][1]*dt) + SF[0]*(P[8][3] + P[5][3]*dt) + SPP[0]*(P[8][0] + P[5][0]*dt) - SPP[1]*(P[8][2] + P[5][2]*dt); - nextP[8][7] = P[8][7] + P[5][7]*dt + dt*(P[8][4] + P[5][4]*dt); - nextP[8][8] = P[8][8] + P[5][8]*dt + dt*(P[8][5] + P[5][5]*dt); - nextP[8][9] = P[8][9] + P[5][9]*dt + dt*(P[8][6] + P[5][6]*dt); - nextP[8][10] = P[8][10] + P[5][10]*dt; - nextP[8][11] = P[8][11] + P[5][11]*dt; - nextP[8][12] = P[8][12] + P[5][12]*dt; - nextP[8][13] = P[8][13] + P[5][13]*dt; - nextP[8][14] = P[8][14] + P[5][14]*dt; - nextP[8][15] = P[8][15] + P[5][15]*dt; - nextP[8][16] = P[8][16] + P[5][16]*dt; - nextP[8][17] = P[8][17] + P[5][17]*dt; - nextP[8][18] = P[8][18] + P[5][18]*dt; - nextP[8][19] = P[8][19] + P[5][19]*dt; - nextP[8][20] = P[8][20] + P[5][20]*dt; - nextP[9][0] = P[9][0] + P[6][0]*dt + SF[6]*(P[9][1] + P[6][1]*dt) + SPP[7]*(P[9][2] + P[6][2]*dt) + SPP[6]*(P[9][3] + P[6][3]*dt) + SPP[5]*(P[9][10] + P[6][10]*dt) + SPP[4]*(P[9][11] + P[6][11]*dt) + SPP[3]*(P[9][12] + P[6][12]*dt); - nextP[9][1] = P[9][1] + P[6][1]*dt + SF[5]*(P[9][0] + P[6][0]*dt) + SF[4]*(P[9][2] + P[6][2]*dt) + SPP[7]*(P[9][3] + P[6][3]*dt) + SPP[3]*(P[9][11] + P[6][11]*dt) - SPP[4]*(P[9][12] + P[6][12]*dt) - (q0*(P[9][10] + P[6][10]*dt))/2; - nextP[9][2] = P[9][2] + P[6][2]*dt + SF[3]*(P[9][0] + P[6][0]*dt) + SF[5]*(P[9][3] + P[6][3]*dt) + SPP[6]*(P[9][1] + P[6][1]*dt) - SPP[3]*(P[9][10] + P[6][10]*dt) + SPP[5]*(P[9][12] + P[6][12]*dt) - (q0*(P[9][11] + P[6][11]*dt))/2; - nextP[9][3] = P[9][3] + P[6][3]*dt + SF[4]*(P[9][0] + P[6][0]*dt) + SF[3]*(P[9][1] + P[6][1]*dt) + SF[6]*(P[9][2] + P[6][2]*dt) + SPP[4]*(P[9][10] + P[6][10]*dt) - SPP[5]*(P[9][11] + P[6][11]*dt) - (q0*(P[9][12] + P[6][12]*dt))/2; - nextP[9][4] = P[9][4] + P[6][4]*dt + SF[0]*(P[9][1] + P[6][1]*dt) + SF[2]*(P[9][0] + P[6][0]*dt) + SPP[0]*(P[9][2] + P[6][2]*dt) - SPP[2]*(P[9][3] + P[6][3]*dt); - nextP[9][5] = P[9][5] + P[6][5]*dt + SF[1]*(P[9][0] + P[6][0]*dt) + SF[0]*(P[9][2] + P[6][2]*dt) + SF[2]*(P[9][3] + P[6][3]*dt) - SPP[0]*(P[9][1] + P[6][1]*dt); - nextP[9][6] = P[9][6] + P[6][6]*dt + SF[1]*(P[9][1] + P[6][1]*dt) + SF[0]*(P[9][3] + P[6][3]*dt) + SPP[0]*(P[9][0] + P[6][0]*dt) - SPP[1]*(P[9][2] + P[6][2]*dt); - nextP[9][7] = P[9][7] + P[6][7]*dt + dt*(P[9][4] + P[6][4]*dt); - nextP[9][8] = P[9][8] + P[6][8]*dt + dt*(P[9][5] + P[6][5]*dt); - nextP[9][9] = P[9][9] + P[6][9]*dt + dt*(P[9][6] + P[6][6]*dt); - nextP[9][10] = P[9][10] + P[6][10]*dt; - nextP[9][11] = P[9][11] + P[6][11]*dt; - nextP[9][12] = P[9][12] + P[6][12]*dt; - nextP[9][13] = P[9][13] + P[6][13]*dt; - nextP[9][14] = P[9][14] + P[6][14]*dt; - nextP[9][15] = P[9][15] + P[6][15]*dt; - nextP[9][16] = P[9][16] + P[6][16]*dt; - nextP[9][17] = P[9][17] + P[6][17]*dt; - nextP[9][18] = P[9][18] + P[6][18]*dt; - nextP[9][19] = P[9][19] + P[6][19]*dt; - nextP[9][20] = P[9][20] + P[6][20]*dt; - nextP[10][0] = P[10][0] + P[10][1]*SF[6] + P[10][2]*SPP[7] + P[10][3]*SPP[6] + P[10][10]*SPP[5] + P[10][11]*SPP[4] + P[10][12]*SPP[3]; - nextP[10][1] = P[10][1] + P[10][0]*SF[5] + P[10][2]*SF[4] + P[10][3]*SPP[7] + P[10][11]*SPP[3] - P[10][12]*SPP[4] - (P[10][10]*q0)/2; - nextP[10][2] = P[10][2] + P[10][0]*SF[3] + P[10][3]*SF[5] + P[10][1]*SPP[6] - P[10][10]*SPP[3] + P[10][12]*SPP[5] - (P[10][11]*q0)/2; - nextP[10][3] = P[10][3] + P[10][0]*SF[4] + P[10][1]*SF[3] + P[10][2]*SF[6] + P[10][10]*SPP[4] - P[10][11]*SPP[5] - (P[10][12]*q0)/2; - nextP[10][4] = P[10][4] + P[10][1]*SF[0] + P[10][0]*SF[2] + P[10][2]*SPP[0] - P[10][3]*SPP[2]; - nextP[10][5] = P[10][5] + P[10][0]*SF[1] + P[10][2]*SF[0] + P[10][3]*SF[2] - P[10][1]*SPP[0]; - nextP[10][6] = P[10][6] + P[10][1]*SF[1] + P[10][3]*SF[0] + P[10][0]*SPP[0] - P[10][2]*SPP[1]; - nextP[10][7] = P[10][7] + P[10][4]*dt; - nextP[10][8] = P[10][8] + P[10][5]*dt; - nextP[10][9] = P[10][9] + P[10][6]*dt; - nextP[10][10] = P[10][10]; - nextP[10][11] = P[10][11]; - nextP[10][12] = P[10][12]; - nextP[10][13] = P[10][13]; - nextP[10][14] = P[10][14]; - nextP[10][15] = P[10][15]; - nextP[10][16] = P[10][16]; - nextP[10][17] = P[10][17]; - nextP[10][18] = P[10][18]; - nextP[10][19] = P[10][19]; - nextP[10][20] = P[10][20]; - nextP[11][0] = P[11][0] + P[11][1]*SF[6] + P[11][2]*SPP[7] + P[11][3]*SPP[6] + P[11][10]*SPP[5] + P[11][11]*SPP[4] + P[11][12]*SPP[3]; - nextP[11][1] = P[11][1] + P[11][0]*SF[5] + P[11][2]*SF[4] + P[11][3]*SPP[7] + P[11][11]*SPP[3] - P[11][12]*SPP[4] - (P[11][10]*q0)/2; - nextP[11][2] = P[11][2] + P[11][0]*SF[3] + P[11][3]*SF[5] + P[11][1]*SPP[6] - P[11][10]*SPP[3] + P[11][12]*SPP[5] - (P[11][11]*q0)/2; - nextP[11][3] = P[11][3] + P[11][0]*SF[4] + P[11][1]*SF[3] + P[11][2]*SF[6] + P[11][10]*SPP[4] - P[11][11]*SPP[5] - (P[11][12]*q0)/2; - nextP[11][4] = P[11][4] + P[11][1]*SF[0] + P[11][0]*SF[2] + P[11][2]*SPP[0] - P[11][3]*SPP[2]; - nextP[11][5] = P[11][5] + P[11][0]*SF[1] + P[11][2]*SF[0] + P[11][3]*SF[2] - P[11][1]*SPP[0]; - nextP[11][6] = P[11][6] + P[11][1]*SF[1] + P[11][3]*SF[0] + P[11][0]*SPP[0] - P[11][2]*SPP[1]; - nextP[11][7] = P[11][7] + P[11][4]*dt; - nextP[11][8] = P[11][8] + P[11][5]*dt; - nextP[11][9] = P[11][9] + P[11][6]*dt; - nextP[11][10] = P[11][10]; - nextP[11][11] = P[11][11]; - nextP[11][12] = P[11][12]; - nextP[11][13] = P[11][13]; - nextP[11][14] = P[11][14]; - nextP[11][15] = P[11][15]; - nextP[11][16] = P[11][16]; - nextP[11][17] = P[11][17]; - nextP[11][18] = P[11][18]; - nextP[11][19] = P[11][19]; - nextP[11][20] = P[11][20]; - nextP[12][0] = P[12][0] + P[12][1]*SF[6] + P[12][2]*SPP[7] + P[12][3]*SPP[6] + P[12][10]*SPP[5] + P[12][11]*SPP[4] + P[12][12]*SPP[3]; - nextP[12][1] = P[12][1] + P[12][0]*SF[5] + P[12][2]*SF[4] + P[12][3]*SPP[7] + P[12][11]*SPP[3] - P[12][12]*SPP[4] - (P[12][10]*q0)/2; - nextP[12][2] = P[12][2] + P[12][0]*SF[3] + P[12][3]*SF[5] + P[12][1]*SPP[6] - P[12][10]*SPP[3] + P[12][12]*SPP[5] - (P[12][11]*q0)/2; - nextP[12][3] = P[12][3] + P[12][0]*SF[4] + P[12][1]*SF[3] + P[12][2]*SF[6] + P[12][10]*SPP[4] - P[12][11]*SPP[5] - (P[12][12]*q0)/2; - nextP[12][4] = P[12][4] + P[12][1]*SF[0] + P[12][0]*SF[2] + P[12][2]*SPP[0] - P[12][3]*SPP[2]; - nextP[12][5] = P[12][5] + P[12][0]*SF[1] + P[12][2]*SF[0] + P[12][3]*SF[2] - P[12][1]*SPP[0]; - nextP[12][6] = P[12][6] + P[12][1]*SF[1] + P[12][3]*SF[0] + P[12][0]*SPP[0] - P[12][2]*SPP[1]; - nextP[12][7] = P[12][7] + P[12][4]*dt; - nextP[12][8] = P[12][8] + P[12][5]*dt; - nextP[12][9] = P[12][9] + P[12][6]*dt; - nextP[12][10] = P[12][10]; - nextP[12][11] = P[12][11]; - nextP[12][12] = P[12][12]; - nextP[12][13] = P[12][13]; - nextP[12][14] = P[12][14]; - nextP[12][15] = P[12][15]; - nextP[12][16] = P[12][16]; - nextP[12][17] = P[12][17]; - nextP[12][18] = P[12][18]; - nextP[12][19] = P[12][19]; - nextP[12][20] = P[12][20]; - nextP[13][0] = P[13][0] + P[13][1]*SF[6] + P[13][2]*SPP[7] + P[13][3]*SPP[6] + P[13][10]*SPP[5] + P[13][11]*SPP[4] + P[13][12]*SPP[3]; - nextP[13][1] = P[13][1] + P[13][0]*SF[5] + P[13][2]*SF[4] + P[13][3]*SPP[7] + P[13][11]*SPP[3] - P[13][12]*SPP[4] - (P[13][10]*q0)/2; - nextP[13][2] = P[13][2] + P[13][0]*SF[3] + P[13][3]*SF[5] + P[13][1]*SPP[6] - P[13][10]*SPP[3] + P[13][12]*SPP[5] - (P[13][11]*q0)/2; - nextP[13][3] = P[13][3] + P[13][0]*SF[4] + P[13][1]*SF[3] + P[13][2]*SF[6] + P[13][10]*SPP[4] - P[13][11]*SPP[5] - (P[13][12]*q0)/2; - nextP[13][4] = P[13][4] + P[13][1]*SF[0] + P[13][0]*SF[2] + P[13][2]*SPP[0] - P[13][3]*SPP[2]; - nextP[13][5] = P[13][5] + P[13][0]*SF[1] + P[13][2]*SF[0] + P[13][3]*SF[2] - P[13][1]*SPP[0]; - nextP[13][6] = P[13][6] + P[13][1]*SF[1] + P[13][3]*SF[0] + P[13][0]*SPP[0] - P[13][2]*SPP[1]; - nextP[13][7] = P[13][7] + P[13][4]*dt; - nextP[13][8] = P[13][8] + P[13][5]*dt; - nextP[13][9] = P[13][9] + P[13][6]*dt; - nextP[13][10] = P[13][10]; - nextP[13][11] = P[13][11]; - nextP[13][12] = P[13][12]; - nextP[13][13] = P[13][13]; - nextP[13][14] = P[13][14]; - nextP[13][15] = P[13][15]; - nextP[13][16] = P[13][16]; - nextP[13][17] = P[13][17]; - nextP[13][18] = P[13][18]; - nextP[13][19] = P[13][19]; - nextP[13][20] = P[13][20]; - nextP[14][0] = P[14][0] + P[14][1]*SF[6] + P[14][2]*SPP[7] + P[14][3]*SPP[6] + P[14][10]*SPP[5] + P[14][11]*SPP[4] + P[14][12]*SPP[3]; - nextP[14][1] = P[14][1] + P[14][0]*SF[5] + P[14][2]*SF[4] + P[14][3]*SPP[7] + P[14][11]*SPP[3] - P[14][12]*SPP[4] - (P[14][10]*q0)/2; - nextP[14][2] = P[14][2] + P[14][0]*SF[3] + P[14][3]*SF[5] + P[14][1]*SPP[6] - P[14][10]*SPP[3] + P[14][12]*SPP[5] - (P[14][11]*q0)/2; - nextP[14][3] = P[14][3] + P[14][0]*SF[4] + P[14][1]*SF[3] + P[14][2]*SF[6] + P[14][10]*SPP[4] - P[14][11]*SPP[5] - (P[14][12]*q0)/2; - nextP[14][4] = P[14][4] + P[14][1]*SF[0] + P[14][0]*SF[2] + P[14][2]*SPP[0] - P[14][3]*SPP[2]; - nextP[14][5] = P[14][5] + P[14][0]*SF[1] + P[14][2]*SF[0] + P[14][3]*SF[2] - P[14][1]*SPP[0]; - nextP[14][6] = P[14][6] + P[14][1]*SF[1] + P[14][3]*SF[0] + P[14][0]*SPP[0] - P[14][2]*SPP[1]; - nextP[14][7] = P[14][7] + P[14][4]*dt; - nextP[14][8] = P[14][8] + P[14][5]*dt; - nextP[14][9] = P[14][9] + P[14][6]*dt; - nextP[14][10] = P[14][10]; - nextP[14][11] = P[14][11]; - nextP[14][12] = P[14][12]; - nextP[14][13] = P[14][13]; - nextP[14][14] = P[14][14]; - nextP[14][15] = P[14][15]; - nextP[14][16] = P[14][16]; - nextP[14][17] = P[14][17]; - nextP[14][18] = P[14][18]; - nextP[14][19] = P[14][19]; - nextP[14][20] = P[14][20]; - nextP[15][0] = P[15][0] + P[15][1]*SF[6] + P[15][2]*SPP[7] + P[15][3]*SPP[6] + P[15][10]*SPP[5] + P[15][11]*SPP[4] + P[15][12]*SPP[3]; - nextP[15][1] = P[15][1] + P[15][0]*SF[5] + P[15][2]*SF[4] + P[15][3]*SPP[7] + P[15][11]*SPP[3] - P[15][12]*SPP[4] - (P[15][10]*q0)/2; - nextP[15][2] = P[15][2] + P[15][0]*SF[3] + P[15][3]*SF[5] + P[15][1]*SPP[6] - P[15][10]*SPP[3] + P[15][12]*SPP[5] - (P[15][11]*q0)/2; - nextP[15][3] = P[15][3] + P[15][0]*SF[4] + P[15][1]*SF[3] + P[15][2]*SF[6] + P[15][10]*SPP[4] - P[15][11]*SPP[5] - (P[15][12]*q0)/2; - nextP[15][4] = P[15][4] + P[15][1]*SF[0] + P[15][0]*SF[2] + P[15][2]*SPP[0] - P[15][3]*SPP[2]; - nextP[15][5] = P[15][5] + P[15][0]*SF[1] + P[15][2]*SF[0] + P[15][3]*SF[2] - P[15][1]*SPP[0]; - nextP[15][6] = P[15][6] + P[15][1]*SF[1] + P[15][3]*SF[0] + P[15][0]*SPP[0] - P[15][2]*SPP[1]; - nextP[15][7] = P[15][7] + P[15][4]*dt; - nextP[15][8] = P[15][8] + P[15][5]*dt; - nextP[15][9] = P[15][9] + P[15][6]*dt; - nextP[15][10] = P[15][10]; - nextP[15][11] = P[15][11]; - nextP[15][12] = P[15][12]; - nextP[15][13] = P[15][13]; - nextP[15][14] = P[15][14]; - nextP[15][15] = P[15][15]; - nextP[15][16] = P[15][16]; - nextP[15][17] = P[15][17]; - nextP[15][18] = P[15][18]; - nextP[15][19] = P[15][19]; - nextP[15][20] = P[15][20]; - nextP[16][0] = P[16][0] + P[16][1]*SF[6] + P[16][2]*SPP[7] + P[16][3]*SPP[6] + P[16][10]*SPP[5] + P[16][11]*SPP[4] + P[16][12]*SPP[3]; - nextP[16][1] = P[16][1] + P[16][0]*SF[5] + P[16][2]*SF[4] + P[16][3]*SPP[7] + P[16][11]*SPP[3] - P[16][12]*SPP[4] - (P[16][10]*q0)/2; - nextP[16][2] = P[16][2] + P[16][0]*SF[3] + P[16][3]*SF[5] + P[16][1]*SPP[6] - P[16][10]*SPP[3] + P[16][12]*SPP[5] - (P[16][11]*q0)/2; - nextP[16][3] = P[16][3] + P[16][0]*SF[4] + P[16][1]*SF[3] + P[16][2]*SF[6] + P[16][10]*SPP[4] - P[16][11]*SPP[5] - (P[16][12]*q0)/2; - nextP[16][4] = P[16][4] + P[16][1]*SF[0] + P[16][0]*SF[2] + P[16][2]*SPP[0] - P[16][3]*SPP[2]; - nextP[16][5] = P[16][5] + P[16][0]*SF[1] + P[16][2]*SF[0] + P[16][3]*SF[2] - P[16][1]*SPP[0]; - nextP[16][6] = P[16][6] + P[16][1]*SF[1] + P[16][3]*SF[0] + P[16][0]*SPP[0] - P[16][2]*SPP[1]; - nextP[16][7] = P[16][7] + P[16][4]*dt; - nextP[16][8] = P[16][8] + P[16][5]*dt; - nextP[16][9] = P[16][9] + P[16][6]*dt; - nextP[16][10] = P[16][10]; - nextP[16][11] = P[16][11]; - nextP[16][12] = P[16][12]; - nextP[16][13] = P[16][13]; - nextP[16][14] = P[16][14]; - nextP[16][15] = P[16][15]; - nextP[16][16] = P[16][16]; - nextP[16][17] = P[16][17]; - nextP[16][18] = P[16][18]; - nextP[16][19] = P[16][19]; - nextP[16][20] = P[16][20]; - nextP[17][0] = P[17][0] + P[17][1]*SF[6] + P[17][2]*SPP[7] + P[17][3]*SPP[6] + P[17][10]*SPP[5] + P[17][11]*SPP[4] + P[17][12]*SPP[3]; - nextP[17][1] = P[17][1] + P[17][0]*SF[5] + P[17][2]*SF[4] + P[17][3]*SPP[7] + P[17][11]*SPP[3] - P[17][12]*SPP[4] - (P[17][10]*q0)/2; - nextP[17][2] = P[17][2] + P[17][0]*SF[3] + P[17][3]*SF[5] + P[17][1]*SPP[6] - P[17][10]*SPP[3] + P[17][12]*SPP[5] - (P[17][11]*q0)/2; - nextP[17][3] = P[17][3] + P[17][0]*SF[4] + P[17][1]*SF[3] + P[17][2]*SF[6] + P[17][10]*SPP[4] - P[17][11]*SPP[5] - (P[17][12]*q0)/2; - nextP[17][4] = P[17][4] + P[17][1]*SF[0] + P[17][0]*SF[2] + P[17][2]*SPP[0] - P[17][3]*SPP[2]; - nextP[17][5] = P[17][5] + P[17][0]*SF[1] + P[17][2]*SF[0] + P[17][3]*SF[2] - P[17][1]*SPP[0]; - nextP[17][6] = P[17][6] + P[17][1]*SF[1] + P[17][3]*SF[0] + P[17][0]*SPP[0] - P[17][2]*SPP[1]; - nextP[17][7] = P[17][7] + P[17][4]*dt; - nextP[17][8] = P[17][8] + P[17][5]*dt; - nextP[17][9] = P[17][9] + P[17][6]*dt; - nextP[17][10] = P[17][10]; - nextP[17][11] = P[17][11]; - nextP[17][12] = P[17][12]; - nextP[17][13] = P[17][13]; - nextP[17][14] = P[17][14]; - nextP[17][15] = P[17][15]; - nextP[17][16] = P[17][16]; - nextP[17][17] = P[17][17]; - nextP[17][18] = P[17][18]; - nextP[17][19] = P[17][19]; - nextP[17][20] = P[17][20]; - nextP[18][0] = P[18][0] + P[18][1]*SF[6] + P[18][2]*SPP[7] + P[18][3]*SPP[6] + P[18][10]*SPP[5] + P[18][11]*SPP[4] + P[18][12]*SPP[3]; - nextP[18][1] = P[18][1] + P[18][0]*SF[5] + P[18][2]*SF[4] + P[18][3]*SPP[7] + P[18][11]*SPP[3] - P[18][12]*SPP[4] - (P[18][10]*q0)/2; - nextP[18][2] = P[18][2] + P[18][0]*SF[3] + P[18][3]*SF[5] + P[18][1]*SPP[6] - P[18][10]*SPP[3] + P[18][12]*SPP[5] - (P[18][11]*q0)/2; - nextP[18][3] = P[18][3] + P[18][0]*SF[4] + P[18][1]*SF[3] + P[18][2]*SF[6] + P[18][10]*SPP[4] - P[18][11]*SPP[5] - (P[18][12]*q0)/2; - nextP[18][4] = P[18][4] + P[18][1]*SF[0] + P[18][0]*SF[2] + P[18][2]*SPP[0] - P[18][3]*SPP[2]; - nextP[18][5] = P[18][5] + P[18][0]*SF[1] + P[18][2]*SF[0] + P[18][3]*SF[2] - P[18][1]*SPP[0]; - nextP[18][6] = P[18][6] + P[18][1]*SF[1] + P[18][3]*SF[0] + P[18][0]*SPP[0] - P[18][2]*SPP[1]; - nextP[18][7] = P[18][7] + P[18][4]*dt; - nextP[18][8] = P[18][8] + P[18][5]*dt; - nextP[18][9] = P[18][9] + P[18][6]*dt; - nextP[18][10] = P[18][10]; - nextP[18][11] = P[18][11]; - nextP[18][12] = P[18][12]; - nextP[18][13] = P[18][13]; - nextP[18][14] = P[18][14]; - nextP[18][15] = P[18][15]; - nextP[18][16] = P[18][16]; - nextP[18][17] = P[18][17]; - nextP[18][18] = P[18][18]; - nextP[18][19] = P[18][19]; - nextP[18][20] = P[18][20]; - nextP[19][0] = P[19][0] + P[19][1]*SF[6] + P[19][2]*SPP[7] + P[19][3]*SPP[6] + P[19][10]*SPP[5] + P[19][11]*SPP[4] + P[19][12]*SPP[3]; - nextP[19][1] = P[19][1] + P[19][0]*SF[5] + P[19][2]*SF[4] + P[19][3]*SPP[7] + P[19][11]*SPP[3] - P[19][12]*SPP[4] - (P[19][10]*q0)/2; - nextP[19][2] = P[19][2] + P[19][0]*SF[3] + P[19][3]*SF[5] + P[19][1]*SPP[6] - P[19][10]*SPP[3] + P[19][12]*SPP[5] - (P[19][11]*q0)/2; - nextP[19][3] = P[19][3] + P[19][0]*SF[4] + P[19][1]*SF[3] + P[19][2]*SF[6] + P[19][10]*SPP[4] - P[19][11]*SPP[5] - (P[19][12]*q0)/2; - nextP[19][4] = P[19][4] + P[19][1]*SF[0] + P[19][0]*SF[2] + P[19][2]*SPP[0] - P[19][3]*SPP[2]; - nextP[19][5] = P[19][5] + P[19][0]*SF[1] + P[19][2]*SF[0] + P[19][3]*SF[2] - P[19][1]*SPP[0]; - nextP[19][6] = P[19][6] + P[19][1]*SF[1] + P[19][3]*SF[0] + P[19][0]*SPP[0] - P[19][2]*SPP[1]; - nextP[19][7] = P[19][7] + P[19][4]*dt; - nextP[19][8] = P[19][8] + P[19][5]*dt; - nextP[19][9] = P[19][9] + P[19][6]*dt; - nextP[19][10] = P[19][10]; - nextP[19][11] = P[19][11]; - nextP[19][12] = P[19][12]; - nextP[19][13] = P[19][13]; - nextP[19][14] = P[19][14]; - nextP[19][15] = P[19][15]; - nextP[19][16] = P[19][16]; - nextP[19][17] = P[19][17]; - nextP[19][18] = P[19][18]; - nextP[19][19] = P[19][19]; - nextP[19][20] = P[19][20]; - nextP[20][0] = P[20][0] + P[20][1]*SF[6] + P[20][2]*SPP[7] + P[20][3]*SPP[6] + P[20][10]*SPP[5] + P[20][11]*SPP[4] + P[20][12]*SPP[3]; - nextP[20][1] = P[20][1] + P[20][0]*SF[5] + P[20][2]*SF[4] + P[20][3]*SPP[7] + P[20][11]*SPP[3] - P[20][12]*SPP[4] - (P[20][10]*q0)/2; - nextP[20][2] = P[20][2] + P[20][0]*SF[3] + P[20][3]*SF[5] + P[20][1]*SPP[6] - P[20][10]*SPP[3] + P[20][12]*SPP[5] - (P[20][11]*q0)/2; - nextP[20][3] = P[20][3] + P[20][0]*SF[4] + P[20][1]*SF[3] + P[20][2]*SF[6] + P[20][10]*SPP[4] - P[20][11]*SPP[5] - (P[20][12]*q0)/2; - nextP[20][4] = P[20][4] + P[20][1]*SF[0] + P[20][0]*SF[2] + P[20][2]*SPP[0] - P[20][3]*SPP[2]; - nextP[20][5] = P[20][5] + P[20][0]*SF[1] + P[20][2]*SF[0] + P[20][3]*SF[2] - P[20][1]*SPP[0]; - nextP[20][6] = P[20][6] + P[20][1]*SF[1] + P[20][3]*SF[0] + P[20][0]*SPP[0] - P[20][2]*SPP[1]; - nextP[20][7] = P[20][7] + P[20][4]*dt; - nextP[20][8] = P[20][8] + P[20][5]*dt; - nextP[20][9] = P[20][9] + P[20][6]*dt; - nextP[20][10] = P[20][10]; - nextP[20][11] = P[20][11]; - nextP[20][12] = P[20][12]; - nextP[20][13] = P[20][13]; - nextP[20][14] = P[20][14]; - nextP[20][15] = P[20][15]; - nextP[20][16] = P[20][16]; - nextP[20][17] = P[20][17]; - nextP[20][18] = P[20][18]; - nextP[20][19] = P[20][19]; - nextP[20][20] = P[20][20]; - - for (unsigned i = 0; i < n_states; i++) - { - nextP[i][i] = nextP[i][i] + processNoise[i]; - } - - // If on ground or no magnetometer fitted, inhibit magnetometer bias updates by - // setting the coresponding covariance terms to zero. - if (onGround || !useCompass) - { - zeroRows(nextP,15,20); - zeroCols(nextP,15,20); - } - - // If on ground or not using airspeed sensing, inhibit wind velocity - // covariance growth. - if (onGround || !useAirspeed) - { - zeroRows(nextP,13,14); - zeroCols(nextP,13,14); - } - - // If the total position variance exceds 1E6 (1000m), then stop covariance - // growth by setting the predicted to the previous values - // This prevent an ill conditioned matrix from occurring for long periods - // without GPS - if ((P[7][7] + P[8][8]) > 1E6f) - { - for (uint8_t i=7; i<=8; i++) - { - for (unsigned j = 0; j < n_states; j++) - { - nextP[i][j] = P[i][j]; - nextP[j][i] = P[j][i]; - } - } - } - - if (onGround || staticMode) { - // copy the portion of the variances we want to - // propagate - for (unsigned i = 0; i < 14; i++) { - P[i][i] = nextP[i][i]; - - // force symmetry for observable states - // force zero for non-observable states - for (unsigned i = 1; i < n_states; i++) - { - for (uint8_t j = 0; j < i; j++) - { - if ((i > 12) || (j > 12)) { - P[i][j] = 0.0f; - } else { - P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); - } - P[j][i] = P[i][j]; - } - } - } - - } else { - - // Copy covariance - for (unsigned i = 0; i < n_states; i++) { - P[i][i] = nextP[i][i]; - } - - // force symmetry for observable states - for (unsigned i = 1; i < n_states; i++) - { - for (uint8_t j = 0; j < i; j++) - { - P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); - P[j][i] = P[i][j]; - } - } - - } - - ConstrainVariances(); -} - -void AttPosEKF::FuseVelposNED() -{ - -// declare variables used by fault isolation logic - uint32_t gpsRetryTime = 30000; // time in msec before GPS fusion will be retried following innovation consistency failure - uint32_t gpsRetryTimeNoTAS = 5000; // retry time if no TAS measurement available - uint32_t hgtRetryTime = 5000; // height measurement retry time - uint32_t horizRetryTime; - -// declare variables used to check measurement errors - float velInnov[3] = {0.0f,0.0f,0.0f}; - float posInnov[2] = {0.0f,0.0f}; - float hgtInnov = 0.0f; - -// declare variables used to control access to arrays - bool fuseData[6] = {false,false,false,false,false,false}; - uint8_t stateIndex; - uint8_t obsIndex; - uint8_t indexLimit; - -// declare variables used by state and covariance update calculations - float velErr; - float posErr; - float R_OBS[6]; - float observation[6]; - float SK; - float quatMag; - -// Perform sequential fusion of GPS measurements. This assumes that the -// errors in the different velocity and position components are -// uncorrelated which is not true, however in the absence of covariance -// data from the GPS receiver it is the only assumption we can make -// so we might as well take advantage of the computational efficiencies -// associated with sequential fusion - if (fuseVelData || fusePosData || fuseHgtData) - { - // set the GPS data timeout depending on whether airspeed data is present - if (useAirspeed) horizRetryTime = gpsRetryTime; - else horizRetryTime = gpsRetryTimeNoTAS; - - // Form the observation vector - for (uint8_t i=0; i<=2; i++) observation[i] = velNED[i]; - for (uint8_t i=3; i<=4; i++) observation[i] = posNE[i-3]; - observation[5] = -(hgtMea); - - // Estimate the GPS Velocity, GPS horiz position and height measurement variances. - velErr = 0.2f*accNavMag; // additional error in GPS velocities caused by manoeuvring - posErr = 0.2f*accNavMag; // additional error in GPS position caused by manoeuvring - R_OBS[0] = 0.04f + sq(velErr); - R_OBS[1] = R_OBS[0]; - R_OBS[2] = 0.08f + sq(velErr); - R_OBS[3] = R_OBS[2]; - R_OBS[4] = 4.0f + sq(posErr); - R_OBS[5] = 4.0f; - - // Set innovation variances to zero default - for (uint8_t i = 0; i<=5; i++) - { - varInnovVelPos[i] = 0.0f; - } - // calculate innovations and check GPS data validity using an innovation consistency check - if (fuseVelData) - { - // test velocity measurements - uint8_t imax = 2; - if (fusionModeGPS == 1) imax = 1; - for (uint8_t i = 0; i<=imax; i++) - { - velInnov[i] = statesAtVelTime[i+4] - velNED[i]; - stateIndex = 4 + i; - varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i]; - } - // apply a 5-sigma threshold - current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0f * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]); - current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime; - if (current_ekf_state.velHealth || current_ekf_state.velTimeout) - { - current_ekf_state.velHealth = true; - current_ekf_state.velFailTime = millis(); - } - else - { - current_ekf_state.velHealth = false; - } - } - if (fusePosData) - { - // test horizontal position measurements - posInnov[0] = statesAtPosTime[7] - posNE[0]; - posInnov[1] = statesAtPosTime[8] - posNE[1]; - varInnovVelPos[3] = P[7][7] + R_OBS[3]; - varInnovVelPos[4] = P[8][8] + R_OBS[4]; - // apply a 10-sigma threshold - current_ekf_state.posHealth = (sq(posInnov[0]) + sq(posInnov[1])) < 100.0f*(varInnovVelPos[3] + varInnovVelPos[4]); - current_ekf_state.posTimeout = (millis() - current_ekf_state.posFailTime) > horizRetryTime; - if (current_ekf_state.posHealth || current_ekf_state.posTimeout) - { - current_ekf_state.posHealth = true; - current_ekf_state.posFailTime = millis(); - } - else - { - current_ekf_state.posHealth = false; - } - } - // test height measurements - if (fuseHgtData) - { - hgtInnov = statesAtHgtTime[9] + hgtMea; - varInnovVelPos[5] = P[9][9] + R_OBS[5]; - // apply a 10-sigma threshold - current_ekf_state.hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5]; - current_ekf_state.hgtTimeout = (millis() - current_ekf_state.hgtFailTime) > hgtRetryTime; - if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout) - { - current_ekf_state.hgtHealth = true; - current_ekf_state.hgtFailTime = millis(); - } - else - { - current_ekf_state.hgtHealth = false; - } - } - // Set range for sequential fusion of velocity and position measurements depending - // on which data is available and its health - if (fuseVelData && fusionModeGPS == 0 && current_ekf_state.velHealth) - { - fuseData[0] = true; - fuseData[1] = true; - fuseData[2] = true; - } - if (fuseVelData && fusionModeGPS == 1 && current_ekf_state.velHealth) - { - fuseData[0] = true; - fuseData[1] = true; - } - if (fusePosData && fusionModeGPS <= 2 && current_ekf_state.posHealth) - { - fuseData[3] = true; - fuseData[4] = true; - } - if (fuseHgtData && current_ekf_state.hgtHealth) - { - fuseData[5] = true; - } - // Limit range of states modified when on ground - if(!onGround) - { - indexLimit = 20; - } - else - { - indexLimit = 12; - } - // Fuse measurements sequentially - for (obsIndex=0; obsIndex<=5; obsIndex++) - { - if (fuseData[obsIndex]) - { - stateIndex = 4 + obsIndex; - // Calculate the measurement innovation, using states from a - // different time coordinate if fusing height data - if (obsIndex >= 0 && obsIndex <= 2) - { - innovVelPos[obsIndex] = statesAtVelTime[stateIndex] - observation[obsIndex]; - } - else if (obsIndex == 3 || obsIndex == 4) - { - innovVelPos[obsIndex] = statesAtPosTime[stateIndex] - observation[obsIndex]; - } - else if (obsIndex == 5) - { - innovVelPos[obsIndex] = statesAtHgtTime[stateIndex] - observation[obsIndex]; - } - // Calculate the Kalman Gain - // Calculate innovation variances - also used for data logging - varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex]; - SK = 1.0/varInnovVelPos[obsIndex]; - for (uint8_t i= 0; i<=indexLimit; i++) - { - Kfusion[i] = P[i][stateIndex]*SK; - } - // Calculate state corrections and re-normalise the quaternions - for (uint8_t i = 0; i<=indexLimit; i++) - { - states[i] = states[i] - Kfusion[i] * innovVelPos[obsIndex]; - } - quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); - if (quatMag > 1e-12f) // divide by 0 protection - { - for (uint8_t i = 0; i<=3; i++) - { - states[i] = states[i] / quatMag; - } - } - // Update the covariance - take advantage of direct observation of a - // single state at index = stateIndex to reduce computations - // Optimised implementation of standard equation P = (I - K*H)*P; - for (uint8_t i= 0; i<=indexLimit; i++) - { - for (uint8_t j= 0; j<=indexLimit; j++) - { - KHP[i][j] = Kfusion[i] * P[stateIndex][j]; - } - } - for (uint8_t i= 0; i<=indexLimit; i++) - { - for (uint8_t j= 0; j<=indexLimit; j++) - { - P[i][j] = P[i][j] - KHP[i][j]; - } - } - } - } - } - - ForceSymmetry(); - ConstrainVariances(); - - //printf("velh: %s, posh: %s, hgth: %s\n", ((velHealth) ? "OK" : "FAIL"), ((posHealth) ? "OK" : "FAIL"), ((hgtHealth) ? "OK" : "FAIL")); -} - -void AttPosEKF::FuseMagnetometer() -{ - uint8_t obsIndex; - uint8_t indexLimit; - float DCM[3][3] = - { - {1.0f,0.0f,0.0f} , - {0.0f,1.0f,0.0f} , - {0.0f,0.0f,1.0f} - }; - float MagPred[3] = {0.0f,0.0f,0.0f}; - float SK_MX[6]; - float SK_MY[5]; - float SK_MZ[6]; - float SH_MAG[9] = {0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f}; - -// Perform sequential fusion of Magnetometer measurements. -// This assumes that the errors in the different components are -// uncorrelated which is not true, however in the absence of covariance -// data fit is the only assumption we can make -// so we might as well take advantage of the computational efficiencies -// associated with sequential fusion - if (useCompass && (fuseMagData || obsIndex == 1 || obsIndex == 2)) - { - // Limit range of states modified when on ground - if(!onGround) - { - indexLimit = 20; - } - else - { - indexLimit = 12; - } - - static float q0 = 0.0f; - static float q1 = 0.0f; - static float q2 = 0.0f; - static float q3 = 1.0f; - static float magN = 0.4f; - static float magE = 0.0f; - static float magD = 0.3f; - - static float R_MAG = 0.0025f; - - float H_MAG[21] = {0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f}; - - // Sequential fusion of XYZ components to spread processing load across - // three prediction time steps. - - // Calculate observation jacobians and Kalman gains - if (fuseMagData) - { - static float magXbias = 0.0f; - static float magYbias = 0.0f; - static float magZbias = 0.0f; - - // Copy required states to local variable names - q0 = statesAtMagMeasTime[0]; - q1 = statesAtMagMeasTime[1]; - q2 = statesAtMagMeasTime[2]; - q3 = statesAtMagMeasTime[3]; - magN = statesAtMagMeasTime[15]; - magE = statesAtMagMeasTime[16]; - magD = statesAtMagMeasTime[17]; - magXbias = statesAtMagMeasTime[18]; - magYbias = statesAtMagMeasTime[19]; - magZbias = statesAtMagMeasTime[20]; - - // rotate predicted earth components into body axes and calculate - // predicted measurments - DCM[0][0] = q0*q0 + q1*q1 - q2*q2 - q3*q3; - DCM[0][1] = 2*(q1*q2 + q0*q3); - DCM[0][2] = 2*(q1*q3-q0*q2); - DCM[1][0] = 2*(q1*q2 - q0*q3); - DCM[1][1] = q0*q0 - q1*q1 + q2*q2 - q3*q3; - DCM[1][2] = 2*(q2*q3 + q0*q1); - DCM[2][0] = 2*(q1*q3 + q0*q2); - DCM[2][1] = 2*(q2*q3 - q0*q1); - DCM[2][2] = q0*q0 - q1*q1 - q2*q2 + q3*q3; - MagPred[0] = DCM[0][0]*magN + DCM[0][1]*magE + DCM[0][2]*magD + magXbias; - MagPred[1] = DCM[1][0]*magN + DCM[1][1]*magE + DCM[1][2]*magD + magYbias; - MagPred[2] = DCM[2][0]*magN + DCM[2][1]*magE + DCM[2][2]*magD + magZbias; - - // scale magnetometer observation error with total angular rate - R_MAG = 0.0025f + sq(0.05f*dAngIMU.length()/dtIMU); - - // Calculate observation jacobians - SH_MAG[0] = 2*magD*q3 + 2*magE*q2 + 2*magN*q1; - SH_MAG[1] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2; - SH_MAG[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3; - SH_MAG[3] = sq(q3); - SH_MAG[4] = sq(q2); - SH_MAG[5] = sq(q1); - SH_MAG[6] = sq(q0); - SH_MAG[7] = 2*magN*q0; - SH_MAG[8] = 2*magE*q3; - - for (uint8_t i=0; i<=20; i++) H_MAG[i] = 0; - H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; - H_MAG[1] = SH_MAG[0]; - H_MAG[2] = 2*magE*q1 - 2*magD*q0 - 2*magN*q2; - H_MAG[3] = SH_MAG[2]; - H_MAG[15] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6]; - H_MAG[16] = 2*q0*q3 + 2*q1*q2; - H_MAG[17] = 2*q1*q3 - 2*q0*q2; - H_MAG[18] = 1.0f; - - // Calculate Kalman gain - SK_MX[0] = 1/(P[18][18] + R_MAG + P[1][18]*SH_MAG[0] + P[3][18]*SH_MAG[2] - P[15][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[18][2] + P[1][2]*SH_MAG[0] + P[3][2]*SH_MAG[2] - P[15][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][2]*(2*q0*q3 + 2*q1*q2) - P[17][2]*(2*q0*q2 - 2*q1*q3) - P[2][2]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[18][0] + P[1][0]*SH_MAG[0] + P[3][0]*SH_MAG[2] - P[15][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][0]*(2*q0*q3 + 2*q1*q2) - P[17][0]*(2*q0*q2 - 2*q1*q3) - P[2][0]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[18][1] + P[1][1]*SH_MAG[0] + P[3][1]*SH_MAG[2] - P[15][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][1]*(2*q0*q3 + 2*q1*q2) - P[17][1]*(2*q0*q2 - 2*q1*q3) - P[2][1]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[2]*(P[18][3] + P[1][3]*SH_MAG[0] + P[3][3]*SH_MAG[2] - P[15][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][3]*(2*q0*q3 + 2*q1*q2) - P[17][3]*(2*q0*q2 - 2*q1*q3) - P[2][3]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[18][15] + P[1][15]*SH_MAG[0] + P[3][15]*SH_MAG[2] - P[15][15]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][15]*(2*q0*q3 + 2*q1*q2) - P[17][15]*(2*q0*q2 - 2*q1*q3) - P[2][15]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][18]*(2*q0*q3 + 2*q1*q2) - P[17][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[18][16] + P[1][16]*SH_MAG[0] + P[3][16]*SH_MAG[2] - P[15][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][16]*(2*q0*q3 + 2*q1*q2) - P[17][16]*(2*q0*q2 - 2*q1*q3) - P[2][16]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(P[18][17] + P[1][17]*SH_MAG[0] + P[3][17]*SH_MAG[2] - P[15][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][17]*(2*q0*q3 + 2*q1*q2) - P[17][17]*(2*q0*q2 - 2*q1*q3) - P[2][17]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); - SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]; - SK_MX[2] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2; - SK_MX[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; - SK_MX[4] = 2*q0*q2 - 2*q1*q3; - SK_MX[5] = 2*q0*q3 + 2*q1*q2; - Kfusion[0] = SK_MX[0]*(P[0][18] + P[0][1]*SH_MAG[0] + P[0][3]*SH_MAG[2] + P[0][0]*SK_MX[3] - P[0][2]*SK_MX[2] - P[0][15]*SK_MX[1] + P[0][16]*SK_MX[5] - P[0][17]*SK_MX[4]); - Kfusion[1] = SK_MX[0]*(P[1][18] + P[1][1]*SH_MAG[0] + P[1][3]*SH_MAG[2] + P[1][0]*SK_MX[3] - P[1][2]*SK_MX[2] - P[1][15]*SK_MX[1] + P[1][16]*SK_MX[5] - P[1][17]*SK_MX[4]); - Kfusion[2] = SK_MX[0]*(P[2][18] + P[2][1]*SH_MAG[0] + P[2][3]*SH_MAG[2] + P[2][0]*SK_MX[3] - P[2][2]*SK_MX[2] - P[2][15]*SK_MX[1] + P[2][16]*SK_MX[5] - P[2][17]*SK_MX[4]); - Kfusion[3] = SK_MX[0]*(P[3][18] + P[3][1]*SH_MAG[0] + P[3][3]*SH_MAG[2] + P[3][0]*SK_MX[3] - P[3][2]*SK_MX[2] - P[3][15]*SK_MX[1] + P[3][16]*SK_MX[5] - P[3][17]*SK_MX[4]); - Kfusion[4] = SK_MX[0]*(P[4][18] + P[4][1]*SH_MAG[0] + P[4][3]*SH_MAG[2] + P[4][0]*SK_MX[3] - P[4][2]*SK_MX[2] - P[4][15]*SK_MX[1] + P[4][16]*SK_MX[5] - P[4][17]*SK_MX[4]); - Kfusion[5] = SK_MX[0]*(P[5][18] + P[5][1]*SH_MAG[0] + P[5][3]*SH_MAG[2] + P[5][0]*SK_MX[3] - P[5][2]*SK_MX[2] - P[5][15]*SK_MX[1] + P[5][16]*SK_MX[5] - P[5][17]*SK_MX[4]); - Kfusion[6] = SK_MX[0]*(P[6][18] + P[6][1]*SH_MAG[0] + P[6][3]*SH_MAG[2] + P[6][0]*SK_MX[3] - P[6][2]*SK_MX[2] - P[6][15]*SK_MX[1] + P[6][16]*SK_MX[5] - P[6][17]*SK_MX[4]); - Kfusion[7] = SK_MX[0]*(P[7][18] + P[7][1]*SH_MAG[0] + P[7][3]*SH_MAG[2] + P[7][0]*SK_MX[3] - P[7][2]*SK_MX[2] - P[7][15]*SK_MX[1] + P[7][16]*SK_MX[5] - P[7][17]*SK_MX[4]); - Kfusion[8] = SK_MX[0]*(P[8][18] + P[8][1]*SH_MAG[0] + P[8][3]*SH_MAG[2] + P[8][0]*SK_MX[3] - P[8][2]*SK_MX[2] - P[8][15]*SK_MX[1] + P[8][16]*SK_MX[5] - P[8][17]*SK_MX[4]); - Kfusion[9] = SK_MX[0]*(P[9][18] + P[9][1]*SH_MAG[0] + P[9][3]*SH_MAG[2] + P[9][0]*SK_MX[3] - P[9][2]*SK_MX[2] - P[9][15]*SK_MX[1] + P[9][16]*SK_MX[5] - P[9][17]*SK_MX[4]); - Kfusion[10] = SK_MX[0]*(P[10][18] + P[10][1]*SH_MAG[0] + P[10][3]*SH_MAG[2] + P[10][0]*SK_MX[3] - P[10][2]*SK_MX[2] - P[10][15]*SK_MX[1] + P[10][16]*SK_MX[5] - P[10][17]*SK_MX[4]); - Kfusion[11] = SK_MX[0]*(P[11][18] + P[11][1]*SH_MAG[0] + P[11][3]*SH_MAG[2] + P[11][0]*SK_MX[3] - P[11][2]*SK_MX[2] - P[11][15]*SK_MX[1] + P[11][16]*SK_MX[5] - P[11][17]*SK_MX[4]); - Kfusion[12] = SK_MX[0]*(P[12][18] + P[12][1]*SH_MAG[0] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[3] - P[12][2]*SK_MX[2] - P[12][15]*SK_MX[1] + P[12][16]*SK_MX[5] - P[12][17]*SK_MX[4]); - Kfusion[13] = SK_MX[0]*(P[13][18] + P[13][1]*SH_MAG[0] + P[13][3]*SH_MAG[2] + P[13][0]*SK_MX[3] - P[13][2]*SK_MX[2] - P[13][15]*SK_MX[1] + P[13][16]*SK_MX[5] - P[13][17]*SK_MX[4]); - Kfusion[14] = SK_MX[0]*(P[14][18] + P[14][1]*SH_MAG[0] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[3] - P[14][2]*SK_MX[2] - P[14][15]*SK_MX[1] + P[14][16]*SK_MX[5] - P[14][17]*SK_MX[4]); - Kfusion[15] = SK_MX[0]*(P[15][18] + P[15][1]*SH_MAG[0] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[3] - P[15][2]*SK_MX[2] - P[15][15]*SK_MX[1] + P[15][16]*SK_MX[5] - P[15][17]*SK_MX[4]); - Kfusion[16] = SK_MX[0]*(P[16][18] + P[16][1]*SH_MAG[0] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[3] - P[16][2]*SK_MX[2] - P[16][15]*SK_MX[1] + P[16][16]*SK_MX[5] - P[16][17]*SK_MX[4]); - Kfusion[17] = SK_MX[0]*(P[17][18] + P[17][1]*SH_MAG[0] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[3] - P[17][2]*SK_MX[2] - P[17][15]*SK_MX[1] + P[17][16]*SK_MX[5] - P[17][17]*SK_MX[4]); - Kfusion[18] = SK_MX[0]*(P[18][18] + P[18][1]*SH_MAG[0] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[3] - P[18][2]*SK_MX[2] - P[18][15]*SK_MX[1] + P[18][16]*SK_MX[5] - P[18][17]*SK_MX[4]); - Kfusion[19] = SK_MX[0]*(P[19][18] + P[19][1]*SH_MAG[0] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[3] - P[19][2]*SK_MX[2] - P[19][15]*SK_MX[1] + P[19][16]*SK_MX[5] - P[19][17]*SK_MX[4]); - Kfusion[20] = SK_MX[0]*(P[20][18] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][15]*SK_MX[1] + P[20][16]*SK_MX[5] - P[20][17]*SK_MX[4]); - varInnovMag[0] = 1.0f/SK_MX[0]; - innovMag[0] = MagPred[0] - magData.x; - - // reset the observation index to 0 (we start by fusing the X - // measurement) - obsIndex = 0; - } - else if (obsIndex == 1) // we are now fusing the Y measurement - { - // Calculate observation jacobians - for (unsigned int i=0; i 5Sigma - if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0) - { - // correct the state vector - for (uint8_t j= 0; j<=indexLimit; j++) - { - states[j] = states[j] - Kfusion[j] * innovMag[obsIndex]; - } - // normalise the quaternion states - float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); - if (quatMag > 1e-12) - { - for (uint8_t j= 0; j<=3; j++) - { - float quatMagInv = 1.0f/quatMag; - states[j] = states[j] * quatMagInv; - } - } - // correct the covariance P = (I - K*H)*P - // take advantage of the empty columns in KH to reduce the - // number of operations - for (uint8_t i = 0; i<=indexLimit; i++) - { - for (uint8_t j = 0; j<=3; j++) - { - KH[i][j] = Kfusion[i] * H_MAG[j]; - } - for (uint8_t j = 4; j<=17; j++) KH[i][j] = 0.0f; - if (!onGround) - { - for (uint8_t j = 15; j<=20; j++) - { - KH[i][j] = Kfusion[i] * H_MAG[j]; - } - } - else - { - for (uint8_t j = 15; j<=20; j++) - { - KH[i][j] = 0.0f; - } - } - } - for (uint8_t i = 0; i<=indexLimit; i++) - { - for (uint8_t j = 0; j<=indexLimit; j++) - { - KHP[i][j] = 0.0f; - for (uint8_t k = 0; k<=3; k++) - { - KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; - } - if (!onGround) - { - for (uint8_t k = 15; k<=20; k++) - { - KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; - } - } - } - } - } - for (uint8_t i = 0; i<=indexLimit; i++) - { - for (uint8_t j = 0; j<=indexLimit; j++) - { - P[i][j] = P[i][j] - KHP[i][j]; - } - } - } - obsIndex = obsIndex + 1; - - ForceSymmetry(); - ConstrainVariances(); -} - -void AttPosEKF::FuseAirspeed() -{ - float vn; - float ve; - float vd; - float vwn; - float vwe; - const float R_TAS = 2.0f; - float SH_TAS[3]; - float Kfusion[21]; - float VtasPred; - - // Copy required states to local variable names - vn = statesAtVtasMeasTime[4]; - ve = statesAtVtasMeasTime[5]; - vd = statesAtVtasMeasTime[6]; - vwn = statesAtVtasMeasTime[13]; - vwe = statesAtVtasMeasTime[14]; - - // Need to check that it is flying before fusing airspeed data - // Calculate the predicted airspeed - VtasPred = sqrtf((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd); - // Perform fusion of True Airspeed measurement - if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f)) - { - // Calculate observation jacobians - SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); - SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f; - SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/2.0f; - - float H_TAS[21]; - for (uint8_t i=0; i<=20; i++) H_TAS[i] = 0.0f; - H_TAS[4] = SH_TAS[2]; - H_TAS[5] = SH_TAS[1]; - H_TAS[6] = vd*SH_TAS[0]; - H_TAS[13] = -SH_TAS[2]; - H_TAS[14] = -SH_TAS[1]; - - // Calculate Kalman gains - float SK_TAS = 1.0f/(R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[13][4]*SH_TAS[2] - P[14][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[13][5]*SH_TAS[2] - P[14][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][13]*SH_TAS[2] + P[5][13]*SH_TAS[1] - P[13][13]*SH_TAS[2] - P[14][13]*SH_TAS[1] + P[6][13]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][14]*SH_TAS[2] + P[5][14]*SH_TAS[1] - P[13][14]*SH_TAS[2] - P[14][14]*SH_TAS[1] + P[6][14]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[13][6]*SH_TAS[2] - P[14][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0])); - Kfusion[0] = SK_TAS*(P[0][4]*SH_TAS[2] - P[0][13]*SH_TAS[2] + P[0][5]*SH_TAS[1] - P[0][14]*SH_TAS[1] + P[0][6]*vd*SH_TAS[0]); - Kfusion[1] = SK_TAS*(P[1][4]*SH_TAS[2] - P[1][13]*SH_TAS[2] + P[1][5]*SH_TAS[1] - P[1][14]*SH_TAS[1] + P[1][6]*vd*SH_TAS[0]); - Kfusion[2] = SK_TAS*(P[2][4]*SH_TAS[2] - P[2][13]*SH_TAS[2] + P[2][5]*SH_TAS[1] - P[2][14]*SH_TAS[1] + P[2][6]*vd*SH_TAS[0]); - Kfusion[3] = SK_TAS*(P[3][4]*SH_TAS[2] - P[3][13]*SH_TAS[2] + P[3][5]*SH_TAS[1] - P[3][14]*SH_TAS[1] + P[3][6]*vd*SH_TAS[0]); - Kfusion[4] = SK_TAS*(P[4][4]*SH_TAS[2] - P[4][13]*SH_TAS[2] + P[4][5]*SH_TAS[1] - P[4][14]*SH_TAS[1] + P[4][6]*vd*SH_TAS[0]); - Kfusion[5] = SK_TAS*(P[5][4]*SH_TAS[2] - P[5][13]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[5][14]*SH_TAS[1] + P[5][6]*vd*SH_TAS[0]); - Kfusion[6] = SK_TAS*(P[6][4]*SH_TAS[2] - P[6][13]*SH_TAS[2] + P[6][5]*SH_TAS[1] - P[6][14]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]); - Kfusion[7] = SK_TAS*(P[7][4]*SH_TAS[2] - P[7][13]*SH_TAS[2] + P[7][5]*SH_TAS[1] - P[7][14]*SH_TAS[1] + P[7][6]*vd*SH_TAS[0]); - Kfusion[8] = SK_TAS*(P[8][4]*SH_TAS[2] - P[8][13]*SH_TAS[2] + P[8][5]*SH_TAS[1] - P[8][14]*SH_TAS[1] + P[8][6]*vd*SH_TAS[0]); - Kfusion[9] = SK_TAS*(P[9][4]*SH_TAS[2] - P[9][13]*SH_TAS[2] + P[9][5]*SH_TAS[1] - P[9][14]*SH_TAS[1] + P[9][6]*vd*SH_TAS[0]); - Kfusion[10] = SK_TAS*(P[10][4]*SH_TAS[2] - P[10][13]*SH_TAS[2] + P[10][5]*SH_TAS[1] - P[10][14]*SH_TAS[1] + P[10][6]*vd*SH_TAS[0]); - Kfusion[11] = SK_TAS*(P[11][4]*SH_TAS[2] - P[11][13]*SH_TAS[2] + P[11][5]*SH_TAS[1] - P[11][14]*SH_TAS[1] + P[11][6]*vd*SH_TAS[0]); - Kfusion[12] = SK_TAS*(P[12][4]*SH_TAS[2] - P[12][13]*SH_TAS[2] + P[12][5]*SH_TAS[1] - P[12][14]*SH_TAS[1] + P[12][6]*vd*SH_TAS[0]); - Kfusion[13] = SK_TAS*(P[13][4]*SH_TAS[2] - P[13][13]*SH_TAS[2] + P[13][5]*SH_TAS[1] - P[13][14]*SH_TAS[1] + P[13][6]*vd*SH_TAS[0]); - Kfusion[14] = SK_TAS*(P[14][4]*SH_TAS[2] - P[14][13]*SH_TAS[2] + P[14][5]*SH_TAS[1] - P[14][14]*SH_TAS[1] + P[14][6]*vd*SH_TAS[0]); - Kfusion[15] = SK_TAS*(P[15][4]*SH_TAS[2] - P[15][13]*SH_TAS[2] + P[15][5]*SH_TAS[1] - P[15][14]*SH_TAS[1] + P[15][6]*vd*SH_TAS[0]); - Kfusion[16] = SK_TAS*(P[16][4]*SH_TAS[2] - P[16][13]*SH_TAS[2] + P[16][5]*SH_TAS[1] - P[16][14]*SH_TAS[1] + P[16][6]*vd*SH_TAS[0]); - Kfusion[17] = SK_TAS*(P[17][4]*SH_TAS[2] - P[17][13]*SH_TAS[2] + P[17][5]*SH_TAS[1] - P[17][14]*SH_TAS[1] + P[17][6]*vd*SH_TAS[0]); - Kfusion[18] = SK_TAS*(P[18][4]*SH_TAS[2] - P[18][13]*SH_TAS[2] + P[18][5]*SH_TAS[1] - P[18][14]*SH_TAS[1] + P[18][6]*vd*SH_TAS[0]); - Kfusion[19] = SK_TAS*(P[19][4]*SH_TAS[2] - P[19][13]*SH_TAS[2] + P[19][5]*SH_TAS[1] - P[19][14]*SH_TAS[1] + P[19][6]*vd*SH_TAS[0]); - Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][13]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][14]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]); - varInnovVtas = 1.0f/SK_TAS; - - // Calculate the measurement innovation - innovVtas = VtasPred - VtasMeas; - // Check the innovation for consistency and don't fuse if > 5Sigma - if ((innovVtas*innovVtas*SK_TAS) < 25.0) - { - // correct the state vector - for (uint8_t j=0; j<=20; j++) - { - states[j] = states[j] - Kfusion[j] * innovVtas; - } - // normalise the quaternion states - float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); - if (quatMag > 1e-12f) - { - for (uint8_t j= 0; j<=3; j++) - { - float quatMagInv = 1.0f/quatMag; - states[j] = states[j] * quatMagInv; - } - } - // correct the covariance P = (I - K*H)*P - // take advantage of the empty columns in H to reduce the - // number of operations - for (uint8_t i = 0; i<=20; i++) - { - for (uint8_t j = 0; j<=3; j++) KH[i][j] = 0.0; - for (uint8_t j = 4; j<=6; j++) - { - KH[i][j] = Kfusion[i] * H_TAS[j]; - } - for (uint8_t j = 7; j<=12; j++) KH[i][j] = 0.0; - for (uint8_t j = 13; j<=14; j++) - { - KH[i][j] = Kfusion[i] * H_TAS[j]; - } - for (uint8_t j = 15; j<=20; j++) KH[i][j] = 0.0; - } - for (uint8_t i = 0; i<=20; i++) - { - for (uint8_t j = 0; j<=20; j++) - { - KHP[i][j] = 0.0; - for (uint8_t k = 4; k<=6; k++) - { - KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; - } - for (uint8_t k = 13; k<=14; k++) - { - KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; - } - } - } - for (uint8_t i = 0; i<=20; i++) - { - for (uint8_t j = 0; j<=20; j++) - { - P[i][j] = P[i][j] - KHP[i][j]; - } - } - } - } - - ForceSymmetry(); - ConstrainVariances(); -} - -void AttPosEKF::zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) -{ - uint8_t row; - uint8_t col; - for (row=first; row<=last; row++) - { - for (col=0; col GPS_FIX_2D); - } -} - -void AttPosEKF::calcEarthRateNED(Vector3f &omega, float latitude) -{ - //Define Earth rotation vector in the NED navigation frame - omega.x = earthRate*cosf(latitude); - omega.y = 0.0f; - omega.z = -earthRate*sinf(latitude); -} - -void AttPosEKF::CovarianceInit() -{ - // Calculate the initial covariance matrix P - P[0][0] = 0.25f * sq(1.0f*deg2rad); - P[1][1] = 0.25f * sq(1.0f*deg2rad); - P[2][2] = 0.25f * sq(1.0f*deg2rad); - P[3][3] = 0.25f * sq(10.0f*deg2rad); - P[4][4] = sq(0.7); - P[5][5] = P[4][4]; - P[6][6] = sq(0.7); - P[7][7] = sq(15.0); - P[8][8] = P[7][7]; - P[9][9] = sq(5.0); - P[10][10] = sq(0.1*deg2rad*dtIMU); - P[11][11] = P[10][10]; - P[12][12] = P[10][10]; - P[13][13] = sq(8.0f); - P[14][4] = P[13][13]; - P[15][15] = sq(0.02f); - P[16][16] = P[15][15]; - P[17][17] = P[15][15]; - P[18][18] = sq(0.02f); - P[19][19] = P[18][18]; - P[20][20] = P[18][18]; -} - -float AttPosEKF::ConstrainFloat(float val, float min, float max) -{ - return (val > max) ? max : ((val < min) ? min : val); -} - -void AttPosEKF::ConstrainVariances() -{ - if (!numericalProtection) { - return; - } - - // State vector: - // 0-3: quaternions (q0, q1, q2, q3) - // 4-6: Velocity - m/sec (North, East, Down) - // 7-9: Position - m (North, East, Down) - // 10-12: Delta Angle bias - rad (X,Y,Z) - // 13-14: Wind Vector - m/sec (North,East) - // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down) - // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z) - - // Constrain quaternion variances - for (unsigned i = 0; i < 4; i++) { - P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); - } - - // Constrain velocitie variances - for (unsigned i = 4; i < 7; i++) { - P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f); - } - - // Constrain position variances - for (unsigned i = 7; i < 10; i++) { - P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e6f); - } - - // Angle bias variances - for (unsigned i = 10; i < 13; i++) { - P[i][i] = ConstrainFloat(P[i][i], 0.0f, sq(0.175f * dtIMU)); - } - - // Wind velocity variances - for (unsigned i = 13; i < 15; i++) { - P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f); - } - - // Earth magnetic field variances - for (unsigned i = 15; i < 18; i++) { - P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); - } - - // Body magnetic field variances - for (unsigned i = 18; i < 21; i++) { - P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); - } - -} - -void AttPosEKF::ConstrainStates() -{ - if (!numericalProtection) { - return; - } - - // State vector: - // 0-3: quaternions (q0, q1, q2, q3) - // 4-6: Velocity - m/sec (North, East, Down) - // 7-9: Position - m (North, East, Down) - // 10-12: Delta Angle bias - rad (X,Y,Z) - // 13-14: Wind Vector - m/sec (North,East) - // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down) - // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z) - - - // Constrain quaternion - for (unsigned i = 0; i < 4; i++) { - states[i] = ConstrainFloat(states[i], -1.0f, 1.0f); - } - - // Constrain velocities to what GPS can do for us - for (unsigned i = 4; i < 7; i++) { - states[i] = ConstrainFloat(states[i], -5.0e2f, 5.0e2f); - } - - // Constrain position to a reasonable vehicle range (in meters) - for (unsigned i = 7; i < 9; i++) { - states[i] = ConstrainFloat(states[i], -1.0e6f, 1.0e6f); - } - - // Constrain altitude - states[9] = ConstrainFloat(states[9], -4.0e4f, 1.0e4f); - - // Angle bias limit - set to 8 degrees / sec - for (unsigned i = 10; i < 13; i++) { - states[i] = ConstrainFloat(states[i], -0.12f * dtIMU, 0.12f * dtIMU); - } - - // Wind velocity limits - assume 120 m/s max velocity - for (unsigned i = 13; i < 15; i++) { - states[i] = ConstrainFloat(states[i], -120.0f, 120.0f); - } - - // Earth magnetic field limits (in Gauss) - for (unsigned i = 15; i < 18; i++) { - states[i] = ConstrainFloat(states[i], -1.0f, 1.0f); - } - - // Body magnetic field variances (in Gauss). - // the max offset should be in this range. - for (unsigned i = 18; i < 21; i++) { - states[i] = ConstrainFloat(states[i], -0.5f, 0.5f); - } - -} - -void AttPosEKF::ForceSymmetry() -{ - if (!numericalProtection) { - return; - } - - // Force symmetry on the covariance matrix to prevent ill-conditioning - // of the matrix which would cause the filter to blow-up - for (unsigned i = 1; i < n_states; i++) - { - for (uint8_t j = 0; j < i; j++) - { - P[i][j] = 0.5f * (P[i][j] + P[j][i]); - P[j][i] = P[i][j]; - } - } -} - -bool AttPosEKF::FilterHealthy() -{ - if (!statesInitialised) { - return false; - } - - // XXX Check state vector for NaNs and ill-conditioning - - // Check if any of the major inputs timed out - if (current_ekf_state.posTimeout || current_ekf_state.velTimeout || current_ekf_state.hgtTimeout) { - return false; - } - - // Nothing fired, return ok. - return true; -} - -/** - * Reset the filter position states - * - * This resets the position to the last GPS measurement - * or to zero in case of static position. - */ -void AttPosEKF::ResetPosition(void) -{ - if (staticMode) { - states[7] = 0; - states[8] = 0; - } else if (GPSstatus >= GPS_FIX_3D) { - - // reset the states from the GPS measurements - states[7] = posNE[0]; - states[8] = posNE[1]; - } -} - -/** - * Reset the height state. - * - * This resets the height state with the last altitude measurements - */ -void AttPosEKF::ResetHeight(void) -{ - // write to the state vector - states[9] = -hgtMea; -} - -/** - * Reset the velocity state. - */ -void AttPosEKF::ResetVelocity(void) -{ - if (staticMode) { - states[4] = 0.0f; - states[5] = 0.0f; - states[6] = 0.0f; - } else if (GPSstatus >= GPS_FIX_3D) { - - states[4] = velNED[0]; // north velocity from last reading - states[5] = velNED[1]; // east velocity from last reading - states[6] = velNED[2]; // down velocity from last reading - } -} - - -void AttPosEKF::FillErrorReport(struct ekf_status_report *err) -{ - for (int i = 0; i < n_states; i++) - { - err->states[i] = states[i]; - } - - err->velHealth = current_ekf_state.velHealth; - err->posHealth = current_ekf_state.posHealth; - err->hgtHealth = current_ekf_state.hgtHealth; - err->velTimeout = current_ekf_state.velTimeout; - err->posTimeout = current_ekf_state.posTimeout; - err->hgtTimeout = current_ekf_state.hgtTimeout; -} - -bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { - bool err = false; - - // check all states and covariance matrices - for (unsigned i = 0; i < n_states; i++) { - for (unsigned j = 0; j < n_states; j++) { - if (!isfinite(KH[i][j])) { - - err_report->covarianceNaN = true; - err = true; - } // intermediate result used for covariance updates - if (!isfinite(KHP[i][j])) { - - err_report->covarianceNaN = true; - err = true; - } // intermediate result used for covariance updates - if (!isfinite(P[i][j])) { - - err_report->covarianceNaN = true; - err = true; - } // covariance matrix - } - - if (!isfinite(Kfusion[i])) { - - err_report->kalmanGainsNaN = true; - err = true; - } // Kalman gains - - if (!isfinite(states[i])) { - - err_report->statesNaN = true; - err = true; - } // state matrix - } - - if (err) { - FillErrorReport(err_report); - } - - return err; - -} - -/** - * Check the filter inputs and bound its operational state - * - * This check will reset the filter states if required - * due to a failure of consistency or timeout checks. - * it should be run after the measurement data has been - * updated, but before any of the fusion steps are - * executed. - */ -int AttPosEKF::CheckAndBound() -{ - - // Store the old filter state - bool currStaticMode = staticMode; - - // Reset the filter if the states went NaN - if (StatesNaN(&last_ekf_error)) { - - InitializeDynamic(velNED, magDeclination); - - return 1; - } - - // Reset the filter if the IMU data is too old - if (dtIMU > 0.2f) { - - ResetVelocity(); - ResetPosition(); - ResetHeight(); - ResetStoredStates(); - - // that's all we can do here, return - return 2; - } - - // Check if we're on ground - this also sets static mode. - OnGroundCheck(); - - // Check if we switched between states - if (currStaticMode != staticMode) { - ResetVelocity(); - ResetPosition(); - ResetHeight(); - ResetStoredStates(); - - return 3; - } - - return 0; -} - -void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat) -{ - float initialRoll, initialPitch; - float cosRoll, sinRoll, cosPitch, sinPitch; - float magX, magY; - float initialHdg, cosHeading, sinHeading; - - initialRoll = atan2(-ay, -az); - initialPitch = atan2(ax, -az); - - cosRoll = cosf(initialRoll); - sinRoll = sinf(initialRoll); - cosPitch = cosf(initialPitch); - sinPitch = sinf(initialPitch); - - magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch; - - magY = my * cosRoll - mz * sinRoll; - - initialHdg = atan2f(-magY, magX); - /* true heading is the mag heading minus declination */ - initialHdg += declination; - - cosRoll = cosf(initialRoll * 0.5f); - sinRoll = sinf(initialRoll * 0.5f); - - cosPitch = cosf(initialPitch * 0.5f); - sinPitch = sinf(initialPitch * 0.5f); - - cosHeading = cosf(initialHdg * 0.5f); - sinHeading = sinf(initialHdg * 0.5f); - - initQuat[0] = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading; - initQuat[1] = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading; - initQuat[2] = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading; - initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; -} - -void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) -{ - - // Clear the init flag - statesInitialised = false; - - magDeclination = declination; - - ZeroVariables(); - - // Calculate initial filter quaternion states from raw measurements - float initQuat[4]; - Vector3f initMagXYZ; - initMagXYZ = magData - magBias; - AttitudeInit(accel.x, accel.y, accel.z, initMagXYZ.x, initMagXYZ.y, initMagXYZ.z, magDeclination, initQuat); - - // Calculate initial Tbn matrix and rotate Mag measurements into NED - // to set initial NED magnetic field states - Mat3f DCM; - quat2Tbn(DCM, initQuat); - Vector3f initMagNED; - initMagXYZ = magData - magBias; - initMagNED.x = DCM.x.x*initMagXYZ.x + DCM.x.y*initMagXYZ.y + DCM.x.z*initMagXYZ.z; - initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z; - initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z; - - - - // write to state vector - for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions - for (uint8_t j=0; j<=2; j++) states[j+4] = initvelNED[j]; // velocities - for (uint8_t j=0; j<=7; j++) states[j+7] = 0.0f; // positiions, dAngBias, windVel - states[15] = initMagNED.x; // Magnetic Field North - states[16] = initMagNED.y; // Magnetic Field East - states[17] = initMagNED.z; // Magnetic Field Down - states[18] = magBias.x; // Magnetic Field Bias X - states[19] = magBias.y; // Magnetic Field Bias Y - states[20] = magBias.z; // Magnetic Field Bias Z - - statesInitialised = true; - - // initialise the covariance matrix - CovarianceInit(); - - //Define Earth rotation vector in the NED navigation frame - calcEarthRateNED(earthRateNED, latRef); - - //Initialise summed variables used by covariance prediction - summedDelAng.x = 0.0f; - summedDelAng.y = 0.0f; - summedDelAng.z = 0.0f; - summedDelVel.x = 0.0f; - summedDelVel.y = 0.0f; - summedDelVel.z = 0.0f; -} - -void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination) -{ - //store initial lat,long and height - latRef = referenceLat; - lonRef = referenceLon; - hgtRef = referenceHgt; - - memset(&last_ekf_error, 0, sizeof(last_ekf_error)); - - InitializeDynamic(initvelNED, declination); -} - -void AttPosEKF::ZeroVariables() -{ - // Do the data structure init - for (unsigned i = 0; i < n_states; i++) { - for (unsigned j = 0; j < n_states; j++) { - KH[i][j] = 0.0f; // intermediate result used for covariance updates - KHP[i][j] = 0.0f; // intermediate result used for covariance updates - P[i][j] = 0.0f; // covariance matrix - } - - Kfusion[i] = 0.0f; // Kalman gains - states[i] = 0.0f; // state matrix - } - - for (unsigned i = 0; i < data_buffer_size; i++) { - - for (unsigned j = 0; j < n_states; j++) { - storedStates[j][i] = 0.0f; - } - - statetimeStamp[i] = 0; - } - - memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); -} - -void AttPosEKF::GetFilterState(struct ekf_status_report *state) -{ - memcpy(state, ¤t_ekf_state, sizeof(state)); -} - -void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error) -{ - memcpy(last_error, &last_ekf_error, sizeof(last_error)); -} diff --git a/src/modules/ekf_att_pos_estimator/estimator_21states.h b/src/modules/ekf_att_pos_estimator/estimator_21states.h deleted file mode 100644 index a19ff19952..0000000000 --- a/src/modules/ekf_att_pos_estimator/estimator_21states.h +++ /dev/null @@ -1,247 +0,0 @@ -#pragma once - -#include "estimator_utilities.h" - -class AttPosEKF { - -public: - - AttPosEKF(); - ~AttPosEKF(); - - /* ############################################## - * - * M A I N F I L T E R P A R A M E T E R S - * - * ########################################### */ - - /* - * parameters are defined here and initialised in - * the InitialiseParameters() (which is just 20 lines down) - */ - - float covTimeStepMax; // maximum time allowed between covariance predictions - float covDelAngMax; // maximum delta angle between covariance predictions - float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis. - - float yawVarScale; - float windVelSigma; - float dAngBiasSigma; - float dVelBiasSigma; - float magEarthSigma; - float magBodySigma; - float gndHgtSigma; - - float vneSigma; - float vdSigma; - float posNeSigma; - float posDSigma; - float magMeasurementSigma; - float airspeedMeasurementSigma; - - float gyroProcessNoise; - float accelProcessNoise; - - float EAS2TAS; // ratio f true to equivalent airspeed - - void InitialiseParameters() - { - covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions - covDelAngMax = 0.02f; // maximum delta angle between covariance predictions - rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis. - EAS2TAS = 1.0f; - - yawVarScale = 1.0f; - windVelSigma = 0.1f; - dAngBiasSigma = 5.0e-7f; - dVelBiasSigma = 1e-4f; - magEarthSigma = 3.0e-4f; - magBodySigma = 3.0e-4f; - gndHgtSigma = 0.02f; // assume 2% terrain gradient 1-sigma - - vneSigma = 0.2f; - vdSigma = 0.3f; - posNeSigma = 2.0f; - posDSigma = 2.0f; - - magMeasurementSigma = 0.05; - airspeedMeasurementSigma = 1.4f; - gyroProcessNoise = 1.4544411e-2f; - accelProcessNoise = 0.5f; - } - - // Global variables - float KH[n_states][n_states]; // intermediate result used for covariance updates - float KHP[n_states][n_states]; // intermediate result used for covariance updates - float P[n_states][n_states]; // covariance matrix - float Kfusion[n_states]; // Kalman gains - float states[n_states]; // state matrix - float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps - uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored - - float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements - float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements - float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement - float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time - float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time - - Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) - Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) - Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad) - Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s) - float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2) - Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) - Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s) - Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2) - Vector3f dVelIMU; - Vector3f dAngIMU; - float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec) - uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity - float innovVelPos[6]; // innovation output - float varInnovVelPos[6]; // innovation variance output - - float velNED[3]; // North, East, Down velocity obs (m/s) - float posNE[2]; // North, East position obs (m) - float hgtMea; // measured height (m) - float posNED[3]; // North, East Down position (m) - - float innovMag[3]; // innovation output - float varInnovMag[3]; // innovation variance output - Vector3f magData; // magnetometer flux radings in X,Y,Z body axes - float innovVtas; // innovation output - float varInnovVtas; // innovation variance output - float VtasMeas; // true airspeed measurement (m/s) - float magDeclination; - float latRef; // WGS-84 latitude of reference point (rad) - float lonRef; // WGS-84 longitude of reference point (rad) - float hgtRef; // WGS-84 height of reference point (m) - Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes - uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction - - // GPS input data variables - float gpsCourse; - float gpsVelD; - float gpsLat; - float gpsLon; - float gpsHgt; - uint8_t GPSstatus; - - // Baro input - float baroHgt; - - bool statesInitialised; - - bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused - bool fusePosData; // this boolean causes the posNE and velNED obs to be fused - bool fuseHgtData; // this boolean causes the hgtMea obs to be fused - bool fuseMagData; // boolean true when magnetometer data is to be fused - bool fuseVtasData; // boolean true when airspeed data is to be fused - - bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying) - bool staticMode; ///< boolean true if no position feedback is fused - bool useAirspeed; ///< boolean true if airspeed data is being used - bool useCompass; ///< boolean true if magnetometer data is being used - - struct ekf_status_report current_ekf_state; - struct ekf_status_report last_ekf_error; - - bool numericalProtection; - - unsigned storeIndex; - - -void UpdateStrapdownEquationsNED(); - -void CovariancePrediction(float dt); - -void FuseVelposNED(); - -void FuseMagnetometer(); - -void FuseAirspeed(); - -void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); - -void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); - -void quatNorm(float (&quatOut)[4], const float quatIn[4]); - -// store staes along with system time stamp in msces -void StoreStates(uint64_t timestamp_ms); - -/** - * Recall the state vector. - * - * Recalls the vector stored at closest time to the one specified by msec - * - * @return zero on success, integer indicating the number of invalid states on failure. - * Does only copy valid states, if the statesForFusion vector was initialized - * correctly by the caller, the result can be safely used, but is a mixture - * time-wise where valid states were updated and invalid remained at the old - * value. - */ -int RecallStates(float statesForFusion[n_states], uint64_t msec); - -void ResetStoredStates(); - -void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]); - -void calcEarthRateNED(Vector3f &omega, float latitude); - -static void eul2quat(float (&quat)[4], const float (&eul)[3]); - -static void quat2eul(float (&eul)[3], const float (&quat)[4]); - -static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); - -static void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); - -static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); - -static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]); - -static float sq(float valIn); - -void OnGroundCheck(); - -void CovarianceInit(); - -void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination); - -float ConstrainFloat(float val, float min, float max); - -void ConstrainVariances(); - -void ConstrainStates(); - -void ForceSymmetry(); - -int CheckAndBound(); - -void ResetPosition(); - -void ResetVelocity(); - -void ZeroVariables(); - -void GetFilterState(struct ekf_status_report *state); - -void GetLastErrorState(struct ekf_status_report *last_error); - -bool StatesNaN(struct ekf_status_report *err_report); -void FillErrorReport(struct ekf_status_report *err); - -void InitializeDynamic(float (&initvelNED)[3], float declination); - -protected: - -bool FilterHealthy(); - -void ResetHeight(void); - -void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat); - -}; - -uint32_t millis(); - diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp similarity index 79% rename from src/modules/ekf_att_pos_estimator/estimator_23states.cpp rename to src/modules/ekf_att_pos_estimator/estimator_22states.cpp index c17e034add..b062097fb3 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -1,4 +1,4 @@ -#include "estimator_23states.h" +#include "estimator_22states.h" #include #include #include @@ -14,9 +14,6 @@ AttPosEKF::AttPosEKF() : covTimeStepMax(0.0f), covDelAngMax(0.0f), rngFinderPitch(0.0f), - a1(0.0f), - a2(0.0f), - a3(0.0f), yawVarScale(0.0f), windVelSigma(0.0f), dAngBiasSigma(0.0f), @@ -71,10 +68,6 @@ AttPosEKF::AttPosEKF() : dtVelPosFilt(0.01f), dtHgtFilt(0.01f), dtGpsFilt(0.1f), - windSpdFiltNorth(0.0f), - windSpdFiltEast(0.0f), - windSpdFiltAltitude(0.0f), - windSpdFiltClimb(0.0f), fusionModeGPS(0), innovVelPos{}, varInnovVelPos{}, @@ -87,7 +80,6 @@ AttPosEKF::AttPosEKF() : innovMag{}, varInnovMag{}, magData{}, - losData{}, innovVtas(0.0f), innovRng(0.0f), innovOptFlow{}, @@ -102,6 +94,8 @@ AttPosEKF::AttPosEKF() : refSet(false), magBias(), covSkipCount(0), + lastFixTime_ms(0), + globalTimeStamp_ms(0), gpsLat(0.0), gpsLon(-M_PI), gpsHgt(0.0f), @@ -123,6 +117,7 @@ AttPosEKF::AttPosEKF() : onGround(true), staticMode(true), + useGPS(false), useAirspeed(true), useCompass(true), useRangeFinder(true), @@ -133,7 +128,7 @@ AttPosEKF::AttPosEKF() : current_ekf_state{}, last_ekf_error{}, numericalProtection(true), - storeIndex(0), + storeIndex(0), storedOmega{}, Popt{}, flowStates{}, @@ -152,6 +147,7 @@ AttPosEKF::AttPosEKF() : minFlowRng(0.0f), moCompR_LOS(0.0f) { + memset(&last_ekf_error, 0, sizeof(last_ekf_error)); memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); ZeroVariables(); @@ -348,11 +344,6 @@ void AttPosEKF::CovariancePrediction(float dt) } else { for (uint8_t i=16; i<=21; i++) processNoise[i] = 0; } - if (!inhibitGndState) { - processNoise[22] = dt * sqrtf(sq(states[4]) + sq(states[5])) * gndHgtSigma; - } else { - processNoise[22] = 0; - } // square all sigmas for (unsigned i = 0; i < n_states; i++) processNoise[i] = sq(processNoise[i]); @@ -451,7 +442,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[0][19] = P[0][19] + P[1][19]*SF[7] + P[2][19]*SF[9] + P[3][19]*SF[8] + P[10][19]*SF[11] + P[11][19]*SPP[7] + P[12][19]*SPP[6]; nextP[0][20] = P[0][20] + P[1][20]*SF[7] + P[2][20]*SF[9] + P[3][20]*SF[8] + P[10][20]*SF[11] + P[11][20]*SPP[7] + P[12][20]*SPP[6]; nextP[0][21] = P[0][21] + P[1][21]*SF[7] + P[2][21]*SF[9] + P[3][21]*SF[8] + P[10][21]*SF[11] + P[11][21]*SPP[7] + P[12][21]*SPP[6]; - nextP[0][22] = P[0][22] + P[1][22]*SF[7] + P[2][22]*SF[9] + P[3][22]*SF[8] + P[10][22]*SF[11] + P[11][22]*SPP[7] + P[12][22]*SPP[6]; nextP[1][0] = P[1][0] + SQ[8] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2 + SF[7]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[9]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) + SF[8]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SF[11]*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2) + SPP[7]*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2) + SPP[6]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2); nextP[1][1] = P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] + daxCov*SQ[9] - (P[10][1]*q0)/2 + SF[6]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[5]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) + SF[9]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SPP[6]*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2) - SPP[7]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2) + (dayCov*sq(q3))/4 + (dazCov*sq(q2))/4 - (q0*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2))/2; nextP[1][2] = P[1][2] + SQ[5] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2 + SF[4]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[8]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[6]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SF[11]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2) - SPP[6]*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2) - (q0*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2))/2; @@ -474,7 +464,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[1][19] = P[1][19] + P[0][19]*SF[6] + P[2][19]*SF[5] + P[3][19]*SF[9] + P[11][19]*SPP[6] - P[12][19]*SPP[7] - (P[10][19]*q0)/2; nextP[1][20] = P[1][20] + P[0][20]*SF[6] + P[2][20]*SF[5] + P[3][20]*SF[9] + P[11][20]*SPP[6] - P[12][20]*SPP[7] - (P[10][20]*q0)/2; nextP[1][21] = P[1][21] + P[0][21]*SF[6] + P[2][21]*SF[5] + P[3][21]*SF[9] + P[11][21]*SPP[6] - P[12][21]*SPP[7] - (P[10][21]*q0)/2; - nextP[1][22] = P[1][22] + P[0][22]*SF[6] + P[2][22]*SF[5] + P[3][22]*SF[9] + P[11][22]*SPP[6] - P[12][22]*SPP[7] - (P[10][22]*q0)/2; nextP[2][0] = P[2][0] + SQ[7] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2 + SF[7]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[9]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) + SF[8]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SF[11]*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2) + SPP[7]*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2) + SPP[6]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2); nextP[2][1] = P[2][1] + SQ[5] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2 + SF[6]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[5]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) + SF[9]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SPP[6]*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2) - SPP[7]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2) - (q0*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2))/2; nextP[2][2] = P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] + dayCov*SQ[9] + (dazCov*SQ[10])/4 - (P[11][2]*q0)/2 + SF[4]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[8]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[6]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SF[11]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2) - SPP[6]*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2) + (daxCov*sq(q3))/4 - (q0*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2))/2; @@ -497,7 +486,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[2][19] = P[2][19] + P[0][19]*SF[4] + P[1][19]*SF[8] + P[3][19]*SF[6] + P[12][19]*SF[11] - P[10][19]*SPP[6] - (P[11][19]*q0)/2; nextP[2][20] = P[2][20] + P[0][20]*SF[4] + P[1][20]*SF[8] + P[3][20]*SF[6] + P[12][20]*SF[11] - P[10][20]*SPP[6] - (P[11][20]*q0)/2; nextP[2][21] = P[2][21] + P[0][21]*SF[4] + P[1][21]*SF[8] + P[3][21]*SF[6] + P[12][21]*SF[11] - P[10][21]*SPP[6] - (P[11][21]*q0)/2; - nextP[2][22] = P[2][22] + P[0][22]*SF[4] + P[1][22]*SF[8] + P[3][22]*SF[6] + P[12][22]*SF[11] - P[10][22]*SPP[6] - (P[11][22]*q0)/2; nextP[3][0] = P[3][0] + SQ[6] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2 + SF[7]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[9]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) + SF[8]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SF[11]*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2) + SPP[7]*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2) + SPP[6]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2); nextP[3][1] = P[3][1] + SQ[4] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2 + SF[6]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[5]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) + SF[9]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SPP[6]*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2) - SPP[7]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2) - (q0*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2))/2; nextP[3][2] = P[3][2] + SQ[3] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2 + SF[4]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[8]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[6]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SF[11]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2) - SPP[6]*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2) - (q0*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2))/2; @@ -520,7 +508,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[3][19] = P[3][19] + P[0][19]*SF[5] + P[1][19]*SF[4] + P[2][19]*SF[7] - P[11][19]*SF[11] + P[10][19]*SPP[7] - (P[12][19]*q0)/2; nextP[3][20] = P[3][20] + P[0][20]*SF[5] + P[1][20]*SF[4] + P[2][20]*SF[7] - P[11][20]*SF[11] + P[10][20]*SPP[7] - (P[12][20]*q0)/2; nextP[3][21] = P[3][21] + P[0][21]*SF[5] + P[1][21]*SF[4] + P[2][21]*SF[7] - P[11][21]*SF[11] + P[10][21]*SPP[7] - (P[12][21]*q0)/2; - nextP[3][22] = P[3][22] + P[0][22]*SF[5] + P[1][22]*SF[4] + P[2][22]*SF[7] - P[11][22]*SF[11] + P[10][22]*SPP[7] - (P[12][22]*q0)/2; nextP[4][0] = P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4] + SF[7]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[9]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) + SF[8]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SF[11]*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]) + SPP[7]*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]) + SPP[6]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]); nextP[4][1] = P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4] + SF[6]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[5]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) + SF[9]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SPP[6]*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]) - SPP[7]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]) - (q0*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]))/2; nextP[4][2] = P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4] + SF[4]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[8]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[6]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SF[11]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]) - SPP[6]*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]) - (q0*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]))/2; @@ -543,7 +530,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[4][19] = P[4][19] + P[0][19]*SF[3] + P[1][19]*SF[1] + P[2][19]*SPP[0] - P[3][19]*SPP[2] - P[13][19]*SPP[4]; nextP[4][20] = P[4][20] + P[0][20]*SF[3] + P[1][20]*SF[1] + P[2][20]*SPP[0] - P[3][20]*SPP[2] - P[13][20]*SPP[4]; nextP[4][21] = P[4][21] + P[0][21]*SF[3] + P[1][21]*SF[1] + P[2][21]*SPP[0] - P[3][21]*SPP[2] - P[13][21]*SPP[4]; - nextP[4][22] = P[4][22] + P[0][22]*SF[3] + P[1][22]*SF[1] + P[2][22]*SPP[0] - P[3][22]*SPP[2] - P[13][22]*SPP[4]; nextP[5][0] = P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3] + SF[7]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[9]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) + SF[8]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SF[11]*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]) + SPP[7]*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]) + SPP[6]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]); nextP[5][1] = P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3] + SF[6]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[5]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) + SF[9]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SPP[6]*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]) - SPP[7]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]) - (q0*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]))/2; nextP[5][2] = P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3] + SF[4]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[8]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[6]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SF[11]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]) - SPP[6]*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]) - (q0*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]))/2; @@ -566,7 +552,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[5][19] = P[5][19] + P[0][19]*SF[2] + P[2][19]*SF[1] + P[3][19]*SF[3] - P[1][19]*SPP[0] + P[13][19]*SPP[3]; nextP[5][20] = P[5][20] + P[0][20]*SF[2] + P[2][20]*SF[1] + P[3][20]*SF[3] - P[1][20]*SPP[0] + P[13][20]*SPP[3]; nextP[5][21] = P[5][21] + P[0][21]*SF[2] + P[2][21]*SF[1] + P[3][21]*SF[3] - P[1][21]*SPP[0] + P[13][21]*SPP[3]; - nextP[5][22] = P[5][22] + P[0][22]*SF[2] + P[2][22]*SF[1] + P[3][22]*SF[3] - P[1][22]*SPP[0] + P[13][22]*SPP[3]; nextP[6][0] = P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[7]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[9]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[8]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[11]*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[7]*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[6]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))); nextP[6][1] = P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[6]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[5]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[9]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[6]*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[7]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - (q0*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))))/2; nextP[6][2] = P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[4]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[8]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[6]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[11]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[6]*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - (q0*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))))/2; @@ -589,7 +574,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[6][19] = P[6][19] + P[1][19]*SF[2] + P[3][19]*SF[1] + P[0][19]*SPP[0] - P[2][19]*SPP[1] - P[13][19]*SPP[5]; nextP[6][20] = P[6][20] + P[1][20]*SF[2] + P[3][20]*SF[1] + P[0][20]*SPP[0] - P[2][20]*SPP[1] - P[13][20]*SPP[5]; nextP[6][21] = P[6][21] + P[1][21]*SF[2] + P[3][21]*SF[1] + P[0][21]*SPP[0] - P[2][21]*SPP[1] - P[13][21]*SPP[5]; - nextP[6][22] = P[6][22] + P[1][22]*SF[2] + P[3][22]*SF[1] + P[0][22]*SPP[0] - P[2][22]*SPP[1] - P[13][22]*SPP[5]; nextP[7][0] = P[7][0] + P[4][0]*dt + SF[7]*(P[7][1] + P[4][1]*dt) + SF[9]*(P[7][2] + P[4][2]*dt) + SF[8]*(P[7][3] + P[4][3]*dt) + SF[11]*(P[7][10] + P[4][10]*dt) + SPP[7]*(P[7][11] + P[4][11]*dt) + SPP[6]*(P[7][12] + P[4][12]*dt); nextP[7][1] = P[7][1] + P[4][1]*dt + SF[6]*(P[7][0] + P[4][0]*dt) + SF[5]*(P[7][2] + P[4][2]*dt) + SF[9]*(P[7][3] + P[4][3]*dt) + SPP[6]*(P[7][11] + P[4][11]*dt) - SPP[7]*(P[7][12] + P[4][12]*dt) - (q0*(P[7][10] + P[4][10]*dt))/2; nextP[7][2] = P[7][2] + P[4][2]*dt + SF[4]*(P[7][0] + P[4][0]*dt) + SF[8]*(P[7][1] + P[4][1]*dt) + SF[6]*(P[7][3] + P[4][3]*dt) + SF[11]*(P[7][12] + P[4][12]*dt) - SPP[6]*(P[7][10] + P[4][10]*dt) - (q0*(P[7][11] + P[4][11]*dt))/2; @@ -612,7 +596,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[7][19] = P[7][19] + P[4][19]*dt; nextP[7][20] = P[7][20] + P[4][20]*dt; nextP[7][21] = P[7][21] + P[4][21]*dt; - nextP[7][22] = P[7][22] + P[4][22]*dt; nextP[8][0] = P[8][0] + P[5][0]*dt + SF[7]*(P[8][1] + P[5][1]*dt) + SF[9]*(P[8][2] + P[5][2]*dt) + SF[8]*(P[8][3] + P[5][3]*dt) + SF[11]*(P[8][10] + P[5][10]*dt) + SPP[7]*(P[8][11] + P[5][11]*dt) + SPP[6]*(P[8][12] + P[5][12]*dt); nextP[8][1] = P[8][1] + P[5][1]*dt + SF[6]*(P[8][0] + P[5][0]*dt) + SF[5]*(P[8][2] + P[5][2]*dt) + SF[9]*(P[8][3] + P[5][3]*dt) + SPP[6]*(P[8][11] + P[5][11]*dt) - SPP[7]*(P[8][12] + P[5][12]*dt) - (q0*(P[8][10] + P[5][10]*dt))/2; nextP[8][2] = P[8][2] + P[5][2]*dt + SF[4]*(P[8][0] + P[5][0]*dt) + SF[8]*(P[8][1] + P[5][1]*dt) + SF[6]*(P[8][3] + P[5][3]*dt) + SF[11]*(P[8][12] + P[5][12]*dt) - SPP[6]*(P[8][10] + P[5][10]*dt) - (q0*(P[8][11] + P[5][11]*dt))/2; @@ -635,7 +618,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[8][19] = P[8][19] + P[5][19]*dt; nextP[8][20] = P[8][20] + P[5][20]*dt; nextP[8][21] = P[8][21] + P[5][21]*dt; - nextP[8][22] = P[8][22] + P[5][22]*dt; nextP[9][0] = P[9][0] + P[6][0]*dt + SF[7]*(P[9][1] + P[6][1]*dt) + SF[9]*(P[9][2] + P[6][2]*dt) + SF[8]*(P[9][3] + P[6][3]*dt) + SF[11]*(P[9][10] + P[6][10]*dt) + SPP[7]*(P[9][11] + P[6][11]*dt) + SPP[6]*(P[9][12] + P[6][12]*dt); nextP[9][1] = P[9][1] + P[6][1]*dt + SF[6]*(P[9][0] + P[6][0]*dt) + SF[5]*(P[9][2] + P[6][2]*dt) + SF[9]*(P[9][3] + P[6][3]*dt) + SPP[6]*(P[9][11] + P[6][11]*dt) - SPP[7]*(P[9][12] + P[6][12]*dt) - (q0*(P[9][10] + P[6][10]*dt))/2; nextP[9][2] = P[9][2] + P[6][2]*dt + SF[4]*(P[9][0] + P[6][0]*dt) + SF[8]*(P[9][1] + P[6][1]*dt) + SF[6]*(P[9][3] + P[6][3]*dt) + SF[11]*(P[9][12] + P[6][12]*dt) - SPP[6]*(P[9][10] + P[6][10]*dt) - (q0*(P[9][11] + P[6][11]*dt))/2; @@ -658,7 +640,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[9][19] = P[9][19] + P[6][19]*dt; nextP[9][20] = P[9][20] + P[6][20]*dt; nextP[9][21] = P[9][21] + P[6][21]*dt; - nextP[9][22] = P[9][22] + P[6][22]*dt; nextP[10][0] = P[10][0] + P[10][1]*SF[7] + P[10][2]*SF[9] + P[10][3]*SF[8] + P[10][10]*SF[11] + P[10][11]*SPP[7] + P[10][12]*SPP[6]; nextP[10][1] = P[10][1] + P[10][0]*SF[6] + P[10][2]*SF[5] + P[10][3]*SF[9] + P[10][11]*SPP[6] - P[10][12]*SPP[7] - (P[10][10]*q0)/2; nextP[10][2] = P[10][2] + P[10][0]*SF[4] + P[10][1]*SF[8] + P[10][3]*SF[6] + P[10][12]*SF[11] - P[10][10]*SPP[6] - (P[10][11]*q0)/2; @@ -681,7 +662,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[10][19] = P[10][19]; nextP[10][20] = P[10][20]; nextP[10][21] = P[10][21]; - nextP[10][22] = P[10][22]; nextP[11][0] = P[11][0] + P[11][1]*SF[7] + P[11][2]*SF[9] + P[11][3]*SF[8] + P[11][10]*SF[11] + P[11][11]*SPP[7] + P[11][12]*SPP[6]; nextP[11][1] = P[11][1] + P[11][0]*SF[6] + P[11][2]*SF[5] + P[11][3]*SF[9] + P[11][11]*SPP[6] - P[11][12]*SPP[7] - (P[11][10]*q0)/2; nextP[11][2] = P[11][2] + P[11][0]*SF[4] + P[11][1]*SF[8] + P[11][3]*SF[6] + P[11][12]*SF[11] - P[11][10]*SPP[6] - (P[11][11]*q0)/2; @@ -704,7 +684,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[11][19] = P[11][19]; nextP[11][20] = P[11][20]; nextP[11][21] = P[11][21]; - nextP[11][22] = P[11][22]; nextP[12][0] = P[12][0] + P[12][1]*SF[7] + P[12][2]*SF[9] + P[12][3]*SF[8] + P[12][10]*SF[11] + P[12][11]*SPP[7] + P[12][12]*SPP[6]; nextP[12][1] = P[12][1] + P[12][0]*SF[6] + P[12][2]*SF[5] + P[12][3]*SF[9] + P[12][11]*SPP[6] - P[12][12]*SPP[7] - (P[12][10]*q0)/2; nextP[12][2] = P[12][2] + P[12][0]*SF[4] + P[12][1]*SF[8] + P[12][3]*SF[6] + P[12][12]*SF[11] - P[12][10]*SPP[6] - (P[12][11]*q0)/2; @@ -727,7 +706,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[12][19] = P[12][19]; nextP[12][20] = P[12][20]; nextP[12][21] = P[12][21]; - nextP[12][22] = P[12][22]; nextP[13][0] = P[13][0] + P[13][1]*SF[7] + P[13][2]*SF[9] + P[13][3]*SF[8] + P[13][10]*SF[11] + P[13][11]*SPP[7] + P[13][12]*SPP[6]; nextP[13][1] = P[13][1] + P[13][0]*SF[6] + P[13][2]*SF[5] + P[13][3]*SF[9] + P[13][11]*SPP[6] - P[13][12]*SPP[7] - (P[13][10]*q0)/2; nextP[13][2] = P[13][2] + P[13][0]*SF[4] + P[13][1]*SF[8] + P[13][3]*SF[6] + P[13][12]*SF[11] - P[13][10]*SPP[6] - (P[13][11]*q0)/2; @@ -750,7 +728,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[13][19] = P[13][19]; nextP[13][20] = P[13][20]; nextP[13][21] = P[13][21]; - nextP[13][22] = P[13][22]; nextP[14][0] = P[14][0] + P[14][1]*SF[7] + P[14][2]*SF[9] + P[14][3]*SF[8] + P[14][10]*SF[11] + P[14][11]*SPP[7] + P[14][12]*SPP[6]; nextP[14][1] = P[14][1] + P[14][0]*SF[6] + P[14][2]*SF[5] + P[14][3]*SF[9] + P[14][11]*SPP[6] - P[14][12]*SPP[7] - (P[14][10]*q0)/2; nextP[14][2] = P[14][2] + P[14][0]*SF[4] + P[14][1]*SF[8] + P[14][3]*SF[6] + P[14][12]*SF[11] - P[14][10]*SPP[6] - (P[14][11]*q0)/2; @@ -773,7 +750,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[14][19] = P[14][19]; nextP[14][20] = P[14][20]; nextP[14][21] = P[14][21]; - nextP[14][22] = P[14][22]; nextP[15][0] = P[15][0] + P[15][1]*SF[7] + P[15][2]*SF[9] + P[15][3]*SF[8] + P[15][10]*SF[11] + P[15][11]*SPP[7] + P[15][12]*SPP[6]; nextP[15][1] = P[15][1] + P[15][0]*SF[6] + P[15][2]*SF[5] + P[15][3]*SF[9] + P[15][11]*SPP[6] - P[15][12]*SPP[7] - (P[15][10]*q0)/2; nextP[15][2] = P[15][2] + P[15][0]*SF[4] + P[15][1]*SF[8] + P[15][3]*SF[6] + P[15][12]*SF[11] - P[15][10]*SPP[6] - (P[15][11]*q0)/2; @@ -796,7 +772,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[15][19] = P[15][19]; nextP[15][20] = P[15][20]; nextP[15][21] = P[15][21]; - nextP[15][22] = P[15][22]; nextP[16][0] = P[16][0] + P[16][1]*SF[7] + P[16][2]*SF[9] + P[16][3]*SF[8] + P[16][10]*SF[11] + P[16][11]*SPP[7] + P[16][12]*SPP[6]; nextP[16][1] = P[16][1] + P[16][0]*SF[6] + P[16][2]*SF[5] + P[16][3]*SF[9] + P[16][11]*SPP[6] - P[16][12]*SPP[7] - (P[16][10]*q0)/2; nextP[16][2] = P[16][2] + P[16][0]*SF[4] + P[16][1]*SF[8] + P[16][3]*SF[6] + P[16][12]*SF[11] - P[16][10]*SPP[6] - (P[16][11]*q0)/2; @@ -819,7 +794,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[16][19] = P[16][19]; nextP[16][20] = P[16][20]; nextP[16][21] = P[16][21]; - nextP[16][22] = P[16][22]; nextP[17][0] = P[17][0] + P[17][1]*SF[7] + P[17][2]*SF[9] + P[17][3]*SF[8] + P[17][10]*SF[11] + P[17][11]*SPP[7] + P[17][12]*SPP[6]; nextP[17][1] = P[17][1] + P[17][0]*SF[6] + P[17][2]*SF[5] + P[17][3]*SF[9] + P[17][11]*SPP[6] - P[17][12]*SPP[7] - (P[17][10]*q0)/2; nextP[17][2] = P[17][2] + P[17][0]*SF[4] + P[17][1]*SF[8] + P[17][3]*SF[6] + P[17][12]*SF[11] - P[17][10]*SPP[6] - (P[17][11]*q0)/2; @@ -842,7 +816,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[17][19] = P[17][19]; nextP[17][20] = P[17][20]; nextP[17][21] = P[17][21]; - nextP[17][22] = P[17][22]; nextP[18][0] = P[18][0] + P[18][1]*SF[7] + P[18][2]*SF[9] + P[18][3]*SF[8] + P[18][10]*SF[11] + P[18][11]*SPP[7] + P[18][12]*SPP[6]; nextP[18][1] = P[18][1] + P[18][0]*SF[6] + P[18][2]*SF[5] + P[18][3]*SF[9] + P[18][11]*SPP[6] - P[18][12]*SPP[7] - (P[18][10]*q0)/2; nextP[18][2] = P[18][2] + P[18][0]*SF[4] + P[18][1]*SF[8] + P[18][3]*SF[6] + P[18][12]*SF[11] - P[18][10]*SPP[6] - (P[18][11]*q0)/2; @@ -865,7 +838,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[18][19] = P[18][19]; nextP[18][20] = P[18][20]; nextP[18][21] = P[18][21]; - nextP[18][22] = P[18][22]; nextP[19][0] = P[19][0] + P[19][1]*SF[7] + P[19][2]*SF[9] + P[19][3]*SF[8] + P[19][10]*SF[11] + P[19][11]*SPP[7] + P[19][12]*SPP[6]; nextP[19][1] = P[19][1] + P[19][0]*SF[6] + P[19][2]*SF[5] + P[19][3]*SF[9] + P[19][11]*SPP[6] - P[19][12]*SPP[7] - (P[19][10]*q0)/2; nextP[19][2] = P[19][2] + P[19][0]*SF[4] + P[19][1]*SF[8] + P[19][3]*SF[6] + P[19][12]*SF[11] - P[19][10]*SPP[6] - (P[19][11]*q0)/2; @@ -888,7 +860,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[19][19] = P[19][19]; nextP[19][20] = P[19][20]; nextP[19][21] = P[19][21]; - nextP[19][22] = P[19][22]; nextP[20][0] = P[20][0] + P[20][1]*SF[7] + P[20][2]*SF[9] + P[20][3]*SF[8] + P[20][10]*SF[11] + P[20][11]*SPP[7] + P[20][12]*SPP[6]; nextP[20][1] = P[20][1] + P[20][0]*SF[6] + P[20][2]*SF[5] + P[20][3]*SF[9] + P[20][11]*SPP[6] - P[20][12]*SPP[7] - (P[20][10]*q0)/2; nextP[20][2] = P[20][2] + P[20][0]*SF[4] + P[20][1]*SF[8] + P[20][3]*SF[6] + P[20][12]*SF[11] - P[20][10]*SPP[6] - (P[20][11]*q0)/2; @@ -911,7 +882,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[20][19] = P[20][19]; nextP[20][20] = P[20][20]; nextP[20][21] = P[20][21]; - nextP[20][22] = P[20][22]; nextP[21][0] = P[21][0] + P[21][1]*SF[7] + P[21][2]*SF[9] + P[21][3]*SF[8] + P[21][10]*SF[11] + P[21][11]*SPP[7] + P[21][12]*SPP[6]; nextP[21][1] = P[21][1] + P[21][0]*SF[6] + P[21][2]*SF[5] + P[21][3]*SF[9] + P[21][11]*SPP[6] - P[21][12]*SPP[7] - (P[21][10]*q0)/2; nextP[21][2] = P[21][2] + P[21][0]*SF[4] + P[21][1]*SF[8] + P[21][3]*SF[6] + P[21][12]*SF[11] - P[21][10]*SPP[6] - (P[21][11]*q0)/2; @@ -934,30 +904,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[21][19] = P[21][19]; nextP[21][20] = P[21][20]; nextP[21][21] = P[21][21]; - nextP[21][22] = P[21][22]; - nextP[22][0] = P[22][0] + P[22][1]*SF[7] + P[22][2]*SF[9] + P[22][3]*SF[8] + P[22][10]*SF[11] + P[22][11]*SPP[7] + P[22][12]*SPP[6]; - nextP[22][1] = P[22][1] + P[22][0]*SF[6] + P[22][2]*SF[5] + P[22][3]*SF[9] + P[22][11]*SPP[6] - P[22][12]*SPP[7] - (P[22][10]*q0)/2; - nextP[22][2] = P[22][2] + P[22][0]*SF[4] + P[22][1]*SF[8] + P[22][3]*SF[6] + P[22][12]*SF[11] - P[22][10]*SPP[6] - (P[22][11]*q0)/2; - nextP[22][3] = P[22][3] + P[22][0]*SF[5] + P[22][1]*SF[4] + P[22][2]*SF[7] - P[22][11]*SF[11] + P[22][10]*SPP[7] - (P[22][12]*q0)/2; - nextP[22][4] = P[22][4] + P[22][1]*SF[1] + P[22][0]*SF[3] + P[22][2]*SPP[0] - P[22][3]*SPP[2] - P[22][13]*SPP[4]; - nextP[22][5] = P[22][5] + P[22][0]*SF[2] + P[22][2]*SF[1] + P[22][3]*SF[3] - P[22][1]*SPP[0] + P[22][13]*SPP[3]; - nextP[22][6] = P[22][6] + P[22][1]*SF[2] + P[22][3]*SF[1] + P[22][0]*SPP[0] - P[22][2]*SPP[1] - P[22][13]*SPP[5]; - nextP[22][7] = P[22][7] + P[22][4]*dt; - nextP[22][8] = P[22][8] + P[22][5]*dt; - nextP[22][9] = P[22][9] + P[22][6]*dt; - nextP[22][10] = P[22][10]; - nextP[22][11] = P[22][11]; - nextP[22][12] = P[22][12]; - nextP[22][13] = P[22][13]; - nextP[22][14] = P[22][14]; - nextP[22][15] = P[22][15]; - nextP[22][16] = P[22][16]; - nextP[22][17] = P[22][17]; - nextP[22][18] = P[22][18]; - nextP[22][19] = P[22][19]; - nextP[22][20] = P[22][20]; - nextP[22][21] = P[22][21]; - nextP[22][22] = P[22][22]; for (unsigned i = 0; i < n_states; i++) { @@ -1031,7 +977,7 @@ void AttPosEKF::FuseVelposNED() bool fuseData[6] = {false,false,false,false,false,false}; uint8_t stateIndex; uint8_t obsIndex; - uint8_t indexLimit = 22; + uint8_t indexLimit = 21; // declare variables used by state and covariance update calculations float velErr; @@ -1235,10 +1181,6 @@ void AttPosEKF::FuseVelposNED() Kfusion[i] = 0; } } - // Don't update terrain state if inhibited - if (inhibitGndState) { - Kfusion[22] = 0; - } // Calculate state corrections and re-normalise the quaternions for (uint8_t i = 0; i<=indexLimit; i++) @@ -1419,11 +1361,6 @@ void AttPosEKF::FuseMagnetometer() Kfusion[i] = 0; } } - if (!inhibitGndState) { - Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[3] - P[22][2]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[5] - P[22][18]*SK_MX[4]); - } else { - Kfusion[22] = 0; - } varInnovMag[0] = 1.0f/SK_MX[0]; innovMag[0] = MagPred[0] - magData.x; } @@ -1669,32 +1606,6 @@ void AttPosEKF::FuseAirspeed() // Perform fusion of True Airspeed measurement if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f)) { - - float altDiff = fabsf(windSpdFiltAltitude - hgtMea); - - if (isfinite(windSpdFiltClimb)) { - windSpdFiltClimb = ((1.0f - 0.0002f) * windSpdFiltClimb) + (0.0002f * states[6]); - } else { - windSpdFiltClimb = states[6]; - } - - if (altDiff < 20.0f) { - // Lowpass the output of the wind estimate - we want a long-term - // stable estimate, but not start to load into the overall dynamics - // of the system (which adjusting covariances would do) - - // Change filter coefficient based on altitude change rate - float windFiltCoeff = ConstrainFloat(fabsf(windSpdFiltClimb) / 1000.0f, 0.00005f, 0.2f); - - windSpdFiltNorth = ((1.0f - windFiltCoeff) * windSpdFiltNorth) + (windFiltCoeff * vwn); - windSpdFiltEast = ((1.0f - windFiltCoeff) * windSpdFiltEast) + (windFiltCoeff * vwe); - } else { - windSpdFiltNorth = vwn; - windSpdFiltEast = vwe; - } - - windSpdFiltAltitude = hgtMea; - // Calculate observation jacobians SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f; @@ -1835,356 +1746,269 @@ void AttPosEKF::zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uin } } -void AttPosEKF::FuseRangeFinder() -{ - - // Local variables - float rngPred; - float SH_RNG[5]; - float H_RNG[23]; - float SK_RNG[6]; - float cosRngTilt; - const float R_RNG = 0.25f; // 0.5 m2 rangefinder measurement variance - - // Copy required states to local variable names - float q0 = statesAtRngTime[0]; - float q1 = statesAtRngTime[1]; - float q2 = statesAtRngTime[2]; - float q3 = statesAtRngTime[3]; - float pd = statesAtRngTime[9]; - float ptd = statesAtRngTime[22]; - - // Need to check that our range finder tilt angle is less than 30 degrees and we are using range finder data - SH_RNG[4] = sinf(rngFinderPitch); - cosRngTilt = - Tbn.z.x * SH_RNG[4] + Tbn.z.z * cosf(rngFinderPitch); - if (useRangeFinder && fuseRngData && cosRngTilt > 0.87f) - { - // Calculate observation jacobian and Kalman gain ignoring all states other than the terrain offset - // This prevents the range finder measurement modifying any of the other filter states and significantly reduces computations - SH_RNG[0] = SH_RNG[4]*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3); - SH_RNG[1] = pd - ptd; - SH_RNG[2] = 1/sq(SH_RNG[0]); - SH_RNG[3] = 1/SH_RNG[0]; - for (uint8_t i = 0; i < n_states; i++) { - H_RNG[i] = 0.0f; - Kfusion[i] = 0.0f; - } - H_RNG[22] = -SH_RNG[3]; - SK_RNG[0] = 1/(R_RNG + SH_RNG[3]*(P[9][9]*SH_RNG[3] - P[22][9]*SH_RNG[3] + P[0][9]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][9]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][9]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][9]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) - SH_RNG[3]*(P[9][22]*SH_RNG[3] - P[22][22]*SH_RNG[3] + P[0][22]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][22]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][22]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][22]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) + SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4])*(P[9][0]*SH_RNG[3] - P[22][0]*SH_RNG[3] + P[0][0]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][0]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][0]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][0]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) - SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4])*(P[9][1]*SH_RNG[3] - P[22][1]*SH_RNG[3] + P[0][1]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][1]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][1]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][1]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) - SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4])*(P[9][2]*SH_RNG[3] - P[22][2]*SH_RNG[3] + P[0][2]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][2]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][2]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][2]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) + SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])*(P[9][3]*SH_RNG[3] - P[22][3]*SH_RNG[3] + P[0][3]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][3]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][3]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][3]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4]))); - SK_RNG[1] = 2*q1 - 2*q3*SH_RNG[4]; - SK_RNG[2] = 2*q0 - 2*q2*SH_RNG[4]; - SK_RNG[3] = 2*q3 + 2*q1*SH_RNG[4]; - SK_RNG[4] = 2*q2 + 2*q0*SH_RNG[4]; - SK_RNG[5] = SH_RNG[2]; - Kfusion[22] = SK_RNG[0]*(P[22][9]*SH_RNG[3] - P[22][22]*SH_RNG[3] + P[22][0]*SH_RNG[1]*SK_RNG[2]*SK_RNG[5] - P[22][1]*SH_RNG[1]*SK_RNG[1]*SK_RNG[5] - P[22][2]*SH_RNG[1]*SK_RNG[4]*SK_RNG[5] + P[22][3]*SH_RNG[1]*SK_RNG[3]*SK_RNG[5]); - - // Calculate the innovation variance for data logging - varInnovRng = 1.0f/SK_RNG[0]; - - // Calculate the measurement innovation - rngPred = (ptd - pd)/cosRngTilt; - innovRng = rngPred - rngMea; - - // calculate the innovation consistency test ratio - auxRngTestRatio = sq(innovRng) / (sq(rngInnovGate) * varInnovRng); - - // Check the innovation for consistency and don't fuse if out of bounds - if (auxRngTestRatio < 1.0f) - { - // correct the state vector - states[22] = states[22] - Kfusion[22] * innovRng; - - // correct the covariance P = (I - K*H)*P - P[22][22] = P[22][22] - Kfusion[22] * H_RNG[22] * P[22][22]; - P[22][22] = ConstrainFloat(P[22][22], 0.0f, 10000.0f); - } - } - -} - void AttPosEKF::FuseOptFlow() { -// static uint8_t obsIndex; -// static float SH_LOS[13]; -// static float SKK_LOS[15]; -// static float SK_LOS[2]; -// static float q0 = 0.0f; -// static float q1 = 0.0f; -// static float q2 = 0.0f; -// static float q3 = 1.0f; -// static float vn = 0.0f; -// static float ve = 0.0f; -// static float vd = 0.0f; -// static float pd = 0.0f; -// static float ptd = 0.0f; -// static float R_LOS = 0.01f; -// static float losPred[2]; + static float SH_LOS[13]; + static float SK_LOS[9]; + static float q0 = 0.0f; + static float q1 = 0.0f; + static float q2 = 0.0f; + static float q3 = 1.0f; + static float vn = 0.0f; + static float ve = 0.0f; + static float vd = 0.0f; + static float pd = 0.0f; + static float ptd = 0.0f; + static float losPred[2]; -// // Transformation matrix from nav to body axes -// Mat3f Tnb_local; -// // Transformation matrix from body to sensor axes -// // assume camera is aligned with Z body axis plus a misalignment -// // defined by 3 small angles about X, Y and Z body axis -// Mat3f Tbs; -// Tbs.x.y = a3; -// Tbs.y.x = -a3; -// Tbs.x.z = -a2; -// Tbs.z.x = a2; -// Tbs.y.z = a1; -// Tbs.z.y = -a1; -// // Transformation matrix from navigation to sensor axes -// Mat3f Tns; -// float H_LOS[n_states]; -// for (uint8_t i = 0; i < n_states; i++) { -// H_LOS[i] = 0.0f; -// } -// Vector3f velNED_local; -// Vector3f relVelSensor; + // Transformation matrix from nav to body axes + float H_LOS[2][n_states]; + float K_LOS[2][n_states]; + Vector3f velNED_local; + Vector3f relVelSensor; -// // Perform sequential fusion of optical flow measurements only when in the air and tilt is less than 30 deg. -// if (useOpticalFlow && (fuseOptFlowData || obsIndex == 1) && !onGround && Tbs.z.z > 0.866f && rngMea > 5.0f && rngMea < 39.0f) -// { -// // Sequential fusion of XY components to spread processing load across -// // two prediction time steps. + // Perform sequential fusion of optical flow measurements only with valid tilt and height + flowStates[1] = maxf(flowStates[1], statesAtFlowTime[9] + minFlowRng); + float heightAboveGndEst = flowStates[1] - statesAtFlowTime[9]; + bool validTilt = Tnb.z.z > 0.71f; + if (validTilt) + { + // Sequential fusion of XY components. -// // Calculate observation jacobians and Kalman gains -// if (fuseOptFlowData) -// { -// // Copy required states to local variable names -// q0 = statesAtOptFlowTime[0]; -// q1 = statesAtOptFlowTime[1]; -// q2 = statesAtOptFlowTime[2]; -// q3 = statesAtOptFlowTime[3]; -// vn = statesAtOptFlowTime[4]; -// ve = statesAtOptFlowTime[5]; -// vd = statesAtOptFlowTime[6]; -// pd = statesAtOptFlowTime[9]; -// ptd = statesAtOptFlowTime[22]; -// velNED_local.x = vn; -// velNED_local.y = ve; -// velNED_local.z = vd; + // Calculate observation jacobians and Kalman gains + if (fuseOptFlowData) + { + // Copy required states to local variable names + q0 = statesAtFlowTime[0]; + q1 = statesAtFlowTime[1]; + q2 = statesAtFlowTime[2]; + q3 = statesAtFlowTime[3]; + vn = statesAtFlowTime[4]; + ve = statesAtFlowTime[5]; + vd = statesAtFlowTime[6]; + pd = statesAtFlowTime[9]; + ptd = flowStates[1]; + velNED_local.x = vn; + velNED_local.y = ve; + velNED_local.z = vd; -// // calculate rotation from NED to body axes -// float q00 = sq(q0); -// float q11 = sq(q1); -// float q22 = sq(q2); -// float q33 = sq(q3); -// float q01 = q0 * q1; -// float q02 = q0 * q2; -// float q03 = q0 * q3; -// float q12 = q1 * q2; -// float q13 = q1 * q3; -// float q23 = q2 * q3; -// Tnb_local.x.x = q00 + q11 - q22 - q33; -// Tnb_local.y.y = q00 - q11 + q22 - q33; -// Tnb_local.z.z = q00 - q11 - q22 + q33; -// Tnb_local.y.x = 2*(q12 - q03); -// Tnb_local.z.x = 2*(q13 + q02); -// Tnb_local.x.y = 2*(q12 + q03); -// Tnb_local.z.y = 2*(q23 - q01); -// Tnb_local.x.z = 2*(q13 - q02); -// Tnb_local.y.z = 2*(q23 + q01); + // calculate range from ground plain to centre of sensor fov assuming flat earth + float range = heightAboveGndEst/Tnb_flow.z.z; -// // calculate transformation from NED to sensor axes -// Tns = Tbs*Tnb_local; + // calculate relative velocity in sensor frame + relVelSensor = Tnb_flow*velNED_local; -// // calculate range from ground plain to centre of sensor fov assuming flat earth -// float range = ConstrainFloat(((ptd - pd)/Tns.z.z),0.5f,100.0f); + // divide velocity by range and include angular rate effects to get predicted angular LOS rates relative to X and Y axes + losPred[0] = relVelSensor.y/range; + losPred[1] = -relVelSensor.x/range; -// // calculate relative velocity in sensor frame -// relVelSensor = Tns*velNED_local; + // Calculate common expressions for observation jacobians + SH_LOS[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3); + SH_LOS[1] = vn*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) - vd*(2*q0*q2 - 2*q1*q3) + ve*(2*q0*q3 + 2*q1*q2); + SH_LOS[2] = ve*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + vd*(2*q0*q1 + 2*q2*q3) - vn*(2*q0*q3 - 2*q1*q2); + SH_LOS[3] = 1/(pd - ptd); + SH_LOS[4] = 1/sq(pd - ptd); -// // divide velocity by range and include angular rate effects to get predicted angular LOS rates relative to X and Y axes -// losPred[0] = relVelSensor.y/range; -// losPred[1] = -relVelSensor.x/range; + // Calculate common expressions for Kalman gains + SK_LOS[0] = 1.0f/((R_LOS + sq(omegaAcrossFlowTime[0] * moCompR_LOS)) + (SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3])*(P[0][0]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][0]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][0]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][0]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][0]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][0]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][0]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][0]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) + (SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3])*(P[0][1]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][1]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][1]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][1]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][1]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][1]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][1]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][1]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) - (SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3])*(P[0][2]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][2]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][2]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][2]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][2]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][2]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][2]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][2]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) + (SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3])*(P[0][3]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][3]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][3]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][3]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][3]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][3]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][3]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][3]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[1]*SH_LOS[4]*(P[0][9]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][9]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][9]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][9]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][9]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][9]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][9]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][9]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))*(P[0][4]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][4]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][4]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][4]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][4]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][4]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][4]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][4]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2)*(P[0][5]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][5]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][5]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][5]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][5]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][5]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][5]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][5]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3)*(P[0][6]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][6]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][6]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][6]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][6]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][6]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][6]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][6]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3)))); + SK_LOS[1] = 1.0f/((R_LOS + sq(omegaAcrossFlowTime[1] * moCompR_LOS))+ (SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3])*(P[0][0]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][0]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][0]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][0]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][0]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][0]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][0]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][0]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + (SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3])*(P[0][1]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][1]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][1]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][1]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][1]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][1]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][1]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][1]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + (SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3])*(P[0][2]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][2]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][2]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][2]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][2]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][2]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][2]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][2]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) - (SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3])*(P[0][3]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][3]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][3]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][3]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][3]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][3]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][3]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][3]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[2]*SH_LOS[4]*(P[0][9]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][9]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][9]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][9]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][9]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][9]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][9]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][9]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))*(P[0][5]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][5]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][5]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][5]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][5]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][5]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][5]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][5]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2)*(P[0][4]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][4]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][4]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][4]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][4]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][4]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][4]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][4]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3)*(P[0][6]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][6]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][6]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][6]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][6]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][6]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][6]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][6]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3)))); + SK_LOS[2] = SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn); + SK_LOS[3] = SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn); + SK_LOS[4] = SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn); + SK_LOS[5] = SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn); + SK_LOS[6] = sq(q0) - sq(q1) + sq(q2) - sq(q3); + SK_LOS[7] = sq(q0) + sq(q1) - sq(q2) - sq(q3); + SK_LOS[8] = SH_LOS[3]; -// //printf("relVelSensor.x=%5.1f, relVelSensor.y=%5.1f\n", relVelSensor.x, relVelSensor.y); -// //printf("Xpred=%5.2f, Xmea=%5.2f, Ypred=%5.2f, Ymea=%5.2f, delAng.x=%4.4f, delAng.y=%4.4f\n", losPred[0], losData[0], losPred[1], losData[1], delAng.x, delAng.y); -// //printf("omegaX=%5.2f, omegaY=%5.2f, velY=%5.1f velX=%5.1f\n, range=%5.1f\n", delAngRel.x/dt, delAngRel.y/dt, relVelSensor.y, relVelSensor.x, range); + // Calculate common intermediate terms + float tempVar[9]; + tempVar[0] = SH_LOS[0]*SK_LOS[6]*SK_LOS[8]; + tempVar[1] = SH_LOS[0]*SH_LOS[2]*SH_LOS[4]; + tempVar[2] = 2.0f*SH_LOS[2]*SK_LOS[8]; + tempVar[3] = SH_LOS[0]*SK_LOS[8]*(2.0f*q0*q1 + 2.0f*q2*q3); + tempVar[4] = SH_LOS[0]*SK_LOS[8]*(2.0f*q0*q3 - 2.0f*q1*q2); + tempVar[5] = (SK_LOS[5] - q2*tempVar[2]); + tempVar[6] = (SK_LOS[2] - q3*tempVar[2]); + tempVar[7] = (SK_LOS[3] - q1*tempVar[2]); + tempVar[8] = (SK_LOS[4] + q0*tempVar[2]); -// // Calculate observation jacobians -// SH_LOS[0] = a1*(2*q0*q1 + 2*q2*q3) + a2*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3); -// SH_LOS[1] = vd*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)) - ve*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + vn*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)); -// SH_LOS[2] = ve*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - vd*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)) + vn*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)); -// SH_LOS[3] = 1/(pd - ptd); -// SH_LOS[4] = 2*q1 - 2*a2*q3 + 2*a3*q2; -// SH_LOS[5] = 2*a2*q2 - 2*q0 + 2*a3*q3; -// SH_LOS[6] = 2*q2 + 2*a2*q0 - 2*a3*q1; -// SH_LOS[7] = 1/sq(pd - ptd); -// SH_LOS[8] = 2*q2 + 2*a1*q3 - 2*a3*q1; -// SH_LOS[9] = 2*q3 - 2*a1*q2 + 2*a3*q0; -// SH_LOS[10] = 2*a1*q1 - 2*q0 + 2*a3*q3; -// SH_LOS[11] = 2*q3 + 2*a2*q1 + 2*a3*q0; -// SH_LOS[12] = 2*q1 + 2*a1*q0 + 2*a3*q2; + // calculate observation jacobians for X LOS rate + for (uint8_t i = 0; i < 23; i++) H_LOS[0][i] = 0; + H_LOS[0][0] = - SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) - 2*q0*SH_LOS[2]*SH_LOS[3]; + H_LOS[0][1] = 2*q1*SH_LOS[2]*SH_LOS[3] - SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn); + H_LOS[0][2] = 2*q2*SH_LOS[2]*SH_LOS[3] - SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn); + H_LOS[0][3] = SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]; + H_LOS[0][4] = SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2); + H_LOS[0][5] = -SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3)); + H_LOS[0][6] = -SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3); + H_LOS[0][9] = tempVar[1]; -// for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0; -// H_LOS[0] = - SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]); -// H_LOS[1] = - SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]); -// H_LOS[2] = SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]) - SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3); -// H_LOS[3] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]); -// H_LOS[4] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)); -// H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)); -// H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)); -// H_LOS[9] = SH_LOS[0]*SH_LOS[2]*SH_LOS[7]; -// H_LOS[22] = -SH_LOS[0]*SH_LOS[2]*SH_LOS[7]; + // calculate Kalman gains for X LOS rate + K_LOS[0][0] = -(P[0][0]*tempVar[8] + P[0][1]*tempVar[7] - P[0][3]*tempVar[6] + P[0][2]*tempVar[5] - P[0][4]*tempVar[4] + P[0][6]*tempVar[3] - P[0][9]*tempVar[1] + P[0][5]*tempVar[0])/(R_LOS + tempVar[8]*(P[0][0]*tempVar[8] + P[1][0]*tempVar[7] + P[2][0]*tempVar[5] - P[3][0]*tempVar[6] - P[4][0]*tempVar[4] + P[6][0]*tempVar[3] - P[9][0]*tempVar[1] + P[5][0]*tempVar[0]) + tempVar[7]*(P[0][1]*tempVar[8] + P[1][1]*tempVar[7] + P[2][1]*tempVar[5] - P[3][1]*tempVar[6] - P[4][1]*tempVar[4] + P[6][1]*tempVar[3] - P[9][1]*tempVar[1] + P[5][1]*tempVar[0]) + tempVar[5]*(P[0][2]*tempVar[8] + P[1][2]*tempVar[7] + P[2][2]*tempVar[5] - P[3][2]*tempVar[6] - P[4][2]*tempVar[4] + P[6][2]*tempVar[3] - P[9][2]*tempVar[1] + P[5][2]*tempVar[0]) - tempVar[6]*(P[0][3]*tempVar[8] + P[1][3]*tempVar[7] + P[2][3]*tempVar[5] - P[3][3]*tempVar[6] - P[4][3]*tempVar[4] + P[6][3]*tempVar[3] - P[9][3]*tempVar[1] + P[5][3]*tempVar[0]) - tempVar[4]*(P[0][4]*tempVar[8] + P[1][4]*tempVar[7] + P[2][4]*tempVar[5] - P[3][4]*tempVar[6] - P[4][4]*tempVar[4] + P[6][4]*tempVar[3] - P[9][4]*tempVar[1] + P[5][4]*tempVar[0]) + tempVar[3]*(P[0][6]*tempVar[8] + P[1][6]*tempVar[7] + P[2][6]*tempVar[5] - P[3][6]*tempVar[6] - P[4][6]*tempVar[4] + P[6][6]*tempVar[3] - P[9][6]*tempVar[1] + P[5][6]*tempVar[0]) - tempVar[1]*(P[0][9]*tempVar[8] + P[1][9]*tempVar[7] + P[2][9]*tempVar[5] - P[3][9]*tempVar[6] - P[4][9]*tempVar[4] + P[6][9]*tempVar[3] - P[9][9]*tempVar[1] + P[5][9]*tempVar[0]) + tempVar[0]*(P[0][5]*tempVar[8] + P[1][5]*tempVar[7] + P[2][5]*tempVar[5] - P[3][5]*tempVar[6] - P[4][5]*tempVar[4] + P[6][5]*tempVar[3] - P[9][5]*tempVar[1] + P[5][5]*tempVar[0])); + K_LOS[0][1] = -SK_LOS[1]*(P[1][0]*tempVar[8] + P[1][1]*tempVar[7] - P[1][3]*tempVar[6] + P[1][2]*tempVar[5] - P[1][4]*tempVar[4] + P[1][6]*tempVar[3] - P[1][9]*tempVar[1] + P[1][5]*tempVar[0]); + K_LOS[0][2] = -SK_LOS[1]*(P[2][0]*tempVar[8] + P[2][1]*tempVar[7] - P[2][3]*tempVar[6] + P[2][2]*tempVar[5] - P[2][4]*tempVar[4] + P[2][6]*tempVar[3] - P[2][9]*tempVar[1] + P[2][5]*tempVar[0]); + K_LOS[0][3] = -SK_LOS[1]*(P[3][0]*tempVar[8] + P[3][1]*tempVar[7] - P[3][3]*tempVar[6] + P[3][2]*tempVar[5] - P[3][4]*tempVar[4] + P[3][6]*tempVar[3] - P[3][9]*tempVar[1] + P[3][5]*tempVar[0]); + K_LOS[0][4] = -SK_LOS[1]*(P[4][0]*tempVar[8] + P[4][1]*tempVar[7] - P[4][3]*tempVar[6] + P[4][2]*tempVar[5] - P[4][4]*tempVar[4] + P[4][6]*tempVar[3] - P[4][9]*tempVar[1] + P[4][5]*tempVar[0]); + K_LOS[0][5] = -SK_LOS[1]*(P[5][0]*tempVar[8] + P[5][1]*tempVar[7] - P[5][3]*tempVar[6] + P[5][2]*tempVar[5] - P[5][4]*tempVar[4] + P[5][6]*tempVar[3] - P[5][9]*tempVar[1] + P[5][5]*tempVar[0]); + K_LOS[0][6] = -SK_LOS[1]*(P[6][0]*tempVar[8] + P[6][1]*tempVar[7] - P[6][3]*tempVar[6] + P[6][2]*tempVar[5] - P[6][4]*tempVar[4] + P[6][6]*tempVar[3] - P[6][9]*tempVar[1] + P[6][5]*tempVar[0]); + K_LOS[0][7] = -SK_LOS[1]*(P[7][0]*tempVar[8] + P[7][1]*tempVar[7] - P[7][3]*tempVar[6] + P[7][2]*tempVar[5] - P[7][4]*tempVar[4] + P[7][6]*tempVar[3] - P[7][9]*tempVar[1] + P[7][5]*tempVar[0]); + K_LOS[0][8] = -SK_LOS[1]*(P[8][0]*tempVar[8] + P[8][1]*tempVar[7] - P[8][3]*tempVar[6] + P[8][2]*tempVar[5] - P[8][4]*tempVar[4] + P[8][6]*tempVar[3] - P[8][9]*tempVar[1] + P[8][5]*tempVar[0]); + K_LOS[0][9] = -SK_LOS[1]*(P[9][0]*tempVar[8] + P[9][1]*tempVar[7] - P[9][3]*tempVar[6] + P[9][2]*tempVar[5] - P[9][4]*tempVar[4] + P[9][6]*tempVar[3] - P[9][9]*tempVar[1] + P[9][5]*tempVar[0]); + K_LOS[0][10] = -SK_LOS[1]*(P[10][0]*tempVar[8] + P[10][1]*tempVar[7] - P[10][3]*tempVar[6] + P[10][2]*tempVar[5] - P[10][4]*tempVar[4] + P[10][6]*tempVar[3] - P[10][9]*tempVar[1] + P[10][5]*tempVar[0]); + K_LOS[0][11] = -SK_LOS[1]*(P[11][0]*tempVar[8] + P[11][1]*tempVar[7] - P[11][3]*tempVar[6] + P[11][2]*tempVar[5] - P[11][4]*tempVar[4] + P[11][6]*tempVar[3] - P[11][9]*tempVar[1] + P[11][5]*tempVar[0]); + K_LOS[0][12] = -SK_LOS[1]*(P[12][0]*tempVar[8] + P[12][1]*tempVar[7] - P[12][3]*tempVar[6] + P[12][2]*tempVar[5] - P[12][4]*tempVar[4] + P[12][6]*tempVar[3] - P[12][9]*tempVar[1] + P[12][5]*tempVar[0]); + // only height measurements are allowed to modify the Z bias state to improve the stability of the estimate + K_LOS[0][13] = 0.0f;//-SK_LOS[1]*(P[13][0]*tempVar[8] + P[13][1]*tempVar[7] - P[13][3]*tempVar[6] + P[13][2]*tempVar[5] - P[13][4]*tempVar[4] + P[13][6]*tempVar[3] - P[13][9]*tempVar[1] + P[13][5]*tempVar[0]); + if (inhibitWindStates) { + K_LOS[0][14] = -SK_LOS[1]*(P[14][0]*tempVar[8] + P[14][1]*tempVar[7] - P[14][3]*tempVar[6] + P[14][2]*tempVar[5] - P[14][4]*tempVar[4] + P[14][6]*tempVar[3] - P[14][9]*tempVar[1] + P[14][5]*tempVar[0]); + K_LOS[0][15] = -SK_LOS[1]*(P[15][0]*tempVar[8] + P[15][1]*tempVar[7] - P[15][3]*tempVar[6] + P[15][2]*tempVar[5] - P[15][4]*tempVar[4] + P[15][6]*tempVar[3] - P[15][9]*tempVar[1] + P[15][5]*tempVar[0]); + } else { + K_LOS[0][14] = 0.0f; + K_LOS[0][15] = 0.0f; + } + if (inhibitMagStates) { + K_LOS[0][16] = -SK_LOS[1]*(P[16][0]*tempVar[8] + P[16][1]*tempVar[7] - P[16][3]*tempVar[6] + P[16][2]*tempVar[5] - P[16][4]*tempVar[4] + P[16][6]*tempVar[3] - P[16][9]*tempVar[1] + P[16][5]*tempVar[0]); + K_LOS[0][17] = -SK_LOS[1]*(P[17][0]*tempVar[8] + P[17][1]*tempVar[7] - P[17][3]*tempVar[6] + P[17][2]*tempVar[5] - P[17][4]*tempVar[4] + P[17][6]*tempVar[3] - P[17][9]*tempVar[1] + P[17][5]*tempVar[0]); + K_LOS[0][18] = -SK_LOS[1]*(P[18][0]*tempVar[8] + P[18][1]*tempVar[7] - P[18][3]*tempVar[6] + P[18][2]*tempVar[5] - P[18][4]*tempVar[4] + P[18][6]*tempVar[3] - P[18][9]*tempVar[1] + P[18][5]*tempVar[0]); + K_LOS[0][19] = -SK_LOS[1]*(P[19][0]*tempVar[8] + P[19][1]*tempVar[7] - P[19][3]*tempVar[6] + P[19][2]*tempVar[5] - P[19][4]*tempVar[4] + P[19][6]*tempVar[3] - P[19][9]*tempVar[1] + P[19][5]*tempVar[0]); + K_LOS[0][20] = -SK_LOS[1]*(P[20][0]*tempVar[8] + P[20][1]*tempVar[7] - P[20][3]*tempVar[6] + P[20][2]*tempVar[5] - P[20][4]*tempVar[4] + P[20][6]*tempVar[3] - P[20][9]*tempVar[1] + P[20][5]*tempVar[0]); + K_LOS[0][21] = -SK_LOS[1]*(P[21][0]*tempVar[8] + P[21][1]*tempVar[7] - P[21][3]*tempVar[6] + P[21][2]*tempVar[5] - P[21][4]*tempVar[4] + P[21][6]*tempVar[3] - P[21][9]*tempVar[1] + P[21][5]*tempVar[0]); + } else { + for (uint8_t i = 16; i <= 21; i++) { + K_LOS[0][i] = 0.0f; + } + } -// // Calculate Kalman gain -// SKK_LOS[0] = a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3); -// SKK_LOS[1] = a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3); -// SKK_LOS[2] = a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3); -// SKK_LOS[3] = a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3); -// SKK_LOS[4] = a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3); -// SKK_LOS[5] = a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3); -// SKK_LOS[6] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]); -// SKK_LOS[7] = SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]); -// SKK_LOS[8] = SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]); -// SKK_LOS[9] = SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]); -// SKK_LOS[10] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]); -// SKK_LOS[11] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]); -// SKK_LOS[12] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]); -// SKK_LOS[13] = SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]); -// SKK_LOS[14] = SH_LOS[0]; + // calculate innovation variance and innovation for X axis observation + varInnovOptFlow[0] = 1.0f/SK_LOS[0]; + innovOptFlow[0] = losPred[0] - flowRadXYcomp[0]; -// SK_LOS[0] = 1/(R_LOS + SKK_LOS[8]*(P[0][0]*SKK_LOS[8] + P[1][0]*SKK_LOS[7] + P[2][0]*SKK_LOS[9] - P[3][0]*SKK_LOS[6] - P[9][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][0]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][0]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[7]*(P[0][1]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] + P[2][1]*SKK_LOS[9] - P[3][1]*SKK_LOS[6] - P[9][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][1]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][1]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[9]*(P[0][2]*SKK_LOS[8] + P[1][2]*SKK_LOS[7] + P[2][2]*SKK_LOS[9] - P[3][2]*SKK_LOS[6] - P[9][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][2]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][2]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SKK_LOS[6]*(P[0][3]*SKK_LOS[8] + P[1][3]*SKK_LOS[7] + P[2][3]*SKK_LOS[9] - P[3][3]*SKK_LOS[6] - P[9][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][3]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][3]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[8] + P[1][9]*SKK_LOS[7] + P[2][9]*SKK_LOS[9] - P[3][9]*SKK_LOS[6] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][9]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][9]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[8] + P[1][22]*SKK_LOS[7] + P[2][22]*SKK_LOS[9] - P[3][22]*SKK_LOS[6] - P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][22]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][22]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14]*(P[0][4]*SKK_LOS[8] + P[1][4]*SKK_LOS[7] + P[2][4]*SKK_LOS[9] - P[3][4]*SKK_LOS[6] - P[9][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][4]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14]*(P[0][5]*SKK_LOS[8] + P[1][5]*SKK_LOS[7] + P[2][5]*SKK_LOS[9] - P[3][5]*SKK_LOS[6] - P[9][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]*(P[0][6]*SKK_LOS[8] + P[1][6]*SKK_LOS[7] + P[2][6]*SKK_LOS[9] - P[3][6]*SKK_LOS[6] - P[9][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14])); -// Kfusion[0] = -SK_LOS[0]*(P[0][0]*SKK_LOS[8] + P[0][1]*SKK_LOS[7] - P[0][3]*SKK_LOS[6] + P[0][2]*SKK_LOS[9] - P[0][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[0][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[0][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[1] = -SK_LOS[0]*(P[1][0]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] - P[1][3]*SKK_LOS[6] + P[1][2]*SKK_LOS[9] - P[1][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[1][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[1][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[2] = -SK_LOS[0]*(P[2][0]*SKK_LOS[8] + P[2][1]*SKK_LOS[7] - P[2][3]*SKK_LOS[6] + P[2][2]*SKK_LOS[9] - P[2][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[2][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[2][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[3] = -SK_LOS[0]*(P[3][0]*SKK_LOS[8] + P[3][1]*SKK_LOS[7] - P[3][3]*SKK_LOS[6] + P[3][2]*SKK_LOS[9] - P[3][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[3][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[3][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[4] = -SK_LOS[0]*(P[4][0]*SKK_LOS[8] + P[4][1]*SKK_LOS[7] - P[4][3]*SKK_LOS[6] + P[4][2]*SKK_LOS[9] - P[4][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[4][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[5] = -SK_LOS[0]*(P[5][0]*SKK_LOS[8] + P[5][1]*SKK_LOS[7] - P[5][3]*SKK_LOS[6] + P[5][2]*SKK_LOS[9] - P[5][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[6] = -SK_LOS[0]*(P[6][0]*SKK_LOS[8] + P[6][1]*SKK_LOS[7] - P[6][3]*SKK_LOS[6] + P[6][2]*SKK_LOS[9] - P[6][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[7] = -SK_LOS[0]*(P[7][0]*SKK_LOS[8] + P[7][1]*SKK_LOS[7] - P[7][3]*SKK_LOS[6] + P[7][2]*SKK_LOS[9] - P[7][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[7][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[7][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[8] = -SK_LOS[0]*(P[8][0]*SKK_LOS[8] + P[8][1]*SKK_LOS[7] - P[8][3]*SKK_LOS[6] + P[8][2]*SKK_LOS[9] - P[8][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[8][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[8][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[9] = -SK_LOS[0]*(P[9][0]*SKK_LOS[8] + P[9][1]*SKK_LOS[7] - P[9][3]*SKK_LOS[6] + P[9][2]*SKK_LOS[9] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[9][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[9][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[10] = -SK_LOS[0]*(P[10][0]*SKK_LOS[8] + P[10][1]*SKK_LOS[7] - P[10][3]*SKK_LOS[6] + P[10][2]*SKK_LOS[9] - P[10][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[10][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[10][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[11] = -SK_LOS[0]*(P[11][0]*SKK_LOS[8] + P[11][1]*SKK_LOS[7] - P[11][3]*SKK_LOS[6] + P[11][2]*SKK_LOS[9] - P[11][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[11][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[11][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[12] = -SK_LOS[0]*(P[12][0]*SKK_LOS[8] + P[12][1]*SKK_LOS[7] - P[12][3]*SKK_LOS[6] + P[12][2]*SKK_LOS[9] - P[12][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[12][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[12][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[13] = 0.0f;//-SK_LOS[0]*(P[13][0]*SKK_LOS[8] + P[13][1]*SKK_LOS[7] - P[13][3]*SKK_LOS[6] + P[13][2]*SKK_LOS[9] - P[13][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[13][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[13][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[14] = -SK_LOS[0]*(P[14][0]*SKK_LOS[8] + P[14][1]*SKK_LOS[7] - P[14][3]*SKK_LOS[6] + P[14][2]*SKK_LOS[9] - P[14][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[14][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[14][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[15] = -SK_LOS[0]*(P[15][0]*SKK_LOS[8] + P[15][1]*SKK_LOS[7] - P[15][3]*SKK_LOS[6] + P[15][2]*SKK_LOS[9] - P[15][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[15][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[15][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[16] = -SK_LOS[0]*(P[16][0]*SKK_LOS[8] + P[16][1]*SKK_LOS[7] - P[16][3]*SKK_LOS[6] + P[16][2]*SKK_LOS[9] - P[16][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[16][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[16][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[17] = -SK_LOS[0]*(P[17][0]*SKK_LOS[8] + P[17][1]*SKK_LOS[7] - P[17][3]*SKK_LOS[6] + P[17][2]*SKK_LOS[9] - P[17][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[17][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[17][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[18] = -SK_LOS[0]*(P[18][0]*SKK_LOS[8] + P[18][1]*SKK_LOS[7] - P[18][3]*SKK_LOS[6] + P[18][2]*SKK_LOS[9] - P[18][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[18][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[18][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[19] = -SK_LOS[0]*(P[19][0]*SKK_LOS[8] + P[19][1]*SKK_LOS[7] - P[19][3]*SKK_LOS[6] + P[19][2]*SKK_LOS[9] - P[19][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[19][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[19][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[20] = -SK_LOS[0]*(P[20][0]*SKK_LOS[8] + P[20][1]*SKK_LOS[7] - P[20][3]*SKK_LOS[6] + P[20][2]*SKK_LOS[9] - P[20][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[20][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[20][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[21] = -SK_LOS[0]*(P[21][0]*SKK_LOS[8] + P[21][1]*SKK_LOS[7] - P[21][3]*SKK_LOS[6] + P[21][2]*SKK_LOS[9] - P[21][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[21][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[21][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[22] = -SK_LOS[0]*(P[22][0]*SKK_LOS[8] + P[22][1]*SKK_LOS[7] - P[22][3]*SKK_LOS[6] + P[22][2]*SKK_LOS[9] - P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[22][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[22][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// varInnovOptFlow[0] = 1.0f/SK_LOS[0]; -// innovOptFlow[0] = losPred[0] - losData[0]; + // calculate intermediate common variables + tempVar[0] = 2.0f*SH_LOS[1]*SK_LOS[8]; + tempVar[1] = (SK_LOS[2] + q0*tempVar[0]); + tempVar[2] = (SK_LOS[5] - q1*tempVar[0]); + tempVar[3] = (SK_LOS[3] + q2*tempVar[0]); + tempVar[4] = (SK_LOS[4] + q3*tempVar[0]); + tempVar[5] = SH_LOS[0]*SK_LOS[8]*(2*q0*q3 + 2*q1*q2); + tempVar[6] = SH_LOS[0]*SK_LOS[8]*(2*q0*q2 - 2*q1*q3); + tempVar[7] = SH_LOS[0]*SH_LOS[1]*SH_LOS[4]; + tempVar[8] = SH_LOS[0]*SK_LOS[7]*SK_LOS[8]; -// // set the observation index to 1 to fuse the y component next time round and reset the commence fusion flag -// obsIndex = 1; -// fuseOptFlowData = false; -// } -// else if (obsIndex == 1) // we are now fusing the Y measurement -// { -// // Calculate observation jacobians -// for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0; -// H_LOS[0] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]); -// H_LOS[1] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]); -// H_LOS[2] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]); -// H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]) - SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1); -// H_LOS[4] = SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)); -// H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)); -// H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)); -// H_LOS[9] = -SH_LOS[0]*SH_LOS[1]*SH_LOS[7]; -// H_LOS[22] = SH_LOS[0]*SH_LOS[1]*SH_LOS[7]; + // Calculate observation jacobians for Y LOS rate + for (uint8_t i = 0; i < 23; i++) H_LOS[1][i] = 0; + H_LOS[1][0] = SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]; + H_LOS[1][1] = SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]; + H_LOS[1][2] = - SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q2*SH_LOS[1]*SH_LOS[3]; + H_LOS[1][3] = SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]; + H_LOS[1][4] = SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3)); + H_LOS[1][5] = SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2); + H_LOS[1][6] = -SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3); + H_LOS[1][9] = -tempVar[7]; -// // Calculate Kalman gains -// SK_LOS[1] = 1/(R_LOS + SKK_LOS[12]*(P[0][2]*SKK_LOS[10] + P[1][2]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[3][2]*SKK_LOS[13] - P[9][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][2]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][2]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SKK_LOS[13]*(P[0][3]*SKK_LOS[10] + P[1][3]*SKK_LOS[11] + P[2][3]*SKK_LOS[12] - P[3][3]*SKK_LOS[13] - P[9][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][3]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][3]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[10]*(P[0][0]*SKK_LOS[10] + P[1][0]*SKK_LOS[11] + P[2][0]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][0]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][0]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][0]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[11]*(P[0][1]*SKK_LOS[10] + P[1][1]*SKK_LOS[11] + P[2][1]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][1]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][1]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][1]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[10] + P[1][9]*SKK_LOS[11] + P[2][9]*SKK_LOS[12] - P[3][9]*SKK_LOS[13] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][9]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][9]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[10] + P[1][22]*SKK_LOS[11] + P[2][22]*SKK_LOS[12] - P[3][22]*SKK_LOS[13] - P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][22]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][22]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14]*(P[0][4]*SKK_LOS[10] + P[1][4]*SKK_LOS[11] + P[2][4]*SKK_LOS[12] - P[3][4]*SKK_LOS[13] - P[9][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][4]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]*(P[0][5]*SKK_LOS[10] + P[1][5]*SKK_LOS[11] + P[2][5]*SKK_LOS[12] - P[3][5]*SKK_LOS[13] - P[9][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]*(P[0][6]*SKK_LOS[10] + P[1][6]*SKK_LOS[11] + P[2][6]*SKK_LOS[12] - P[3][6]*SKK_LOS[13] - P[9][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14])); -// Kfusion[0] = SK_LOS[1]*(P[0][0]*SKK_LOS[10] - P[0][3]*SKK_LOS[13] + P[0][1]*SKK_LOS[11] + P[0][2]*SKK_LOS[12] - P[0][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[0][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[1] = SK_LOS[1]*(P[1][0]*SKK_LOS[10] - P[1][3]*SKK_LOS[13] + P[1][1]*SKK_LOS[11] + P[1][2]*SKK_LOS[12] - P[1][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[1][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[2] = SK_LOS[1]*(P[2][0]*SKK_LOS[10] - P[2][3]*SKK_LOS[13] + P[2][1]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[2][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[2][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[3] = SK_LOS[1]*(P[3][0]*SKK_LOS[10] - P[3][3]*SKK_LOS[13] + P[3][1]*SKK_LOS[11] + P[3][2]*SKK_LOS[12] - P[3][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[3][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[4] = SK_LOS[1]*(P[4][0]*SKK_LOS[10] - P[4][3]*SKK_LOS[13] + P[4][1]*SKK_LOS[11] + P[4][2]*SKK_LOS[12] - P[4][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[4][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[5] = SK_LOS[1]*(P[5][0]*SKK_LOS[10] - P[5][3]*SKK_LOS[13] + P[5][1]*SKK_LOS[11] + P[5][2]*SKK_LOS[12] - P[5][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[6] = SK_LOS[1]*(P[6][0]*SKK_LOS[10] - P[6][3]*SKK_LOS[13] + P[6][1]*SKK_LOS[11] + P[6][2]*SKK_LOS[12] - P[6][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[7] = SK_LOS[1]*(P[7][0]*SKK_LOS[10] - P[7][3]*SKK_LOS[13] + P[7][1]*SKK_LOS[11] + P[7][2]*SKK_LOS[12] - P[7][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[7][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[8] = SK_LOS[1]*(P[8][0]*SKK_LOS[10] - P[8][3]*SKK_LOS[13] + P[8][1]*SKK_LOS[11] + P[8][2]*SKK_LOS[12] - P[8][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[8][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[9] = SK_LOS[1]*(P[9][0]*SKK_LOS[10] - P[9][3]*SKK_LOS[13] + P[9][1]*SKK_LOS[11] + P[9][2]*SKK_LOS[12] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[9][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[10] = SK_LOS[1]*(P[10][0]*SKK_LOS[10] - P[10][3]*SKK_LOS[13] + P[10][1]*SKK_LOS[11] + P[10][2]*SKK_LOS[12] - P[10][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[10][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[11] = SK_LOS[1]*(P[11][0]*SKK_LOS[10] - P[11][3]*SKK_LOS[13] + P[11][1]*SKK_LOS[11] + P[11][2]*SKK_LOS[12] - P[11][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[11][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[12] = SK_LOS[1]*(P[12][0]*SKK_LOS[10] - P[12][3]*SKK_LOS[13] + P[12][1]*SKK_LOS[11] + P[12][2]*SKK_LOS[12] - P[12][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[12][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[13] = 0.0f;//SK_LOS[1]*(P[13][0]*SKK_LOS[10] - P[13][3]*SKK_LOS[13] + P[13][1]*SKK_LOS[11] + P[13][2]*SKK_LOS[12] - P[13][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[13][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[14] = SK_LOS[1]*(P[14][0]*SKK_LOS[10] - P[14][3]*SKK_LOS[13] + P[14][1]*SKK_LOS[11] + P[14][2]*SKK_LOS[12] - P[14][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[14][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[15] = SK_LOS[1]*(P[15][0]*SKK_LOS[10] - P[15][3]*SKK_LOS[13] + P[15][1]*SKK_LOS[11] + P[15][2]*SKK_LOS[12] - P[15][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[15][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[16] = SK_LOS[1]*(P[16][0]*SKK_LOS[10] - P[16][3]*SKK_LOS[13] + P[16][1]*SKK_LOS[11] + P[16][2]*SKK_LOS[12] - P[16][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[16][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[17] = SK_LOS[1]*(P[17][0]*SKK_LOS[10] - P[17][3]*SKK_LOS[13] + P[17][1]*SKK_LOS[11] + P[17][2]*SKK_LOS[12] - P[17][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[17][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[18] = SK_LOS[1]*(P[18][0]*SKK_LOS[10] - P[18][3]*SKK_LOS[13] + P[18][1]*SKK_LOS[11] + P[18][2]*SKK_LOS[12] - P[18][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[18][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[19] = SK_LOS[1]*(P[19][0]*SKK_LOS[10] - P[19][3]*SKK_LOS[13] + P[19][1]*SKK_LOS[11] + P[19][2]*SKK_LOS[12] - P[19][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[19][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[20] = SK_LOS[1]*(P[20][0]*SKK_LOS[10] - P[20][3]*SKK_LOS[13] + P[20][1]*SKK_LOS[11] + P[20][2]*SKK_LOS[12] - P[20][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[20][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[21] = SK_LOS[1]*(P[21][0]*SKK_LOS[10] - P[21][3]*SKK_LOS[13] + P[21][1]*SKK_LOS[11] + P[21][2]*SKK_LOS[12] - P[21][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[21][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[22] = SK_LOS[1]*(P[22][0]*SKK_LOS[10] - P[22][3]*SKK_LOS[13] + P[22][1]*SKK_LOS[11] + P[22][2]*SKK_LOS[12] - P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[22][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// varInnovOptFlow[1] = 1.0f/SK_LOS[1]; -// innovOptFlow[1] = losPred[1] - losData[1]; + // Calculate Kalman gains for Y LOS rate + K_LOS[1][0] = SK_LOS[0]*(P[0][0]*tempVar[1] + P[0][1]*tempVar[2] - P[0][2]*tempVar[3] + P[0][3]*tempVar[4] + P[0][5]*tempVar[5] - P[0][6]*tempVar[6] - P[0][9]*tempVar[7] + P[0][4]*tempVar[8]); + K_LOS[1][1] = SK_LOS[0]*(P[1][0]*tempVar[1] + P[1][1]*tempVar[2] - P[1][2]*tempVar[3] + P[1][3]*tempVar[4] + P[1][5]*tempVar[5] - P[1][6]*tempVar[6] - P[1][9]*tempVar[7] + P[1][4]*tempVar[8]); + K_LOS[1][2] = SK_LOS[0]*(P[2][0]*tempVar[1] + P[2][1]*tempVar[2] - P[2][2]*tempVar[3] + P[2][3]*tempVar[4] + P[2][5]*tempVar[5] - P[2][6]*tempVar[6] - P[2][9]*tempVar[7] + P[2][4]*tempVar[8]); + K_LOS[1][3] = SK_LOS[0]*(P[3][0]*tempVar[1] + P[3][1]*tempVar[2] - P[3][2]*tempVar[3] + P[3][3]*tempVar[4] + P[3][5]*tempVar[5] - P[3][6]*tempVar[6] - P[3][9]*tempVar[7] + P[3][4]*tempVar[8]); + K_LOS[1][4] = SK_LOS[0]*(P[4][0]*tempVar[1] + P[4][1]*tempVar[2] - P[4][2]*tempVar[3] + P[4][3]*tempVar[4] + P[4][5]*tempVar[5] - P[4][6]*tempVar[6] - P[4][9]*tempVar[7] + P[4][4]*tempVar[8]); + K_LOS[1][5] = SK_LOS[0]*(P[5][0]*tempVar[1] + P[5][1]*tempVar[2] - P[5][2]*tempVar[3] + P[5][3]*tempVar[4] + P[5][5]*tempVar[5] - P[5][6]*tempVar[6] - P[5][9]*tempVar[7] + P[5][4]*tempVar[8]); + K_LOS[1][6] = SK_LOS[0]*(P[6][0]*tempVar[1] + P[6][1]*tempVar[2] - P[6][2]*tempVar[3] + P[6][3]*tempVar[4] + P[6][5]*tempVar[5] - P[6][6]*tempVar[6] - P[6][9]*tempVar[7] + P[6][4]*tempVar[8]); + K_LOS[1][7] = SK_LOS[0]*(P[7][0]*tempVar[1] + P[7][1]*tempVar[2] - P[7][2]*tempVar[3] + P[7][3]*tempVar[4] + P[7][5]*tempVar[5] - P[7][6]*tempVar[6] - P[7][9]*tempVar[7] + P[7][4]*tempVar[8]); + K_LOS[1][8] = SK_LOS[0]*(P[8][0]*tempVar[1] + P[8][1]*tempVar[2] - P[8][2]*tempVar[3] + P[8][3]*tempVar[4] + P[8][5]*tempVar[5] - P[8][6]*tempVar[6] - P[8][9]*tempVar[7] + P[8][4]*tempVar[8]); + K_LOS[1][9] = SK_LOS[0]*(P[9][0]*tempVar[1] + P[9][1]*tempVar[2] - P[9][2]*tempVar[3] + P[9][3]*tempVar[4] + P[9][5]*tempVar[5] - P[9][6]*tempVar[6] - P[9][9]*tempVar[7] + P[9][4]*tempVar[8]); + K_LOS[1][10] = SK_LOS[0]*(P[10][0]*tempVar[1] + P[10][1]*tempVar[2] - P[10][2]*tempVar[3] + P[10][3]*tempVar[4] + P[10][5]*tempVar[5] - P[10][6]*tempVar[6] - P[10][9]*tempVar[7] + P[10][4]*tempVar[8]); + K_LOS[1][11] = SK_LOS[0]*(P[11][0]*tempVar[1] + P[11][1]*tempVar[2] - P[11][2]*tempVar[3] + P[11][3]*tempVar[4] + P[11][5]*tempVar[5] - P[11][6]*tempVar[6] - P[11][9]*tempVar[7] + P[11][4]*tempVar[8]); + K_LOS[1][12] = SK_LOS[0]*(P[12][0]*tempVar[1] + P[12][1]*tempVar[2] - P[12][2]*tempVar[3] + P[12][3]*tempVar[4] + P[12][5]*tempVar[5] - P[12][6]*tempVar[6] - P[12][9]*tempVar[7] + P[12][4]*tempVar[8]); + // only height measurements are allowed to modify the Z bias state to improve the stability of the estimate + K_LOS[1][13] = 0.0f;//SK_LOS[0]*(P[13][0]*tempVar[1] + P[13][1]*tempVar[2] - P[13][2]*tempVar[3] + P[13][3]*tempVar[4] + P[13][5]*tempVar[5] - P[13][6]*tempVar[6] - P[13][9]*tempVar[7] + P[13][4]*tempVar[8]); + if (inhibitWindStates) { + K_LOS[1][14] = SK_LOS[0]*(P[14][0]*tempVar[1] + P[14][1]*tempVar[2] - P[14][2]*tempVar[3] + P[14][3]*tempVar[4] + P[14][5]*tempVar[5] - P[14][6]*tempVar[6] - P[14][9]*tempVar[7] + P[14][4]*tempVar[8]); + K_LOS[1][15] = SK_LOS[0]*(P[15][0]*tempVar[1] + P[15][1]*tempVar[2] - P[15][2]*tempVar[3] + P[15][3]*tempVar[4] + P[15][5]*tempVar[5] - P[15][6]*tempVar[6] - P[15][9]*tempVar[7] + P[15][4]*tempVar[8]); + } else { + K_LOS[1][14] = 0.0f; + K_LOS[1][15] = 0.0f; + } + if (inhibitMagStates) { + K_LOS[1][16] = SK_LOS[0]*(P[16][0]*tempVar[1] + P[16][1]*tempVar[2] - P[16][2]*tempVar[3] + P[16][3]*tempVar[4] + P[16][5]*tempVar[5] - P[16][6]*tempVar[6] - P[16][9]*tempVar[7] + P[16][4]*tempVar[8]); + K_LOS[1][17] = SK_LOS[0]*(P[17][0]*tempVar[1] + P[17][1]*tempVar[2] - P[17][2]*tempVar[3] + P[17][3]*tempVar[4] + P[17][5]*tempVar[5] - P[17][6]*tempVar[6] - P[17][9]*tempVar[7] + P[17][4]*tempVar[8]); + K_LOS[1][18] = SK_LOS[0]*(P[18][0]*tempVar[1] + P[18][1]*tempVar[2] - P[18][2]*tempVar[3] + P[18][3]*tempVar[4] + P[18][5]*tempVar[5] - P[18][6]*tempVar[6] - P[18][9]*tempVar[7] + P[18][4]*tempVar[8]); + K_LOS[1][19] = SK_LOS[0]*(P[19][0]*tempVar[1] + P[19][1]*tempVar[2] - P[19][2]*tempVar[3] + P[19][3]*tempVar[4] + P[19][5]*tempVar[5] - P[19][6]*tempVar[6] - P[19][9]*tempVar[7] + P[19][4]*tempVar[8]); + K_LOS[1][20] = SK_LOS[0]*(P[20][0]*tempVar[1] + P[20][1]*tempVar[2] - P[20][2]*tempVar[3] + P[20][3]*tempVar[4] + P[20][5]*tempVar[5] - P[20][6]*tempVar[6] - P[20][9]*tempVar[7] + P[20][4]*tempVar[8]); + K_LOS[1][21] = SK_LOS[0]*(P[21][0]*tempVar[1] + P[21][1]*tempVar[2] - P[21][2]*tempVar[3] + P[21][3]*tempVar[4] + P[21][5]*tempVar[5] - P[21][6]*tempVar[6] - P[21][9]*tempVar[7] + P[21][4]*tempVar[8]); + } else { + for (uint8_t i = 16; i <= 21; i++) { + K_LOS[1][i] = 0.0f; + } + } -// // reset the observation index -// obsIndex = 0; -// fuseOptFlowData = false; -// } + // calculate variance and innovation for Y observation + varInnovOptFlow[1] = 1.0f/SK_LOS[1]; + innovOptFlow[1] = losPred[1] - flowRadXYcomp[1]; -// // Check the innovation for consistency and don't fuse if > 3Sigma -// if ((innovOptFlow[obsIndex]*innovOptFlow[obsIndex]/varInnovOptFlow[obsIndex]) < 9.0f) -// { -// // correct the state vector -// for (uint8_t j = 0; j < n_states; j++) -// { -// states[j] = states[j] - Kfusion[j] * innovOptFlow[obsIndex]; -// } -// // normalise the quaternion states -// float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); -// if (quatMag > 1e-12f) -// { -// for (uint8_t j= 0; j<=3; j++) -// { -// float quatMagInv = 1.0f/quatMag; -// states[j] = states[j] * quatMagInv; -// } -// } -// // correct the covariance P = (I - K*H)*P -// // take advantage of the empty columns in KH to reduce the -// // number of operations -// for (uint8_t i = 0; i < n_states; i++) -// { -// for (uint8_t j = 0; j <= 6; j++) -// { -// KH[i][j] = Kfusion[i] * H_LOS[j]; -// } -// for (uint8_t j = 7; j <= 8; j++) -// { -// KH[i][j] = 0.0f; -// } -// KH[i][9] = Kfusion[i] * H_LOS[9]; -// for (uint8_t j = 10; j <= 21; j++) -// { -// KH[i][j] = 0.0f; -// } -// KH[i][22] = Kfusion[i] * H_LOS[22]; -// } -// for (uint8_t i = 0; i < n_states; i++) -// { -// for (uint8_t j = 0; j < n_states; j++) -// { -// KHP[i][j] = 0.0f; -// for (uint8_t k = 0; k <= 6; k++) -// { -// KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; -// } -// KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j]; -// KHP[i][j] = KHP[i][j] + KH[i][22] * P[2][j]; -// } -// } -// } -// for (uint8_t i = 0; i < n_states; i++) -// { -// for (uint8_t j = 0; j < n_states; j++) -// { -// P[i][j] = P[i][j] - KHP[i][j]; -// } -// } -// ForceSymmetry(); -// ConstrainVariances(); -// } + } + + // loop through the X and Y observations and fuse them sequentially + for (uint8_t obsIndex = 0; obsIndex < 2; obsIndex++) { + // Check the innovation for consistency and don't fuse if > 5Sigma + if ((innovOptFlow[obsIndex]*innovOptFlow[obsIndex]/varInnovOptFlow[obsIndex]) < 25.0f) { + // correct the state vector + for (uint8_t j = 0; j < n_states; j++) + { + states[j] = states[j] - K_LOS[obsIndex][j] * innovOptFlow[obsIndex]; + } + // normalise the quaternion states + float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); + if (quatMag > 1e-12f) + { + for (uint8_t j= 0; j<=3; j++) + { + float quatMagInv = 1.0f/quatMag; + states[j] = states[j] * quatMagInv; + } + } + // correct the covariance P = (I - K*H)*P + // take advantage of the empty columns in KH to reduce the + // number of operations + for (uint8_t i = 0; i < n_states; i++) + { + for (uint8_t j = 0; j <= 6; j++) + { + KH[i][j] = K_LOS[obsIndex][i] * H_LOS[obsIndex][j]; + } + for (uint8_t j = 7; j <= 8; j++) + { + KH[i][j] = 0.0f; + } + KH[i][9] = K_LOS[obsIndex][i] * H_LOS[obsIndex][9]; + for (uint8_t j = 10; j <= 21; j++) + { + KH[i][j] = 0.0f; + } + } + for (uint8_t i = 0; i < n_states; i++) + { + for (uint8_t j = 0; j < n_states; j++) + { + KHP[i][j] = 0.0f; + for (uint8_t k = 0; k <= 6; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j]; + } + } + for (uint8_t i = 0; i < n_states; i++) + { + for (uint8_t j = 0; j < n_states; j++) + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + } + } + ForceSymmetry(); + ConstrainVariances(); + } } /* @@ -2192,27 +2016,29 @@ Estimation of optical flow sensor focal length scale factor and terrain height u This fiter requires optical flow rates that are not motion compensated Range to ground measurement is assumed to be via a narrow beam type sensor - eg laser */ -void AttPosEKF::GroundEKF() +void AttPosEKF::OpticalFlowEKF() { // propagate ground position state noise each time this is called using the difference in position since the last observations and an RMS gradient assumption // limit distance to prevent intialisation afer bad gps causing bad numerical conditioning if (!inhibitGndState) { float distanceTravelledSq; - distanceTravelledSq = sq(statesAtRngTime[7] - prevPosN) + sq(statesAtRngTime[8] - prevPosE); - prevPosN = statesAtRngTime[7]; - prevPosE = statesAtRngTime[8]; + if (fuseRngData) { + distanceTravelledSq = sq(statesAtRngTime[7] - prevPosN) + sq(statesAtRngTime[8] - prevPosE); + prevPosN = statesAtRngTime[7]; + prevPosE = statesAtRngTime[8]; + } else if (fuseOptFlowData) { + distanceTravelledSq = sq(statesAtFlowTime[7] - prevPosN) + sq(statesAtFlowTime[8] - prevPosE); + prevPosN = statesAtFlowTime[7]; + prevPosE = statesAtFlowTime[8]; + } else { + return; + } distanceTravelledSq = min(distanceTravelledSq, 100.0f); Popt[1][1] += (distanceTravelledSq * sq(gndHgtSigma)); } - // we aren't using optical flow measurements in this hacked implementation so set the covariances for this state to zero to avoid numerical problems - Popt[0][0] = 0.0f; - Popt[0][1] = 0.0f; - Popt[1][0] = 0.0f; - // Fuse range finder data - // Need to check that our range finder tilt angle is less than 30 degrees - float cosRngTilt = - Tbn.z.x * sinf(rngFinderPitch) + Tbn.z.z * cosf(rngFinderPitch); - if (useRangeFinder && fuseRngData && cosRngTilt > 0.87f) { + // fuse range finder data + if (fuseRngData) { float range; // range from camera to centre of image float q0; // quaternion at optical flow measurement time float q1; // quaternion at optical flow measurement time @@ -2266,10 +2092,7 @@ void AttPosEKF::GroundEKF() flowStates[i] -= K_RNG[i] * innovRng; } // constrain the states - - // constrain focal length to 0.1 to 10 mm flowStates[0] = ConstrainFloat(flowStates[0], 0.1f, 10.0f); - // constrain altitude flowStates[1] = maxf(flowStates[1], statesAtRngTime[9] + minFlowRng); // correct the covariance matrix @@ -2285,6 +2108,132 @@ void AttPosEKF::GroundEKF() Popt[1][0] = Popt[0][1]; } } + + if (fuseOptFlowData) { + Vector3f vel; // velocity of sensor relative to ground in NED axes + Vector3f relVelSensor; // velocity of sensor relative to ground in sensor axes + float losPred[2]; // predicted optical flow angular rate measurements + float range; // range from camera to centre of image + float q0; // quaternion at optical flow measurement time + float q1; // quaternion at optical flow measurement time + float q2; // quaternion at optical flow measurement time + float q3; // quaternion at optical flow measurement time + float HP[2]; + float SH_OPT[6]; + float SK_OPT[3]; + float K_OPT[2][2]; + float H_OPT[2][2]; + float nextPopt[2][2]; + + // propagate scale factor state noise + if (!inhibitScaleState) { + Popt[0][0] += 1e-8f; + } else { + Popt[0][0] = 0.0f; + } + + // Copy required states to local variable names + q0 = statesAtFlowTime[0]; + q1 = statesAtFlowTime[1]; + q2 = statesAtFlowTime[2]; + q3 = statesAtFlowTime[3]; + vel.x = statesAtFlowTime[4]; + vel.y = statesAtFlowTime[5]; + vel.z = statesAtFlowTime[6]; + + // constrain terrain height to be below the vehicle + flowStates[1] = maxf(flowStates[1], statesAtFlowTime[9] + minFlowRng); + + // estimate range to centre of image + range = (flowStates[1] - statesAtFlowTime[9]) / Tnb_flow.z.z; + + // calculate relative velocity in sensor frame + relVelSensor = Tnb_flow * vel; + + // divide velocity by range, subtract body rates and apply scale factor to + // get predicted sensed angular optical rates relative to X and Y sensor axes + losPred[0] = flowStates[0]*( relVelSensor.y / range) - omegaAcrossFlowTime[0]; + losPred[1] = flowStates[0]*(-relVelSensor.x / range) - omegaAcrossFlowTime[1]; + + // calculate innovations + auxFlowObsInnov[0] = losPred[0] - flowRadXY[0]; + auxFlowObsInnov[1] = losPred[1] - flowRadXY[1]; + + // calculate Kalman gains + SH_OPT[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3); + SH_OPT[1] = vel.x*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + vel.y*(2*q0*q3 + 2*q1*q2) - vel.z*(2*q0*q2 - 2*q1*q3); + SH_OPT[2] = vel.y*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) - vel.x*(2*q0*q3 - 2*q1*q2) + vel.z*(2*q0*q1 + 2*q2*q3); + SH_OPT[3] = statesAtFlowTime[9] - flowStates[1]; + SH_OPT[4] = 1.0f/sq(SH_OPT[3]); + SH_OPT[5] = 1.0f/SH_OPT[3]; + float SH015 = SH_OPT[0]*SH_OPT[1]*SH_OPT[5]; + float SH025 = SH_OPT[0]*SH_OPT[2]*SH_OPT[5]; + float SH014 = SH_OPT[0]*SH_OPT[1]*SH_OPT[4]; + float SH024 = SH_OPT[0]*SH_OPT[2]*SH_OPT[4]; + SK_OPT[0] = 1.0f/(R_LOS + SH015*(Popt[0][0]*SH015 + Popt[1][0]*flowStates[0]*SH014) + flowStates[0]*SH014*(Popt[0][1]*SH015 + Popt[1][1]*flowStates[0]*SH014)); + SK_OPT[1] = 1.0f/(R_LOS + SH025*(Popt[0][0]*SH025 + Popt[1][0]*flowStates[0]*SH024) + flowStates[0]*SH024*(Popt[0][1]*SH025 + Popt[1][1]*flowStates[0]*SH024)); + SK_OPT[2] = SH_OPT[0]; + if (!inhibitScaleState) { + K_OPT[0][0] = -SK_OPT[1]*(Popt[0][0]*SH_OPT[2]*SH_OPT[5]*SK_OPT[2] + Popt[0][1]*flowStates[0]*SH_OPT[2]*SH_OPT[4]*SK_OPT[2]); + K_OPT[0][1] = SK_OPT[0]*(Popt[0][0]*SH_OPT[1]*SH_OPT[5]*SK_OPT[2] + Popt[0][1]*flowStates[0]*SH_OPT[1]*SH_OPT[4]*SK_OPT[2]); + } else { + K_OPT[0][0] = 0.0f; + K_OPT[0][1] = 0.0f; + } + if (!inhibitGndState) { + K_OPT[1][0] = -SK_OPT[1]*(Popt[1][0]*SH_OPT[2]*SH_OPT[5]*SK_OPT[2] + Popt[1][1]*flowStates[0]*SH_OPT[2]*SH_OPT[4]*SK_OPT[2]); + K_OPT[1][1] = SK_OPT[0]*(Popt[1][0]*SH_OPT[1]*SH_OPT[5]*SK_OPT[2] + Popt[1][1]*flowStates[0]*SH_OPT[1]*SH_OPT[4]*SK_OPT[2]); + } else { + K_OPT[1][0] = 0.0f; + K_OPT[1][1] = 0.0f; + } + + // calculate innovation variances + auxFlowObsInnovVar[0] = 1.0f/SK_OPT[1]; + auxFlowObsInnovVar[1] = 1.0f/SK_OPT[0]; + + // calculate observations jacobians + H_OPT[0][0] = -SH025; + H_OPT[0][1] = -flowStates[0]*SH024; + H_OPT[1][0] = SH015; + H_OPT[1][1] = flowStates[0]*SH014; + + // Check the innovation for consistency and don't fuse if > threshold + for (uint8_t obsIndex = 0; obsIndex < 2; obsIndex++) { + + // calculate the innovation consistency test ratio + auxFlowTestRatio[obsIndex] = sq(auxFlowObsInnov[obsIndex]) / (sq(auxFlowInnovGate) * auxFlowObsInnovVar[obsIndex]); + if (auxFlowTestRatio[obsIndex] < 1.0f) { + // correct the state + for (uint8_t i = 0; i < 2 ; i++) { + flowStates[i] -= K_OPT[i][obsIndex] * auxFlowObsInnov[obsIndex]; + } + // constrain the states + flowStates[0] = ConstrainFloat(flowStates[0], 0.1f, 10.0f); + flowStates[1] = maxf(flowStates[1], statesAtFlowTime[9] + minFlowRng); + + // correct the covariance matrix + for (uint8_t i = 0; i < 2 ; i++) { + HP[i] = 0.0f; + for (uint8_t j = 0; j < 2 ; j++) { + HP[i] += H_OPT[obsIndex][j] * P[j][i]; + } + } + for (uint8_t i = 0; i < 2 ; i++) { + for (uint8_t j = 0; j < 2 ; j++) { + nextPopt[i][j] = P[i][j] - K_OPT[i][obsIndex] * HP[j]; + } + } + + // prevent the state variances from becoming negative and maintain symmetry + Popt[0][0] = maxf(nextPopt[0][0],0.0f); + Popt[1][1] = maxf(nextPopt[1][1],0.0f); + Popt[0][1] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]); + Popt[1][0] = Popt[0][1]; + } + } + } + } void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) @@ -2328,6 +2277,9 @@ void AttPosEKF::StoreStates(uint64_t timestamp_ms) { for (unsigned i=0; i 0) + { + for (unsigned i=0; i < 3; i++) { + omegaForFusion[i] += storedOmega[i][storeIndexLocal]; + } + sumIndex += 1; + } + } + if (sumIndex >= 1) { + for (unsigned i=0; i < 3; i++) { + omegaForFusion[i] = omegaForFusion[i] / float(sumIndex); + } + } else { + omegaForFusion[0] = angRate.x; + omegaForFusion[1] = angRate.y; + omegaForFusion[2] = angRate.z; + } +} + void AttPosEKF::quat2Tnb(Mat3f &Tnb, const float (&quat)[4]) { // Calculate the nav to body cosine matrix @@ -2502,6 +2481,7 @@ void AttPosEKF::calcLLH(float posNED[3], double &lat, double &lon, float &hgt, d void AttPosEKF::OnGroundCheck() { onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 8.0f)); + if (staticMode) { staticMode = (!refSet || (GPSstatus < GPS_FIX_3D)); } @@ -2517,12 +2497,25 @@ void AttPosEKF::OnGroundCheck() } else { inhibitMagStates = false; } - // don't update terrain offset state if on ground - if (onGround) { + // don't update terrain offset state if there is no range finder and flying at low velocity or without GPS + if ((onGround || !useGPS) && !useRangeFinder) { inhibitGndState = true; } else { inhibitGndState = false; } + // don't update terrain offset state if there is no range finder and flying at low velocity, or without GPS, as it is poorly observable + if ((onGround || (globalTimeStamp_ms - lastFixTime_ms) > 1000) && !useRangeFinder) { + inhibitGndState = true; + } else { + inhibitGndState = false; + } + // Don't update focal length offset state if there is no range finder or optical flow sensor + // we need both sensors to do this estimation + if (!useRangeFinder || !useOpticalFlow) { + inhibitScaleState = true; + } else { + inhibitScaleState = false; + } } void AttPosEKF::calcEarthRateNED(Vector3f &omega, float latitude) @@ -2558,7 +2551,9 @@ void AttPosEKF::CovarianceInit() P[19][19] = sq(0.02f); P[20][20] = P[19][19]; P[21][21] = P[19][19]; - P[22][22] = sq(0.5f); + + fScaleFactorVar = 0.001f; // focal length scale factor variance + Popt[0][0] = 0.001f; } float AttPosEKF::ConstrainFloat(float val, float min_val, float max_val) @@ -2596,7 +2591,6 @@ void AttPosEKF::ConstrainVariances() // 14-15: Wind Vector - m/sec (North,East) // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) - // 22: Terrain offset - m // Constrain quaternion variances for (unsigned i = 0; i <= 3; i++) { @@ -2636,8 +2630,6 @@ void AttPosEKF::ConstrainVariances() P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); } - // Constrain terrain offset variance - P[22][22] = ConstrainFloat(P[22][22], 0.0f, 10000.0f); } void AttPosEKF::ConstrainStates() @@ -2655,7 +2647,6 @@ void AttPosEKF::ConstrainStates() // 14-15: Wind Vector - m/sec (North,East) // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) - // 22: Terrain offset - m // Constrain quaternion for (unsigned i = 0; i <= 3; i++) { @@ -2673,7 +2664,8 @@ void AttPosEKF::ConstrainStates() } // Constrain altitude - states[9] = ConstrainFloat(states[9], -4.0e4f, 1.0e4f); + // NOT FOR FLIGHT : Upper value of 0.0 is a temporary fix to get around lack of range finder data during development testing + states[9] = ConstrainFloat(states[9], -4.0e4f, 4.0e4f); // Angle bias limit - set to 8 degrees / sec for (unsigned i = 10; i <= 12; i++) { @@ -2699,9 +2691,6 @@ void AttPosEKF::ConstrainStates() states[i] = ConstrainFloat(states[i], -0.5f, 0.5f); } - // Constrain terrain offset - states[22] = ConstrainFloat(states[22], -1000.0f, 1000.0f); - } void AttPosEKF::ForceSymmetry() @@ -3162,12 +3151,14 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) states[19] = magBias.x; // Magnetic Field Bias X states[20] = magBias.y; // Magnetic Field Bias Y states[21] = magBias.z; // Magnetic Field Bias Z - states[22] = 0.0f; // terrain height ResetVelocity(); ResetPosition(); ResetHeight(); + // initialise focal length scale factor estimator states + flowStates[0] = 1.0f; + statesInitialised = true; // initialise the covariance matrix @@ -3223,6 +3214,9 @@ void AttPosEKF::ZeroVariables() states[i] = 0.0f; // state matrix } + // initialise the variables for the focal length scale factor to unity + flowStates[0] = 1.0f; + correctedDelAng.zero(); summedDelAng.zero(); summedDelVel.zero(); @@ -3230,12 +3224,9 @@ void AttPosEKF::ZeroVariables() dVelIMU.zero(); lastGyroOffset.zero(); - windSpdFiltNorth = 0.0f; - windSpdFiltEast = 0.0f; - // setting the altitude to zero will give us a higher - // gain to adjust faster in the first step - windSpdFiltAltitude = 0.0f; - windSpdFiltClimb = 0.0f; + // initialise states used by OpticalFlowEKF + flowStates[0] = 1.0f; + flowStates[1] = 0.0f; for (unsigned i = 0; i < data_buffer_size; i++) { diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h similarity index 91% rename from src/modules/ekf_att_pos_estimator/estimator_23states.h rename to src/modules/ekf_att_pos_estimator/estimator_22states.h index a607955a89..b1d71bd748 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -2,7 +2,7 @@ #include "estimator_utilities.h" -const unsigned int n_states = 23; +const unsigned int n_states = 22; const unsigned int data_buffer_size = 50; class AttPosEKF { @@ -29,10 +29,6 @@ public: float covDelAngMax; // maximum delta angle between covariance predictions float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis. - float a1; // optical flow sensor misalgnment angle about X axis (rad) - float a2; // optical flow sensor misalgnment angle about Y axis (rad) - float a3; // optical flow sensor misalgnment angle about Z axis (rad) - float yawVarScale; float windVelSigma; float dAngBiasSigma; @@ -59,9 +55,6 @@ public: covDelAngMax = 0.02f; // maximum delta angle between covariance predictions rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis. EAS2TAS = 1.0f; - a1 = 0.0f; // optical flow sensor misalgnment angle about X axis (rad) - a2 = 0.0f; // optical flow sensor misalgnment angle about Y axis (rad) - a3 = 0.0f; // optical flow sensor misalgnment angle about Z axis (rad) yawVarScale = 1.0f; windVelSigma = 0.1f; @@ -69,7 +62,6 @@ public: dVelBiasSigma = 1e-4f; magEarthSigma = 3.0e-4f; magBodySigma = 3.0e-4f; - gndHgtSigma = 0.02f; // assume 2% terrain gradient 1-sigma vneSigma = 0.2f; vdSigma = 0.3f; @@ -82,12 +74,13 @@ public: accelProcessNoise = 0.5f; gndHgtSigma = 0.1f; // terrain gradient 1-sigma - R_LOS = 0.03f; // optical flow measurement noise variance (rad/sec)^2 + R_LOS = 0.3f; // optical flow measurement noise variance (rad/sec)^2 flowInnovGate = 3.0f; // number of standard deviations applied to the optical flow innovation consistency check auxFlowInnovGate = 10.0f; // number of standard deviations applied to the optical flow innovation consistency check used by the auxiliary filter - rngInnovGate = 10.0f; // number of standard deviations applied to the rnage finder innovation consistency check - minFlowRng = 0.01f; //minimum range between ground and flow sensor - moCompR_LOS = 0.2; // scaler from sensor gyro rate to uncertainty in LOS rate + rngInnovGate = 5.0f; // number of standard deviations applied to the range finder innovation consistency check + minFlowRng = 0.3f; //minimum range between ground and flow sensor + moCompR_LOS = 0.0; // scaler from sensor gyro rate to uncertainty in LOS rate + } struct mag_state_struct { @@ -134,6 +127,7 @@ public: float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time float statesAtRngTime[n_states]; // filter states at the effective measurement time float statesAtFlowTime[n_states]; // States at the effective optical flow measurement time + float omegaAcrossFlowTime[3]; // angular rates at the effective optical flow measurement time Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) @@ -145,7 +139,7 @@ public: Vector3f lastGyroOffset; // Last gyro offset Vector3f delAngTotal; - Mat3f Tbn; // transformation matrix from body to NED coordinates + Mat3f Tbn; // transformation matrix from body to NED coordinatesFuseOptFlow Mat3f Tnb; // transformation amtrix from NED to body coordinates Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2) @@ -157,10 +151,6 @@ public: float dtVelPosFilt; // average time between position / velocity fusion steps float dtHgtFilt; // average time between height measurement updates float dtGpsFilt; // average time between gps measurement updates - float windSpdFiltNorth; // average wind speed north component - float windSpdFiltEast; // average wind speed east component - float windSpdFiltAltitude; // the last altitude used to filter wind speed - float windSpdFiltClimb; // filtered climb rate uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity float innovVelPos[6]; // innovation output float varInnovVelPos[6]; // innovation variance output @@ -175,7 +165,8 @@ public: float innovMag[3]; // innovation output float varInnovMag[3]; // innovation variance output Vector3f magData; // magnetometer flux radings in X,Y,Z body axes - float losData[2]; // optical flow LOS rate measurements (rad/sec) + float flowRadXYcomp[2]; // motion compensated optical flow angular rates(rad/sec) + float flowRadXY[2]; // raw (non motion compensated) optical flow angular rates (rad/sec) float innovVtas; // innovation output float innovRng; ///< Range finder innovation float innovOptFlow[2]; // optical flow LOS innovations (rad/sec) @@ -190,6 +181,8 @@ public: bool refSet; ///< flag to indicate if the reference position has been set Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes unsigned covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction + uint32_t lastFixTime_ms; // Number of msec since last GPS Fix that was used + uint32_t globalTimeStamp_ms; // time in msec of current prediction cycle // GPS input data variables double gpsLat; @@ -217,6 +210,7 @@ public: bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying) bool staticMode; ///< boolean true if no position feedback is fused + bool useGPS; // boolean true if GPS data is being used bool useAirspeed; ///< boolean true if airspeed data is being used bool useCompass; ///< boolean true if magnetometer data is being used bool useRangeFinder; ///< true when rangefinder is being used @@ -267,11 +261,9 @@ void FuseMagnetometer(); void FuseAirspeed(); -void FuseRangeFinder(); - void FuseOptFlow(); -void GroundEKF(); +void OpticalFlowEKF(); void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); @@ -286,7 +278,7 @@ void StoreStates(uint64_t timestamp_ms); * Recall the state vector. * * Recalls the vector stored at closest time to the one specified by msec - * + *FuseOptFlow * @return zero on success, integer indicating the number of invalid states on failure. * Does only copy valid states, if the statesForFusion vector was initialized * correctly by the caller, the result can be safely used, but is a mixture @@ -295,6 +287,8 @@ void StoreStates(uint64_t timestamp_ms); */ int RecallStates(float *statesForFusion, uint64_t msec); +void RecallOmega(float *omegaForFusion, uint64_t msec); + void ResetStoredStates(); void quat2Tbn(Mat3f &TBodyNed, const float (&quat)[4]); @@ -325,7 +319,7 @@ void CovarianceInit(); void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination); -float ConstrainFloat(float val, float min, float max); +float ConstrainFloat(float val, float min, float maxf); void ConstrainVariances(); diff --git a/src/modules/ekf_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk index 843dde9789..f519621137 100644 --- a/src/modules/ekf_att_pos_estimator/module.mk +++ b/src/modules/ekf_att_pos_estimator/module.mk @@ -39,7 +39,7 @@ MODULE_COMMAND = ekf_att_pos_estimator SRCS = ekf_att_pos_estimator_main.cpp \ ekf_att_pos_estimator_params.c \ - estimator_23states.cpp \ + estimator_22states.cpp \ estimator_utilities.cpp EXTRACXXFLAGS = -Weffc++ -Wframe-larger-than=6100 From ddf5be50e1058cbb18771393d3c70aed9d8655a6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 11 Jan 2015 16:10:47 +0100 Subject: [PATCH 003/269] README: Add link to contribution.md --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index 33b8cdec79..4ab1cbd2d8 100644 --- a/README.md +++ b/README.md @@ -21,6 +21,7 @@ Please refer to the [user documentation](https://pixhawk.org/users/start) for fl ### Developers ### Contributing guide: +https://github.com/PX4/Firmware/blob/master/CONTRIBUTING.md http://px4.io/dev/contributing Developer guide: From 9e42d5bed2c73ee49dd7692499e0de6c6ff76475 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 11 Jan 2015 16:13:17 +0100 Subject: [PATCH 004/269] README: Update contribution link style --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 4ab1cbd2d8..80f5a585fe 100644 --- a/README.md +++ b/README.md @@ -21,8 +21,8 @@ Please refer to the [user documentation](https://pixhawk.org/users/start) for fl ### Developers ### Contributing guide: -https://github.com/PX4/Firmware/blob/master/CONTRIBUTING.md -http://px4.io/dev/contributing + * [CONTRIBUTING.md](https://github.com/PX4/Firmware/blob/master/CONTRIBUTING.md) + * [PX4 Contribution Guide](http://px4.io/dev/contributing) Developer guide: http://px4.io/dev/ From 3c98c7119ebe956d1dc9c10379e9158567d16642 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Mon, 22 Dec 2014 13:47:20 -0700 Subject: [PATCH 005/269] use non-uniform 1st order IIR lowpass filters for baro_gps_offset estimation --- src/drivers/ms5611/ms5611.cpp | 2 +- .../mathlib/math/filter/LowPassFilter2p.cpp | 5 +- .../ekf_att_pos_estimator_main.cpp | 59 ++++++++++++++++--- 3 files changed, 57 insertions(+), 9 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 0a793cbc97..6eda6933d9 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -90,7 +90,7 @@ static const int ERROR = -1; /* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */ #define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */ -#define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ +#define MS5611_MEASUREMENT_RATIO 100 /* pressure measurements per temperature measurement */ #define MS5611_BARO_DEVICE_PATH_EXT "/dev/ms5611_ext" #define MS5611_BARO_DEVICE_PATH_INT "/dev/ms5611_int" diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp index 6f640c9f9a..c709a28348 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp @@ -66,6 +66,7 @@ float LowPassFilter2p::apply(float sample) // no filtering return sample; } + // do the filtering float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2; if (isnan(delay_element_0) || isinf(delay_element_0)) { @@ -82,7 +83,9 @@ float LowPassFilter2p::apply(float sample) } float LowPassFilter2p::reset(float sample) { - _delay_element_1 = _delay_element_2 = sample; + float dval = sample / (_b0 + _b1 + _b2); + _delay_element_1 = dval; + _delay_element_2 = dval; return apply(sample); } diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index c41777968e..1770a75e53 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -82,12 +82,11 @@ #include #include #include +#include #include #include "estimator_23states.h" - - /** * estimator app start / stop handling function * @@ -101,7 +100,7 @@ __EXPORT uint64_t getMicros(); static uint64_t IMUmsec = 0; static uint64_t IMUusec = 0; -static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; +static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; // units: microseconds uint32_t millis() { @@ -222,8 +221,10 @@ private: float _baro_ref; /**< barometer reference altitude */ float _baro_ref_offset; /**< offset between initial baro reference and GPS init baro altitude */ float _baro_gps_offset; /**< offset between baro altitude (at GPS init time) and GPS altitude */ + hrt_abstime _last_debug_print = 0; perf_counter_t _loop_perf; ///< loop performance counter + perf_counter_t _loop_intvl; ///< loop rate counter perf_counter_t _perf_gyro; ///gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; _ekf->gpsHgt = _gps.alt / 1e3f; + // update LPF + _gps_alt_filt += (1 - expf(-gps_elapsed * rc)) * (_gps_last - _gps_alt_filt); + _gps_last = _ekf->gpsHgt; + + //warnx("gps alt: %6.1f, interval: %6.3f", (double)_ekf->gpsHgt, (double)gps_elapsed); + // if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) { // _ekf->vneSigma = sqrtf(_gps.s_variance_m_s); // } else { @@ -1070,7 +1085,6 @@ FixedwingEstimator::task_main() // warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m); newDataGps = true; - last_gps = _gps.timestamp_position; } @@ -1082,15 +1096,16 @@ FixedwingEstimator::task_main() if (baro_updated) { - hrt_abstime baro_last = _baro.timestamp; - orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro); float baro_elapsed = (_baro.timestamp - baro_last) / 1e6f; + baro_last = _baro.timestamp; _ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f)); _ekf->baroHgt = _baro.altitude; + _baro_alt_filt += (1 - expf(-baro_elapsed * rc)) * (_baro_last - _baro_alt_filt); + _baro_last = _baro.altitude; if (!_baro_init) { _baro_ref = _baro.altitude; @@ -1180,6 +1195,24 @@ FixedwingEstimator::task_main() float initVelNED[3]; + // maintain filtered baro and gps altitudes to calculate weather offset + // baro sample rate is ~70Hz and measurement bandwidth is high + // gps sample rate is 5Hz and altitude is assumed accurate when averaged over 30 seconds + // maintain heavily filtered values for both baro and gps altitude + // Assume the filtered output should be identical for both sensors + _baro_gps_offset = _baro_alt_filt - _gps_alt_filt; + if (hrt_elapsed_time(&_last_debug_print) >= 5e6) { + _last_debug_print = hrt_absolute_time(); + perf_print_counter(_perf_baro); + perf_reset(_perf_baro); + warnx("gpsoff: %5.1f, baro_alt_filt: %6.1f, gps_alt_filt: %6.1f, gpos.alt: %5.1f, lpos.z: %6.1f", + (double)_baro_gps_offset, + (double)_baro_alt_filt, + (double)_gps_alt_filt, + (double)_global_pos.alt, + (double)_local_pos.z); + } + /* Initialize the filter first */ if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) { @@ -1195,7 +1228,13 @@ FixedwingEstimator::task_main() // Set up height correctly orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro); _baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame - _baro_gps_offset = _baro.altitude - gps_alt; + + // init filtered gps and baro altitudes + _gps_alt_filt = gps_alt; + _gps_last = gps_alt; + _baro_alt_filt = _baro.altitude; + _baro_last = _baro.altitude; + _ekf->baroHgt = _baro.altitude; _ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref)); @@ -1502,6 +1541,12 @@ FixedwingEstimator::task_main() _global_pos.timestamp = _local_pos.timestamp; + // FIXME: usurp terrain alt field for baro_gps_offset + _global_pos.terrain_alt = _baro_gps_offset; + _global_pos.terrain_alt_valid = true; + _global_pos.eph = _baro_alt_filt; + _global_pos.epv = _gps_alt_filt; + /* lazily publish the global position only once available */ if (_global_pos_pub > 0) { /* publish the global position */ From ea57dec24b14d94ec82f04b67bc31775b1f4a9cf Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Mon, 12 Jan 2015 17:15:38 -0700 Subject: [PATCH 006/269] revert to original ms5611 temperature sample rate --- src/drivers/ms5611/ms5611.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 6eda6933d9..0a793cbc97 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -90,7 +90,7 @@ static const int ERROR = -1; /* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */ #define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */ -#define MS5611_MEASUREMENT_RATIO 100 /* pressure measurements per temperature measurement */ +#define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ #define MS5611_BARO_DEVICE_PATH_EXT "/dev/ms5611_ext" #define MS5611_BARO_DEVICE_PATH_INT "/dev/ms5611_int" From dba0a5a90fd9fb87ba5a0083b0a009e8ac74b0ea Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Mon, 12 Jan 2015 17:23:30 -0700 Subject: [PATCH 007/269] comment out debug warnx --- .../ekf_att_pos_estimator_main.cpp | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 1770a75e53..8acf09977e 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1201,17 +1201,17 @@ FixedwingEstimator::task_main() // maintain heavily filtered values for both baro and gps altitude // Assume the filtered output should be identical for both sensors _baro_gps_offset = _baro_alt_filt - _gps_alt_filt; - if (hrt_elapsed_time(&_last_debug_print) >= 5e6) { - _last_debug_print = hrt_absolute_time(); - perf_print_counter(_perf_baro); - perf_reset(_perf_baro); - warnx("gpsoff: %5.1f, baro_alt_filt: %6.1f, gps_alt_filt: %6.1f, gpos.alt: %5.1f, lpos.z: %6.1f", - (double)_baro_gps_offset, - (double)_baro_alt_filt, - (double)_gps_alt_filt, - (double)_global_pos.alt, - (double)_local_pos.z); - } +// if (hrt_elapsed_time(&_last_debug_print) >= 5e6) { +// _last_debug_print = hrt_absolute_time(); +// perf_print_counter(_perf_baro); +// perf_reset(_perf_baro); +// warnx("gpsoff: %5.1f, baro_alt_filt: %6.1f, gps_alt_filt: %6.1f, gpos.alt: %5.1f, lpos.z: %6.1f", +// (double)_baro_gps_offset, +// (double)_baro_alt_filt, +// (double)_gps_alt_filt, +// (double)_global_pos.alt, +// (double)_local_pos.z); +// } /* Initialize the filter first */ if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) { From b795705640c6a14ba359e381c935351f9377aea9 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Tue, 13 Jan 2015 08:53:11 -0700 Subject: [PATCH 008/269] improve efficiency of non-uniform rate LPFs --- .../ekf_att_pos_estimator_main.cpp | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 8acf09977e..4ee2b70e65 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -786,8 +786,7 @@ FixedwingEstimator::task_main() // init lowpass filters for baro and gps altitude float _gps_alt_filt = 0, _baro_alt_filt = 0; - float _gps_last = 0, _baro_last = 0; - float rc = (1.0f/10); // actually 1/RC time constant of 1st order LPF + float rc = 10.0f; // RC time constant of 1st order LPF in seconds hrt_abstime baro_last = 0; _task_running = true; @@ -1065,8 +1064,7 @@ FixedwingEstimator::task_main() _ekf->gpsHgt = _gps.alt / 1e3f; // update LPF - _gps_alt_filt += (1 - expf(-gps_elapsed * rc)) * (_gps_last - _gps_alt_filt); - _gps_last = _ekf->gpsHgt; + _gps_alt_filt += (gps_elapsed / (rc + gps_elapsed)) * (_ekf->gpsHgt - _gps_alt_filt); //warnx("gps alt: %6.1f, interval: %6.3f", (double)_ekf->gpsHgt, (double)gps_elapsed); @@ -1104,8 +1102,7 @@ FixedwingEstimator::task_main() _ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f)); _ekf->baroHgt = _baro.altitude; - _baro_alt_filt += (1 - expf(-baro_elapsed * rc)) * (_baro_last - _baro_alt_filt); - _baro_last = _baro.altitude; + _baro_alt_filt += (baro_elapsed/(rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt); if (!_baro_init) { _baro_ref = _baro.altitude; @@ -1231,9 +1228,7 @@ FixedwingEstimator::task_main() // init filtered gps and baro altitudes _gps_alt_filt = gps_alt; - _gps_last = gps_alt; _baro_alt_filt = _baro.altitude; - _baro_last = _baro.altitude; _ekf->baroHgt = _baro.altitude; _ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref)); From d0af62783d1b18da6a15cb2da63e7ea88f5c398a Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 11:39:22 +0100 Subject: [PATCH 009/269] uORB: Added vehicle_landed uORB topic --- src/modules/uORB/objects_common.cpp | 3 +++ .../uORB/topics/vehicle_land_detected.h | 23 +++++++++++++++++++ 2 files changed, 26 insertions(+) create mode 100644 src/modules/uORB/topics/vehicle_land_detected.h diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 293412455e..78fdf4de75 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -83,6 +83,9 @@ ORB_DEFINE(sensor_combined, struct sensor_combined_s); #include "topics/vehicle_gps_position.h" ORB_DEFINE(vehicle_gps_position, struct vehicle_gps_position_s); +#include "topics/vehicle_land_detected.h" +ORB_DEFINE(vehicle_land_detected, struct vehicle_land_detected_s); + #include "topics/satellite_info.h" ORB_DEFINE(satellite_info, struct satellite_info_s); diff --git a/src/modules/uORB/topics/vehicle_land_detected.h b/src/modules/uORB/topics/vehicle_land_detected.h new file mode 100644 index 0000000000..0de29498d4 --- /dev/null +++ b/src/modules/uORB/topics/vehicle_land_detected.h @@ -0,0 +1,23 @@ +#ifndef __TOPIC_VEHICLE_LANDED_H__ +#define __TOPIC_VEHICLE_LANDED_H__ + +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +struct vehicle_land_detected_s { + uint64_t timestamp; /**< timestamp of the setpoint */ + bool landed; /**< true if vehicle is currently landed on the ground*/ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_land_detected); + +#endif //__TOPIC_VEHICLE_LANDED_H__ From 642063c3b8fdcd3f5e748666d1bb0412ea434b5f Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 11:40:11 +0100 Subject: [PATCH 010/269] LandDetector: Added crude land detectors for multicopter and fixedwing --- makefiles/config_px4fmu-v2_default.mk | 6 + .../FixedwingLandDetector.cpp | 171 ++++++++++++++ .../fw_land_detector/FixedwingLandDetector.h | 107 +++++++++ .../fw_land_detector_main.cpp | 195 ++++++++++++++++ src/modules/fw_land_detector/module.mk | 10 + .../MulticopterLandDetector.cpp | 212 ++++++++++++++++++ .../MulticopterLandDetector.h | 116 ++++++++++ .../mc_land_detector_main.cpp | 195 ++++++++++++++++ src/modules/mc_land_detector/module.mk | 10 + 9 files changed, 1022 insertions(+) create mode 100644 src/modules/fw_land_detector/FixedwingLandDetector.cpp create mode 100644 src/modules/fw_land_detector/FixedwingLandDetector.h create mode 100644 src/modules/fw_land_detector/fw_land_detector_main.cpp create mode 100644 src/modules/fw_land_detector/module.mk create mode 100644 src/modules/mc_land_detector/MulticopterLandDetector.cpp create mode 100644 src/modules/mc_land_detector/MulticopterLandDetector.h create mode 100644 src/modules/mc_land_detector/mc_land_detector_main.cpp create mode 100644 src/modules/mc_land_detector/module.mk diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 577140f054..8ef1c333ae 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -72,6 +72,12 @@ MODULES += modules/mavlink MODULES += modules/gpio_led MODULES += modules/uavcan +# +# Vehicle land detection +# +MODULES += modules/mc_land_detector +MODULES += modules/fw_land_detector + # # Estimation modules (EKF/ SO3 / other filters) # diff --git a/src/modules/fw_land_detector/FixedwingLandDetector.cpp b/src/modules/fw_land_detector/FixedwingLandDetector.cpp new file mode 100644 index 0000000000..6f5eca3ce0 --- /dev/null +++ b/src/modules/fw_land_detector/FixedwingLandDetector.cpp @@ -0,0 +1,171 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file FixedwingLandDetector.cpp + * Land detection algorithm for fixedwings + * + * @author Johan Jansen + */ + +#include "FixedwingLandDetector.h" + +#include +#include +#include +#include //usleep + +FixedwingLandDetector::FixedwingLandDetector() : + _landDetectedPub(-1), + _landDetected({0,false}), + + _vehicleLocalPositionSub(-1), + _vehicleLocalPosition({}), + _airspeedSub(-1), + _airspeed({}), + + _velocity_xy_filtered(0.0f), + _velocity_z_filtered(0.0f), + _airspeed_filtered(0.0f), + _landDetectTrigger(0), + + _taskShouldExit(false), + _taskIsRunning(false) +{ + //Advertise the first land detected uORB + _landDetected.timestamp = hrt_absolute_time(); + _landDetected.landed = false; + _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); +} + +FixedwingLandDetector::~FixedwingLandDetector() +{ + _taskShouldExit = true; + close(_landDetectedPub); + } + +void FixedwingLandDetector::shutdown() +{ + _taskShouldExit = true; +} + +/** +* @brief Convinience function for polling uORB subscriptions +* @return true if there was new data and it was successfully copied +**/ +static bool orb_update(const struct orb_metadata *meta, int handle, void *buffer) +{ + bool newData = false; + + //Check if there is new data to grab + if(orb_check(handle, &newData) != OK) { + return false; + } + + if(!newData) { + return false; + } + + if(orb_copy(meta, handle, buffer) != OK) { + return false; + } + + return true; +} + +void FixedwingLandDetector::updateSubscriptions() +{ + orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition); + orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); +} + +void FixedwingLandDetector::landDetectorLoop() +{ + //This should never happen! + if(_taskIsRunning) return; + + //Subscribe to local position and airspeed data + _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position)); + _airspeedSub = orb_subscribe(ORB_ID(airspeed)); + + _taskIsRunning = true; + _taskShouldExit = false; + while (!_taskShouldExit) { + + //First poll for new data from our subscriptions + updateSubscriptions(); + + const uint64_t now = hrt_absolute_time(); + bool landDetected = false; + + //TODO: reset filtered values on arming? + _velocity_xy_filtered = 0.95f*_velocity_xy_filtered + 0.05f*sqrtf(_vehicleLocalPosition.vx*_vehicleLocalPosition.vx + _vehicleLocalPosition.vy*_vehicleLocalPosition.vy); + _velocity_z_filtered = 0.95f*_velocity_z_filtered + 0.05f*fabsf(_vehicleLocalPosition.vz); + _airspeed_filtered = 0.95f*_airspeed_filtered + 0.05f*_airspeed.true_airspeed_m_s; + + /* crude land detector for fixedwing */ + if (_velocity_xy_filtered < FW_LAND_DETECTOR_VELOCITY_MAX + && _velocity_z_filtered < FW_LAND_DETECTOR_CLIMBRATE_MAX + && _airspeed_filtered < FW_LAND_DETECTOR_AIRSPEED_MAX) { + + //These conditions need to be stable for a period of time before we trust them + if(now > _landDetectTrigger) { + landDetected = true; + } + } + else { + //reset land detect trigger + _landDetectTrigger = now + FW_LAND_DETECTOR_TRIGGER_TIME; + } + + //Publish if land detection state has changed + if(_landDetected.landed != landDetected) { + _landDetected.timestamp = now; + _landDetected.landed = landDetected; + + /* publish the land detected broadcast */ + orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); + } + + //Limit loop rate + usleep(1000000 / FW_LAND_DETECTOR_UPDATE_RATE); + } + + _taskIsRunning = false; + _exit(0); +} + +bool FixedwingLandDetector::isRunning() const +{ + return _taskIsRunning; +} diff --git a/src/modules/fw_land_detector/FixedwingLandDetector.h b/src/modules/fw_land_detector/FixedwingLandDetector.h new file mode 100644 index 0000000000..6a0f951dd1 --- /dev/null +++ b/src/modules/fw_land_detector/FixedwingLandDetector.h @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file FixedwingLandDetector.h + * Land detection algorithm for multicopters + * + * @author Johan Jansen + * @author Morten Lysgaard + */ + +#ifndef __FIXED_WING_LAND_DETECTOR_H__ +#define __FIXED_WING_LAND_DETECTOR_H__ + +#include +#include +#include +#include + +class FixedwingLandDetector { +public: + FixedwingLandDetector(); + ~FixedwingLandDetector(); + + /** + * @brief Executes the main loop of the land detector in a separate deamon thread + * @returns OK if task was successfully launched + **/ + int createDeamonThread(); + + /** + * @returns true if this task is currently running + **/ + bool isRunning() const; + + /** + * @brief blocking loop, should be run in a separate thread or task. Runs at 50Hz + **/ + void landDetectorLoop(); + + /** + * @brief Tells the Land Detector task that it should exit + **/ + void shutdown(); + +protected: + /** + * @brief polls all subscriptions and pulls any data that has changed + **/ + void updateSubscriptions(); + + //Algorithm parameters (TODO: should probably be externalized) + static constexpr uint32_t FW_LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */ + static constexpr uint64_t FW_LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold before triggering a land */ + static constexpr float FW_LAND_DETECTOR_VELOCITY_MAX = 5.0f; /**< maximum horizontal movement m/s*/ + static constexpr float FW_LAND_DETECTOR_CLIMBRATE_MAX = 10.00f; /**< +- climb rate in m/s*/ + static constexpr float FW_LAND_DETECTOR_AIRSPEED_MAX = 10.00f; /**< airspeed max m/s*/ + +private: + orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ + struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ + + int _vehicleLocalPositionSub; /**< notification of local position */ + struct vehicle_local_position_s _vehicleLocalPosition; /**< the result from local position subscription */ + int _airspeedSub; + struct airspeed_s _airspeed; + + float _velocity_xy_filtered; + float _velocity_z_filtered; + float _airspeed_filtered; + uint64_t _landDetectTrigger; + + bool _taskShouldExit; /**< true if it is requested that this task should exit */ + bool _taskIsRunning; /**< task has reached main loop and is currently running */ +}; + +#endif //__FIXED_WING_LAND_DETECTOR_H__ diff --git a/src/modules/fw_land_detector/fw_land_detector_main.cpp b/src/modules/fw_land_detector/fw_land_detector_main.cpp new file mode 100644 index 0000000000..a9eefc4064 --- /dev/null +++ b/src/modules/fw_land_detector/fw_land_detector_main.cpp @@ -0,0 +1,195 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file fw_land_detector_main.cpp + * Land detection algorithm for fixed wings + * + * @author Johan Jansen + */ + +#include //usleep +#include +#include +#include +#include +#include +#include //Scheduler +#include //print to console + +#include "FixedwingLandDetector.h" + +//Function prototypes +static int fw_land_detector_start(); +static void fw_land_detector_stop(); + +/** + * land detector app start / stop handling function + * This makes the land detector module accessible from the nuttx shell + * @ingroup apps + */ +extern "C" __EXPORT int fw_land_detector_main(int argc, char *argv[]); + +//Private variables +static FixedwingLandDetector* fw_land_detector_task = nullptr; +static int _landDetectorTaskID = -1; + +/** +* Deamon thread function +**/ +static void fw_land_detector_deamon_thread(int argc, char *argv[]) +{ + fw_land_detector_task->landDetectorLoop(); +} + +/** +* Stop the task, force killing it if it doesn't stop by itself +**/ +static void fw_land_detector_stop() +{ + if(fw_land_detector_task == nullptr || _landDetectorTaskID == -1) { + errx(1, "not running"); + return; + } + + fw_land_detector_task->shutdown(); + + //Wait for task to die + int i = 0; + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_landDetectorTaskID); + break; + } + } while (fw_land_detector_task->isRunning()); + + + delete fw_land_detector_task; + fw_land_detector_task = nullptr; + _landDetectorTaskID = -1; + warn("fw_land_detector has been stopped"); +} + +/** +* Start new task, fails if it is already running. Returns OK if successful +**/ +static int fw_land_detector_start() +{ + if(fw_land_detector_task != nullptr || _landDetectorTaskID != -1) { + errx(1, "already running"); + return -1; + } + + //Allocate memory + fw_land_detector_task = new FixedwingLandDetector(); + if (fw_land_detector_task == nullptr) { + errx(1, "alloc failed"); + return -1; + } + + //Start new thread task + _landDetectorTaskID = task_spawn_cmd("fw_land_detector", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 1024, + (main_t)&fw_land_detector_deamon_thread, + nullptr); + + if (_landDetectorTaskID < 0) { + errx(1, "task start failed: %d", -errno); + return -1; + } + + /* avoid memory fragmentation by not exiting start handler until the task has fully started */ + const uint32_t timeout = hrt_absolute_time() + 5000000; //5 second timeout + while (!fw_land_detector_task->isRunning()) { + usleep(50000); + printf("."); + fflush(stdout); + + if(hrt_absolute_time() > timeout) { + err(1, "start failed - timeout"); + fw_land_detector_stop(); + exit(1); + } + } + printf("\n"); + + exit(0); + return 0; +} + +/** +* Main entry point for this module +**/ +int fw_land_detector_main(int argc, char *argv[]) +{ + + if (argc < 1) { + warnx("usage: fw_land_detector {start|stop|status}"); + exit(0); + } + + if (!strcmp(argv[1], "start")) { + fw_land_detector_start(); + } + + if (!strcmp(argv[1], "stop")) { + fw_land_detector_stop(); + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (fw_land_detector_task) { + + if(fw_land_detector_task->isRunning()) { + warnx("running"); + } + else { + errx(1, "exists, but not running"); + } + + exit(0); + + } else { + errx(1, "not running"); + } + } + + warn("usage: fw_land_detector {start|stop|status}"); + return 1; +} diff --git a/src/modules/fw_land_detector/module.mk b/src/modules/fw_land_detector/module.mk new file mode 100644 index 0000000000..894b29a7b5 --- /dev/null +++ b/src/modules/fw_land_detector/module.mk @@ -0,0 +1,10 @@ +# +# Fixedwing land detector +# + +MODULE_COMMAND = fw_land_detector + +SRCS = fw_land_detector_main.cpp \ + FixedwingLandDetector.cpp + +EXTRACXXFLAGS = -Weffc++ \ No newline at end of file diff --git a/src/modules/mc_land_detector/MulticopterLandDetector.cpp b/src/modules/mc_land_detector/MulticopterLandDetector.cpp new file mode 100644 index 0000000000..39064de6b5 --- /dev/null +++ b/src/modules/mc_land_detector/MulticopterLandDetector.cpp @@ -0,0 +1,212 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file MultiCopterLandDetector.cpp + * Land detection algorithm for multicopters + * + * @author Johan Jansen + * @author Morten Lysgaard + */ + +#include "MulticopterLandDetector.h" + +#include +#include +#include +#include //usleep + +MulticopterLandDetector::MulticopterLandDetector() : + _landDetectedPub(-1), + _landDetected({0,false}), + + _vehicleGlobalPositionSub(-1), + _sensorsCombinedSub(-1), + _waypointSub(-1), + _actuatorsSub(-1), + _armingSub(-1), + + _vehicleGlobalPosition({}), + _sensors({}), + _waypoint({}), + _actuators({}), + _arming({}), + + _taskShouldExit(false), + _taskIsRunning(false), + _landTimer(0) +{ + //Advertise the first land detected uORB + _landDetected.timestamp = hrt_absolute_time(); + _landDetected.landed = false; + _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); +} + +MulticopterLandDetector::~MulticopterLandDetector() +{ + _taskShouldExit = true; + close(_landDetectedPub); +} + +/** +* @brief Convinience function for polling uORB subscriptions +* @return true if there was new data and it was successfully copied +**/ +static bool orb_update(const struct orb_metadata *meta, int handle, void *buffer) +{ + bool newData = false; + + //Check if there is new data to grab + if(orb_check(handle, &newData) != OK) { + return false; + } + + if(!newData) { + return false; + } + + if(orb_copy(meta, handle, buffer) != OK) { + return false; + } + + return true; +} + +void MulticopterLandDetector::shutdown() +{ + _taskShouldExit = true; +} + +void MulticopterLandDetector::updateSubscriptions() +{ + orb_update(ORB_ID(vehicle_global_position), _vehicleGlobalPositionSub, &_vehicleGlobalPosition); + orb_update(ORB_ID(sensor_combined), _sensorsCombinedSub, &_sensors); + orb_update(ORB_ID(position_setpoint_triplet), _waypointSub, &_waypoint); + orb_update(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _actuatorsSub, &_actuators); + orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); +} + +void MulticopterLandDetector::landDetectorLoop() +{ + //This should never happen! + if(_taskIsRunning) return; + + //Subscribe to position, attitude, arming and velocity changes + _vehicleGlobalPositionSub = orb_subscribe(ORB_ID(vehicle_global_position)); + _sensorsCombinedSub = orb_subscribe(ORB_ID(sensor_combined)); + _waypointSub = orb_subscribe(ORB_ID(position_setpoint_triplet)); + _actuatorsSub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + _armingSub = orb_subscribe(ORB_ID(actuator_armed)); + + //Begin task + _taskIsRunning = true; + _taskShouldExit = false; + while (!_taskShouldExit) { + + //First poll for new data from our subscriptions + updateSubscriptions(); + + const uint64_t now = hrt_absolute_time(); + + //only detect landing if the autopilot is actively trying to land + if(!_waypoint.current.valid || _waypoint.current.type != SETPOINT_TYPE_LAND) { + _landTimer = now; + } + else { + /* + static int debugcnt = 0; + if(debugcnt++ > 12) { + debugcnt = 0; + mavlink_log_critical(_mavlinkFd, "T: %.4f R: %.4f", (double)_actuators.control[3], + sqrt( _sensors.gyro_rad_s[0]*_sensors.gyro_rad_s[0]+ + _sensors.gyro_rad_s[1]*_sensors.gyro_rad_s[1]+ + _sensors.gyro_rad_s[2]*_sensors.gyro_rad_s[2])); + } + */ + + //Check if we are moving vertically + bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > MC_LAND_DETECTOR_CLIMBRATE_MAX; + + bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n*_vehicleGlobalPosition.vel_n + + _vehicleGlobalPosition.vel_e*_vehicleGlobalPosition.vel_e) > MC_LAND_DETECTOR_VELOCITY_MAX; + + //Next look if all rotation angles are not moving + bool rotating = sqrtf(_sensors.gyro_rad_s[0]*_sensors.gyro_rad_s[0]+ + _sensors.gyro_rad_s[1]*_sensors.gyro_rad_s[1]+ + _sensors.gyro_rad_s[2]*_sensors.gyro_rad_s[2]) > MC_LAND_DETECTOR_ROTATION_MAX; + + //Check if thrust output is minimal (about half of default) + bool minimalThrust = _arming.armed && _actuators.control[3] <= MC_LAND_DETECTOR_THRUST_MAX; + + if(verticalMovement || rotating || !minimalThrust || horizontalMovement) { + //Sensed movement, so reset the land detector + _landTimer = now; + } + + } + + // if we have detected a landing for 2 continious seconds + if(now-_landTimer > 2000000) { + if(!_landDetected.landed) + { + _landDetected.timestamp = now; + _landDetected.landed = true; + + /* publish the land detected broadcast */ + orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); + } + } + else { + // if we currently think we have landed, but the latest data says we are flying + if(_landDetected.landed) + { + _landDetected.timestamp = now; + _landDetected.landed = false; + + /* publish the land detected broadcast */ + orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); + } + } + + //Limit loop rate + usleep(1000000 / MC_LAND_DETECTOR_UPDATE_RATE); + } + + _taskIsRunning = false; + _exit(0); +} + +bool MulticopterLandDetector::isRunning() const +{ + return _taskIsRunning; +} diff --git a/src/modules/mc_land_detector/MulticopterLandDetector.h b/src/modules/mc_land_detector/MulticopterLandDetector.h new file mode 100644 index 0000000000..7421898c12 --- /dev/null +++ b/src/modules/mc_land_detector/MulticopterLandDetector.h @@ -0,0 +1,116 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file MultiCopterLandDetector.h + * Land detection algorithm for multicopters + * + * @author Johan Jansen + * @author Morten Lysgaard + */ + +#ifndef __MULTICOPTER_LAND_DETECTOR_H__ +#define __MULTICOPTER_LAND_DETECTOR_H__ + +#include +#include +#include +#include +#include +#include +#include + +//TODO: add crash detection to this module? +class MulticopterLandDetector { +public: + MulticopterLandDetector(); + ~MulticopterLandDetector(); + + /** + * @brief Executes the main loop of the land detector in a separate deamon thread + * @returns OK if task was successfully launched + **/ + int createDeamonThread(); + + /** + * @returns true if this task is currently running + **/ + bool isRunning() const; + + /** + * @brief blocking loop, should be run in a separate thread or task. Runs at 50Hz + **/ + void landDetectorLoop(); + + /** + * @brief Tells the Land Detector task that it should exit + **/ + void shutdown(); + +protected: + /** + * @brief polls all subscriptions and pulls any data that has changed + **/ + void updateSubscriptions(); + + //Algorithm parameters (TODO: should probably be externalized) + static constexpr float MC_LAND_DETECTOR_CLIMBRATE_MAX = 0.30f; /**< max +- climb rate in m/s */ + static constexpr float MC_LAND_DETECTOR_ROTATION_MAX = 0.5f; /**< max rotation in rad/sec (= 30 deg/s) */ + static constexpr float MC_LAND_DETECTOR_THRUST_MAX = 0.2f; + static constexpr float MC_LAND_DETECTOR_VELOCITY_MAX = 1.0f; /**< max +- horizontal movement in m/s */ + static constexpr uint32_t MC_LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */ + static constexpr uint32_t MC_LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold before triggering a land */ + +private: + orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ + struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ + + int _vehicleGlobalPositionSub; /**< notification of global position */ + int _sensorsCombinedSub; + int _waypointSub; + int _actuatorsSub; + int _armingSub; + + struct vehicle_global_position_s _vehicleGlobalPosition; /**< the result from global position subscription */ + struct sensor_combined_s _sensors; /**< subscribe to sensor readings */ + struct position_setpoint_triplet_s _waypoint; /**< subscribe to autopilot navigation */ + struct actuator_controls_s _actuators; + struct actuator_armed_s _arming; + + bool _taskShouldExit; /**< true if it is requested that this task should exit */ + bool _taskIsRunning; /**< task has reached main loop and is currently running */ + + uint64_t _landTimer; /**< timestamp in microseconds since a possible land was detected*/ +}; + +#endif //__MULTICOPTER_LAND_DETECTOR_H__ diff --git a/src/modules/mc_land_detector/mc_land_detector_main.cpp b/src/modules/mc_land_detector/mc_land_detector_main.cpp new file mode 100644 index 0000000000..2992ead99c --- /dev/null +++ b/src/modules/mc_land_detector/mc_land_detector_main.cpp @@ -0,0 +1,195 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_land_detector_main.cpp + * Land detection algorithm for multicopters + * + * @author Johan Jansen + */ + +#include //usleep +#include +#include +#include +#include +#include +#include //Scheduler +#include //print to console + +#include "MulticopterLandDetector.h" + +//Function prototypes +static int mc_land_detector_start(); +static void mc_land_detector_stop(); + +/** + * land detector app start / stop handling function + * This makes the land detector module accessible from the nuttx shell + * @ingroup apps + */ +extern "C" __EXPORT int mc_land_detector_main(int argc, char *argv[]); + +//Private variables +static MulticopterLandDetector* mc_land_detector_task = nullptr; +static int _landDetectorTaskID = -1; + +/** +* Deamon thread function +**/ +static void mc_land_detector_deamon_thread(int argc, char *argv[]) +{ + mc_land_detector_task->landDetectorLoop(); +} + +/** +* Stop the task, force killing it if it doesn't stop by itself +**/ +static void mc_land_detector_stop() +{ + if(mc_land_detector_task == nullptr || _landDetectorTaskID == -1) { + errx(1, "not running"); + return; + } + + mc_land_detector_task->shutdown(); + + //Wait for task to die + int i = 0; + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_landDetectorTaskID); + break; + } + } while (mc_land_detector_task->isRunning()); + + + delete mc_land_detector_task; + mc_land_detector_task = nullptr; + _landDetectorTaskID = -1; + warn("mc_land_detector has been stopped"); +} + +/** +* Start new task, fails if it is already running. Returns OK if successful +**/ +static int mc_land_detector_start() +{ + if(mc_land_detector_task != nullptr || _landDetectorTaskID != -1) { + errx(1, "already running"); + return -1; + } + + //Allocate memory + mc_land_detector_task = new MulticopterLandDetector(); + if (mc_land_detector_task == nullptr) { + errx(1, "alloc failed"); + return -1; + } + + //Start new thread task + _landDetectorTaskID = task_spawn_cmd("mc_land_detector", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 1024, + (main_t)&mc_land_detector_deamon_thread, + nullptr); + + if (_landDetectorTaskID < 0) { + errx(1, "task start failed: %d", -errno); + return -1; + } + + /* avoid memory fragmentation by not exiting start handler until the task has fully started */ + const uint32_t timeout = hrt_absolute_time() + 5000000; //5 second timeout + while (!mc_land_detector_task->isRunning()) { + usleep(50000); + printf("."); + fflush(stdout); + + if(hrt_absolute_time() > timeout) { + err(1, "start failed - timeout"); + mc_land_detector_stop(); + exit(1); + } + } + printf("\n"); + + exit(0); + return 0; +} + +/** +* Main entry point for this module +**/ +int mc_land_detector_main(int argc, char *argv[]) +{ + + if (argc < 1) { + warnx("usage: mc_land_detector {start|stop|status}"); + exit(0); + } + + if (!strcmp(argv[1], "start")) { + mc_land_detector_start(); + } + + if (!strcmp(argv[1], "stop")) { + mc_land_detector_stop(); + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (mc_land_detector_task) { + + if(mc_land_detector_task->isRunning()) { + warnx("running"); + } + else { + errx(1, "exists, but not running"); + } + + exit(0); + + } else { + errx(1, "not running"); + } + } + + warn("usage: mc_land_detector {start|stop|status}"); + return 1; +} diff --git a/src/modules/mc_land_detector/module.mk b/src/modules/mc_land_detector/module.mk new file mode 100644 index 0000000000..023d48979a --- /dev/null +++ b/src/modules/mc_land_detector/module.mk @@ -0,0 +1,10 @@ +# +# Multirotor land detector +# + +MODULE_COMMAND = mc_land_detector + +SRCS = mc_land_detector_main.cpp \ + MulticopterLandDetector.cpp + +EXTRACXXFLAGS = -Weffc++ From cffba8440e056b71a01003a0d90a7e6f4cffd648 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 11:46:59 +0100 Subject: [PATCH 011/269] EKF: Removed the fixed wing land detector in the EKF module --- .../ekf_att_pos_estimator_main.cpp | 25 +------------------ 1 file changed, 1 insertion(+), 24 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index c41777968e..866e5dc841 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -290,10 +290,6 @@ private: AttPosEKF *_ekf; - float _velocity_xy_filtered; - float _velocity_z_filtered; - float _airspeed_filtered; - /** * Update our local parameter cache. */ @@ -422,10 +418,7 @@ FixedwingEstimator::FixedwingEstimator() : _mavlink_fd(-1), _parameters{}, _parameter_handles{}, - _ekf(nullptr), - _velocity_xy_filtered(0.0f), - _velocity_z_filtered(0.0f), - _airspeed_filtered(0.0f) + _ekf(nullptr) { _last_run = hrt_absolute_time(); @@ -1434,22 +1427,6 @@ FixedwingEstimator::task_main() _local_pos.v_z_valid = true; _local_pos.xy_global = true; - _velocity_xy_filtered = 0.95f*_velocity_xy_filtered + 0.05f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy); - _velocity_z_filtered = 0.95f*_velocity_z_filtered + 0.05f*fabsf(_local_pos.vz); - _airspeed_filtered = 0.95f*_airspeed_filtered + 0.05f*_airspeed.true_airspeed_m_s; - - - /* crude land detector for fixedwing only, - * TODO: adapt so that it works for both, maybe move to another location - */ - if (_velocity_xy_filtered < 5 - && _velocity_z_filtered < 10 - && _airspeed_filtered < 10) { - _local_pos.landed = true; - } else { - _local_pos.landed = false; - } - _local_pos.z_global = false; _local_pos.yaw = _att.yaw; From 051a6972281dc9f8c15b5d8c73ac808416944932 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 11:53:00 +0100 Subject: [PATCH 012/269] uORB: Added missing license header --- .../uORB/topics/vehicle_land_detected.h | 40 +++++++++++++++++++ 1 file changed, 40 insertions(+) diff --git a/src/modules/uORB/topics/vehicle_land_detected.h b/src/modules/uORB/topics/vehicle_land_detected.h index 0de29498d4..51b3568e82 100644 --- a/src/modules/uORB/topics/vehicle_land_detected.h +++ b/src/modules/uORB/topics/vehicle_land_detected.h @@ -1,3 +1,43 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vehicle_land_detected.h + * Land detected uORB topic + * + * @author Johan Jansen + */ + #ifndef __TOPIC_VEHICLE_LANDED_H__ #define __TOPIC_VEHICLE_LANDED_H__ From 3a4b3d094a07d41c84efabdc134763bec9372597 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 11:53:47 +0100 Subject: [PATCH 013/269] LandDetector: Removed commented debug info --- .../mc_land_detector/MulticopterLandDetector.cpp | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/src/modules/mc_land_detector/MulticopterLandDetector.cpp b/src/modules/mc_land_detector/MulticopterLandDetector.cpp index 39064de6b5..63dc54d5ef 100644 --- a/src/modules/mc_land_detector/MulticopterLandDetector.cpp +++ b/src/modules/mc_land_detector/MulticopterLandDetector.cpp @@ -143,16 +143,6 @@ void MulticopterLandDetector::landDetectorLoop() _landTimer = now; } else { - /* - static int debugcnt = 0; - if(debugcnt++ > 12) { - debugcnt = 0; - mavlink_log_critical(_mavlinkFd, "T: %.4f R: %.4f", (double)_actuators.control[3], - sqrt( _sensors.gyro_rad_s[0]*_sensors.gyro_rad_s[0]+ - _sensors.gyro_rad_s[1]*_sensors.gyro_rad_s[1]+ - _sensors.gyro_rad_s[2]*_sensors.gyro_rad_s[2])); - } - */ //Check if we are moving vertically bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > MC_LAND_DETECTOR_CLIMBRATE_MAX; From b5c7c6a15b4badf56c335746b32ef138afaca539 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 11:59:15 +0100 Subject: [PATCH 014/269] ROMFS: Added the respective land detector to the startup scripts --- ROMFS/px4fmu_common/init.d/rc.fw_apps | 9 +++++++++ ROMFS/px4fmu_common/init.d/rc.mc_apps | 5 +++++ 2 files changed, 14 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index 9aca3fc5f4..c6947009bb 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -13,3 +13,12 @@ ekf_att_pos_estimator start # fw_att_control start fw_pos_control_l1 start + +# +# Start Land Detector +# +fw_land_detector start + +# +# Misc apps +# \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 268eb9bba0..9c3d6352b3 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -10,3 +10,8 @@ position_estimator_inav start mc_att_control start mc_pos_control start + +# +# Start Land Detector +# +mc_land_detector start From eefbf366fbc2bef58f0bc283f7d02fb49023faa6 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 12:16:07 +0100 Subject: [PATCH 015/269] LandDetector: Fixed some typos and magic constant --- src/modules/fw_land_detector/FixedwingLandDetector.h | 3 +-- src/modules/mc_land_detector/MulticopterLandDetector.cpp | 5 +++-- src/modules/mc_land_detector/MulticopterLandDetector.h | 1 - 3 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/modules/fw_land_detector/FixedwingLandDetector.h b/src/modules/fw_land_detector/FixedwingLandDetector.h index 6a0f951dd1..9004457400 100644 --- a/src/modules/fw_land_detector/FixedwingLandDetector.h +++ b/src/modules/fw_land_detector/FixedwingLandDetector.h @@ -33,10 +33,9 @@ /** * @file FixedwingLandDetector.h - * Land detection algorithm for multicopters + * Land detection algorithm for fixedwing * * @author Johan Jansen - * @author Morten Lysgaard */ #ifndef __FIXED_WING_LAND_DETECTOR_H__ diff --git a/src/modules/mc_land_detector/MulticopterLandDetector.cpp b/src/modules/mc_land_detector/MulticopterLandDetector.cpp index 63dc54d5ef..90b36a795c 100644 --- a/src/modules/mc_land_detector/MulticopterLandDetector.cpp +++ b/src/modules/mc_land_detector/MulticopterLandDetector.cpp @@ -147,6 +147,7 @@ void MulticopterLandDetector::landDetectorLoop() //Check if we are moving vertically bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > MC_LAND_DETECTOR_CLIMBRATE_MAX; + //Check if we are moving horizontally bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n*_vehicleGlobalPosition.vel_n + _vehicleGlobalPosition.vel_e*_vehicleGlobalPosition.vel_e) > MC_LAND_DETECTOR_VELOCITY_MAX; @@ -165,8 +166,8 @@ void MulticopterLandDetector::landDetectorLoop() } - // if we have detected a landing for 2 continious seconds - if(now-_landTimer > 2000000) { + // if we have detected a landing for 2 continuous seconds + if(now-_landTimer > MC_LAND_DETECTOR_TRIGGER_TIME) { if(!_landDetected.landed) { _landDetected.timestamp = now; diff --git a/src/modules/mc_land_detector/MulticopterLandDetector.h b/src/modules/mc_land_detector/MulticopterLandDetector.h index 7421898c12..1aeaa0d62a 100644 --- a/src/modules/mc_land_detector/MulticopterLandDetector.h +++ b/src/modules/mc_land_detector/MulticopterLandDetector.h @@ -50,7 +50,6 @@ #include #include -//TODO: add crash detection to this module? class MulticopterLandDetector { public: MulticopterLandDetector(); From 28adc8850075da70206864c9f285456bb32c086c Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 12:25:18 +0100 Subject: [PATCH 016/269] Commander: Subscribe and use land detector --- src/modules/commander/commander.cpp | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f511f38762..142ff978c4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -84,6 +84,7 @@ #include #include #include + #include #include #include @@ -964,6 +965,11 @@ int commander_thread_main(int argc, char *argv[]) struct vehicle_local_position_s local_position; memset(&local_position, 0, sizeof(local_position)); + /* Subscribe to land detector */ + int land_detector_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); + struct vehicle_land_detected_s land_detector; + memset(&land_detector, 0, sizeof(land_detector)); + /* * The home position is set based on GPS only, to prevent a dependency between * position estimator and commander. RAW GPS is more than good enough for a @@ -1379,9 +1385,15 @@ int commander_thread_main(int argc, char *argv[]) check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); + /* Update land detector */ + orb_check(land_detector_sub, &updated); + if(updated) { + orb_copy(ORB_ID(vehicle_land_detected), land_detector_sub, &land_detector); + } + if (status.condition_local_altitude_valid) { - if (status.condition_landed != local_position.landed) { - status.condition_landed = local_position.landed; + if (status.condition_landed != land_detector.landed) { + status.condition_landed = land_detector.landed; status_changed = true; if (status.condition_landed) { From 98ab83142c1bf7e5170e7527dde1a9e5132b5422 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 12:31:05 +0100 Subject: [PATCH 017/269] InertialNav: Removed land detector from position estimator --- .../position_estimator_inav_main.c | 15 +++++++++------ src/modules/uORB/topics/vehicle_local_position.h | 2 +- 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 2f972fc9f2..b418e3a76b 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -246,9 +246,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float baro_offset = 0.0f; // baro offset for reference altitude, initialized on start, then adjusted float surface_offset = 0.0f; // ground level offset from reference altitude float surface_offset_rate = 0.0f; // surface offset change rate - float alt_avg = 0.0f; - bool landed = true; - hrt_abstime landed_time = 0; + //float alt_avg = 0.0f; + //bool landed = true; + //hrt_abstime landed_time = 0; hrt_abstime accel_timestamp = 0; hrt_abstime baro_timestamp = 0; @@ -1068,12 +1068,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v); } - /* detect land */ + +/* + // detect land alt_avg += (- z_est[0] - alt_avg) * dt / params.land_t; float alt_disp2 = - z_est[0] - alt_avg; alt_disp2 = alt_disp2 * alt_disp2; float land_disp2 = params.land_disp * params.land_disp; - /* get actual thrust output */ + // get actual thrust output float thrust = armed.armed ? actuator.control[3] : 0.0f; if (landed) { @@ -1097,6 +1099,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) landed_time = 0; } } +*/ if (verbose_mode) { /* print updates rate */ @@ -1148,7 +1151,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.vy = y_est[1]; local_pos.z = z_est[0]; local_pos.vz = z_est[1]; - local_pos.landed = landed; + local_pos.landed = false; local_pos.yaw = att.yaw; local_pos.dist_bottom_valid = dist_bottom_valid; local_pos.eph = eph; diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index 5d39c897d4..0e0bc9781a 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -77,7 +77,7 @@ struct vehicle_local_position_s { double ref_lat; /**< Reference point latitude in degrees */ double ref_lon; /**< Reference point longitude in degrees */ float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */ - bool landed; /**< true if vehicle is landed */ + bool landed; /**< true if vehicle is landed TODO: deprecated, use vehicle_land_detected instead (remove this line?)*/ /* Distance to surface */ float dist_bottom; /**< Distance to bottom surface (ground) */ float dist_bottom_rate; /**< Distance to bottom surface (ground) change rate */ From 546b5727b442ac7520d7ce72e15732378a1a0799 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 13:33:35 +0100 Subject: [PATCH 018/269] Formatting: Run AStyle to fix indenting --- .../FixedwingLandDetector.cpp | 156 +++++++------- .../fw_land_detector/FixedwingLandDetector.h | 88 ++++---- .../fw_land_detector_main.cpp | 172 +++++++-------- .../MulticopterLandDetector.cpp | 203 +++++++++--------- .../MulticopterLandDetector.h | 95 ++++---- .../mc_land_detector_main.cpp | 30 +-- 6 files changed, 378 insertions(+), 366 deletions(-) diff --git a/src/modules/fw_land_detector/FixedwingLandDetector.cpp b/src/modules/fw_land_detector/FixedwingLandDetector.cpp index 6f5eca3ce0..5fbeb2f45b 100644 --- a/src/modules/fw_land_detector/FixedwingLandDetector.cpp +++ b/src/modules/fw_land_detector/FixedwingLandDetector.cpp @@ -46,37 +46,37 @@ #include //usleep FixedwingLandDetector::FixedwingLandDetector() : - _landDetectedPub(-1), - _landDetected({0,false}), - - _vehicleLocalPositionSub(-1), - _vehicleLocalPosition({}), - _airspeedSub(-1), - _airspeed({}), + _landDetectedPub(-1), + _landDetected({0, false}), - _velocity_xy_filtered(0.0f), - _velocity_z_filtered(0.0f), - _airspeed_filtered(0.0f), - _landDetectTrigger(0), + _vehicleLocalPositionSub(-1), + _vehicleLocalPosition({}), + _airspeedSub(-1), + _airspeed({}), - _taskShouldExit(false), - _taskIsRunning(false) + _velocity_xy_filtered(0.0f), + _velocity_z_filtered(0.0f), + _airspeed_filtered(0.0f), + _landDetectTrigger(0), + + _taskShouldExit(false), + _taskIsRunning(false) { - //Advertise the first land detected uORB - _landDetected.timestamp = hrt_absolute_time(); - _landDetected.landed = false; - _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); + //Advertise the first land detected uORB + _landDetected.timestamp = hrt_absolute_time(); + _landDetected.landed = false; + _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); } FixedwingLandDetector::~FixedwingLandDetector() { - _taskShouldExit = true; - close(_landDetectedPub); - } + _taskShouldExit = true; + close(_landDetectedPub); +} void FixedwingLandDetector::shutdown() { - _taskShouldExit = true; + _taskShouldExit = true; } /** @@ -85,87 +85,89 @@ void FixedwingLandDetector::shutdown() **/ static bool orb_update(const struct orb_metadata *meta, int handle, void *buffer) { - bool newData = false; + bool newData = false; - //Check if there is new data to grab - if(orb_check(handle, &newData) != OK) { - return false; - } + //Check if there is new data to grab + if (orb_check(handle, &newData) != OK) { + return false; + } - if(!newData) { - return false; - } + if (!newData) { + return false; + } - if(orb_copy(meta, handle, buffer) != OK) { - return false; - } + if (orb_copy(meta, handle, buffer) != OK) { + return false; + } - return true; + return true; } void FixedwingLandDetector::updateSubscriptions() { - orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition); - orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); + orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition); + orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); } void FixedwingLandDetector::landDetectorLoop() { - //This should never happen! - if(_taskIsRunning) return; + //This should never happen! + if (_taskIsRunning) { return; } - //Subscribe to local position and airspeed data - _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position)); - _airspeedSub = orb_subscribe(ORB_ID(airspeed)); + //Subscribe to local position and airspeed data + _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position)); + _airspeedSub = orb_subscribe(ORB_ID(airspeed)); - _taskIsRunning = true; - _taskShouldExit = false; - while (!_taskShouldExit) { + _taskIsRunning = true; + _taskShouldExit = false; - //First poll for new data from our subscriptions - updateSubscriptions(); + while (!_taskShouldExit) { - const uint64_t now = hrt_absolute_time(); - bool landDetected = false; + //First poll for new data from our subscriptions + updateSubscriptions(); - //TODO: reset filtered values on arming? - _velocity_xy_filtered = 0.95f*_velocity_xy_filtered + 0.05f*sqrtf(_vehicleLocalPosition.vx*_vehicleLocalPosition.vx + _vehicleLocalPosition.vy*_vehicleLocalPosition.vy); - _velocity_z_filtered = 0.95f*_velocity_z_filtered + 0.05f*fabsf(_vehicleLocalPosition.vz); - _airspeed_filtered = 0.95f*_airspeed_filtered + 0.05f*_airspeed.true_airspeed_m_s; + const uint64_t now = hrt_absolute_time(); + bool landDetected = false; - /* crude land detector for fixedwing */ - if (_velocity_xy_filtered < FW_LAND_DETECTOR_VELOCITY_MAX - && _velocity_z_filtered < FW_LAND_DETECTOR_CLIMBRATE_MAX - && _airspeed_filtered < FW_LAND_DETECTOR_AIRSPEED_MAX) { + //TODO: reset filtered values on arming? + _velocity_xy_filtered = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx * + _vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy); + _velocity_z_filtered = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz); + _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; - //These conditions need to be stable for a period of time before we trust them - if(now > _landDetectTrigger) { - landDetected = true; - } - } - else { - //reset land detect trigger - _landDetectTrigger = now + FW_LAND_DETECTOR_TRIGGER_TIME; - } + /* crude land detector for fixedwing */ + if (_velocity_xy_filtered < FW_LAND_DETECTOR_VELOCITY_MAX + && _velocity_z_filtered < FW_LAND_DETECTOR_CLIMBRATE_MAX + && _airspeed_filtered < FW_LAND_DETECTOR_AIRSPEED_MAX) { - //Publish if land detection state has changed - if(_landDetected.landed != landDetected) { - _landDetected.timestamp = now; - _landDetected.landed = landDetected; + //These conditions need to be stable for a period of time before we trust them + if (now > _landDetectTrigger) { + landDetected = true; + } - /* publish the land detected broadcast */ - orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); - } + } else { + //reset land detect trigger + _landDetectTrigger = now + FW_LAND_DETECTOR_TRIGGER_TIME; + } - //Limit loop rate - usleep(1000000 / FW_LAND_DETECTOR_UPDATE_RATE); - } + //Publish if land detection state has changed + if (_landDetected.landed != landDetected) { + _landDetected.timestamp = now; + _landDetected.landed = landDetected; - _taskIsRunning = false; - _exit(0); + /* publish the land detected broadcast */ + orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); + } + + //Limit loop rate + usleep(1000000 / FW_LAND_DETECTOR_UPDATE_RATE); + } + + _taskIsRunning = false; + _exit(0); } bool FixedwingLandDetector::isRunning() const { - return _taskIsRunning; + return _taskIsRunning; } diff --git a/src/modules/fw_land_detector/FixedwingLandDetector.h b/src/modules/fw_land_detector/FixedwingLandDetector.h index 9004457400..be9b17b74b 100644 --- a/src/modules/fw_land_detector/FixedwingLandDetector.h +++ b/src/modules/fw_land_detector/FixedwingLandDetector.h @@ -46,61 +46,63 @@ #include #include -class FixedwingLandDetector { +class FixedwingLandDetector +{ public: - FixedwingLandDetector(); - ~FixedwingLandDetector(); + FixedwingLandDetector(); + ~FixedwingLandDetector(); - /** - * @brief Executes the main loop of the land detector in a separate deamon thread - * @returns OK if task was successfully launched - **/ - int createDeamonThread(); + /** + * @brief Executes the main loop of the land detector in a separate deamon thread + * @returns OK if task was successfully launched + **/ + int createDeamonThread(); - /** - * @returns true if this task is currently running - **/ - bool isRunning() const; + /** + * @returns true if this task is currently running + **/ + bool isRunning() const; - /** - * @brief blocking loop, should be run in a separate thread or task. Runs at 50Hz - **/ - void landDetectorLoop(); + /** + * @brief blocking loop, should be run in a separate thread or task. Runs at 50Hz + **/ + void landDetectorLoop(); - /** - * @brief Tells the Land Detector task that it should exit - **/ - void shutdown(); + /** + * @brief Tells the Land Detector task that it should exit + **/ + void shutdown(); protected: - /** - * @brief polls all subscriptions and pulls any data that has changed - **/ - void updateSubscriptions(); + /** + * @brief polls all subscriptions and pulls any data that has changed + **/ + void updateSubscriptions(); - //Algorithm parameters (TODO: should probably be externalized) - static constexpr uint32_t FW_LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */ - static constexpr uint64_t FW_LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold before triggering a land */ - static constexpr float FW_LAND_DETECTOR_VELOCITY_MAX = 5.0f; /**< maximum horizontal movement m/s*/ - static constexpr float FW_LAND_DETECTOR_CLIMBRATE_MAX = 10.00f; /**< +- climb rate in m/s*/ - static constexpr float FW_LAND_DETECTOR_AIRSPEED_MAX = 10.00f; /**< airspeed max m/s*/ + //Algorithm parameters (TODO: should probably be externalized) + static constexpr uint32_t FW_LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */ + static constexpr uint64_t FW_LAND_DETECTOR_TRIGGER_TIME = + 2000000; /**< usec that landing conditions have to hold before triggering a land */ + static constexpr float FW_LAND_DETECTOR_VELOCITY_MAX = 5.0f; /**< maximum horizontal movement m/s*/ + static constexpr float FW_LAND_DETECTOR_CLIMBRATE_MAX = 10.00f; /**< +- climb rate in m/s*/ + static constexpr float FW_LAND_DETECTOR_AIRSPEED_MAX = 10.00f; /**< airspeed max m/s*/ private: - orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ - struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ + orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ + struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ - int _vehicleLocalPositionSub; /**< notification of local position */ - struct vehicle_local_position_s _vehicleLocalPosition; /**< the result from local position subscription */ - int _airspeedSub; - struct airspeed_s _airspeed; + int _vehicleLocalPositionSub; /**< notification of local position */ + struct vehicle_local_position_s _vehicleLocalPosition; /**< the result from local position subscription */ + int _airspeedSub; + struct airspeed_s _airspeed; - float _velocity_xy_filtered; - float _velocity_z_filtered; - float _airspeed_filtered; - uint64_t _landDetectTrigger; - - bool _taskShouldExit; /**< true if it is requested that this task should exit */ - bool _taskIsRunning; /**< task has reached main loop and is currently running */ + float _velocity_xy_filtered; + float _velocity_z_filtered; + float _airspeed_filtered; + uint64_t _landDetectTrigger; + + bool _taskShouldExit; /**< true if it is requested that this task should exit */ + bool _taskIsRunning; /**< task has reached main loop and is currently running */ }; #endif //__FIXED_WING_LAND_DETECTOR_H__ diff --git a/src/modules/fw_land_detector/fw_land_detector_main.cpp b/src/modules/fw_land_detector/fw_land_detector_main.cpp index a9eefc4064..a45a518639 100644 --- a/src/modules/fw_land_detector/fw_land_detector_main.cpp +++ b/src/modules/fw_land_detector/fw_land_detector_main.cpp @@ -61,7 +61,7 @@ static void fw_land_detector_stop(); extern "C" __EXPORT int fw_land_detector_main(int argc, char *argv[]); //Private variables -static FixedwingLandDetector* fw_land_detector_task = nullptr; +static FixedwingLandDetector *fw_land_detector_task = nullptr; static int _landDetectorTaskID = -1; /** @@ -69,7 +69,7 @@ static int _landDetectorTaskID = -1; **/ static void fw_land_detector_deamon_thread(int argc, char *argv[]) { - fw_land_detector_task->landDetectorLoop(); + fw_land_detector_task->landDetectorLoop(); } /** @@ -77,31 +77,32 @@ static void fw_land_detector_deamon_thread(int argc, char *argv[]) **/ static void fw_land_detector_stop() { - if(fw_land_detector_task == nullptr || _landDetectorTaskID == -1) { - errx(1, "not running"); - return; - } + if (fw_land_detector_task == nullptr || _landDetectorTaskID == -1) { + errx(1, "not running"); + return; + } - fw_land_detector_task->shutdown(); + fw_land_detector_task->shutdown(); - //Wait for task to die - int i = 0; - do { - /* wait 20ms */ - usleep(20000); + //Wait for task to die + int i = 0; - /* if we have given up, kill it */ - if (++i > 50) { - task_delete(_landDetectorTaskID); - break; - } - } while (fw_land_detector_task->isRunning()); + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_landDetectorTaskID); + break; + } + } while (fw_land_detector_task->isRunning()); - delete fw_land_detector_task; - fw_land_detector_task = nullptr; - _landDetectorTaskID = -1; - warn("fw_land_detector has been stopped"); + delete fw_land_detector_task; + fw_land_detector_task = nullptr; + _landDetectorTaskID = -1; + warn("fw_land_detector has been stopped"); } /** @@ -109,48 +110,51 @@ static void fw_land_detector_stop() **/ static int fw_land_detector_start() { - if(fw_land_detector_task != nullptr || _landDetectorTaskID != -1) { - errx(1, "already running"); - return -1; - } + if (fw_land_detector_task != nullptr || _landDetectorTaskID != -1) { + errx(1, "already running"); + return -1; + } - //Allocate memory - fw_land_detector_task = new FixedwingLandDetector(); - if (fw_land_detector_task == nullptr) { - errx(1, "alloc failed"); - return -1; - } + //Allocate memory + fw_land_detector_task = new FixedwingLandDetector(); - //Start new thread task - _landDetectorTaskID = task_spawn_cmd("fw_land_detector", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 1024, - (main_t)&fw_land_detector_deamon_thread, - nullptr); + if (fw_land_detector_task == nullptr) { + errx(1, "alloc failed"); + return -1; + } - if (_landDetectorTaskID < 0) { - errx(1, "task start failed: %d", -errno); - return -1; - } + //Start new thread task + _landDetectorTaskID = task_spawn_cmd("fw_land_detector", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 1024, + (main_t)&fw_land_detector_deamon_thread, + nullptr); - /* avoid memory fragmentation by not exiting start handler until the task has fully started */ - const uint32_t timeout = hrt_absolute_time() + 5000000; //5 second timeout - while (!fw_land_detector_task->isRunning()) { - usleep(50000); - printf("."); - fflush(stdout); + if (_landDetectorTaskID < 0) { + errx(1, "task start failed: %d", -errno); + return -1; + } - if(hrt_absolute_time() > timeout) { - err(1, "start failed - timeout"); - fw_land_detector_stop(); - exit(1); - } - } - printf("\n"); + /* avoid memory fragmentation by not exiting start handler until the task has fully started */ + const uint32_t timeout = hrt_absolute_time() + 5000000; //5 second timeout - exit(0); - return 0; + while (!fw_land_detector_task->isRunning()) { + usleep(50000); + printf("."); + fflush(stdout); + + if (hrt_absolute_time() > timeout) { + err(1, "start failed - timeout"); + fw_land_detector_stop(); + exit(1); + } + } + + printf("\n"); + + exit(0); + return 0; } /** @@ -159,37 +163,37 @@ static int fw_land_detector_start() int fw_land_detector_main(int argc, char *argv[]) { - if (argc < 1) { - warnx("usage: fw_land_detector {start|stop|status}"); - exit(0); - } + if (argc < 1) { + warnx("usage: fw_land_detector {start|stop|status}"); + exit(0); + } - if (!strcmp(argv[1], "start")) { - fw_land_detector_start(); - } + if (!strcmp(argv[1], "start")) { + fw_land_detector_start(); + } - if (!strcmp(argv[1], "stop")) { - fw_land_detector_stop(); - exit(0); - } + if (!strcmp(argv[1], "stop")) { + fw_land_detector_stop(); + exit(0); + } - if (!strcmp(argv[1], "status")) { - if (fw_land_detector_task) { + if (!strcmp(argv[1], "status")) { + if (fw_land_detector_task) { - if(fw_land_detector_task->isRunning()) { - warnx("running"); - } - else { - errx(1, "exists, but not running"); - } + if (fw_land_detector_task->isRunning()) { + warnx("running"); - exit(0); + } else { + errx(1, "exists, but not running"); + } - } else { - errx(1, "not running"); - } - } + exit(0); - warn("usage: fw_land_detector {start|stop|status}"); - return 1; + } else { + errx(1, "not running"); + } + } + + warn("usage: fw_land_detector {start|stop|status}"); + return 1; } diff --git a/src/modules/mc_land_detector/MulticopterLandDetector.cpp b/src/modules/mc_land_detector/MulticopterLandDetector.cpp index 90b36a795c..d23ff9017a 100644 --- a/src/modules/mc_land_detector/MulticopterLandDetector.cpp +++ b/src/modules/mc_land_detector/MulticopterLandDetector.cpp @@ -47,35 +47,35 @@ #include //usleep MulticopterLandDetector::MulticopterLandDetector() : - _landDetectedPub(-1), - _landDetected({0,false}), - - _vehicleGlobalPositionSub(-1), - _sensorsCombinedSub(-1), - _waypointSub(-1), - _actuatorsSub(-1), - _armingSub(-1), + _landDetectedPub(-1), + _landDetected({0, false}), - _vehicleGlobalPosition({}), - _sensors({}), - _waypoint({}), - _actuators({}), - _arming({}), + _vehicleGlobalPositionSub(-1), + _sensorsCombinedSub(-1), + _waypointSub(-1), + _actuatorsSub(-1), + _armingSub(-1), - _taskShouldExit(false), - _taskIsRunning(false), - _landTimer(0) + _vehicleGlobalPosition({}), + _sensors({}), + _waypoint({}), + _actuators({}), + _arming({}), + + _taskShouldExit(false), + _taskIsRunning(false), + _landTimer(0) { - //Advertise the first land detected uORB - _landDetected.timestamp = hrt_absolute_time(); - _landDetected.landed = false; - _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); + //Advertise the first land detected uORB + _landDetected.timestamp = hrt_absolute_time(); + _landDetected.landed = false; + _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); } MulticopterLandDetector::~MulticopterLandDetector() { - _taskShouldExit = true; - close(_landDetectedPub); + _taskShouldExit = true; + close(_landDetectedPub); } /** @@ -84,120 +84,119 @@ MulticopterLandDetector::~MulticopterLandDetector() **/ static bool orb_update(const struct orb_metadata *meta, int handle, void *buffer) { - bool newData = false; + bool newData = false; - //Check if there is new data to grab - if(orb_check(handle, &newData) != OK) { - return false; - } + //Check if there is new data to grab + if (orb_check(handle, &newData) != OK) { + return false; + } - if(!newData) { - return false; - } + if (!newData) { + return false; + } - if(orb_copy(meta, handle, buffer) != OK) { - return false; - } + if (orb_copy(meta, handle, buffer) != OK) { + return false; + } - return true; + return true; } void MulticopterLandDetector::shutdown() { - _taskShouldExit = true; + _taskShouldExit = true; } void MulticopterLandDetector::updateSubscriptions() { - orb_update(ORB_ID(vehicle_global_position), _vehicleGlobalPositionSub, &_vehicleGlobalPosition); - orb_update(ORB_ID(sensor_combined), _sensorsCombinedSub, &_sensors); - orb_update(ORB_ID(position_setpoint_triplet), _waypointSub, &_waypoint); - orb_update(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _actuatorsSub, &_actuators); - orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); + orb_update(ORB_ID(vehicle_global_position), _vehicleGlobalPositionSub, &_vehicleGlobalPosition); + orb_update(ORB_ID(sensor_combined), _sensorsCombinedSub, &_sensors); + orb_update(ORB_ID(position_setpoint_triplet), _waypointSub, &_waypoint); + orb_update(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _actuatorsSub, &_actuators); + orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); } void MulticopterLandDetector::landDetectorLoop() { - //This should never happen! - if(_taskIsRunning) return; + //This should never happen! + if (_taskIsRunning) { return; } - //Subscribe to position, attitude, arming and velocity changes - _vehicleGlobalPositionSub = orb_subscribe(ORB_ID(vehicle_global_position)); - _sensorsCombinedSub = orb_subscribe(ORB_ID(sensor_combined)); - _waypointSub = orb_subscribe(ORB_ID(position_setpoint_triplet)); - _actuatorsSub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - _armingSub = orb_subscribe(ORB_ID(actuator_armed)); + //Subscribe to position, attitude, arming and velocity changes + _vehicleGlobalPositionSub = orb_subscribe(ORB_ID(vehicle_global_position)); + _sensorsCombinedSub = orb_subscribe(ORB_ID(sensor_combined)); + _waypointSub = orb_subscribe(ORB_ID(position_setpoint_triplet)); + _actuatorsSub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + _armingSub = orb_subscribe(ORB_ID(actuator_armed)); - //Begin task - _taskIsRunning = true; - _taskShouldExit = false; - while (!_taskShouldExit) { + //Begin task + _taskIsRunning = true; + _taskShouldExit = false; - //First poll for new data from our subscriptions - updateSubscriptions(); + while (!_taskShouldExit) { - const uint64_t now = hrt_absolute_time(); + //First poll for new data from our subscriptions + updateSubscriptions(); - //only detect landing if the autopilot is actively trying to land - if(!_waypoint.current.valid || _waypoint.current.type != SETPOINT_TYPE_LAND) { - _landTimer = now; - } - else { + const uint64_t now = hrt_absolute_time(); - //Check if we are moving vertically - bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > MC_LAND_DETECTOR_CLIMBRATE_MAX; + //only detect landing if the autopilot is actively trying to land + if (!_waypoint.current.valid || _waypoint.current.type != SETPOINT_TYPE_LAND) { + _landTimer = now; - //Check if we are moving horizontally - bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n*_vehicleGlobalPosition.vel_n - + _vehicleGlobalPosition.vel_e*_vehicleGlobalPosition.vel_e) > MC_LAND_DETECTOR_VELOCITY_MAX; + } else { - //Next look if all rotation angles are not moving - bool rotating = sqrtf(_sensors.gyro_rad_s[0]*_sensors.gyro_rad_s[0]+ - _sensors.gyro_rad_s[1]*_sensors.gyro_rad_s[1]+ - _sensors.gyro_rad_s[2]*_sensors.gyro_rad_s[2]) > MC_LAND_DETECTOR_ROTATION_MAX; + //Check if we are moving vertically + bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > MC_LAND_DETECTOR_CLIMBRATE_MAX; - //Check if thrust output is minimal (about half of default) - bool minimalThrust = _arming.armed && _actuators.control[3] <= MC_LAND_DETECTOR_THRUST_MAX; + //Check if we are moving horizontally + bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n + + _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > MC_LAND_DETECTOR_VELOCITY_MAX; - if(verticalMovement || rotating || !minimalThrust || horizontalMovement) { - //Sensed movement, so reset the land detector - _landTimer = now; - } + //Next look if all rotation angles are not moving + bool rotating = sqrtf(_sensors.gyro_rad_s[0] * _sensors.gyro_rad_s[0] + + _sensors.gyro_rad_s[1] * _sensors.gyro_rad_s[1] + + _sensors.gyro_rad_s[2] * _sensors.gyro_rad_s[2]) > MC_LAND_DETECTOR_ROTATION_MAX; - } + //Check if thrust output is minimal (about half of default) + bool minimalThrust = _arming.armed && _actuators.control[3] <= MC_LAND_DETECTOR_THRUST_MAX; - // if we have detected a landing for 2 continuous seconds - if(now-_landTimer > MC_LAND_DETECTOR_TRIGGER_TIME) { - if(!_landDetected.landed) - { - _landDetected.timestamp = now; - _landDetected.landed = true; + if (verticalMovement || rotating || !minimalThrust || horizontalMovement) { + //Sensed movement, so reset the land detector + _landTimer = now; + } - /* publish the land detected broadcast */ - orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); - } - } - else { - // if we currently think we have landed, but the latest data says we are flying - if(_landDetected.landed) - { - _landDetected.timestamp = now; - _landDetected.landed = false; + } - /* publish the land detected broadcast */ - orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); - } - } + // if we have detected a landing for 2 continuous seconds + if (now - _landTimer > MC_LAND_DETECTOR_TRIGGER_TIME) { + if (!_landDetected.landed) { + _landDetected.timestamp = now; + _landDetected.landed = true; - //Limit loop rate - usleep(1000000 / MC_LAND_DETECTOR_UPDATE_RATE); - } + /* publish the land detected broadcast */ + orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); + } - _taskIsRunning = false; - _exit(0); + } else { + // if we currently think we have landed, but the latest data says we are flying + if (_landDetected.landed) { + _landDetected.timestamp = now; + _landDetected.landed = false; + + /* publish the land detected broadcast */ + orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); + } + } + + //Limit loop rate + usleep(1000000 / MC_LAND_DETECTOR_UPDATE_RATE); + } + + _taskIsRunning = false; + _exit(0); } bool MulticopterLandDetector::isRunning() const { - return _taskIsRunning; + return _taskIsRunning; } diff --git a/src/modules/mc_land_detector/MulticopterLandDetector.h b/src/modules/mc_land_detector/MulticopterLandDetector.h index 1aeaa0d62a..fe18374c3b 100644 --- a/src/modules/mc_land_detector/MulticopterLandDetector.h +++ b/src/modules/mc_land_detector/MulticopterLandDetector.h @@ -50,66 +50,67 @@ #include #include -class MulticopterLandDetector { +class MulticopterLandDetector +{ public: - MulticopterLandDetector(); - ~MulticopterLandDetector(); + MulticopterLandDetector(); + ~MulticopterLandDetector(); - /** - * @brief Executes the main loop of the land detector in a separate deamon thread - * @returns OK if task was successfully launched - **/ - int createDeamonThread(); + /** + * @brief Executes the main loop of the land detector in a separate deamon thread + * @returns OK if task was successfully launched + **/ + int createDeamonThread(); - /** - * @returns true if this task is currently running - **/ - bool isRunning() const; + /** + * @returns true if this task is currently running + **/ + bool isRunning() const; - /** - * @brief blocking loop, should be run in a separate thread or task. Runs at 50Hz - **/ - void landDetectorLoop(); + /** + * @brief blocking loop, should be run in a separate thread or task. Runs at 50Hz + **/ + void landDetectorLoop(); - /** - * @brief Tells the Land Detector task that it should exit - **/ - void shutdown(); + /** + * @brief Tells the Land Detector task that it should exit + **/ + void shutdown(); protected: - /** - * @brief polls all subscriptions and pulls any data that has changed - **/ - void updateSubscriptions(); + /** + * @brief polls all subscriptions and pulls any data that has changed + **/ + void updateSubscriptions(); - //Algorithm parameters (TODO: should probably be externalized) - static constexpr float MC_LAND_DETECTOR_CLIMBRATE_MAX = 0.30f; /**< max +- climb rate in m/s */ - static constexpr float MC_LAND_DETECTOR_ROTATION_MAX = 0.5f; /**< max rotation in rad/sec (= 30 deg/s) */ - static constexpr float MC_LAND_DETECTOR_THRUST_MAX = 0.2f; - static constexpr float MC_LAND_DETECTOR_VELOCITY_MAX = 1.0f; /**< max +- horizontal movement in m/s */ - static constexpr uint32_t MC_LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */ - static constexpr uint32_t MC_LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold before triggering a land */ + //Algorithm parameters (TODO: should probably be externalized) + static constexpr float MC_LAND_DETECTOR_CLIMBRATE_MAX = 0.30f; /**< max +- climb rate in m/s */ + static constexpr float MC_LAND_DETECTOR_ROTATION_MAX = 0.5f; /**< max rotation in rad/sec (= 30 deg/s) */ + static constexpr float MC_LAND_DETECTOR_THRUST_MAX = 0.2f; + static constexpr float MC_LAND_DETECTOR_VELOCITY_MAX = 1.0f; /**< max +- horizontal movement in m/s */ + static constexpr uint32_t MC_LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */ + static constexpr uint32_t MC_LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold before triggering a land */ private: - orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ - struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ + orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ + struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ - int _vehicleGlobalPositionSub; /**< notification of global position */ - int _sensorsCombinedSub; - int _waypointSub; - int _actuatorsSub; - int _armingSub; + int _vehicleGlobalPositionSub; /**< notification of global position */ + int _sensorsCombinedSub; + int _waypointSub; + int _actuatorsSub; + int _armingSub; - struct vehicle_global_position_s _vehicleGlobalPosition; /**< the result from global position subscription */ - struct sensor_combined_s _sensors; /**< subscribe to sensor readings */ - struct position_setpoint_triplet_s _waypoint; /**< subscribe to autopilot navigation */ - struct actuator_controls_s _actuators; - struct actuator_armed_s _arming; - - bool _taskShouldExit; /**< true if it is requested that this task should exit */ - bool _taskIsRunning; /**< task has reached main loop and is currently running */ + struct vehicle_global_position_s _vehicleGlobalPosition; /**< the result from global position subscription */ + struct sensor_combined_s _sensors; /**< subscribe to sensor readings */ + struct position_setpoint_triplet_s _waypoint; /**< subscribe to autopilot navigation */ + struct actuator_controls_s _actuators; + struct actuator_armed_s _arming; - uint64_t _landTimer; /**< timestamp in microseconds since a possible land was detected*/ + bool _taskShouldExit; /**< true if it is requested that this task should exit */ + bool _taskIsRunning; /**< task has reached main loop and is currently running */ + + uint64_t _landTimer; /**< timestamp in microseconds since a possible land was detected*/ }; #endif //__MULTICOPTER_LAND_DETECTOR_H__ diff --git a/src/modules/mc_land_detector/mc_land_detector_main.cpp b/src/modules/mc_land_detector/mc_land_detector_main.cpp index 2992ead99c..6ad3f82a45 100644 --- a/src/modules/mc_land_detector/mc_land_detector_main.cpp +++ b/src/modules/mc_land_detector/mc_land_detector_main.cpp @@ -61,7 +61,7 @@ static void mc_land_detector_stop(); extern "C" __EXPORT int mc_land_detector_main(int argc, char *argv[]); //Private variables -static MulticopterLandDetector* mc_land_detector_task = nullptr; +static MulticopterLandDetector *mc_land_detector_task = nullptr; static int _landDetectorTaskID = -1; /** @@ -77,7 +77,7 @@ static void mc_land_detector_deamon_thread(int argc, char *argv[]) **/ static void mc_land_detector_stop() { - if(mc_land_detector_task == nullptr || _landDetectorTaskID == -1) { + if (mc_land_detector_task == nullptr || _landDetectorTaskID == -1) { errx(1, "not running"); return; } @@ -86,6 +86,7 @@ static void mc_land_detector_stop() //Wait for task to die int i = 0; + do { /* wait 20ms */ usleep(20000); @@ -109,13 +110,14 @@ static void mc_land_detector_stop() **/ static int mc_land_detector_start() { - if(mc_land_detector_task != nullptr || _landDetectorTaskID != -1) { + if (mc_land_detector_task != nullptr || _landDetectorTaskID != -1) { errx(1, "already running"); return -1; } //Allocate memory mc_land_detector_task = new MulticopterLandDetector(); + if (mc_land_detector_task == nullptr) { errx(1, "alloc failed"); return -1; @@ -123,11 +125,11 @@ static int mc_land_detector_start() //Start new thread task _landDetectorTaskID = task_spawn_cmd("mc_land_detector", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 1024, - (main_t)&mc_land_detector_deamon_thread, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 1024, + (main_t)&mc_land_detector_deamon_thread, + nullptr); if (_landDetectorTaskID < 0) { errx(1, "task start failed: %d", -errno); @@ -136,17 +138,19 @@ static int mc_land_detector_start() /* avoid memory fragmentation by not exiting start handler until the task has fully started */ const uint32_t timeout = hrt_absolute_time() + 5000000; //5 second timeout + while (!mc_land_detector_task->isRunning()) { usleep(50000); printf("."); fflush(stdout); - if(hrt_absolute_time() > timeout) { + if (hrt_absolute_time() > timeout) { err(1, "start failed - timeout"); mc_land_detector_stop(); exit(1); } } + printf("\n"); exit(0); @@ -170,16 +174,16 @@ int mc_land_detector_main(int argc, char *argv[]) if (!strcmp(argv[1], "stop")) { mc_land_detector_stop(); - exit(0); + exit(0); } if (!strcmp(argv[1], "status")) { if (mc_land_detector_task) { - if(mc_land_detector_task->isRunning()) { + if (mc_land_detector_task->isRunning()) { warnx("running"); - } - else { + + } else { errx(1, "exists, but not running"); } From 57ed27304a394fb9fec8e2ae4bfca9b2a77d6c7e Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 13:56:06 +0100 Subject: [PATCH 019/269] HIL: Added land detector to HIL simulation --- src/modules/mavlink/mavlink_receiver.cpp | 21 ++++++++++++++++++--- src/modules/mavlink/mavlink_receiver.h | 3 +++ 2 files changed, 21 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 97108270c5..dfbf00b664 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -92,6 +92,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _mavlink(parent), status{}, hil_local_pos{}, + hil_land_detector{}, _control_mode{}, _global_pos_pub(-1), _local_pos_pub(-1), @@ -118,6 +119,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _telemetry_status_pub(-1), _rc_pub(-1), _manual_pub(-1), + _land_detector_pub(-1), _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), _hil_frames(0), _old_timestamp(0), @@ -1353,9 +1355,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_local_pos.xy_global = true; hil_local_pos.z_global = true; - bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve? - hil_local_pos.landed = landed; - if (_local_pos_pub < 0) { _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos); @@ -1364,6 +1363,22 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) } } + /* land detector */ + { + bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve? + if(hil_land_detector.landed != landed) { + hil_land_detector.landed = landed; + hil_land_detector.timestamp = hrt_absolute_time(); + + if (_land_detector_pub < 0) { + _land_detector_pub = orb_advertise(ORB_ID(vehicle_land_detected), &hil_land_detector); + + } else { + orb_publish(ORB_ID(vehicle_land_detected), _land_detector_pub, &hil_land_detector); + } + } + } + /* accelerometer */ { struct accel_report accel; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 4afc9b3721..699996860a 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -50,6 +50,7 @@ #include #include #include +#include #include #include #include @@ -145,6 +146,7 @@ private: mavlink_status_t status; struct vehicle_local_position_s hil_local_pos; + struct vehicle_land_detected_s hil_land_detector; struct vehicle_control_mode_s _control_mode; orb_advert_t _global_pos_pub; orb_advert_t _local_pos_pub; @@ -171,6 +173,7 @@ private: orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; + orb_advert_t _land_detector_pub; int _control_mode_sub; int _hil_frames; uint64_t _old_timestamp; From 6978ed6a61212859d1c58c769ce75f343bc2e4ca Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 13:56:36 +0100 Subject: [PATCH 020/269] INAV: Removed all references to land detector logic --- .../position_estimator_inav_main.c | 37 ------------------- 1 file changed, 37 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index b418e3a76b..39294e3c00 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -246,9 +246,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float baro_offset = 0.0f; // baro offset for reference altitude, initialized on start, then adjusted float surface_offset = 0.0f; // ground level offset from reference altitude float surface_offset_rate = 0.0f; // surface offset change rate - //float alt_avg = 0.0f; - //bool landed = true; - //hrt_abstime landed_time = 0; hrt_abstime accel_timestamp = 0; hrt_abstime baro_timestamp = 0; @@ -1068,39 +1065,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v); } - -/* - // detect land - alt_avg += (- z_est[0] - alt_avg) * dt / params.land_t; - float alt_disp2 = - z_est[0] - alt_avg; - alt_disp2 = alt_disp2 * alt_disp2; - float land_disp2 = params.land_disp * params.land_disp; - // get actual thrust output - float thrust = armed.armed ? actuator.control[3] : 0.0f; - - if (landed) { - if (alt_disp2 > land_disp2 || thrust > params.land_thr) { - landed = false; - landed_time = 0; - } - } else { - if (alt_disp2 < land_disp2 && thrust < params.land_thr) { - if (landed_time == 0) { - landed_time = t; // land detected first time - - } else { - if (t > landed_time + params.land_t * 1000000.0f) { - landed = true; - landed_time = 0; - } - } - - } else { - landed_time = 0; - } - } -*/ - if (verbose_mode) { /* print updates rate */ if (t > updates_counter_start + updates_counter_len) { @@ -1151,7 +1115,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.vy = y_est[1]; local_pos.z = z_est[0]; local_pos.vz = z_est[1]; - local_pos.landed = false; local_pos.yaw = att.yaw; local_pos.dist_bottom_valid = dist_bottom_valid; local_pos.eph = eph; From 6edb54ff7755fbb30c695984cf53b246ff497141 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 13:57:27 +0100 Subject: [PATCH 021/269] sdlog2: Added land detector log message (removed from local pos) --- src/modules/sdlog2/sdlog2.c | 12 +++++++++++- src/modules/sdlog2/sdlog2_messages.h | 7 +++++++ 2 files changed, 18 insertions(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index d35b702391..b470eefacc 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -72,6 +72,7 @@ #include #include #include +#include #include #include #include @@ -981,6 +982,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_local_position_s local_pos; struct vehicle_local_position_setpoint_s local_pos_sp; struct vehicle_global_position_s global_pos; + struct vehicle_land_detected_s land_detector; struct position_setpoint_triplet_s triplet; struct vehicle_vicon_position_s vicon_pos; struct vision_position_estimate vision_pos; @@ -1016,6 +1018,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_SENS_s log_SENS; struct log_LPOS_s log_LPOS; struct log_LPSP_s log_LPSP; + struct log_LAND_s log_LAND; struct log_GPS_s log_GPS; struct log_ATTC_s log_ATTC; struct log_STAT_s log_STAT; @@ -1082,6 +1085,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int servorail_status_sub; int wind_sub; int encoders_sub; + int land_detector_sub; } subs; subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -1112,6 +1116,8 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.system_power_sub = orb_subscribe(ORB_ID(system_power)); subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status)); subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate)); + subs.land_detector_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); + /* we need to rate-limit wind, as we do not need the full update rate */ orb_set_interval(subs.wind_sub, 90); subs.encoders_sub = orb_subscribe(ORB_ID(encoders)); @@ -1514,13 +1520,17 @@ int sdlog2_thread_main(int argc, char *argv[]) (buf.local_pos.v_z_valid ? 8 : 0) | (buf.local_pos.xy_global ? 16 : 0) | (buf.local_pos.z_global ? 32 : 0); - log_msg.body.log_LPOS.landed = buf.local_pos.landed; log_msg.body.log_LPOS.ground_dist_flags = (buf.local_pos.dist_bottom_valid ? 1 : 0); log_msg.body.log_LPOS.eph = buf.local_pos.eph; log_msg.body.log_LPOS.epv = buf.local_pos.epv; LOGBUFFER_WRITE_AND_COUNT(LPOS); } + /* --- LAND DETECTED --- */ + if (copy_if_updated(ORB_ID(vehicle_land_detected), subs.land_detector_sub, &buf.land_detector)) { + log_msg.body.log_LAND.landed = buf.land_detector.landed; + } + /* --- LOCAL POSITION SETPOINT --- */ if (copy_if_updated(ORB_ID(vehicle_local_position_setpoint), subs.local_pos_sp_sub, &buf.local_pos_sp)) { log_msg.msg_type = LOG_LPSP_MSG; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 5941bfac0e..246f5129f0 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -427,6 +427,12 @@ struct log_ENCD_s { /* --- AIR SPEED SENSORS - DIFF. PRESSURE --- */ #define LOG_AIR1_MSG 40 +/* --- LAND - LAND DETECTOR --- */ +#define LOG_LAND_MSG 41 +struct log_LAND_s { + uint8_t landed; +}; + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -495,6 +501,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(TECS, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), LOG_FORMAT(ENCD, "qfqf", "cnt0,vel0,cnt1,vel1"), + LOG_FORMAT(LAND, "B", "landed"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ From 2da6439f742f7743adca22ad3a887e936e6c2277 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 13:57:46 +0100 Subject: [PATCH 022/269] uORB: Removed landed boolean flag from vehicle_local_position topic --- src/modules/uORB/topics/vehicle_local_position.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index 0e0bc9781a..8b46c5a3f3 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -77,7 +77,6 @@ struct vehicle_local_position_s { double ref_lat; /**< Reference point latitude in degrees */ double ref_lon; /**< Reference point longitude in degrees */ float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */ - bool landed; /**< true if vehicle is landed TODO: deprecated, use vehicle_land_detected instead (remove this line?)*/ /* Distance to surface */ float dist_bottom; /**< Distance to bottom surface (ground) */ float dist_bottom_rate; /**< Distance to bottom surface (ground) change rate */ From 784fa9f4699c434671edfd1e4fb48897bea54d8f Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Wed, 7 Jan 2015 22:12:48 +0100 Subject: [PATCH 023/269] sdlog2: Removed vehicle_land_detected topic from logging --- src/modules/sdlog2/sdlog2.c | 10 ---------- src/modules/sdlog2/sdlog2_messages.h | 9 +-------- 2 files changed, 1 insertion(+), 18 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index b470eefacc..7b7949239e 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -72,7 +72,6 @@ #include #include #include -#include #include #include #include @@ -982,7 +981,6 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_local_position_s local_pos; struct vehicle_local_position_setpoint_s local_pos_sp; struct vehicle_global_position_s global_pos; - struct vehicle_land_detected_s land_detector; struct position_setpoint_triplet_s triplet; struct vehicle_vicon_position_s vicon_pos; struct vision_position_estimate vision_pos; @@ -1018,7 +1016,6 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_SENS_s log_SENS; struct log_LPOS_s log_LPOS; struct log_LPSP_s log_LPSP; - struct log_LAND_s log_LAND; struct log_GPS_s log_GPS; struct log_ATTC_s log_ATTC; struct log_STAT_s log_STAT; @@ -1085,7 +1082,6 @@ int sdlog2_thread_main(int argc, char *argv[]) int servorail_status_sub; int wind_sub; int encoders_sub; - int land_detector_sub; } subs; subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -1116,7 +1112,6 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.system_power_sub = orb_subscribe(ORB_ID(system_power)); subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status)); subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate)); - subs.land_detector_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); /* we need to rate-limit wind, as we do not need the full update rate */ orb_set_interval(subs.wind_sub, 90); @@ -1526,11 +1521,6 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(LPOS); } - /* --- LAND DETECTED --- */ - if (copy_if_updated(ORB_ID(vehicle_land_detected), subs.land_detector_sub, &buf.land_detector)) { - log_msg.body.log_LAND.landed = buf.land_detector.landed; - } - /* --- LOCAL POSITION SETPOINT --- */ if (copy_if_updated(ORB_ID(vehicle_local_position_setpoint), subs.local_pos_sp_sub, &buf.local_pos_sp)) { log_msg.msg_type = LOG_LPSP_MSG; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 246f5129f0..99f70a948e 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -113,7 +113,6 @@ struct log_LPOS_s { int32_t ref_lon; float ref_alt; uint8_t pos_flags; - uint8_t landed; uint8_t ground_dist_flags; float eph; float epv; @@ -427,11 +426,6 @@ struct log_ENCD_s { /* --- AIR SPEED SENSORS - DIFF. PRESSURE --- */ #define LOG_AIR1_MSG 40 -/* --- LAND - LAND DETECTOR --- */ -#define LOG_LAND_MSG 41 -struct log_LAND_s { - uint8_t landed; -}; /********** SYSTEM MESSAGES, ID > 0x80 **********/ @@ -468,7 +462,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT_S(IMU2, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT_S(SENS, SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), LOG_FORMAT_S(AIR1, SENS, "fffff", "BaroPa,BaroAlt,BaroTmp,DiffPres,DiffPresF"), - LOG_FORMAT(LPOS, "ffffffffLLfBBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,LFlg,GFlg,EPH,EPV"), + LOG_FORMAT(LPOS, "ffffffffLLfBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,GFlg,EPH,EPV"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"), LOG_FORMAT_S(ATTC, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), @@ -501,7 +495,6 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(TECS, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), LOG_FORMAT(ENCD, "qfqf", "cnt0,vel0,cnt1,vel1"), - LOG_FORMAT(LAND, "B", "landed"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ From 10a2dd8a346a6a08a0d9b52739f20b842d460646 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Wed, 7 Jan 2015 23:19:35 +0100 Subject: [PATCH 024/269] LandDetector: Merged fixedwing and multicopter into one module handling both algorithms --- makefiles/config_px4fmu-v2_default.mk | 7 +- .../FixedwingLandDetector.cpp | 173 --------------- .../fw_land_detector/FixedwingLandDetector.h | 108 ---------- .../fw_land_detector_main.cpp | 199 ----------------- src/modules/fw_land_detector/module.mk | 10 - .../land_detector/FixedwingLandDetector.cpp | 103 +++++++++ .../land_detector/FixedwingLandDetector.h | 88 ++++++++ src/modules/land_detector/LandDetector.cpp | 77 +++++++ src/modules/land_detector/LandDetector.h | 101 +++++++++ .../land_detector/MulticopterLandDetector.cpp | 120 +++++++++++ .../MulticopterLandDetector.h | 48 ++--- .../land_detector_main.cpp} | 81 ++++--- src/modules/land_detector/module.mk | 12 ++ .../MulticopterLandDetector.cpp | 202 ------------------ src/modules/mc_land_detector/module.mk | 10 - 15 files changed, 564 insertions(+), 775 deletions(-) delete mode 100644 src/modules/fw_land_detector/FixedwingLandDetector.cpp delete mode 100644 src/modules/fw_land_detector/FixedwingLandDetector.h delete mode 100644 src/modules/fw_land_detector/fw_land_detector_main.cpp delete mode 100644 src/modules/fw_land_detector/module.mk create mode 100644 src/modules/land_detector/FixedwingLandDetector.cpp create mode 100644 src/modules/land_detector/FixedwingLandDetector.h create mode 100644 src/modules/land_detector/LandDetector.cpp create mode 100644 src/modules/land_detector/LandDetector.h create mode 100644 src/modules/land_detector/MulticopterLandDetector.cpp rename src/modules/{mc_land_detector => land_detector}/MulticopterLandDetector.h (75%) rename src/modules/{mc_land_detector/mc_land_detector_main.cpp => land_detector/land_detector_main.cpp} (65%) create mode 100644 src/modules/land_detector/module.mk delete mode 100644 src/modules/mc_land_detector/MulticopterLandDetector.cpp delete mode 100644 src/modules/mc_land_detector/module.mk diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 8ef1c333ae..e13a88ca57 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -71,12 +71,7 @@ MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/gpio_led MODULES += modules/uavcan - -# -# Vehicle land detection -# -MODULES += modules/mc_land_detector -MODULES += modules/fw_land_detector +MODULES += modules/land_detector # # Estimation modules (EKF/ SO3 / other filters) diff --git a/src/modules/fw_land_detector/FixedwingLandDetector.cpp b/src/modules/fw_land_detector/FixedwingLandDetector.cpp deleted file mode 100644 index 5fbeb2f45b..0000000000 --- a/src/modules/fw_land_detector/FixedwingLandDetector.cpp +++ /dev/null @@ -1,173 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file FixedwingLandDetector.cpp - * Land detection algorithm for fixedwings - * - * @author Johan Jansen - */ - -#include "FixedwingLandDetector.h" - -#include -#include -#include -#include //usleep - -FixedwingLandDetector::FixedwingLandDetector() : - _landDetectedPub(-1), - _landDetected({0, false}), - - _vehicleLocalPositionSub(-1), - _vehicleLocalPosition({}), - _airspeedSub(-1), - _airspeed({}), - - _velocity_xy_filtered(0.0f), - _velocity_z_filtered(0.0f), - _airspeed_filtered(0.0f), - _landDetectTrigger(0), - - _taskShouldExit(false), - _taskIsRunning(false) -{ - //Advertise the first land detected uORB - _landDetected.timestamp = hrt_absolute_time(); - _landDetected.landed = false; - _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); -} - -FixedwingLandDetector::~FixedwingLandDetector() -{ - _taskShouldExit = true; - close(_landDetectedPub); -} - -void FixedwingLandDetector::shutdown() -{ - _taskShouldExit = true; -} - -/** -* @brief Convinience function for polling uORB subscriptions -* @return true if there was new data and it was successfully copied -**/ -static bool orb_update(const struct orb_metadata *meta, int handle, void *buffer) -{ - bool newData = false; - - //Check if there is new data to grab - if (orb_check(handle, &newData) != OK) { - return false; - } - - if (!newData) { - return false; - } - - if (orb_copy(meta, handle, buffer) != OK) { - return false; - } - - return true; -} - -void FixedwingLandDetector::updateSubscriptions() -{ - orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition); - orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); -} - -void FixedwingLandDetector::landDetectorLoop() -{ - //This should never happen! - if (_taskIsRunning) { return; } - - //Subscribe to local position and airspeed data - _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position)); - _airspeedSub = orb_subscribe(ORB_ID(airspeed)); - - _taskIsRunning = true; - _taskShouldExit = false; - - while (!_taskShouldExit) { - - //First poll for new data from our subscriptions - updateSubscriptions(); - - const uint64_t now = hrt_absolute_time(); - bool landDetected = false; - - //TODO: reset filtered values on arming? - _velocity_xy_filtered = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx * - _vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy); - _velocity_z_filtered = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz); - _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; - - /* crude land detector for fixedwing */ - if (_velocity_xy_filtered < FW_LAND_DETECTOR_VELOCITY_MAX - && _velocity_z_filtered < FW_LAND_DETECTOR_CLIMBRATE_MAX - && _airspeed_filtered < FW_LAND_DETECTOR_AIRSPEED_MAX) { - - //These conditions need to be stable for a period of time before we trust them - if (now > _landDetectTrigger) { - landDetected = true; - } - - } else { - //reset land detect trigger - _landDetectTrigger = now + FW_LAND_DETECTOR_TRIGGER_TIME; - } - - //Publish if land detection state has changed - if (_landDetected.landed != landDetected) { - _landDetected.timestamp = now; - _landDetected.landed = landDetected; - - /* publish the land detected broadcast */ - orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); - } - - //Limit loop rate - usleep(1000000 / FW_LAND_DETECTOR_UPDATE_RATE); - } - - _taskIsRunning = false; - _exit(0); -} - -bool FixedwingLandDetector::isRunning() const -{ - return _taskIsRunning; -} diff --git a/src/modules/fw_land_detector/FixedwingLandDetector.h b/src/modules/fw_land_detector/FixedwingLandDetector.h deleted file mode 100644 index be9b17b74b..0000000000 --- a/src/modules/fw_land_detector/FixedwingLandDetector.h +++ /dev/null @@ -1,108 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file FixedwingLandDetector.h - * Land detection algorithm for fixedwing - * - * @author Johan Jansen - */ - -#ifndef __FIXED_WING_LAND_DETECTOR_H__ -#define __FIXED_WING_LAND_DETECTOR_H__ - -#include -#include -#include -#include - -class FixedwingLandDetector -{ -public: - FixedwingLandDetector(); - ~FixedwingLandDetector(); - - /** - * @brief Executes the main loop of the land detector in a separate deamon thread - * @returns OK if task was successfully launched - **/ - int createDeamonThread(); - - /** - * @returns true if this task is currently running - **/ - bool isRunning() const; - - /** - * @brief blocking loop, should be run in a separate thread or task. Runs at 50Hz - **/ - void landDetectorLoop(); - - /** - * @brief Tells the Land Detector task that it should exit - **/ - void shutdown(); - -protected: - /** - * @brief polls all subscriptions and pulls any data that has changed - **/ - void updateSubscriptions(); - - //Algorithm parameters (TODO: should probably be externalized) - static constexpr uint32_t FW_LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */ - static constexpr uint64_t FW_LAND_DETECTOR_TRIGGER_TIME = - 2000000; /**< usec that landing conditions have to hold before triggering a land */ - static constexpr float FW_LAND_DETECTOR_VELOCITY_MAX = 5.0f; /**< maximum horizontal movement m/s*/ - static constexpr float FW_LAND_DETECTOR_CLIMBRATE_MAX = 10.00f; /**< +- climb rate in m/s*/ - static constexpr float FW_LAND_DETECTOR_AIRSPEED_MAX = 10.00f; /**< airspeed max m/s*/ - -private: - orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ - struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ - - int _vehicleLocalPositionSub; /**< notification of local position */ - struct vehicle_local_position_s _vehicleLocalPosition; /**< the result from local position subscription */ - int _airspeedSub; - struct airspeed_s _airspeed; - - float _velocity_xy_filtered; - float _velocity_z_filtered; - float _airspeed_filtered; - uint64_t _landDetectTrigger; - - bool _taskShouldExit; /**< true if it is requested that this task should exit */ - bool _taskIsRunning; /**< task has reached main loop and is currently running */ -}; - -#endif //__FIXED_WING_LAND_DETECTOR_H__ diff --git a/src/modules/fw_land_detector/fw_land_detector_main.cpp b/src/modules/fw_land_detector/fw_land_detector_main.cpp deleted file mode 100644 index a45a518639..0000000000 --- a/src/modules/fw_land_detector/fw_land_detector_main.cpp +++ /dev/null @@ -1,199 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file fw_land_detector_main.cpp - * Land detection algorithm for fixed wings - * - * @author Johan Jansen - */ - -#include //usleep -#include -#include -#include -#include -#include -#include //Scheduler -#include //print to console - -#include "FixedwingLandDetector.h" - -//Function prototypes -static int fw_land_detector_start(); -static void fw_land_detector_stop(); - -/** - * land detector app start / stop handling function - * This makes the land detector module accessible from the nuttx shell - * @ingroup apps - */ -extern "C" __EXPORT int fw_land_detector_main(int argc, char *argv[]); - -//Private variables -static FixedwingLandDetector *fw_land_detector_task = nullptr; -static int _landDetectorTaskID = -1; - -/** -* Deamon thread function -**/ -static void fw_land_detector_deamon_thread(int argc, char *argv[]) -{ - fw_land_detector_task->landDetectorLoop(); -} - -/** -* Stop the task, force killing it if it doesn't stop by itself -**/ -static void fw_land_detector_stop() -{ - if (fw_land_detector_task == nullptr || _landDetectorTaskID == -1) { - errx(1, "not running"); - return; - } - - fw_land_detector_task->shutdown(); - - //Wait for task to die - int i = 0; - - do { - /* wait 20ms */ - usleep(20000); - - /* if we have given up, kill it */ - if (++i > 50) { - task_delete(_landDetectorTaskID); - break; - } - } while (fw_land_detector_task->isRunning()); - - - delete fw_land_detector_task; - fw_land_detector_task = nullptr; - _landDetectorTaskID = -1; - warn("fw_land_detector has been stopped"); -} - -/** -* Start new task, fails if it is already running. Returns OK if successful -**/ -static int fw_land_detector_start() -{ - if (fw_land_detector_task != nullptr || _landDetectorTaskID != -1) { - errx(1, "already running"); - return -1; - } - - //Allocate memory - fw_land_detector_task = new FixedwingLandDetector(); - - if (fw_land_detector_task == nullptr) { - errx(1, "alloc failed"); - return -1; - } - - //Start new thread task - _landDetectorTaskID = task_spawn_cmd("fw_land_detector", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 1024, - (main_t)&fw_land_detector_deamon_thread, - nullptr); - - if (_landDetectorTaskID < 0) { - errx(1, "task start failed: %d", -errno); - return -1; - } - - /* avoid memory fragmentation by not exiting start handler until the task has fully started */ - const uint32_t timeout = hrt_absolute_time() + 5000000; //5 second timeout - - while (!fw_land_detector_task->isRunning()) { - usleep(50000); - printf("."); - fflush(stdout); - - if (hrt_absolute_time() > timeout) { - err(1, "start failed - timeout"); - fw_land_detector_stop(); - exit(1); - } - } - - printf("\n"); - - exit(0); - return 0; -} - -/** -* Main entry point for this module -**/ -int fw_land_detector_main(int argc, char *argv[]) -{ - - if (argc < 1) { - warnx("usage: fw_land_detector {start|stop|status}"); - exit(0); - } - - if (!strcmp(argv[1], "start")) { - fw_land_detector_start(); - } - - if (!strcmp(argv[1], "stop")) { - fw_land_detector_stop(); - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (fw_land_detector_task) { - - if (fw_land_detector_task->isRunning()) { - warnx("running"); - - } else { - errx(1, "exists, but not running"); - } - - exit(0); - - } else { - errx(1, "not running"); - } - } - - warn("usage: fw_land_detector {start|stop|status}"); - return 1; -} diff --git a/src/modules/fw_land_detector/module.mk b/src/modules/fw_land_detector/module.mk deleted file mode 100644 index 894b29a7b5..0000000000 --- a/src/modules/fw_land_detector/module.mk +++ /dev/null @@ -1,10 +0,0 @@ -# -# Fixedwing land detector -# - -MODULE_COMMAND = fw_land_detector - -SRCS = fw_land_detector_main.cpp \ - FixedwingLandDetector.cpp - -EXTRACXXFLAGS = -Weffc++ \ No newline at end of file diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp new file mode 100644 index 0000000000..42735f38c8 --- /dev/null +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file FixedwingLandDetector.cpp + * Land detection algorithm for fixedwings + * + * @author Johan Jansen + */ + +#include "FixedwingLandDetector.h" + +#include +#include + +FixedwingLandDetector::FixedwingLandDetector() : + _vehicleLocalPositionSub(-1), + _vehicleLocalPosition({}), + _airspeedSub(-1), + _airspeed({}), + + _velocity_xy_filtered(0.0f), + _velocity_z_filtered(0.0f), + _airspeed_filtered(0.0f), + _landDetectTrigger(0) +{ + //ctor +} + +void FixedwingLandDetector::initialize() +{ + //Subscribe to local position and airspeed data + _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position)); + _airspeedSub = orb_subscribe(ORB_ID(airspeed)); +} + +void FixedwingLandDetector::updateSubscriptions() +{ + orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition); + orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); +} + +bool FixedwingLandDetector::update() +{ + //First poll for new data from our subscriptions + updateSubscriptions(); + + const uint64_t now = hrt_absolute_time(); + bool landDetected = false; + + //TODO: reset filtered values on arming? + _velocity_xy_filtered = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx * + _vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy); + _velocity_z_filtered = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz); + _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; + + /* crude land detector for fixedwing */ + if (_velocity_xy_filtered < FW_LAND_DETECTOR_VELOCITY_MAX + && _velocity_z_filtered < FW_LAND_DETECTOR_CLIMBRATE_MAX + && _airspeed_filtered < FW_LAND_DETECTOR_AIRSPEED_MAX) { + + //These conditions need to be stable for a period of time before we trust them + if (now > _landDetectTrigger) { + landDetected = true; + } + + } else { + //reset land detect trigger + _landDetectTrigger = now + FW_LAND_DETECTOR_TRIGGER_TIME; + } + + return landDetected; +} diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h new file mode 100644 index 0000000000..faeece6f3c --- /dev/null +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file FixedwingLandDetector.h + * Land detection algorithm for fixedwing + * + * @author Johan Jansen + */ + +#ifndef __FIXED_WING_LAND_DETECTOR_H__ +#define __FIXED_WING_LAND_DETECTOR_H__ + +#include "LandDetector.h" +#include +#include + +class FixedwingLandDetector : public LandDetector +{ +public: + FixedwingLandDetector(); + +protected: + /** + * @brief blocking loop, should be run in a separate thread or task. Runs at 50Hz + **/ + bool update() override; + + /** + * @brief Initializes the land detection algorithm + **/ + void initialize() override; + + /** + * @brief polls all subscriptions and pulls any data that has changed + **/ + void updateSubscriptions(); + + //Algorithm parameters (TODO: should probably be externalized) + static constexpr uint64_t FW_LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold + before triggering a land */ + static constexpr float FW_LAND_DETECTOR_VELOCITY_MAX = 5.0f; /**< maximum horizontal movement m/s*/ + static constexpr float FW_LAND_DETECTOR_CLIMBRATE_MAX = 10.00f; /**< +- climb rate in m/s*/ + static constexpr float FW_LAND_DETECTOR_AIRSPEED_MAX = 10.00f; /**< airspeed max m/s*/ + +private: + int _vehicleLocalPositionSub; /**< notification of local position */ + struct vehicle_local_position_s _vehicleLocalPosition; /**< the result from local position subscription */ + int _airspeedSub; + struct airspeed_s _airspeed; + + float _velocity_xy_filtered; + float _velocity_z_filtered; + float _airspeed_filtered; + uint64_t _landDetectTrigger; +}; + +#endif //__FIXED_WING_LAND_DETECTOR_H__ diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp new file mode 100644 index 0000000000..a39e538185 --- /dev/null +++ b/src/modules/land_detector/LandDetector.cpp @@ -0,0 +1,77 @@ +#include "LandDetector.h" +#include //usleep +#include + +LandDetector::LandDetector() : + _landDetectedPub(-1), + _landDetected({0, false}), + _taskShouldExit(false), + _taskIsRunning(false) +{ + //Advertise the first land detected uORB + _landDetected.timestamp = hrt_absolute_time(); + _landDetected.landed = false; + _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); +} + +LandDetector::~LandDetector() +{ + _taskShouldExit = true; + close(_landDetectedPub); +} + +void LandDetector::shutdown() +{ + _taskShouldExit = true; +} + +void LandDetector::start() +{ + //Make sure this method has not already been called by another thread + if(isRunning()) { + return; + } + + //Task is now running, keep doing so until shutdown() has been called + _taskIsRunning = true; + _taskShouldExit = false; + while(isRunning()) { + + bool landDetected = update(); + + //Publish if land detection state has changed + if (_landDetected.landed != landDetected) { + _landDetected.timestamp = hrt_absolute_time(); + _landDetected.landed = landDetected; + + /* publish the land detected broadcast */ + orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); + } + + //Limit loop rate + usleep(1000000 / LAND_DETECTOR_UPDATE_RATE); + } + + _taskIsRunning = false; + _exit(0); +} + +bool LandDetector::orb_update(const struct orb_metadata *meta, int handle, void *buffer) +{ + bool newData = false; + + //Check if there is new data to grab + if (orb_check(handle, &newData) != OK) { + return false; + } + + if (!newData) { + return false; + } + + if (orb_copy(meta, handle, buffer) != OK) { + return false; + } + + return true; +} diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h new file mode 100644 index 0000000000..6d74656917 --- /dev/null +++ b/src/modules/land_detector/LandDetector.h @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file LandDetector.h + * Abstract interface for land detector algorithms + * + * @author Johan Jansen + */ + +#ifndef __LAND_DETECTOR_H__ +#define __LAND_DETECTOR_H__ + +#include +#include + +class LandDetector +{ +public: + + LandDetector(); + virtual ~LandDetector(); + + /** + * @return true if this task is currently running + **/ + inline bool isRunning() const {return _taskIsRunning;} + + /** + * @brief Tells the Land Detector task that it should exit + **/ + void shutdown(); + + /** + * @brief Blocking function that should be called from it's own task thread. This method will + * run the underlying algorithm at the desired update rate and publish if the landing state changes. + **/ + void start(); + +protected: + + /** + * @brief Pure abstract method that must be overriden by sub-classes. This actually runs the underlying algorithm + * @return true if a landing was detected and this should be broadcast to the rest of the system + **/ + virtual bool update() = 0; + + /** + * @brief Pure abstract method that is called by this class once for initializing the uderlying algorithm (memory allocation, + * uORB topic subscription, creating file descriptors, etc.) + **/ + virtual void initialize() = 0; + + /** + * @brief Convinience function for polling uORB subscriptions + * @return true if there was new data and it was successfully copied + **/ + bool orb_update(const struct orb_metadata *meta, int handle, void *buffer); + + static constexpr uint32_t LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */ + +protected: + orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ + struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ + +private: + bool _taskShouldExit; /**< true if it is requested that this task should exit */ + bool _taskIsRunning; /**< task has reached main loop and is currently running */ +}; + +#endif //__LAND_DETECTOR_H__ \ No newline at end of file diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp new file mode 100644 index 0000000000..36d92fd798 --- /dev/null +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -0,0 +1,120 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file MulticopterLandDetector.cpp + * Land detection algorithm + * + * @author Johan Jansen + * @author Morten Lysgaard + */ + +#include "MulticopterLandDetector.h" + +#include +#include + +MulticopterLandDetector::MulticopterLandDetector() : + _vehicleGlobalPositionSub(-1), + _sensorsCombinedSub(-1), + _waypointSub(-1), + _actuatorsSub(-1), + _armingSub(-1), + + _vehicleGlobalPosition({}), + _sensors({}), + _waypoint({}), + _actuators({}), + _arming({}), + + _landTimer(0) +{ + //ctor +} + +void MulticopterLandDetector::initialize() +{ + //Subscribe to position, attitude, arming and velocity changes + _vehicleGlobalPositionSub = orb_subscribe(ORB_ID(vehicle_global_position)); + _sensorsCombinedSub = orb_subscribe(ORB_ID(sensor_combined)); + _waypointSub = orb_subscribe(ORB_ID(position_setpoint_triplet)); + _actuatorsSub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + _armingSub = orb_subscribe(ORB_ID(actuator_armed)); +} + +void MulticopterLandDetector::updateSubscriptions() +{ + orb_update(ORB_ID(vehicle_global_position), _vehicleGlobalPositionSub, &_vehicleGlobalPosition); + orb_update(ORB_ID(sensor_combined), _sensorsCombinedSub, &_sensors); + orb_update(ORB_ID(position_setpoint_triplet), _waypointSub, &_waypoint); + orb_update(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _actuatorsSub, &_actuators); + orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); +} + +bool MulticopterLandDetector::update() +{ + //First poll for new data from our subscriptions + updateSubscriptions(); + + const uint64_t now = hrt_absolute_time(); + + //only detect landing if the autopilot is actively trying to land + if (!_waypoint.current.valid || _waypoint.current.type != SETPOINT_TYPE_LAND) { + _landTimer = now; + + } else { + + //Check if we are moving vertically + bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > MC_LAND_DETECTOR_CLIMBRATE_MAX; + + //Check if we are moving horizontally + bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n + + _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > MC_LAND_DETECTOR_VELOCITY_MAX; + + //Next look if all rotation angles are not moving + bool rotating = sqrtf(_sensors.gyro_rad_s[0] * _sensors.gyro_rad_s[0] + + _sensors.gyro_rad_s[1] * _sensors.gyro_rad_s[1] + + _sensors.gyro_rad_s[2] * _sensors.gyro_rad_s[2]) > MC_LAND_DETECTOR_ROTATION_MAX; + + //Check if thrust output is minimal (about half of default) + bool minimalThrust = _arming.armed && _actuators.control[3] <= MC_LAND_DETECTOR_THRUST_MAX; + + if (verticalMovement || rotating || !minimalThrust || horizontalMovement) { + //Sensed movement, so reset the land detector + _landTimer = now; + } + + } + + return now - _landTimer > MC_LAND_DETECTOR_TRIGGER_TIME; +} diff --git a/src/modules/mc_land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h similarity index 75% rename from src/modules/mc_land_detector/MulticopterLandDetector.h rename to src/modules/land_detector/MulticopterLandDetector.h index fe18374c3b..08a8132ba5 100644 --- a/src/modules/mc_land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -32,50 +32,27 @@ ****************************************************************************/ /** - * @file MultiCopterLandDetector.h + * @file MulticopterLandDetector.h * Land detection algorithm for multicopters * * @author Johan Jansen - * @author Morten Lysgaard + * @author Morten Lysgaard */ #ifndef __MULTICOPTER_LAND_DETECTOR_H__ #define __MULTICOPTER_LAND_DETECTOR_H__ -#include -#include +#include "LandDetector.h" #include #include #include #include #include -class MulticopterLandDetector +class MulticopterLandDetector : public LandDetector { public: MulticopterLandDetector(); - ~MulticopterLandDetector(); - - /** - * @brief Executes the main loop of the land detector in a separate deamon thread - * @returns OK if task was successfully launched - **/ - int createDeamonThread(); - - /** - * @returns true if this task is currently running - **/ - bool isRunning() const; - - /** - * @brief blocking loop, should be run in a separate thread or task. Runs at 50Hz - **/ - void landDetectorLoop(); - - /** - * @brief Tells the Land Detector task that it should exit - **/ - void shutdown(); protected: /** @@ -83,18 +60,24 @@ protected: **/ void updateSubscriptions(); + /** + * @brief Runs one iteration of the land detection algorithm + **/ + bool update() override; + + /** + * @brief Initializes the land detection algorithm + **/ + void initialize() override; + //Algorithm parameters (TODO: should probably be externalized) static constexpr float MC_LAND_DETECTOR_CLIMBRATE_MAX = 0.30f; /**< max +- climb rate in m/s */ static constexpr float MC_LAND_DETECTOR_ROTATION_MAX = 0.5f; /**< max rotation in rad/sec (= 30 deg/s) */ static constexpr float MC_LAND_DETECTOR_THRUST_MAX = 0.2f; static constexpr float MC_LAND_DETECTOR_VELOCITY_MAX = 1.0f; /**< max +- horizontal movement in m/s */ - static constexpr uint32_t MC_LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */ static constexpr uint32_t MC_LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold before triggering a land */ private: - orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ - struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ - int _vehicleGlobalPositionSub; /**< notification of global position */ int _sensorsCombinedSub; int _waypointSub; @@ -107,9 +90,6 @@ private: struct actuator_controls_s _actuators; struct actuator_armed_s _arming; - bool _taskShouldExit; /**< true if it is requested that this task should exit */ - bool _taskIsRunning; /**< task has reached main loop and is currently running */ - uint64_t _landTimer; /**< timestamp in microseconds since a possible land was detected*/ }; diff --git a/src/modules/mc_land_detector/mc_land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp similarity index 65% rename from src/modules/mc_land_detector/mc_land_detector_main.cpp rename to src/modules/land_detector/land_detector_main.cpp index 6ad3f82a45..2bc89e7526 100644 --- a/src/modules/mc_land_detector/mc_land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -32,8 +32,8 @@ ****************************************************************************/ /** - * @file mc_land_detector_main.cpp - * Land detection algorithm for multicopters + * @file land_detector_main.cpp + * Land detection algorithm * * @author Johan Jansen */ @@ -47,42 +47,44 @@ #include //Scheduler #include //print to console +#include "FixedwingLandDetector.h" #include "MulticopterLandDetector.h" //Function prototypes -static int mc_land_detector_start(); -static void mc_land_detector_stop(); +static int land_detector_start(const char* mode); +static void land_detector_stop(); /** * land detector app start / stop handling function * This makes the land detector module accessible from the nuttx shell * @ingroup apps */ -extern "C" __EXPORT int mc_land_detector_main(int argc, char *argv[]); +extern "C" __EXPORT int land_detector_main(int argc, char *argv[]); //Private variables -static MulticopterLandDetector *mc_land_detector_task = nullptr; +static LandDetector *land_detector_task = nullptr; static int _landDetectorTaskID = -1; +static char _currentMode[12]; /** * Deamon thread function **/ -static void mc_land_detector_deamon_thread(int argc, char *argv[]) +static void land_detector_deamon_thread(int argc, char *argv[]) { - mc_land_detector_task->landDetectorLoop(); + land_detector_task->start(); } /** * Stop the task, force killing it if it doesn't stop by itself **/ -static void mc_land_detector_stop() +static void land_detector_stop() { - if (mc_land_detector_task == nullptr || _landDetectorTaskID == -1) { + if (land_detector_task == nullptr || _landDetectorTaskID == -1) { errx(1, "not running"); return; } - mc_land_detector_task->shutdown(); + land_detector_task->shutdown(); //Wait for task to die int i = 0; @@ -96,39 +98,49 @@ static void mc_land_detector_stop() task_delete(_landDetectorTaskID); break; } - } while (mc_land_detector_task->isRunning()); + } while (land_detector_task->isRunning()); - delete mc_land_detector_task; - mc_land_detector_task = nullptr; + delete land_detector_task; + land_detector_task = nullptr; _landDetectorTaskID = -1; - warn("mc_land_detector has been stopped"); + warn("land_detector has been stopped"); } /** * Start new task, fails if it is already running. Returns OK if successful **/ -static int mc_land_detector_start() +static int land_detector_start(const char* mode) { - if (mc_land_detector_task != nullptr || _landDetectorTaskID != -1) { + if (land_detector_task != nullptr || _landDetectorTaskID != -1) { errx(1, "already running"); return -1; } //Allocate memory - mc_land_detector_task = new MulticopterLandDetector(); + if(!strcmp(mode, "fixedwing")) { + land_detector_task = new FixedwingLandDetector(); + } + else if(!strcmp(mode, "multicopter")) { + land_detector_task = new MulticopterLandDetector(); + } + else { + errx(1, "[mode] must be either 'fixedwing' or 'multicopter'"); + return -1; + } - if (mc_land_detector_task == nullptr) { + //Check if alloc worked + if (land_detector_task == nullptr) { errx(1, "alloc failed"); return -1; } //Start new thread task - _landDetectorTaskID = task_spawn_cmd("mc_land_detector", + _landDetectorTaskID = task_spawn_cmd("land_detector", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1024, - (main_t)&mc_land_detector_deamon_thread, + (main_t)&land_detector_deamon_thread, nullptr); if (_landDetectorTaskID < 0) { @@ -139,20 +151,23 @@ static int mc_land_detector_start() /* avoid memory fragmentation by not exiting start handler until the task has fully started */ const uint32_t timeout = hrt_absolute_time() + 5000000; //5 second timeout - while (!mc_land_detector_task->isRunning()) { + while (!land_detector_task->isRunning()) { usleep(50000); printf("."); fflush(stdout); if (hrt_absolute_time() > timeout) { err(1, "start failed - timeout"); - mc_land_detector_stop(); + land_detector_stop(); exit(1); } } printf("\n"); + //Remember current active mode + strncpy(_currentMode, mode, 12); + exit(0); return 0; } @@ -160,31 +175,31 @@ static int mc_land_detector_start() /** * Main entry point for this module **/ -int mc_land_detector_main(int argc, char *argv[]) +int land_detector_main(int argc, char *argv[]) { if (argc < 1) { - warnx("usage: mc_land_detector {start|stop|status}"); + warnx("usage: land_detector {start|stop|status} [mode]\nmode can either be 'fixedwing' or 'multicopter'"); exit(0); } - if (!strcmp(argv[1], "start")) { - mc_land_detector_start(); + if (argc >= 2 && !strcmp(argv[1], "start")) { + land_detector_start(argv[2]); } if (!strcmp(argv[1], "stop")) { - mc_land_detector_stop(); + land_detector_stop(); exit(0); } if (!strcmp(argv[1], "status")) { - if (mc_land_detector_task) { + if (land_detector_task) { - if (mc_land_detector_task->isRunning()) { - warnx("running"); + if (land_detector_task->isRunning()) { + warnx("running (%s)", _currentMode); } else { - errx(1, "exists, but not running"); + errx(1, "exists, but not running (%s)", _currentMode); } exit(0); @@ -194,6 +209,6 @@ int mc_land_detector_main(int argc, char *argv[]) } } - warn("usage: mc_land_detector {start|stop|status}"); + warn("usage: land_detector {start|stop|status} [mode]\nmode can either be 'fixedwing' or 'multicopter'"); return 1; } diff --git a/src/modules/land_detector/module.mk b/src/modules/land_detector/module.mk new file mode 100644 index 0000000000..db0e184472 --- /dev/null +++ b/src/modules/land_detector/module.mk @@ -0,0 +1,12 @@ +# +# Land detector +# + +MODULE_COMMAND = land_detector + +SRCS = land_detector_main.cpp \ + LandDetector.cpp \ + MulticopterLandDetector.cpp \ + FixedwingLandDetector.cpp + +EXTRACXXFLAGS = -Weffc++ -Os diff --git a/src/modules/mc_land_detector/MulticopterLandDetector.cpp b/src/modules/mc_land_detector/MulticopterLandDetector.cpp deleted file mode 100644 index d23ff9017a..0000000000 --- a/src/modules/mc_land_detector/MulticopterLandDetector.cpp +++ /dev/null @@ -1,202 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file MultiCopterLandDetector.cpp - * Land detection algorithm for multicopters - * - * @author Johan Jansen - * @author Morten Lysgaard - */ - -#include "MulticopterLandDetector.h" - -#include -#include -#include -#include //usleep - -MulticopterLandDetector::MulticopterLandDetector() : - _landDetectedPub(-1), - _landDetected({0, false}), - - _vehicleGlobalPositionSub(-1), - _sensorsCombinedSub(-1), - _waypointSub(-1), - _actuatorsSub(-1), - _armingSub(-1), - - _vehicleGlobalPosition({}), - _sensors({}), - _waypoint({}), - _actuators({}), - _arming({}), - - _taskShouldExit(false), - _taskIsRunning(false), - _landTimer(0) -{ - //Advertise the first land detected uORB - _landDetected.timestamp = hrt_absolute_time(); - _landDetected.landed = false; - _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); -} - -MulticopterLandDetector::~MulticopterLandDetector() -{ - _taskShouldExit = true; - close(_landDetectedPub); -} - -/** -* @brief Convinience function for polling uORB subscriptions -* @return true if there was new data and it was successfully copied -**/ -static bool orb_update(const struct orb_metadata *meta, int handle, void *buffer) -{ - bool newData = false; - - //Check if there is new data to grab - if (orb_check(handle, &newData) != OK) { - return false; - } - - if (!newData) { - return false; - } - - if (orb_copy(meta, handle, buffer) != OK) { - return false; - } - - return true; -} - -void MulticopterLandDetector::shutdown() -{ - _taskShouldExit = true; -} - -void MulticopterLandDetector::updateSubscriptions() -{ - orb_update(ORB_ID(vehicle_global_position), _vehicleGlobalPositionSub, &_vehicleGlobalPosition); - orb_update(ORB_ID(sensor_combined), _sensorsCombinedSub, &_sensors); - orb_update(ORB_ID(position_setpoint_triplet), _waypointSub, &_waypoint); - orb_update(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _actuatorsSub, &_actuators); - orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); -} - -void MulticopterLandDetector::landDetectorLoop() -{ - //This should never happen! - if (_taskIsRunning) { return; } - - //Subscribe to position, attitude, arming and velocity changes - _vehicleGlobalPositionSub = orb_subscribe(ORB_ID(vehicle_global_position)); - _sensorsCombinedSub = orb_subscribe(ORB_ID(sensor_combined)); - _waypointSub = orb_subscribe(ORB_ID(position_setpoint_triplet)); - _actuatorsSub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - _armingSub = orb_subscribe(ORB_ID(actuator_armed)); - - //Begin task - _taskIsRunning = true; - _taskShouldExit = false; - - while (!_taskShouldExit) { - - //First poll for new data from our subscriptions - updateSubscriptions(); - - const uint64_t now = hrt_absolute_time(); - - //only detect landing if the autopilot is actively trying to land - if (!_waypoint.current.valid || _waypoint.current.type != SETPOINT_TYPE_LAND) { - _landTimer = now; - - } else { - - //Check if we are moving vertically - bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > MC_LAND_DETECTOR_CLIMBRATE_MAX; - - //Check if we are moving horizontally - bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n - + _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > MC_LAND_DETECTOR_VELOCITY_MAX; - - //Next look if all rotation angles are not moving - bool rotating = sqrtf(_sensors.gyro_rad_s[0] * _sensors.gyro_rad_s[0] + - _sensors.gyro_rad_s[1] * _sensors.gyro_rad_s[1] + - _sensors.gyro_rad_s[2] * _sensors.gyro_rad_s[2]) > MC_LAND_DETECTOR_ROTATION_MAX; - - //Check if thrust output is minimal (about half of default) - bool minimalThrust = _arming.armed && _actuators.control[3] <= MC_LAND_DETECTOR_THRUST_MAX; - - if (verticalMovement || rotating || !minimalThrust || horizontalMovement) { - //Sensed movement, so reset the land detector - _landTimer = now; - } - - } - - // if we have detected a landing for 2 continuous seconds - if (now - _landTimer > MC_LAND_DETECTOR_TRIGGER_TIME) { - if (!_landDetected.landed) { - _landDetected.timestamp = now; - _landDetected.landed = true; - - /* publish the land detected broadcast */ - orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); - } - - } else { - // if we currently think we have landed, but the latest data says we are flying - if (_landDetected.landed) { - _landDetected.timestamp = now; - _landDetected.landed = false; - - /* publish the land detected broadcast */ - orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); - } - } - - //Limit loop rate - usleep(1000000 / MC_LAND_DETECTOR_UPDATE_RATE); - } - - _taskIsRunning = false; - _exit(0); -} - -bool MulticopterLandDetector::isRunning() const -{ - return _taskIsRunning; -} diff --git a/src/modules/mc_land_detector/module.mk b/src/modules/mc_land_detector/module.mk deleted file mode 100644 index 023d48979a..0000000000 --- a/src/modules/mc_land_detector/module.mk +++ /dev/null @@ -1,10 +0,0 @@ -# -# Multirotor land detector -# - -MODULE_COMMAND = mc_land_detector - -SRCS = mc_land_detector_main.cpp \ - MulticopterLandDetector.cpp - -EXTRACXXFLAGS = -Weffc++ From 9ea086bf2d9b3d9d3d480f6ae83447b9669f3603 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Wed, 7 Jan 2015 23:28:20 +0100 Subject: [PATCH 025/269] Astyle: Run astyle to fix code formatting --- makefiles/config_px4fmu-v2_default.mk | 2 +- .../land_detector/FixedwingLandDetector.cpp | 74 +++++++-------- .../land_detector/FixedwingLandDetector.h | 52 +++++------ src/modules/land_detector/LandDetector.cpp | 89 ++++++++++--------- src/modules/land_detector/LandDetector.h | 70 +++++++-------- .../land_detector/MulticopterLandDetector.h | 3 +- .../land_detector/land_detector_main.cpp | 16 ++-- 7 files changed, 154 insertions(+), 152 deletions(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index e13a88ca57..3abebd82fa 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -71,7 +71,7 @@ MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/gpio_led MODULES += modules/uavcan -MODULES += modules/land_detector +MODULES += modules/land_detector # # Estimation modules (EKF/ SO3 / other filters) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 42735f38c8..7df89257f7 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -39,65 +39,65 @@ */ #include "FixedwingLandDetector.h" - + #include #include FixedwingLandDetector::FixedwingLandDetector() : - _vehicleLocalPositionSub(-1), - _vehicleLocalPosition({}), - _airspeedSub(-1), - _airspeed({}), + _vehicleLocalPositionSub(-1), + _vehicleLocalPosition({}), + _airspeedSub(-1), + _airspeed({}), - _velocity_xy_filtered(0.0f), - _velocity_z_filtered(0.0f), - _airspeed_filtered(0.0f), - _landDetectTrigger(0) + _velocity_xy_filtered(0.0f), + _velocity_z_filtered(0.0f), + _airspeed_filtered(0.0f), + _landDetectTrigger(0) { - //ctor + //ctor } void FixedwingLandDetector::initialize() { - //Subscribe to local position and airspeed data - _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position)); - _airspeedSub = orb_subscribe(ORB_ID(airspeed)); + //Subscribe to local position and airspeed data + _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position)); + _airspeedSub = orb_subscribe(ORB_ID(airspeed)); } void FixedwingLandDetector::updateSubscriptions() { - orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition); - orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); + orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition); + orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); } bool FixedwingLandDetector::update() { - //First poll for new data from our subscriptions - updateSubscriptions(); + //First poll for new data from our subscriptions + updateSubscriptions(); - const uint64_t now = hrt_absolute_time(); - bool landDetected = false; + const uint64_t now = hrt_absolute_time(); + bool landDetected = false; - //TODO: reset filtered values on arming? - _velocity_xy_filtered = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx * - _vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy); - _velocity_z_filtered = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz); - _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; + //TODO: reset filtered values on arming? + _velocity_xy_filtered = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx * + _vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy); + _velocity_z_filtered = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz); + _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; - /* crude land detector for fixedwing */ - if (_velocity_xy_filtered < FW_LAND_DETECTOR_VELOCITY_MAX - && _velocity_z_filtered < FW_LAND_DETECTOR_CLIMBRATE_MAX - && _airspeed_filtered < FW_LAND_DETECTOR_AIRSPEED_MAX) { + /* crude land detector for fixedwing */ + if (_velocity_xy_filtered < FW_LAND_DETECTOR_VELOCITY_MAX + && _velocity_z_filtered < FW_LAND_DETECTOR_CLIMBRATE_MAX + && _airspeed_filtered < FW_LAND_DETECTOR_AIRSPEED_MAX) { - //These conditions need to be stable for a period of time before we trust them - if (now > _landDetectTrigger) { - landDetected = true; - } + //These conditions need to be stable for a period of time before we trust them + if (now > _landDetectTrigger) { + landDetected = true; + } - } else { - //reset land detect trigger - _landDetectTrigger = now + FW_LAND_DETECTOR_TRIGGER_TIME; - } + } else { + //reset land detect trigger + _landDetectTrigger = now + FW_LAND_DETECTOR_TRIGGER_TIME; + } - return landDetected; + return landDetected; } diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index faeece6f3c..6737af68a1 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -48,41 +48,41 @@ class FixedwingLandDetector : public LandDetector { public: - FixedwingLandDetector(); + FixedwingLandDetector(); protected: - /** - * @brief blocking loop, should be run in a separate thread or task. Runs at 50Hz - **/ - bool update() override; + /** + * @brief blocking loop, should be run in a separate thread or task. Runs at 50Hz + **/ + bool update() override; - /** - * @brief Initializes the land detection algorithm - **/ - void initialize() override; + /** + * @brief Initializes the land detection algorithm + **/ + void initialize() override; - /** - * @brief polls all subscriptions and pulls any data that has changed - **/ - void updateSubscriptions(); + /** + * @brief polls all subscriptions and pulls any data that has changed + **/ + void updateSubscriptions(); - //Algorithm parameters (TODO: should probably be externalized) - static constexpr uint64_t FW_LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold + //Algorithm parameters (TODO: should probably be externalized) + static constexpr uint64_t FW_LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold before triggering a land */ - static constexpr float FW_LAND_DETECTOR_VELOCITY_MAX = 5.0f; /**< maximum horizontal movement m/s*/ - static constexpr float FW_LAND_DETECTOR_CLIMBRATE_MAX = 10.00f; /**< +- climb rate in m/s*/ - static constexpr float FW_LAND_DETECTOR_AIRSPEED_MAX = 10.00f; /**< airspeed max m/s*/ + static constexpr float FW_LAND_DETECTOR_VELOCITY_MAX = 5.0f; /**< maximum horizontal movement m/s*/ + static constexpr float FW_LAND_DETECTOR_CLIMBRATE_MAX = 10.00f; /**< +- climb rate in m/s*/ + static constexpr float FW_LAND_DETECTOR_AIRSPEED_MAX = 10.00f; /**< airspeed max m/s*/ private: - int _vehicleLocalPositionSub; /**< notification of local position */ - struct vehicle_local_position_s _vehicleLocalPosition; /**< the result from local position subscription */ - int _airspeedSub; - struct airspeed_s _airspeed; + int _vehicleLocalPositionSub; /**< notification of local position */ + struct vehicle_local_position_s _vehicleLocalPosition; /**< the result from local position subscription */ + int _airspeedSub; + struct airspeed_s _airspeed; - float _velocity_xy_filtered; - float _velocity_z_filtered; - float _airspeed_filtered; - uint64_t _landDetectTrigger; + float _velocity_xy_filtered; + float _velocity_z_filtered; + float _airspeed_filtered; + uint64_t _landDetectTrigger; }; #endif //__FIXED_WING_LAND_DETECTOR_H__ diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index a39e538185..5029ce185b 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -3,75 +3,76 @@ #include LandDetector::LandDetector() : - _landDetectedPub(-1), - _landDetected({0, false}), - _taskShouldExit(false), - _taskIsRunning(false) + _landDetectedPub(-1), + _landDetected({0, false}), + _taskShouldExit(false), + _taskIsRunning(false) { - //Advertise the first land detected uORB - _landDetected.timestamp = hrt_absolute_time(); - _landDetected.landed = false; - _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); + //Advertise the first land detected uORB + _landDetected.timestamp = hrt_absolute_time(); + _landDetected.landed = false; + _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); } LandDetector::~LandDetector() { - _taskShouldExit = true; - close(_landDetectedPub); + _taskShouldExit = true; + close(_landDetectedPub); } void LandDetector::shutdown() { - _taskShouldExit = true; + _taskShouldExit = true; } void LandDetector::start() { - //Make sure this method has not already been called by another thread - if(isRunning()) { - return; - } + //Make sure this method has not already been called by another thread + if (isRunning()) { + return; + } - //Task is now running, keep doing so until shutdown() has been called - _taskIsRunning = true; - _taskShouldExit = false; - while(isRunning()) { + //Task is now running, keep doing so until shutdown() has been called + _taskIsRunning = true; + _taskShouldExit = false; - bool landDetected = update(); + while (isRunning()) { - //Publish if land detection state has changed - if (_landDetected.landed != landDetected) { - _landDetected.timestamp = hrt_absolute_time(); - _landDetected.landed = landDetected; + bool landDetected = update(); - /* publish the land detected broadcast */ - orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); - } + //Publish if land detection state has changed + if (_landDetected.landed != landDetected) { + _landDetected.timestamp = hrt_absolute_time(); + _landDetected.landed = landDetected; - //Limit loop rate - usleep(1000000 / LAND_DETECTOR_UPDATE_RATE); - } + /* publish the land detected broadcast */ + orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); + } - _taskIsRunning = false; - _exit(0); + //Limit loop rate + usleep(1000000 / LAND_DETECTOR_UPDATE_RATE); + } + + _taskIsRunning = false; + _exit(0); } bool LandDetector::orb_update(const struct orb_metadata *meta, int handle, void *buffer) { - bool newData = false; + bool newData = false; - //Check if there is new data to grab - if (orb_check(handle, &newData) != OK) { - return false; - } + //Check if there is new data to grab + if (orb_check(handle, &newData) != OK) { + return false; + } - if (!newData) { - return false; - } + if (!newData) { + return false; + } - if (orb_copy(meta, handle, buffer) != OK) { - return false; - } + if (orb_copy(meta, handle, buffer) != OK) { + return false; + } - return true; + return true; } diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index 6d74656917..f7120434c4 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -48,54 +48,54 @@ class LandDetector { public: - LandDetector(); - virtual ~LandDetector(); + LandDetector(); + virtual ~LandDetector(); - /** - * @return true if this task is currently running - **/ - inline bool isRunning() const {return _taskIsRunning;} + /** + * @return true if this task is currently running + **/ + inline bool isRunning() const {return _taskIsRunning;} - /** - * @brief Tells the Land Detector task that it should exit - **/ - void shutdown(); + /** + * @brief Tells the Land Detector task that it should exit + **/ + void shutdown(); - /** - * @brief Blocking function that should be called from it's own task thread. This method will - * run the underlying algorithm at the desired update rate and publish if the landing state changes. - **/ - void start(); + /** + * @brief Blocking function that should be called from it's own task thread. This method will + * run the underlying algorithm at the desired update rate and publish if the landing state changes. + **/ + void start(); protected: - /** - * @brief Pure abstract method that must be overriden by sub-classes. This actually runs the underlying algorithm - * @return true if a landing was detected and this should be broadcast to the rest of the system - **/ - virtual bool update() = 0; + /** + * @brief Pure abstract method that must be overriden by sub-classes. This actually runs the underlying algorithm + * @return true if a landing was detected and this should be broadcast to the rest of the system + **/ + virtual bool update() = 0; - /** - * @brief Pure abstract method that is called by this class once for initializing the uderlying algorithm (memory allocation, - * uORB topic subscription, creating file descriptors, etc.) - **/ - virtual void initialize() = 0; + /** + * @brief Pure abstract method that is called by this class once for initializing the uderlying algorithm (memory allocation, + * uORB topic subscription, creating file descriptors, etc.) + **/ + virtual void initialize() = 0; - /** - * @brief Convinience function for polling uORB subscriptions - * @return true if there was new data and it was successfully copied - **/ - bool orb_update(const struct orb_metadata *meta, int handle, void *buffer); + /** + * @brief Convinience function for polling uORB subscriptions + * @return true if there was new data and it was successfully copied + **/ + bool orb_update(const struct orb_metadata *meta, int handle, void *buffer); - static constexpr uint32_t LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */ + static constexpr uint32_t LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */ protected: - orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ - struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ + orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ + struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ private: - bool _taskShouldExit; /**< true if it is requested that this task should exit */ - bool _taskIsRunning; /**< task has reached main loop and is currently running */ + bool _taskShouldExit; /**< true if it is requested that this task should exit */ + bool _taskIsRunning; /**< task has reached main loop and is currently running */ }; #endif //__LAND_DETECTOR_H__ \ No newline at end of file diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 08a8132ba5..5b4172a36d 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -75,7 +75,8 @@ protected: static constexpr float MC_LAND_DETECTOR_ROTATION_MAX = 0.5f; /**< max rotation in rad/sec (= 30 deg/s) */ static constexpr float MC_LAND_DETECTOR_THRUST_MAX = 0.2f; static constexpr float MC_LAND_DETECTOR_VELOCITY_MAX = 1.0f; /**< max +- horizontal movement in m/s */ - static constexpr uint32_t MC_LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold before triggering a land */ + static constexpr uint32_t MC_LAND_DETECTOR_TRIGGER_TIME = + 2000000; /**< usec that landing conditions have to hold before triggering a land */ private: int _vehicleGlobalPositionSub; /**< notification of global position */ diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index 2bc89e7526..7c0ffb82ad 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -51,7 +51,7 @@ #include "MulticopterLandDetector.h" //Function prototypes -static int land_detector_start(const char* mode); +static int land_detector_start(const char *mode); static void land_detector_stop(); /** @@ -110,7 +110,7 @@ static void land_detector_stop() /** * Start new task, fails if it is already running. Returns OK if successful **/ -static int land_detector_start(const char* mode) +static int land_detector_start(const char *mode) { if (land_detector_task != nullptr || _landDetectorTaskID != -1) { errx(1, "already running"); @@ -118,15 +118,15 @@ static int land_detector_start(const char* mode) } //Allocate memory - if(!strcmp(mode, "fixedwing")) { + if (!strcmp(mode, "fixedwing")) { land_detector_task = new FixedwingLandDetector(); - } - else if(!strcmp(mode, "multicopter")) { + + } else if (!strcmp(mode, "multicopter")) { land_detector_task = new MulticopterLandDetector(); - } - else { + + } else { errx(1, "[mode] must be either 'fixedwing' or 'multicopter'"); - return -1; + return -1; } //Check if alloc worked From 73c7b44f6a684df80b550a4ce52c883348684045 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 8 Jan 2015 14:22:39 +0100 Subject: [PATCH 026/269] ROMFS: Corrected land detector startup scripts --- ROMFS/px4fmu_common/init.d/rc.fw_apps | 2 +- ROMFS/px4fmu_common/init.d/rc.mc_apps | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index c6947009bb..173c1846b2 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -17,7 +17,7 @@ fw_pos_control_l1 start # # Start Land Detector # -fw_land_detector start +land_detector start fixedwing # # Misc apps diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 9c3d6352b3..033b3b6406 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -14,4 +14,4 @@ mc_pos_control start # # Start Land Detector # -mc_land_detector start +land_detector start multicopter From 1356c44f0e27e9ab4d1c2df4cccbe4ac6fb2f1c4 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Wed, 14 Jan 2015 17:41:26 +0100 Subject: [PATCH 027/269] LandDetector: Fix land detection algorithm not being initialized --- src/modules/land_detector/LandDetector.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 5029ce185b..2f1310cab0 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -8,10 +8,7 @@ LandDetector::LandDetector() : _taskShouldExit(false), _taskIsRunning(false) { - //Advertise the first land detected uORB - _landDetected.timestamp = hrt_absolute_time(); - _landDetected.landed = false; - _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); + //ctor } LandDetector::~LandDetector() @@ -32,6 +29,14 @@ void LandDetector::start() return; } + //Advertise the first land detected uORB + _landDetected.timestamp = hrt_absolute_time(); + _landDetected.landed = false; + _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); + + //Initialize land detection algorithm + initialize(); + //Task is now running, keep doing so until shutdown() has been called _taskIsRunning = true; _taskShouldExit = false; From f1587da4c46e92474687f37ad49fbd003b7c91db Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Wed, 14 Jan 2015 17:47:17 +0100 Subject: [PATCH 028/269] MulticopterLandDetector: Detect land even if autopilot is not landing --- .../land_detector/FixedwingLandDetector.cpp | 2 +- .../land_detector/MulticopterLandDetector.cpp | 50 +++++++++---------- .../land_detector/land_detector_main.cpp | 2 +- 3 files changed, 26 insertions(+), 28 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 7df89257f7..38f6c00a95 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -43,7 +43,7 @@ #include #include -FixedwingLandDetector::FixedwingLandDetector() : +FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), _vehicleLocalPositionSub(-1), _vehicleLocalPosition({}), _airspeedSub(-1), diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 36d92fd798..81af029fb1 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -44,7 +44,7 @@ #include #include -MulticopterLandDetector::MulticopterLandDetector() : +MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _vehicleGlobalPositionSub(-1), _sensorsCombinedSub(-1), _waypointSub(-1), @@ -86,34 +86,32 @@ bool MulticopterLandDetector::update() //First poll for new data from our subscriptions updateSubscriptions(); + //Only trigger flight conditions if we are armed + if(!_arming.armed) { + return true; + } + const uint64_t now = hrt_absolute_time(); - //only detect landing if the autopilot is actively trying to land - if (!_waypoint.current.valid || _waypoint.current.type != SETPOINT_TYPE_LAND) { + //Check if we are moving vertically + bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > MC_LAND_DETECTOR_CLIMBRATE_MAX; + + //Check if we are moving horizontally + bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n + + _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > MC_LAND_DETECTOR_VELOCITY_MAX; + + //Next look if all rotation angles are not moving + bool rotating = sqrtf(_sensors.gyro_rad_s[0] * _sensors.gyro_rad_s[0] + + _sensors.gyro_rad_s[1] * _sensors.gyro_rad_s[1] + + _sensors.gyro_rad_s[2] * _sensors.gyro_rad_s[2]) > MC_LAND_DETECTOR_ROTATION_MAX; + + //Check if thrust output is minimal (about half of default) + bool minimalThrust = _actuators.control[3] <= MC_LAND_DETECTOR_THRUST_MAX; + + if (verticalMovement || rotating || !minimalThrust || horizontalMovement) { + //Sensed movement, so reset the land detector _landTimer = now; - - } else { - - //Check if we are moving vertically - bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > MC_LAND_DETECTOR_CLIMBRATE_MAX; - - //Check if we are moving horizontally - bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n - + _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > MC_LAND_DETECTOR_VELOCITY_MAX; - - //Next look if all rotation angles are not moving - bool rotating = sqrtf(_sensors.gyro_rad_s[0] * _sensors.gyro_rad_s[0] + - _sensors.gyro_rad_s[1] * _sensors.gyro_rad_s[1] + - _sensors.gyro_rad_s[2] * _sensors.gyro_rad_s[2]) > MC_LAND_DETECTOR_ROTATION_MAX; - - //Check if thrust output is minimal (about half of default) - bool minimalThrust = _arming.armed && _actuators.control[3] <= MC_LAND_DETECTOR_THRUST_MAX; - - if (verticalMovement || rotating || !minimalThrust || horizontalMovement) { - //Sensed movement, so reset the land detector - _landTimer = now; - } - + return false; } return now - _landTimer > MC_LAND_DETECTOR_TRIGGER_TIME; diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index 7c0ffb82ad..9e135b5f17 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -104,7 +104,7 @@ static void land_detector_stop() delete land_detector_task; land_detector_task = nullptr; _landDetectorTaskID = -1; - warn("land_detector has been stopped"); + errx(0, "land_detector has been stopped"); } /** From e40d207311126b05a7fd87739fb72d2ae8d7d500 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 15 Jan 2015 10:06:56 +0100 Subject: [PATCH 029/269] AStyle: Fixed file formatting --- ROMFS/px4fmu_common/init.d/rc.fw_apps | 2 +- src/modules/land_detector/LandDetector.cpp | 2 +- src/modules/land_detector/LandDetector.h | 2 +- src/modules/land_detector/MulticopterLandDetector.cpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index 173c1846b2..9e2d1f6ffe 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -21,4 +21,4 @@ land_detector start fixedwing # # Misc apps -# \ No newline at end of file +# diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 2f1310cab0..1dd09b6a67 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -32,7 +32,7 @@ void LandDetector::start() //Advertise the first land detected uORB _landDetected.timestamp = hrt_absolute_time(); _landDetected.landed = false; - _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); + _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); //Initialize land detection algorithm initialize(); diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index f7120434c4..ba15ad498b 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -98,4 +98,4 @@ private: bool _taskIsRunning; /**< task has reached main loop and is currently running */ }; -#endif //__LAND_DETECTOR_H__ \ No newline at end of file +#endif //__LAND_DETECTOR_H__ diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 81af029fb1..8c25ae5fe7 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -87,7 +87,7 @@ bool MulticopterLandDetector::update() updateSubscriptions(); //Only trigger flight conditions if we are armed - if(!_arming.armed) { + if (!_arming.armed) { return true; } From 92add9cf8003c4fd8b01143626c3a101062dd9dd Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 15 Jan 2015 14:36:04 +0100 Subject: [PATCH 030/269] LandDetector: Externalized algorithm parameters --- .../land_detector/FixedwingLandDetector.cpp | 33 +++++- .../land_detector/FixedwingLandDetector.h | 29 ++++- src/modules/land_detector/LandDetector.h | 3 + .../land_detector/MulticopterLandDetector.cpp | 41 ++++++- .../land_detector/MulticopterLandDetector.h | 33 ++++-- .../land_detector/land_detector_params.c | 105 ++++++++++++++++++ src/modules/land_detector/module.mk | 1 + 7 files changed, 221 insertions(+), 24 deletions(-) create mode 100644 src/modules/land_detector/land_detector_params.c diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 38f6c00a95..0d6f373100 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -44,17 +44,22 @@ #include FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), + _paramHandle(), + _params(), _vehicleLocalPositionSub(-1), _vehicleLocalPosition({}), _airspeedSub(-1), _airspeed({}), + _parameterSub(-1), _velocity_xy_filtered(0.0f), _velocity_z_filtered(0.0f), _airspeed_filtered(0.0f), _landDetectTrigger(0) { - //ctor + _paramHandle.maxVelocity = param_find("LAND_FW_VELOCITY_MAX"); + _paramHandle.maxClimbRate = param_find("LAND_FW_MAX_CLIMB_RATE"); + _paramHandle.maxAirSpeed = param_find("LAND_FW_AIR_SPEED_MAX"); } void FixedwingLandDetector::initialize() @@ -85,9 +90,9 @@ bool FixedwingLandDetector::update() _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; /* crude land detector for fixedwing */ - if (_velocity_xy_filtered < FW_LAND_DETECTOR_VELOCITY_MAX - && _velocity_z_filtered < FW_LAND_DETECTOR_CLIMBRATE_MAX - && _airspeed_filtered < FW_LAND_DETECTOR_AIRSPEED_MAX) { + if (_velocity_xy_filtered < _params.maxVelocity + && _velocity_z_filtered < _params.maxClimbRate + && _airspeed_filtered < _params.maxAirSpeed) { //These conditions need to be stable for a period of time before we trust them if (now > _landDetectTrigger) { @@ -96,8 +101,26 @@ bool FixedwingLandDetector::update() } else { //reset land detect trigger - _landDetectTrigger = now + FW_LAND_DETECTOR_TRIGGER_TIME; + _landDetectTrigger = now + LAND_DETECTOR_TRIGGER_TIME; } return landDetected; } + +void FixedwingLandDetector::updateParameterCache(const bool force) +{ + bool updated; + parameter_update_s paramUpdate; + + orb_check(_parameterSub, &updated); + + if (updated) { + orb_copy(ORB_ID(parameter_update), _parameterSub, ¶mUpdate); + } + + if (updated || force) { + param_get(_paramHandle.maxVelocity, &_params.maxVelocity); + param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate); + param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed); + } +} diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index 6737af68a1..0e9c092ee0 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -44,6 +44,8 @@ #include "LandDetector.h" #include #include +#include +#include class FixedwingLandDetector : public LandDetector { @@ -66,18 +68,33 @@ protected: **/ void updateSubscriptions(); - //Algorithm parameters (TODO: should probably be externalized) - static constexpr uint64_t FW_LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold - before triggering a land */ - static constexpr float FW_LAND_DETECTOR_VELOCITY_MAX = 5.0f; /**< maximum horizontal movement m/s*/ - static constexpr float FW_LAND_DETECTOR_CLIMBRATE_MAX = 10.00f; /**< +- climb rate in m/s*/ - static constexpr float FW_LAND_DETECTOR_AIRSPEED_MAX = 10.00f; /**< airspeed max m/s*/ +private: + /** + * @brief download and update local parameter cache + **/ + void updateParameterCache(const bool force); + + /** + * @brief Handles for interesting parameters + **/ + struct { + param_t maxVelocity; + param_t maxClimbRate; + param_t maxAirSpeed; + } _paramHandle; + + struct { + float maxVelocity; + float maxClimbRate; + float maxAirSpeed; + } _params; private: int _vehicleLocalPositionSub; /**< notification of local position */ struct vehicle_local_position_s _vehicleLocalPosition; /**< the result from local position subscription */ int _airspeedSub; struct airspeed_s _airspeed; + int _parameterSub; float _velocity_xy_filtered; float _velocity_z_filtered; diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index ba15ad498b..09db6e4746 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -89,6 +89,9 @@ protected: static constexpr uint32_t LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */ + static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold + before triggering a land */ + protected: orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 8c25ae5fe7..278b0cd51e 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -45,11 +45,14 @@ #include MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), + _paramHandle(), + _params(), _vehicleGlobalPositionSub(-1), _sensorsCombinedSub(-1), _waypointSub(-1), _actuatorsSub(-1), _armingSub(-1), + _parameterSub(-1), _vehicleGlobalPosition({}), _sensors({}), @@ -59,7 +62,10 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _landTimer(0) { - //ctor + _paramHandle.maxRotation = param_find("LAND_MC_MAX_CLIMB_RATE"); + _paramHandle.maxVelocity = param_find("LAND_MC_VELOCITY_MAX"); + _paramHandle.maxClimbRate = param_find("LAND_MC_ROTATION_MAX"); + _paramHandle.maxThrottle = param_find("LAND_MC_THRUST_MAX"); } void MulticopterLandDetector::initialize() @@ -70,6 +76,10 @@ void MulticopterLandDetector::initialize() _waypointSub = orb_subscribe(ORB_ID(position_setpoint_triplet)); _actuatorsSub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); _armingSub = orb_subscribe(ORB_ID(actuator_armed)); + _parameterSub = orb_subscribe(ORB_ID(parameter_update)); + + //Download parameters + updateParameterCache(true); } void MulticopterLandDetector::updateSubscriptions() @@ -94,19 +104,19 @@ bool MulticopterLandDetector::update() const uint64_t now = hrt_absolute_time(); //Check if we are moving vertically - bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > MC_LAND_DETECTOR_CLIMBRATE_MAX; + bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > _params.maxClimbRate; //Check if we are moving horizontally bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n - + _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > MC_LAND_DETECTOR_VELOCITY_MAX; + + _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > _params.maxVelocity; //Next look if all rotation angles are not moving bool rotating = sqrtf(_sensors.gyro_rad_s[0] * _sensors.gyro_rad_s[0] + _sensors.gyro_rad_s[1] * _sensors.gyro_rad_s[1] + - _sensors.gyro_rad_s[2] * _sensors.gyro_rad_s[2]) > MC_LAND_DETECTOR_ROTATION_MAX; + _sensors.gyro_rad_s[2] * _sensors.gyro_rad_s[2]) > _params.maxRotation; //Check if thrust output is minimal (about half of default) - bool minimalThrust = _actuators.control[3] <= MC_LAND_DETECTOR_THRUST_MAX; + bool minimalThrust = _actuators.control[3] <= _params.maxThrottle; if (verticalMovement || rotating || !minimalThrust || horizontalMovement) { //Sensed movement, so reset the land detector @@ -114,5 +124,24 @@ bool MulticopterLandDetector::update() return false; } - return now - _landTimer > MC_LAND_DETECTOR_TRIGGER_TIME; + return now - _landTimer > LAND_DETECTOR_TRIGGER_TIME; +} + +void MulticopterLandDetector::updateParameterCache(const bool force) +{ + bool updated; + parameter_update_s paramUpdate; + + orb_check(_parameterSub, &updated); + + if (updated) { + orb_copy(ORB_ID(parameter_update), _parameterSub, ¶mUpdate); + } + + if (updated || force) { + param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate); + param_get(_paramHandle.maxVelocity, &_params.maxVelocity); + param_get(_paramHandle.maxRotation, &_params.maxRotation); + param_get(_paramHandle.maxThrottle, &_params.maxThrottle); + } } diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 5b4172a36d..7bb7f1a907 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -48,6 +48,8 @@ #include #include #include +#include +#include class MulticopterLandDetector : public LandDetector { @@ -70,13 +72,29 @@ protected: **/ void initialize() override; - //Algorithm parameters (TODO: should probably be externalized) - static constexpr float MC_LAND_DETECTOR_CLIMBRATE_MAX = 0.30f; /**< max +- climb rate in m/s */ - static constexpr float MC_LAND_DETECTOR_ROTATION_MAX = 0.5f; /**< max rotation in rad/sec (= 30 deg/s) */ - static constexpr float MC_LAND_DETECTOR_THRUST_MAX = 0.2f; - static constexpr float MC_LAND_DETECTOR_VELOCITY_MAX = 1.0f; /**< max +- horizontal movement in m/s */ - static constexpr uint32_t MC_LAND_DETECTOR_TRIGGER_TIME = - 2000000; /**< usec that landing conditions have to hold before triggering a land */ + +private: + /** + * @brief download and update local parameter cache + **/ + void updateParameterCache(const bool force); + + /** + * @brief Handles for interesting parameters + **/ + struct { + param_t maxClimbRate; + param_t maxVelocity; + param_t maxRotation; + param_t maxThrottle; + } _paramHandle; + + struct { + float maxClimbRate; + float maxVelocity; + float maxRotation; + float maxThrottle; + } _params; private: int _vehicleGlobalPositionSub; /**< notification of global position */ @@ -84,6 +102,7 @@ private: int _waypointSub; int _actuatorsSub; int _armingSub; + int _parameterSub; struct vehicle_global_position_s _vehicleGlobalPosition; /**< the result from global position subscription */ struct sensor_combined_s _sensors; /**< subscribe to sensor readings */ diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c new file mode 100644 index 0000000000..4aecae5372 --- /dev/null +++ b/src/modules/land_detector/land_detector_params.c @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Anton Babushkin + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file land_detector.c + * Land detector algorithm parameters. + * + * @author Johan Jansen + */ + +#include + +/** + * Multicopter max climb rate + * + * Maximum vertical velocity allowed to trigger a land (m/s up and down) + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LAND_MC_MAX_CLIMB_RATE, 0.30f); + +/** + * Multicopter max horizontal velocity + * + * Maximum horizontal velocity allowed to trigger a land (m/s) + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LAND_MC_VELOCITY_MAX, 1.00f); + +/** + * Multicopter max rotation + * + * Maximum allowed around each axis to trigger a land (radians per second) + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LAND_MC_MAX_ROTATION, 0.20f); + +/** + * Multicopter max throttle + * + * Maximum actuator output on throttle before triggering a land + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LAND_MC_MAX_THROTTLE, 0.20f); + +/** + * Fixedwing max horizontal velocity + * + * Maximum horizontal velocity allowed to trigger a land (m/s) + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LAND_FW_VELOCITY_MAX, 0.20f); + +/** + * Fixedwing max climb rate + * + * Maximum vertical velocity allowed to trigger a land (m/s up and down) + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LAND_FW_MAX_CLIMB_RATE, 10.00f); + +/** + * Airspeed max + * + * Maximum airspeed allowed to trigger a land (m/s) + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LAND_FW_AIRSPEED_MAX, 10.00f); diff --git a/src/modules/land_detector/module.mk b/src/modules/land_detector/module.mk index db0e184472..e08a4b7a81 100644 --- a/src/modules/land_detector/module.mk +++ b/src/modules/land_detector/module.mk @@ -5,6 +5,7 @@ MODULE_COMMAND = land_detector SRCS = land_detector_main.cpp \ + land_detector_params.c \ LandDetector.cpp \ MulticopterLandDetector.cpp \ FixedwingLandDetector.cpp From 510a314386529f95978078d27da25368435d8d90 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 15 Jan 2015 14:58:06 +0100 Subject: [PATCH 031/269] LandDetector: Shorter and less ambiguous param names --- src/modules/land_detector/FixedwingLandDetector.cpp | 6 +++--- .../land_detector/MulticopterLandDetector.cpp | 8 ++++---- src/modules/land_detector/land_detector_params.c | 12 ++++++------ 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 0d6f373100..52084e4c05 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -57,9 +57,9 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), _airspeed_filtered(0.0f), _landDetectTrigger(0) { - _paramHandle.maxVelocity = param_find("LAND_FW_VELOCITY_MAX"); - _paramHandle.maxClimbRate = param_find("LAND_FW_MAX_CLIMB_RATE"); - _paramHandle.maxAirSpeed = param_find("LAND_FW_AIR_SPEED_MAX"); + _paramHandle.maxVelocity = param_find("LAND_FW_VEL_XY_MAX"); + _paramHandle.maxClimbRate = param_find("LAND_FW_VEL_Z_MAX"); + _paramHandle.maxAirSpeed = param_find("LAND_FW_AIRSPEED_MAX"); } void FixedwingLandDetector::initialize() diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 278b0cd51e..5371346181 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -62,10 +62,10 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _landTimer(0) { - _paramHandle.maxRotation = param_find("LAND_MC_MAX_CLIMB_RATE"); - _paramHandle.maxVelocity = param_find("LAND_MC_VELOCITY_MAX"); - _paramHandle.maxClimbRate = param_find("LAND_MC_ROTATION_MAX"); - _paramHandle.maxThrottle = param_find("LAND_MC_THRUST_MAX"); + _paramHandle.maxRotation = param_find("LAND_MC_Z_VEL_MAX"); + _paramHandle.maxVelocity = param_find("LAND_MC_XY_VEL_MAX"); + _paramHandle.maxClimbRate = param_find("LAND_MC_ROT_MAX"); + _paramHandle.maxThrottle = param_find("LAND_MC_THR_MAX"); } void MulticopterLandDetector::initialize() diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index 4aecae5372..e2acf42b3d 100644 --- a/src/modules/land_detector/land_detector_params.c +++ b/src/modules/land_detector/land_detector_params.c @@ -48,7 +48,7 @@ * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LAND_MC_MAX_CLIMB_RATE, 0.30f); +PARAM_DEFINE_FLOAT(LAND_MC_Z_VEL_MAX, 0.30f); /** * Multicopter max horizontal velocity @@ -57,7 +57,7 @@ PARAM_DEFINE_FLOAT(LAND_MC_MAX_CLIMB_RATE, 0.30f); * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LAND_MC_VELOCITY_MAX, 1.00f); +PARAM_DEFINE_FLOAT(LAND_MC_XY_VEL_MAX, 1.00f); /** * Multicopter max rotation @@ -66,7 +66,7 @@ PARAM_DEFINE_FLOAT(LAND_MC_VELOCITY_MAX, 1.00f); * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LAND_MC_MAX_ROTATION, 0.20f); +PARAM_DEFINE_FLOAT(LAND_MC_ROT_MAX, 0.20f); /** * Multicopter max throttle @@ -75,7 +75,7 @@ PARAM_DEFINE_FLOAT(LAND_MC_MAX_ROTATION, 0.20f); * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LAND_MC_MAX_THROTTLE, 0.20f); +PARAM_DEFINE_FLOAT(LAND_MC_THR_MAX, 0.20f); /** * Fixedwing max horizontal velocity @@ -84,7 +84,7 @@ PARAM_DEFINE_FLOAT(LAND_MC_MAX_THROTTLE, 0.20f); * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LAND_FW_VELOCITY_MAX, 0.20f); +PARAM_DEFINE_FLOAT(LAND_FW_VEL_XY_MAX, 0.20f); /** * Fixedwing max climb rate @@ -93,7 +93,7 @@ PARAM_DEFINE_FLOAT(LAND_FW_VELOCITY_MAX, 0.20f); * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LAND_FW_MAX_CLIMB_RATE, 10.00f); +PARAM_DEFINE_FLOAT(LAND_FW_VEL_Z_MAX, 10.00f); /** * Airspeed max From 818ccf7a04fd1aacef4d9e1c6d96493a37302006 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Fri, 16 Jan 2015 15:51:57 +0100 Subject: [PATCH 032/269] Commander: Fix for resetting home timestamp on arm --- src/modules/commander/commander.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f511f38762..bae1ed2e43 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1860,7 +1860,8 @@ int commander_thread_main(int argc, char *argv[]) } - hrt_abstime t1 = hrt_absolute_time(); + //Get current timestamp + const hrt_abstime now = hrt_absolute_time(); /* print new state */ if (arming_state_changed) { @@ -1871,7 +1872,8 @@ int commander_thread_main(int argc, char *argv[]) if (armed.armed && !was_armed && hrt_absolute_time() > start_time + 2000000 && status.condition_global_position_valid && (global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) { - // TODO remove code duplication + // TODO remove code duplication (setting home is also done somewhere else in this file) + home.timestamp = now; home.lat = global_position.lat; home.lon = global_position.lon; home.alt = global_position.alt; @@ -1932,13 +1934,13 @@ int commander_thread_main(int argc, char *argv[]) /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { set_control_mode(); - control_mode.timestamp = t1; + control_mode.timestamp = now; orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); - status.timestamp = t1; + status.timestamp = now; orb_publish(ORB_ID(vehicle_status), status_pub, &status); - armed.timestamp = t1; + armed.timestamp = now; orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } From a581ae8ed60f0f08d4bf407aeeb0f3979b6ba38a Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Fri, 16 Jan 2015 16:43:11 +0100 Subject: [PATCH 033/269] Commander: Remove duplicate code for setting home position --- src/modules/commander/commander.cpp | 126 ++++++++++++++-------------- 1 file changed, 64 insertions(+), 62 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bae1ed2e43..57f94259ca 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -240,6 +240,12 @@ transition_result_t check_navigation_state_machine(struct vehicle_status_s *stat transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy); +/** +* @brief This function initializes the home position of the vehicle. This happens first time we get a good GPS fix and each +* time the vehicle is armed with a good GPS fix. +**/ +static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home, const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition); + /** * Loop that runs at a lower rate and priority for calibration and parameter tasks. */ @@ -716,6 +722,52 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s return true; } +/** +* @brief This function initializes the home position of the vehicle. This happens first time we get a good GPS fix and each +* time the vehicle is armed with a good GPS fix. +**/ +static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home, const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition) +{ + //Need global position fix to be able to set home + if(!status.condition_global_position_valid) { + return; + } + + //Ensure that the GPS accuracy is good enough for intializing home + if(globalPosition.eph > eph_threshold || globalPosition.epv > epv_threshold) { + return; + } + + //Set home position + home.timestamp = hrt_absolute_time(); + home.lat = globalPosition.lat; + home.lon = globalPosition.lon; + home.alt = globalPosition.alt; + + home.x = localPosition.x; + home.y = localPosition.y; + home.z = localPosition.z; + + warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); + mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); + + /* announce new home position */ + if (homePub > 0) { + orb_publish(ORB_ID(home_position), homePub, &home); + + } else { + homePub = orb_advertise(ORB_ID(home_position), &home); + } + + //Play tune first time we initialize HOME + if(!status.condition_home_position_valid) { + tune_positive(true); + } + + /* mark home position as set */ + status.condition_home_position_valid = true; +} + int commander_thread_main(int argc, char *argv[]) { /* not yet initialized */ @@ -904,7 +956,6 @@ int commander_thread_main(int argc, char *argv[]) bool critical_battery_voltage_actions_done = false; hrt_abstime last_idle_time = 0; - hrt_abstime start_time = 0; bool status_changed = true; bool param_init_forced = true; @@ -1035,7 +1086,7 @@ int commander_thread_main(int argc, char *argv[]) commander_initialized = true; thread_running = true; - start_time = hrt_absolute_time(); + const hrt_abstime commander_boot_timestamp = hrt_absolute_time(); transition_result_t arming_ret; @@ -1325,34 +1376,6 @@ int commander_thread_main(int argc, char *argv[]) check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), &status_changed); - /* update home position */ - if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed && - (global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) { - - home.lat = global_position.lat; - home.lon = global_position.lon; - home.alt = global_position.alt; - - home.x = local_position.x; - home.y = local_position.y; - home.z = local_position.z; - - warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); - mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); - - /* announce new home position */ - if (home_pub > 0) { - orb_publish(ORB_ID(home_position), home_pub, &home); - - } else { - home_pub = orb_advertise(ORB_ID(home_position), &home); - } - - /* mark home position as set */ - status.condition_home_position_valid = true; - tune_positive(true); - } - /* update condition_local_position_valid and condition_local_altitude_valid */ /* hysteresis for EPH */ bool local_eph_good; @@ -1401,7 +1424,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls); /* only consider battery voltage if system has been running 2s and battery voltage is valid */ - if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) { + if (hrt_absolute_time() > commander_boot_timestamp + 2000000 && battery.voltage_filtered_v > 0.0f) { status.battery_voltage = battery.voltage_filtered_v; status.battery_current = battery.current_a; status.condition_battery_voltage_valid = true; @@ -1859,44 +1882,23 @@ int commander_thread_main(int argc, char *argv[]) } } - //Get current timestamp const hrt_abstime now = hrt_absolute_time(); + //First time home position update + if(!status.condition_home_position_valid) { + commander_set_home_position(home_pub, home, local_position, global_position); + } + + /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ + else if(arming_state_changed && armed.armed && !was_armed && now > commander_boot_timestamp + 2000000) { + commander_set_home_position(home_pub, home, local_position, global_position); + } + /* print new state */ if (arming_state_changed) { status_changed = true; mavlink_log_info(mavlink_fd, "[cmd] arming state: %s", arming_states_str[status.arming_state]); - - /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ - if (armed.armed && !was_armed && hrt_absolute_time() > start_time + 2000000 && status.condition_global_position_valid && - (global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) { - - // TODO remove code duplication (setting home is also done somewhere else in this file) - home.timestamp = now; - home.lat = global_position.lat; - home.lon = global_position.lon; - home.alt = global_position.alt; - - home.x = local_position.x; - home.y = local_position.y; - home.z = local_position.z; - - warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); - mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); - - /* announce new home position */ - if (home_pub > 0) { - orb_publish(ORB_ID(home_position), home_pub, &home); - - } else { - home_pub = orb_advertise(ORB_ID(home_position), &home); - } - - /* mark home position as set */ - status.condition_home_position_valid = true; - } - arming_state_changed = false; } From 9ecadcd9b46ccf9bc58ea008b77f8408155c3665 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Fri, 16 Jan 2015 16:46:16 +0100 Subject: [PATCH 034/269] Astyle: Fix format for commander.cpp --- src/modules/commander/commander.cpp | 50 +++++++++++++++++------------ 1 file changed, 29 insertions(+), 21 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 57f94259ca..aac6b20023 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -244,7 +244,8 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armed * @brief This function initializes the home position of the vehicle. This happens first time we get a good GPS fix and each * time the vehicle is armed with a good GPS fix. **/ -static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home, const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition); +static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home, + const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition); /** * Loop that runs at a lower rate and priority for calibration and parameter tasks. @@ -566,7 +567,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } break; - /* Flight termination */ + /* Flight termination */ case VEHICLE_CMD_DO_FLIGHTTERMINATION: { if (cmd->param1 > 0.5f) { //XXX update state machine? @@ -726,15 +727,16 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s * @brief This function initializes the home position of the vehicle. This happens first time we get a good GPS fix and each * time the vehicle is armed with a good GPS fix. **/ -static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home, const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition) +static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home, + const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition) { //Need global position fix to be able to set home - if(!status.condition_global_position_valid) { + if (!status.condition_global_position_valid) { return; } //Ensure that the GPS accuracy is good enough for intializing home - if(globalPosition.eph > eph_threshold || globalPosition.epv > epv_threshold) { + if (globalPosition.eph > eph_threshold || globalPosition.epv > epv_threshold) { return; } @@ -760,7 +762,7 @@ static void commander_set_home_position(orb_advert_t &homePub, home_position_s & } //Play tune first time we initialize HOME - if(!status.condition_home_position_valid) { + if (!status.condition_home_position_valid) { tune_positive(true); } @@ -1147,8 +1149,8 @@ int commander_thread_main(int argc, char *argv[]) } /* set vehicle_status.is_vtol flag */ - status.is_vtol = (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR) || - (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR); + status.is_vtol = (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR) || + (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR); /* check and update system / component ID */ param_get(_param_system_id, &(status.system_id)); @@ -1310,12 +1312,12 @@ int commander_thread_main(int argc, char *argv[]) } //Notify the user if the status of the safety switch changes - if(safety.safety_switch_available && previous_safety_off != safety.safety_off) { + if (safety.safety_switch_available && previous_safety_off != safety.safety_off) { - if(safety.safety_off) { + if (safety.safety_off) { set_tune(TONE_NOTIFY_POSITIVE_TUNE); - } - else { + + } else { tune_neutral(true); } @@ -1330,6 +1332,7 @@ int commander_thread_main(int argc, char *argv[]) /* vtol status changed */ orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status); status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab; + /* Make sure that this is only adjusted if vehicle realy is of type vtol*/ if (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR || VEHICLE_TYPE_VTOL_QUADROTOR) { status.is_rotary_wing = vtol_status.vtol_in_rw_mode; @@ -1632,7 +1635,8 @@ int commander_thread_main(int argc, char *argv[]) } else { if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %llums",(hrt_absolute_time()-status.rc_signal_lost_timestamp)/1000); + mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %llums", + (hrt_absolute_time() - status.rc_signal_lost_timestamp) / 1000); status_changed = true; } } @@ -1733,9 +1737,9 @@ int commander_thread_main(int argc, char *argv[]) } else { if (!status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST (at t=%llums)",hrt_absolute_time()/1000); + mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST (at t=%llums)", hrt_absolute_time() / 1000); status.rc_signal_lost = true; - status.rc_signal_lost_timestamp=sp_man.timestamp; + status.rc_signal_lost_timestamp = sp_man.timestamp; status_changed = true; } } @@ -1886,13 +1890,13 @@ int commander_thread_main(int argc, char *argv[]) const hrt_abstime now = hrt_absolute_time(); //First time home position update - if(!status.condition_home_position_valid) { - commander_set_home_position(home_pub, home, local_position, global_position); + if (!status.condition_home_position_valid) { + commander_set_home_position(home_pub, home, local_position, global_position); } /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ - else if(arming_state_changed && armed.armed && !was_armed && now > commander_boot_timestamp + 2000000) { - commander_set_home_position(home_pub, home, local_position, global_position); + else if (arming_state_changed && armed.armed && !was_armed && now > commander_boot_timestamp + 2000000) { + commander_set_home_position(home_pub, home, local_position, global_position); } /* print new state */ @@ -1919,11 +1923,14 @@ int commander_thread_main(int argc, char *argv[]) if (status.failsafe != failsafe_old) { status_changed = true; + if (status.failsafe) { mavlink_log_critical(mavlink_fd, "failsafe mode on"); + } else { mavlink_log_critical(mavlink_fd, "failsafe mode off"); } + failsafe_old = status.failsafe; } @@ -1969,7 +1976,7 @@ int commander_thread_main(int argc, char *argv[]) if (!armed.armed || (safety.safety_switch_available && !safety.safety_off)) { //Notify the user that it is safe to approach the vehicle - if(arm_tune_played) { + if (arm_tune_played) { tune_neutral(true); } @@ -2436,6 +2443,7 @@ set_control_mode() control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; } + break; default: @@ -2474,7 +2482,7 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul { switch (result) { case VEHICLE_CMD_RESULT_ACCEPTED: - tune_positive(true); + tune_positive(true); break; case VEHICLE_CMD_RESULT_DENIED: From e18e5ca5b5d82a140d9a525cc3ba9bfddf4dcb7f Mon Sep 17 00:00:00 2001 From: hauptmech Date: Thu, 8 Jan 2015 12:38:33 +1300 Subject: [PATCH 035/269] Add SENS_MAG_ID parameter Record devid to SENS_MAG_ID during mag calibration Verify devid matches SENS_MAG_ID during preflight_check --- src/drivers/drv_mag.h | 2 ++ src/modules/commander/mag_calibration.cpp | 7 +++++++ src/modules/sensors/sensor_params.c | 6 ++++++ src/systemcmds/config/config.c | 6 +++++- src/systemcmds/preflight_check/preflight_check.c | 11 +++++++++++ 5 files changed, 31 insertions(+), 1 deletion(-) diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index 5ddf5d08e8..d341e89472 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -41,9 +41,11 @@ #include #include +#include "drv_device.h" #include "drv_sensor.h" #include "drv_orb_dev.h" + #define MAG_DEVICE_PATH "/dev/mag" /** diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 7be8de9c6b..9d2abd3ceb 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -65,6 +65,7 @@ static const char *sensor_name = "mag"; int do_mag_calibration(int mavlink_fd) { + int32_t device_id; mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); mavlink_log_info(mavlink_fd, "don't move system"); @@ -88,6 +89,9 @@ int do_mag_calibration(int mavlink_fd) /* erase old calibration */ int fd = open(MAG_DEVICE_PATH, O_RDONLY); + + device_id = ioctl(fd, DEVIOCGDEVICEID, 0); + res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null); if (res != OK) { @@ -253,6 +257,9 @@ int do_mag_calibration(int mavlink_fd) if (res == OK) { /* set parameters */ + if (param_set(param_find("SENS_MAG_ID"), &(device_id))) { + res = ERROR; + } if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) { res = ERROR; } diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index a065541b91..bf5708e0b7 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -98,6 +98,12 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f); */ PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f); +/** + * ID of Magnetometer the calibration is for. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(SENS_MAG_ID, 0); /** * Magnetometer X-axis offset diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 4a97d328cf..077bc47c90 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -58,6 +58,7 @@ #include "systemlib/systemlib.h" #include "systemlib/err.h" +#include "systemlib/param/param.h" __EXPORT int config_main(int argc, char *argv[]); @@ -264,8 +265,11 @@ do_mag(int argc, char *argv[]) int srate = ioctl(fd, MAGIOCGSAMPLERATE, 0); int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); int range = ioctl(fd, MAGIOCGRANGE, 0); + int id = ioctl(fd, DEVIOCGDEVICEID,0); + int32_t calibration_id = 0; + param_get(param_find("SENS_MAG_ID"), &(calibration_id)); - warnx("mag: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", srate, prate, range); + warnx("mag: \n\tdevice id:\t%x\t(calibration is for device id %x)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", id, calibration_id, srate, prate, range); close(fd); } diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index 86e4ff545d..bbd90b961f 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -84,6 +84,7 @@ int preflight_check_main(int argc, char *argv[]) /* open text message output path */ int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); int ret; + int32_t mag_devid,mag_calibration_devid; /* give the system some time to sample the sensors in the background */ usleep(150000); @@ -96,6 +97,16 @@ int preflight_check_main(int argc, char *argv[]) system_ok = false; goto system_eval; } + + mag_devid = ioctl(fd, DEVIOCGDEVICEID,0); + param_get(param_find("SENS_MAG_ID"), &(mag_calibration_devid)); + if (mag_devid != mag_calibration_devid){ + warnx("magnetometer calibration is for a different device - calibrate magnetometer first"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CAL ID"); + system_ok = false; + goto system_eval; + } + ret = ioctl(fd, MAGIOCSELFTEST, 0); if (ret != OK) { From 6324513b91702d8fa877e915c91a6dcef9790faa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 17 Jan 2015 18:48:41 +0100 Subject: [PATCH 036/269] MS5611: Comment-only fix --- src/drivers/ms5611/ms5611.h | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/drivers/ms5611/ms5611.h b/src/drivers/ms5611/ms5611.h index f0b3fd61d7..3f1f6c4738 100644 --- a/src/drivers/ms5611/ms5611.h +++ b/src/drivers/ms5611/ms5611.h @@ -37,12 +37,12 @@ * Shared defines for the ms5611 driver. */ -#define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */ -#define ADDR_CMD_CONVERT_D1 0x48 /* write to this address to start temperature conversion */ -#define ADDR_CMD_CONVERT_D2 0x58 /* write to this address to start pressure conversion */ -#define ADDR_DATA 0x00 /* address of 3 bytes / 32bit pressure data */ -#define ADDR_PROM_SETUP 0xA0 /* address of 8x 2 bytes factory and calibration data */ -#define ADDR_PROM_C1 0xA2 /* address of 6x 2 bytes calibration data */ +#define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */ +#define ADDR_CMD_CONVERT_D1 0x48 /* write to this address to start pressure conversion */ +#define ADDR_CMD_CONVERT_D2 0x58 /* write to this address to start temperature conversion */ +#define ADDR_DATA 0x00 /* address of 3 bytes / 32bit pressure data */ +#define ADDR_PROM_SETUP 0xA0 /* address of 8x 2 bytes factory and calibration data */ +#define ADDR_PROM_C1 0xA2 /* address of 6x 2 bytes calibration data */ /* interface ioctls */ #define IOCTL_RESET 2 From b1127315b453e129753e1f59aff0b0f6906cbaac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 17 Jan 2015 23:26:43 +0100 Subject: [PATCH 037/269] Fixed land detector param names --- .../land_detector/FixedwingLandDetector.cpp | 6 +-- src/modules/land_detector/LandDetector.cpp | 41 +++++++++++++++++++ .../land_detector/MulticopterLandDetector.cpp | 10 ++--- .../land_detector/land_detector_main.cpp | 2 +- .../land_detector/land_detector_params.c | 17 ++++---- 5 files changed, 58 insertions(+), 18 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 52084e4c05..74a197bd23 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -57,9 +57,9 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), _airspeed_filtered(0.0f), _landDetectTrigger(0) { - _paramHandle.maxVelocity = param_find("LAND_FW_VEL_XY_MAX"); - _paramHandle.maxClimbRate = param_find("LAND_FW_VEL_Z_MAX"); - _paramHandle.maxAirSpeed = param_find("LAND_FW_AIRSPEED_MAX"); + _paramHandle.maxVelocity = param_find("LNDFW_VEL_XY_MAX"); + _paramHandle.maxClimbRate = param_find("LNDFW_VEL_Z_MAX"); + _paramHandle.maxAirSpeed = param_find("LNDFW_AIRSPD_MAX"); } void FixedwingLandDetector::initialize() diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 1dd09b6a67..61e678b41c 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -1,3 +1,44 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file LandDetector.cpp + * Land detection algorithm + * + * @author Johan Jansen + * @author Morten Lysgaard + */ + #include "LandDetector.h" #include //usleep #include diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 5371346181..cc984f11fa 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -33,7 +33,7 @@ /** * @file MulticopterLandDetector.cpp - * Land detection algorithm + * Land detection algorithm for multicopters * * @author Johan Jansen * @author Morten Lysgaard @@ -62,10 +62,10 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _landTimer(0) { - _paramHandle.maxRotation = param_find("LAND_MC_Z_VEL_MAX"); - _paramHandle.maxVelocity = param_find("LAND_MC_XY_VEL_MAX"); - _paramHandle.maxClimbRate = param_find("LAND_MC_ROT_MAX"); - _paramHandle.maxThrottle = param_find("LAND_MC_THR_MAX"); + _paramHandle.maxRotation = param_find("LNDMC_Z_VEL_MAX"); + _paramHandle.maxVelocity = param_find("LNDMC_XY_VEL_MAX"); + _paramHandle.maxClimbRate = param_find("LNDMC_ROT_MAX"); + _paramHandle.maxThrottle = param_find("LNDMC_THR_MAX"); } void MulticopterLandDetector::initialize() diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index 9e135b5f17..1e43e7ad5d 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -139,7 +139,7 @@ static int land_detector_start(const char *mode) _landDetectorTaskID = task_spawn_cmd("land_detector", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 1024, + 1200, (main_t)&land_detector_deamon_thread, nullptr); diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index e2acf42b3d..0302bc7c14 100644 --- a/src/modules/land_detector/land_detector_params.c +++ b/src/modules/land_detector/land_detector_params.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Anton Babushkin + * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -48,7 +47,7 @@ * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LAND_MC_Z_VEL_MAX, 0.30f); +PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.30f); /** * Multicopter max horizontal velocity @@ -57,7 +56,7 @@ PARAM_DEFINE_FLOAT(LAND_MC_Z_VEL_MAX, 0.30f); * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LAND_MC_XY_VEL_MAX, 1.00f); +PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.00f); /** * Multicopter max rotation @@ -66,7 +65,7 @@ PARAM_DEFINE_FLOAT(LAND_MC_XY_VEL_MAX, 1.00f); * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LAND_MC_ROT_MAX, 0.20f); +PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 0.20f); /** * Multicopter max throttle @@ -75,7 +74,7 @@ PARAM_DEFINE_FLOAT(LAND_MC_ROT_MAX, 0.20f); * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LAND_MC_THR_MAX, 0.20f); +PARAM_DEFINE_FLOAT(LNDMC_THR_MAX, 0.20f); /** * Fixedwing max horizontal velocity @@ -84,7 +83,7 @@ PARAM_DEFINE_FLOAT(LAND_MC_THR_MAX, 0.20f); * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LAND_FW_VEL_XY_MAX, 0.20f); +PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 0.20f); /** * Fixedwing max climb rate @@ -93,7 +92,7 @@ PARAM_DEFINE_FLOAT(LAND_FW_VEL_XY_MAX, 0.20f); * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LAND_FW_VEL_Z_MAX, 10.00f); +PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 10.00f); /** * Airspeed max @@ -102,4 +101,4 @@ PARAM_DEFINE_FLOAT(LAND_FW_VEL_Z_MAX, 10.00f); * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LAND_FW_AIRSPEED_MAX, 10.00f); +PARAM_DEFINE_FLOAT(LNDFW_AIRSPD_MAX, 10.00f); From 378dcc53d63a18811f17ab6f60c66b8315d8db25 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 17 Jan 2015 23:32:00 +0100 Subject: [PATCH 038/269] Code style: minor comment styling --- .../land_detector/FixedwingLandDetector.cpp | 13 ++++++------ src/modules/land_detector/LandDetector.cpp | 18 ++++++++--------- .../land_detector/MulticopterLandDetector.cpp | 20 +++++++++---------- 3 files changed, 24 insertions(+), 27 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 74a197bd23..8e5bcf7bae 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -51,7 +51,6 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), _airspeedSub(-1), _airspeed({}), _parameterSub(-1), - _velocity_xy_filtered(0.0f), _velocity_z_filtered(0.0f), _airspeed_filtered(0.0f), @@ -64,7 +63,7 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), void FixedwingLandDetector::initialize() { - //Subscribe to local position and airspeed data + // Subscribe to local position and airspeed data _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position)); _airspeedSub = orb_subscribe(ORB_ID(airspeed)); } @@ -77,30 +76,30 @@ void FixedwingLandDetector::updateSubscriptions() bool FixedwingLandDetector::update() { - //First poll for new data from our subscriptions + // First poll for new data from our subscriptions updateSubscriptions(); const uint64_t now = hrt_absolute_time(); bool landDetected = false; - //TODO: reset filtered values on arming? + // TODO: reset filtered values on arming? _velocity_xy_filtered = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx * _vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy); _velocity_z_filtered = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz); _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; - /* crude land detector for fixedwing */ + // crude land detector for fixedwing if (_velocity_xy_filtered < _params.maxVelocity && _velocity_z_filtered < _params.maxClimbRate && _airspeed_filtered < _params.maxAirSpeed) { - //These conditions need to be stable for a period of time before we trust them + // these conditions need to be stable for a period of time before we trust them if (now > _landDetectTrigger) { landDetected = true; } } else { - //reset land detect trigger + // reset land detect trigger _landDetectTrigger = now + LAND_DETECTOR_TRIGGER_TIME; } diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 61e678b41c..a4fbb9861f 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -49,7 +49,7 @@ LandDetector::LandDetector() : _taskShouldExit(false), _taskIsRunning(false) { - //ctor + // ctor } LandDetector::~LandDetector() @@ -65,20 +65,20 @@ void LandDetector::shutdown() void LandDetector::start() { - //Make sure this method has not already been called by another thread + // make sure this method has not already been called by another thread if (isRunning()) { return; } - //Advertise the first land detected uORB + // advertise the first land detected uORB _landDetected.timestamp = hrt_absolute_time(); _landDetected.landed = false; _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); - //Initialize land detection algorithm + // initialize land detection algorithm initialize(); - //Task is now running, keep doing so until shutdown() has been called + // task is now running, keep doing so until shutdown() has been called _taskIsRunning = true; _taskShouldExit = false; @@ -86,16 +86,16 @@ void LandDetector::start() bool landDetected = update(); - //Publish if land detection state has changed + // publish if land detection state has changed if (_landDetected.landed != landDetected) { _landDetected.timestamp = hrt_absolute_time(); _landDetected.landed = landDetected; - /* publish the land detected broadcast */ + // publish the land detected broadcast orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); } - //Limit loop rate + // limit loop rate usleep(1000000 / LAND_DETECTOR_UPDATE_RATE); } @@ -107,7 +107,7 @@ bool LandDetector::orb_update(const struct orb_metadata *meta, int handle, void { bool newData = false; - //Check if there is new data to grab + // check if there is new data to grab if (orb_check(handle, &newData) != OK) { return false; } diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index cc984f11fa..277cb9363f 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -53,13 +53,11 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _actuatorsSub(-1), _armingSub(-1), _parameterSub(-1), - _vehicleGlobalPosition({}), _sensors({}), _waypoint({}), _actuators({}), _arming({}), - _landTimer(0) { _paramHandle.maxRotation = param_find("LNDMC_Z_VEL_MAX"); @@ -70,7 +68,7 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), void MulticopterLandDetector::initialize() { - //Subscribe to position, attitude, arming and velocity changes + // subscribe to position, attitude, arming and velocity changes _vehicleGlobalPositionSub = orb_subscribe(ORB_ID(vehicle_global_position)); _sensorsCombinedSub = orb_subscribe(ORB_ID(sensor_combined)); _waypointSub = orb_subscribe(ORB_ID(position_setpoint_triplet)); @@ -78,7 +76,7 @@ void MulticopterLandDetector::initialize() _armingSub = orb_subscribe(ORB_ID(actuator_armed)); _parameterSub = orb_subscribe(ORB_ID(parameter_update)); - //Download parameters + // download parameters updateParameterCache(true); } @@ -93,33 +91,33 @@ void MulticopterLandDetector::updateSubscriptions() bool MulticopterLandDetector::update() { - //First poll for new data from our subscriptions + // first poll for new data from our subscriptions updateSubscriptions(); - //Only trigger flight conditions if we are armed + // only trigger flight conditions if we are armed if (!_arming.armed) { return true; } const uint64_t now = hrt_absolute_time(); - //Check if we are moving vertically + // check if we are moving vertically bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > _params.maxClimbRate; - //Check if we are moving horizontally + // check if we are moving horizontally bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n + _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > _params.maxVelocity; - //Next look if all rotation angles are not moving + // next look if all rotation angles are not moving bool rotating = sqrtf(_sensors.gyro_rad_s[0] * _sensors.gyro_rad_s[0] + _sensors.gyro_rad_s[1] * _sensors.gyro_rad_s[1] + _sensors.gyro_rad_s[2] * _sensors.gyro_rad_s[2]) > _params.maxRotation; - //Check if thrust output is minimal (about half of default) + // check if thrust output is minimal (about half of default) bool minimalThrust = _actuators.control[3] <= _params.maxThrottle; if (verticalMovement || rotating || !minimalThrust || horizontalMovement) { - //Sensed movement, so reset the land detector + // sensed movement, so reset the land detector _landTimer = now; return false; } From beab89367f5cc2765c747fb463a27ce001206dd9 Mon Sep 17 00:00:00 2001 From: tumbili Date: Sun, 18 Jan 2015 17:46:01 +0100 Subject: [PATCH 039/269] calculate total airflow over elevons using physical of flow behind propeller. read local position topic for future use. --- src/modules/uORB/topics/vtol_vehicle_status.h | 1 + .../vtol_att_control_main.cpp | 104 ++++++++++++++++-- .../vtol_att_control_params.c | 35 +++++- 3 files changed, 129 insertions(+), 11 deletions(-) diff --git a/src/modules/uORB/topics/vtol_vehicle_status.h b/src/modules/uORB/topics/vtol_vehicle_status.h index 7b4e22bc88..968c2b6bdf 100644 --- a/src/modules/uORB/topics/vtol_vehicle_status.h +++ b/src/modules/uORB/topics/vtol_vehicle_status.h @@ -55,6 +55,7 @@ struct vtol_vehicle_status_s { uint64_t timestamp; /**< Microseconds since system boot */ bool vtol_in_rw_mode; /*true: vtol vehicle is in rotating wing mode */ bool fw_permanent_stab; /**< In fw mode stabilize attitude even if in manual mode*/ + float airspeed_tot; /*< Estimated airspeed over control surfaces */ }; /** diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index c72a85ecd4..8e68730b8b 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -65,6 +65,8 @@ #include #include #include +#include +#include #include #include #include @@ -105,7 +107,9 @@ private: int _params_sub; //parameter updates subscription int _manual_control_sp_sub; //manual control setpoint subscription int _armed_sub; //arming status subscription + int _local_pos_sub; // sensor subscription int _airspeed_sub; // airspeed subscription + int _battery_status_sub; // battery status subscription int _actuator_inputs_mc; //topic on which the mc_att_controller publishes actuator inputs int _actuator_inputs_fw; //topic on which the fw_att_controller publishes actuator inputs @@ -129,7 +133,9 @@ private: struct actuator_controls_s _actuators_mc_in; //actuator controls from mc_att_control struct actuator_controls_s _actuators_fw_in; //actuator controls from fw_att_control struct actuator_armed_s _armed; //actuator arming status + struct vehicle_local_position_s _local_pos; struct airspeed_s _airspeed; // airspeed + struct battery_status_s _batt_status; // battery status struct { param_t idle_pwm_mc; //pwm value for idle in mc mode @@ -139,6 +145,9 @@ private: float mc_airspeed_trim; // trim airspeed in multicopter mode float mc_airspeed_max; // max airpseed in multicopter mode float fw_pitch_trim; // trim for neutral elevon position in fw mode + float power_max; // maximum power of one engine + float prop_eff; // factor to calculate prop efficiency + float arsp_lp_gain; // total airspeed estimate low pass gain } _params; struct { @@ -149,6 +158,9 @@ private: param_t mc_airspeed_trim; param_t mc_airspeed_max; param_t fw_pitch_trim; + param_t power_max; + param_t prop_eff; + param_t arsp_lp_gain; } _params_handles; perf_counter_t _loop_perf; /**< loop performance counter */ @@ -159,6 +171,7 @@ private: * to waste energy when gliding. */ bool flag_idle_mc; //false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode" unsigned _motor_count; // number of motors + float _airspeed_tot; //*****************Member functions*********************************************************************** @@ -172,7 +185,9 @@ private: void actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output void vehicle_rates_sp_mc_poll(); void vehicle_rates_sp_fw_poll(); + void vehicle_local_pos_poll(); // Check for changes in sensor values void vehicle_airspeed_poll(); // Check for changes in airspeed + void vehicle_battery_poll(); // Check for battery updates void parameters_update_poll(); //Check if parameters have changed int parameters_update(); //Update local paraemter cache void fill_mc_att_control_output(); //write mc_att_control results to actuator message @@ -182,6 +197,7 @@ private: void set_idle_fw(); void set_idle_mc(); void scale_mc_output(); + void calc_tot_airspeed(); // estimated airspeed seen by elevons }; namespace VTOL_att_control @@ -205,7 +221,9 @@ VtolAttitudeControl::VtolAttitudeControl() : _params_sub(-1), _manual_control_sp_sub(-1), _armed_sub(-1), + _local_pos_sub(-1), _airspeed_sub(-1), + _battery_status_sub(-1), //init publication handlers _actuators_0_pub(-1), @@ -218,6 +236,7 @@ VtolAttitudeControl::VtolAttitudeControl() : { flag_idle_mc = true; + _airspeed_tot = 0.0f; memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status)); _vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/ @@ -234,7 +253,9 @@ VtolAttitudeControl::VtolAttitudeControl() : memset(&_actuators_mc_in, 0, sizeof(_actuators_mc_in)); memset(&_actuators_fw_in, 0, sizeof(_actuators_fw_in)); memset(&_armed, 0, sizeof(_armed)); + memset(&_local_pos,0,sizeof(_local_pos)); memset(&_airspeed,0,sizeof(_airspeed)); + memset(&_batt_status,0,sizeof(_batt_status)); _params.idle_pwm_mc = PWM_LOWEST_MIN; _params.vtol_motor_count = 0; @@ -247,6 +268,9 @@ VtolAttitudeControl::VtolAttitudeControl() : _params_handles.mc_airspeed_max = param_find("VT_MC_ARSPD_MAX"); _params_handles.mc_airspeed_trim = param_find("VT_MC_ARSPD_TRIM"); _params_handles.fw_pitch_trim = param_find("VT_FW_PITCH_TRIM"); + _params_handles.power_max = param_find("VT_POWER_MAX"); + _params_handles.prop_eff = param_find("VT_PROP_EFF"); + _params_handles.arsp_lp_gain = param_find("VT_ARSP_LP_GAIN"); /* fetch initial parameter values */ parameters_update(); @@ -387,6 +411,19 @@ VtolAttitudeControl::vehicle_airspeed_poll() { } } +/** +* Check for battery updates. +*/ +void +VtolAttitudeControl::vehicle_battery_poll() { + bool updated; + orb_check(_battery_status_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(battery_status), _battery_status_sub , &_batt_status); + } +} + /** * Check for parameter updates. */ @@ -405,6 +442,22 @@ VtolAttitudeControl::parameters_update_poll() } } +/** +* Check for sensor updates. +*/ +void +VtolAttitudeControl::vehicle_local_pos_poll() +{ + bool updated; + /* Check if parameters have changed */ + orb_check(_local_pos_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub , &_local_pos); + } + +} + /** * Update parameters. */ @@ -437,6 +490,18 @@ VtolAttitudeControl::parameters_update() param_get(_params_handles.fw_pitch_trim, &v); _params.fw_pitch_trim = v; + /* vtol maximum power engine can produce */ + param_get(_params_handles.power_max, &v); + _params.power_max = v; + + /* vtol propeller efficiency factor */ + param_get(_params_handles.prop_eff, &v); + _params.prop_eff = v; + + /* vtol total airspeed estimate low pass gain */ + param_get(_params_handles.arsp_lp_gain, &v); + _params.arsp_lp_gain = v; + return OK; } @@ -555,7 +620,7 @@ void VtolAttitudeControl::scale_mc_output() { // scale around tuning airspeed float airspeed; - + calc_tot_airspeed(); // estimate air velocity seen by elevons // if airspeed is not updating, we assume the normal average speed if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) || hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { @@ -563,16 +628,12 @@ VtolAttitudeControl::scale_mc_output() { if (nonfinite) { perf_count(_nonfinite_input_perf); } - } else { - // prevent numerical drama by requiring 0.5 m/s minimal speed - airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s); - } - - // Sanity check if airspeed is consistent with throttle - if(_manual_control_sp.z >= 0.35f && airspeed < _params.mc_airspeed_trim) { // XXX magic number, should be hover throttle param - airspeed = _params.mc_airspeed_trim; + } else { + airspeed = _airspeed_tot; + airspeed = math::constrain(airspeed,_params.mc_airspeed_min, _params.mc_airspeed_max); } + _vtol_vehicle_status.airspeed_tot = airspeed; // save value for logging /* * For scaling our actuators using anything less than the min (close to stall) * speed doesn't make any sense - its the strongest reasonable deflection we @@ -584,6 +645,23 @@ VtolAttitudeControl::scale_mc_output() { _actuators_mc_in.control[1] = math::constrain(_actuators_mc_in.control[1]*airspeed_scaling*airspeed_scaling,-1.0f,1.0f); } +void VtolAttitudeControl::calc_tot_airspeed() { + float airspeed = math::max(1.0f, _airspeed.true_airspeed_m_s); // prevent numerical drama + // calculate momentary power of one engine + float P = _batt_status.voltage_filtered_v * _batt_status.current_a / _params.vtol_motor_count; + P = math::constrain(P,1.0f,_params.power_max); + // calculate prop efficiency + float power_factor = 1.0f - P*_params.prop_eff/_params.power_max; + float eta = (1.0f/(1 + expf(-0.4f * power_factor * airspeed)) - 0.5f)*2.0f; + eta = math::constrain(eta,0.001f,1.0f); // live on the safe side + // calculate induced airspeed by propeller + float v_ind = (airspeed/eta - airspeed)*2.0f; + // calculate total airspeed + float airspeed_raw = airspeed + v_ind; + // apply low-pass filter + _airspeed_tot = _params.arsp_lp_gain * (_airspeed_tot - airspeed_raw) + airspeed_raw; +} + void VtolAttitudeControl::task_main_trampoline(int argc, char *argv[]) { @@ -604,7 +682,9 @@ void VtolAttitudeControl::task_main() _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + _battery_status_sub = orb_subscribe(ORB_ID(battery_status)); _actuator_inputs_mc = orb_subscribe(ORB_ID(actuator_controls_virtual_mc)); _actuator_inputs_fw = orb_subscribe(ORB_ID(actuator_controls_virtual_fw)); @@ -674,7 +754,10 @@ void VtolAttitudeControl::task_main() vehicle_rates_sp_mc_poll(); vehicle_rates_sp_fw_poll(); parameters_update_poll(); + vehicle_local_pos_poll(); // Check for new sensor values vehicle_airspeed_poll(); + vehicle_battery_poll(); + if (_manual_control_sp.aux1 <= 0.0f) { /* vehicle is in mc mode */ _vtol_vehicle_status.vtol_in_rw_mode = true; @@ -688,7 +771,8 @@ void VtolAttitudeControl::task_main() if (fds[0].revents & POLLIN) { vehicle_manual_poll(); /* update remote input */ orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in); - // scale pitch control with airspeed + + // scale pitch control with total airspeed scale_mc_output(); fill_mc_att_control_output(); diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index d1d4697f3f..33752b2c43 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -64,7 +64,7 @@ PARAM_DEFINE_INT32(VT_IDLE_PWM_MC,900); * @min 0.0 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MIN,2.0f); +PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MIN,10.0f); /** * Maximum airspeed in multicopter mode @@ -109,3 +109,36 @@ PARAM_DEFINE_INT32(VT_FW_PERM_STAB,0); */ PARAM_DEFINE_FLOAT(VT_FW_PITCH_TRIM,0.0f); +/** + * Motor max power + * + * Indicates the maximum power the motor is able to produce. Used to calculate + * propeller efficiency map. + * + * @min 1 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_POWER_MAX,120.0f); + +/** + * Propeller efficiency parameter + * + * Influences propeller efficiency at different power settings. Should be tuned beforehand. + * + * @min 0.5 + * @max 0.9 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_PROP_EFF,0.0f); + +/** + * Total airspeed estimate low-pass filter gain + * + * Gain for tuning the low-pass filter for the total airspeed estimate + * + * @min 0.0 + * @max 0.99 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_ARSP_LP_GAIN,0.3f); + From 3a05b571690ce675f56184cd5c5168f7699f9a03 Mon Sep 17 00:00:00 2001 From: tumbili Date: Sun, 18 Jan 2015 17:46:25 +0100 Subject: [PATCH 040/269] log total airspeed for vtol --- src/modules/sdlog2/sdlog2.c | 12 ++++++++++++ src/modules/sdlog2/sdlog2_messages.h | 6 ++++++ 2 files changed, 18 insertions(+) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 7b7949239e..0cc193b6c1 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -94,6 +94,7 @@ #include #include #include +#include #include #include @@ -1000,6 +1001,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct satellite_info_s sat_info; struct wind_estimate_s wind_estimate; struct encoders_s encoders; + struct vtol_vehicle_status_s vtol_status; } buf; memset(&buf, 0, sizeof(buf)); @@ -1019,6 +1021,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GPS_s log_GPS; struct log_ATTC_s log_ATTC; struct log_STAT_s log_STAT; + struct log_VTOL_s log_VTOL; struct log_RC_s log_RC; struct log_OUT0_s log_OUT0; struct log_AIRS_s log_AIRS; @@ -1053,6 +1056,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct { int cmd_sub; int status_sub; + int vtol_status_sub; int sensor_sub; int att_sub; int att_sp_sub; @@ -1086,6 +1090,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); subs.status_sub = orb_subscribe(ORB_ID(vehicle_status)); + subs.vtol_status_sub = orb_subscribe(ORB_ID(vtol_vehicle_status)); subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); @@ -1219,6 +1224,13 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(STAT); } + /* --- VTOL VEHICLE STATUS --- */ + if(copy_if_updated(ORB_ID(vtol_vehicle_status), subs.vtol_status_sub, &buf.vtol_status)) { + log_msg.msg_type = LOG_VTOL_MSG; + log_msg.body.log_VTOL.airspeed_tot = buf.vtol_status.airspeed_tot; + LOGBUFFER_WRITE_AND_COUNT(VTOL); + } + /* --- GPS POSITION - UNIT #1 --- */ if (gps_pos_updated) { diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 99f70a948e..c9221d58a6 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -426,6 +426,11 @@ struct log_ENCD_s { /* --- AIR SPEED SENSORS - DIFF. PRESSURE --- */ #define LOG_AIR1_MSG 40 +/* --- VTOL - VTOL VEHICLE STATUS */ +#define LOG_VTOL_MSG 42 +struct log_VTOL_s { + float airspeed_tot; +}; /********** SYSTEM MESSAGES, ID > 0x80 **********/ @@ -468,6 +473,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT_S(ATTC, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT_S(ATC1, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT(STAT, "BBBfBB", "MainState,ArmState,FailsafeState,BatRem,BatWarn,Landed"), + LOG_FORMAT(VTOL, "f", "Arsp"), LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"), From b85f0ab3cddeb93e92064de92a2065297c34b28c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Jan 2015 13:16:44 +0100 Subject: [PATCH 041/269] README.md: Add coverity logo --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 80f5a585fe..db125e06d4 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ ## PX4 Flight Control Stack and Middleware ## -[![Build Status](https://travis-ci.org/PX4/Firmware.svg?branch=master)](https://travis-ci.org/PX4/Firmware) +[![Build Status](https://travis-ci.org/PX4/Firmware.svg?branch=master)](https://travis-ci.org/PX4/Firmware) [![Coverity Scan](https://scan.coverity.com/projects/3966/badge.svg?flat=1)](https://scan.coverity.com/projects/3966?tab=overview) [![Gitter](https://badges.gitter.im/Join%20Chat.svg)](https://gitter.im/PX4/Firmware?utm_source=badge&utm_medium=badge&utm_campaign=pr-badge&utm_content=badge) From 7d13318df87a97d2793e7c0bd078660cd58dc7f7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Jan 2015 13:22:44 +0100 Subject: [PATCH 042/269] Fix coverity CID 12451 --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3c71195fbc..adf1b0950a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1340,7 +1340,7 @@ int commander_thread_main(int argc, char *argv[]) status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab; /* Make sure that this is only adjusted if vehicle realy is of type vtol*/ - if (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR || VEHICLE_TYPE_VTOL_QUADROTOR) { + if ((status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR) || (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR)) { status.is_rotary_wing = vtol_status.vtol_in_rw_mode; } } From a1a5a65dfadaaea721e9c87c57b1b1b6bf1d4184 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Jan 2015 13:38:50 +0100 Subject: [PATCH 043/269] Fixed Coverity CID #12412 --- src/modules/uORB/uORB.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index 149b8f6d24..cc6617aea9 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (Cc) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -230,7 +230,7 @@ ORBDevNode::open(struct file *filp) ret = CDev::open(filp); if (ret != OK) - free(sd); + delete sd; return ret; } From 5aa75c8f00eede4f502b4c8e94efc65d3cc7f6dc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Jan 2015 13:52:33 +0100 Subject: [PATCH 044/269] Fixed coverity CID #12543 --- src/drivers/px4fmu/fmu.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 4366720407..d3fee16263 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1637,12 +1637,15 @@ sensor_reset(int ms) fd = open(PX4FMU_DEVICE_PATH, O_RDWR); - if (fd < 0) + if (fd < 0) { errx(1, "open fail"); + } - if (ioctl(fd, GPIO_SENSOR_RAIL_RESET, ms) < 0) - err(1, "servo arm failed"); + if (ioctl(fd, GPIO_SENSOR_RAIL_RESET, ms) < 0) { + warnx("sensor rail reset failed"); + } + close(fd); } void From 65915e5d01cd0b1b8aed95c827a4886f3a57e545 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Jan 2015 14:42:51 +0100 Subject: [PATCH 045/269] EKF: Fix race condition in accel measurement assignment --- .../ekf_att_pos_estimator/estimator_22states.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index b062097fb3..6cff4b948a 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -181,9 +181,12 @@ void AttPosEKF::UpdateStrapdownEquationsNED() correctedDelAng.x = dAngIMU.x - states[10]; correctedDelAng.y = dAngIMU.y - states[11]; correctedDelAng.z = dAngIMU.z - states[12]; - dVelIMU.x = dVelIMU.x; - dVelIMU.y = dVelIMU.y; - dVelIMU.z = dVelIMU.z - states[13]; + + Vector3f dVelIMURel; + + dVelIMURel.x = dVelIMU.x; + dVelIMURel.y = dVelIMU.y; + dVelIMURel.z = dVelIMU.z - states[13]; delAngTotal.x += correctedDelAng.x; delAngTotal.y += correctedDelAng.y; @@ -263,9 +266,9 @@ void AttPosEKF::UpdateStrapdownEquationsNED() // transform body delta velocities to delta velocities in the nav frame // * and + operators have been overloaded //delVelNav = Tbn*dVelIMU + gravityNED*dtIMU; - delVelNav.x = Tbn.x.x*dVelIMU.x + Tbn.x.y*dVelIMU.y + Tbn.x.z*dVelIMU.z + gravityNED.x*dtIMU; - delVelNav.y = Tbn.y.x*dVelIMU.x + Tbn.y.y*dVelIMU.y + Tbn.y.z*dVelIMU.z + gravityNED.y*dtIMU; - delVelNav.z = Tbn.z.x*dVelIMU.x + Tbn.z.y*dVelIMU.y + Tbn.z.z*dVelIMU.z + gravityNED.z*dtIMU; + delVelNav.x = Tbn.x.x*dVelIMURel.x + Tbn.x.y*dVelIMURel.y + Tbn.x.z*dVelIMURel.z + gravityNED.x*dtIMU; + delVelNav.y = Tbn.y.x*dVelIMURel.x + Tbn.y.y*dVelIMURel.y + Tbn.y.z*dVelIMURel.z + gravityNED.y*dtIMU; + delVelNav.z = Tbn.z.x*dVelIMURel.x + Tbn.z.y*dVelIMURel.y + Tbn.z.z*dVelIMURel.z + gravityNED.z*dtIMU; // calculate the magnitude of the nav acceleration (required for GPS // variance estimation) From 0a99a26fcc4bea0f19482f632d5492a905026654 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Mon, 19 Jan 2015 16:51:08 +0100 Subject: [PATCH 046/269] Cleanup uploader output. --- Tools/px_uploader.py | 53 +++++++++++++++++++++++++------------------- 1 file changed, 30 insertions(+), 23 deletions(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index c46f6bede1..95a3d40469 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -266,18 +266,19 @@ class uploader(object): self.__getSync() return value - def __drawProgressBar(self, progress, maxVal): + def __drawProgressBar(self, label, progress, maxVal): if maxVal < progress: progress = maxVal percent = (float(progress) / float(maxVal)) * 100.0 - sys.stdout.write("\rprogress:[%-20s] %.2f%%" % ('='*int(percent/5.0), percent)) + sys.stdout.write("\r%s: [%-20s] %.1f%%" % (label, '='*int(percent/5.0), percent)) sys.stdout.flush() # send the CHIP_ERASE command and wait for the bootloader to become ready - def __erase(self): + def __erase(self, label): + print("\n", end='') self.__send(uploader.CHIP_ERASE + uploader.EOC) @@ -288,15 +289,14 @@ class uploader(object): #Draw progress bar (erase usually takes about 9 seconds to complete) estimatedTimeRemaining = deadline-time.time() if estimatedTimeRemaining >= 9.0: - self.__drawProgressBar(20.0-estimatedTimeRemaining, 9.0) + self.__drawProgressBar(label, 20.0-estimatedTimeRemaining, 9.0) else: - self.__drawProgressBar(10.0, 10.0) + self.__drawProgressBar(label, 10.0, 10.0) sys.stdout.write(" (timeout: %d seconds) " % int(deadline-time.time()) ) sys.stdout.flush() if self.__trySync(): - self.__drawProgressBar(10.0, 10.0) - sys.stdout.write("\nerase complete!\n") + self.__drawProgressBar(label, 10.0, 10.0) return; raise RuntimeError("timed out waiting for erase") @@ -350,7 +350,8 @@ class uploader(object): return [seq[i:i+length] for i in range(0, len(seq), length)] # upload code - def __program(self, fw): + def __program(self, label, fw): + print("\n", end='') code = fw.image groups = self.__split_len(code, uploader.PROG_MULTI_MAX) @@ -361,31 +362,40 @@ class uploader(object): #Print upload progress (throttled, so it does not delay upload progress) uploadProgress += 1 if uploadProgress % 256 == 0: - self.__drawProgressBar(uploadProgress, len(groups)) - self.__drawProgressBar(100, 100) - print("\nprogram complete!") + self.__drawProgressBar(label, uploadProgress, len(groups)) + self.__drawProgressBar(label, 100, 100) # verify code - def __verify_v2(self, fw): + def __verify_v2(self, label, fw): + print("\n", end='') self.__send(uploader.CHIP_VERIFY + uploader.EOC) self.__getSync() code = fw.image groups = self.__split_len(code, uploader.READ_MULTI_MAX) + verifyProgress = 0 for bytes in groups: + verifyProgress += 1 + if verifyProgress % 256 == 0: + self.__drawProgressBar(label, verifyProgress, len(groups)) if (not self.__verify_multi(bytes)): raise RuntimeError("Verification failed") + self.__drawProgressBar(label, 100, 100) - def __verify_v3(self, fw): - expect_crc = fw.crc(self.fw_maxsize) + def __verify_v3(self, label, fw): + print("\n", end='') + self.__drawProgressBar(label, 1, 100) + expect_crc = fw.crc(self.fw_maxsize) self.__send(uploader.GET_CRC + uploader.EOC) report_crc = self.__recv_int() self.__getSync() + verifyProgress = 0 if report_crc != expect_crc: print("Expected 0x%x" % expect_crc) print("Got 0x%x" % report_crc) raise RuntimeError("Program CRC failed") + self.__drawProgressBar(label, 100, 100) # get basic data about the board def identify(self): @@ -439,19 +449,16 @@ class uploader(object): except Exception: # ignore bad character encodings pass - print("erase...") - self.__erase() + + self.__erase("Erase ") + self.__program("Program", fw) - print("program...") - self.__program(fw) - - print("verify...") if self.bl_rev == 2: - self.__verify_v2(fw) + self.__verify_v2("Verify ", fw) else: - self.__verify_v3(fw) + self.__verify_v3("Verify ", fw) - print("done, rebooting.") + print("\nRebooting.\n") self.__reboot() self.port.close() From 8c8315044c4c085a6b8f45f5375a28d19ea655ec Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Jan 2015 20:15:03 +0100 Subject: [PATCH 047/269] NuttX update to include Coverity fixes --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index dbcccb2455..574bac488f 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit dbcccb2455d759b789d549d25e1fbf489b2d3c83 +Subproject commit 574bac488f384ddaa344378e25653c27124a2b69 From fa4d09018527676d3803582a432df8e25c8e5010 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Jan 2015 20:18:44 +0100 Subject: [PATCH 048/269] Hotfix: Fix EKF estimator and remove debug output which should not have been on master anyway --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 3237a1e200..923aa28614 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -1518,12 +1518,6 @@ FixedwingEstimator::task_main() _global_pos.timestamp = _local_pos.timestamp; - // FIXME: usurp terrain alt field for baro_gps_offset - _global_pos.terrain_alt = _baro_gps_offset; - _global_pos.terrain_alt_valid = true; - _global_pos.eph = _baro_alt_filt; - _global_pos.epv = _gps_alt_filt; - /* lazily publish the global position only once available */ if (_global_pos_pub > 0) { /* publish the global position */ From 0af6303d0b70cbac281450b49a11b649852ccbfc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 20 Jan 2015 14:29:53 +1100 Subject: [PATCH 049/269] hmc5883: fixed mixup of internal and external hmc5883 I2C bus options --- src/drivers/hmc5883/hmc5883.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 2a10b00632..9d63479f38 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1305,9 +1305,9 @@ struct hmc5883_bus_option { uint8_t busnum; HMC5883 *dev; } bus_options[] = { - { HMC5883_BUS_I2C_INTERNAL, "/dev/hmc5883_int", &HMC5883_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL }, + { HMC5883_BUS_I2C_EXTERNAL, "/dev/hmc5883_ext", &HMC5883_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL }, #ifdef PX4_I2C_BUS_ONBOARD - { HMC5883_BUS_I2C_EXTERNAL, "/dev/hmc5883_ext", &HMC5883_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL }, + { HMC5883_BUS_I2C_INTERNAL, "/dev/hmc5883_int", &HMC5883_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL }, #endif #ifdef PX4_SPIDEV_HMC { HMC5883_BUS_SPI, "/dev/hmc5883_spi", &HMC5883_SPI_interface, PX4_SPI_BUS_SENSORS, NULL }, From 9b10395e94497ef16c99390460808409e08d468f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 20 Jan 2015 15:07:05 +1100 Subject: [PATCH 050/269] hmc5883: fixed DEVIOCGDEVICEID ioctl we need to pass the ioctl through to the bus interface thanks to Jon Challinger for noticing this bug --- src/drivers/hmc5883/hmc5883.cpp | 4 ++++ src/drivers/hmc5883/hmc5883_i2c.cpp | 5 +++++ src/drivers/hmc5883/hmc5883_spi.cpp | 5 +++++ 3 files changed, 14 insertions(+) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 9d63479f38..95fbed0ba3 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -66,6 +66,7 @@ #include #include #include +#include #include #include @@ -725,6 +726,9 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) debug("MAGIOCGEXTERNAL in main driver"); return _interface->ioctl(cmd, dummy); + case DEVIOCGDEVICEID: + return _interface->ioctl(cmd, dummy); + default: /* give it to the superclass */ return CDev::ioctl(filp, cmd, arg); diff --git a/src/drivers/hmc5883/hmc5883_i2c.cpp b/src/drivers/hmc5883/hmc5883_i2c.cpp index 782ea62fe7..f86c1af6b1 100644 --- a/src/drivers/hmc5883/hmc5883_i2c.cpp +++ b/src/drivers/hmc5883/hmc5883_i2c.cpp @@ -53,6 +53,7 @@ #include #include +#include #include "hmc5883.h" @@ -90,6 +91,7 @@ HMC5883_I2C_interface(int bus) HMC5883_I2C::HMC5883_I2C(int bus) : I2C("HMC5883_I2C", nullptr, bus, HMC5883L_ADDRESS, 400000) { + _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; } HMC5883_I2C::~HMC5883_I2C() @@ -117,6 +119,9 @@ HMC5883_I2C::ioctl(unsigned operation, unsigned &arg) return 0; } + case DEVIOCGDEVICEID: + return CDev::ioctl(nullptr, operation, arg); + default: ret = -EINVAL; } diff --git a/src/drivers/hmc5883/hmc5883_spi.cpp b/src/drivers/hmc5883/hmc5883_spi.cpp index 25a2f2b405..aec990ca88 100644 --- a/src/drivers/hmc5883/hmc5883_spi.cpp +++ b/src/drivers/hmc5883/hmc5883_spi.cpp @@ -53,6 +53,7 @@ #include #include +#include #include "hmc5883.h" #include @@ -92,6 +93,7 @@ HMC5883_SPI_interface(int bus) HMC5883_SPI::HMC5883_SPI(int bus, spi_dev_e device) : SPI("HMC5883_SPI", nullptr, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz */) { + _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; } HMC5883_SPI::~HMC5883_SPI() @@ -146,6 +148,9 @@ HMC5883_SPI::ioctl(unsigned operation, unsigned &arg) return 0; } + case DEVIOCGDEVICEID: + return CDev::ioctl(nullptr, operation, arg); + default: { ret = -EINVAL; From 275a9df928590fbe590fcfed9eb30d794fb3fa6b Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 20 Jan 2015 01:09:20 -0500 Subject: [PATCH 051/269] fixes #1621 - travis after_script upload all Images to s3 with build status and index --- .travis.yml | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/.travis.yml b/.travis.yml index fd2a6b6d1c..fcc273437e 100644 --- a/.travis.yml +++ b/.travis.yml @@ -52,6 +52,16 @@ script: - echo -en 'travis_fold:end:script.3\\r' - zip Firmware.zip Images/*.px4 +after_script: + - git clone git://github.com/PX4/CI-Tools.git + - ./CI-Tools/s3cmd-configure +# upload newest build for this branch with s3 index + - ./CI-Tools/s3cmd-put Images/px4*.px4 CI-Tools/directory/index.html $TRAVIS_BRANCH/ +# archive newest build by date with s3 index + - ./CI-Tools/s3cmd-put Images/px4*.px4 CI-Tools/directory/index.html $TRAVIS_BRANCH/archives/`date "+%Y-%m-%d"`-$TRAVIS_BUILD_ID/ +# upload top level index.html and timestamp.html + - ./CI-Tools/s3cmd-put CI-Tools/index.html timestamp.html / + # We use an encrypted env variable to ensure this only executes when artifacts are uploaded. #after_script: # - echo "Branch $TRAVIS_BRANCH (pull request: $TRAVIS_PULL_REQUEST) ready for flight testing." >> $PX4_REPORT From 0da66c33ad217ae78a52f134067a1c3bfa400dfb Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 20 Jan 2015 01:10:46 -0500 Subject: [PATCH 052/269] travis delete commented after_script PX4_REPORT template --- .travis.yml | 15 --------------- 1 file changed, 15 deletions(-) diff --git a/.travis.yml b/.travis.yml index fcc273437e..69efadfd2b 100644 --- a/.travis.yml +++ b/.travis.yml @@ -62,21 +62,6 @@ after_script: # upload top level index.html and timestamp.html - ./CI-Tools/s3cmd-put CI-Tools/index.html timestamp.html / -# We use an encrypted env variable to ensure this only executes when artifacts are uploaded. -#after_script: -# - echo "Branch $TRAVIS_BRANCH (pull request: $TRAVIS_PULL_REQUEST) ready for flight testing." >> $PX4_REPORT -# - git log -n1 > $PX4_REPORT -# - echo " " >> $PX4_REPORT -# - echo "Files available at:" >> $PX4_REPORT -# - echo "https://px4-travis.s3.amazonaws.com/PX4/Firmware/$TRAVIS_BUILD_NUMBER/$TRAVIS_BUILD_NUMBER.1/Firmware.zip" >> $PX4_REPORT -# - echo "Description of desired tests is available at:" >> $PX4_REPORT -# - echo "https://github.com/PX4/Firmware/pull/$TRAVIS_PULL_REQUEST" >> $PX4_REPORT -# - echo " " >> $PX4_REPORT -# - echo "Thanks for testing!" >> $PX4_REPORT -# - echo " " >> $PX4_REPORT -# - /usr/bin/mail -s "$SUBJECT ($TRAVIS_COMMIT)" "$PX4_EMAIL" < "$PX4_REPORT" -# - s3cmd put --acl-public --guess-mime-type --config=.s3cfg Firmware.zip s3://s3-website-us-east-1.amazonaws.com/#$TRAVIS_JOB_ID/ - deploy: provider: releases api_key: From 12e54d1adb437b71bd63973357a647c65a8d77a7 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 20 Jan 2015 01:11:26 -0500 Subject: [PATCH 053/269] travis delete redundant addons artifacts s3 upload --- .travis.yml | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/.travis.yml b/.travis.yml index 69efadfd2b..9f71b4e3a0 100644 --- a/.travis.yml +++ b/.travis.yml @@ -73,18 +73,6 @@ deploy: all_branches: true repo: PX4/Firmware -addons: - artifacts: - paths: - - "Firmware.zip" - key: - secure: j4y9x9KXUiarGrnpFBLPIkEKIH8X6oSRUO61TwxTOamsE0eEKnIaCz1Xq83q7DoqzomHBD3qXAFPV9dhLr1zdKEPJDIyV45GVD4ClIQIzh/P3Uc7kDNxKzdmxY12SH6D0orMpC4tCf1sNK7ETepltWfcnjaDk1Rjs9+TVY7LuzM= - secret: - secure: CJC7VPGtEhJu8Pix85iPF8xUvMPZvTgnHyd9MrSlPKCFFMrlgz9eMT0WWW/TPQ+s4LPwJIfEQx2Q0BRT5tOXuvsTLuOG68mplVddhTWbHb0m0qTQErXFHEppvW4ayuSdeLJ4TjTWphBVainL0mcLLRwQfuAJJDDs/sGan3WrG+Y= - bucket: px4-travis - region: us-east-1 - endpoint: s3-website-us-east-1.amazonaws.com - notifications: webhooks: urls: From 287e802bb5f4f747477b5b69b006d3e6f5295b4b Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 20 Jan 2015 01:54:13 -0500 Subject: [PATCH 054/269] travis s3 move archive location and store Firmware.zip instead of individual images --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 9f71b4e3a0..f0008b3b45 100644 --- a/.travis.yml +++ b/.travis.yml @@ -58,7 +58,7 @@ after_script: # upload newest build for this branch with s3 index - ./CI-Tools/s3cmd-put Images/px4*.px4 CI-Tools/directory/index.html $TRAVIS_BRANCH/ # archive newest build by date with s3 index - - ./CI-Tools/s3cmd-put Images/px4*.px4 CI-Tools/directory/index.html $TRAVIS_BRANCH/archives/`date "+%Y-%m-%d"`-$TRAVIS_BUILD_ID/ + - ./CI-Tools/s3cmd-put Firmware.zip CI-Tools/directory/index.html archives/$TRAVIS_BRANCH/`date "+%Y-%m-%d"`-$TRAVIS_BUILD_ID/ # upload top level index.html and timestamp.html - ./CI-Tools/s3cmd-put CI-Tools/index.html timestamp.html / From 70c7764b9f9e780664ae13d0962a55dd226117e6 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 19 Jan 2015 09:56:42 +0100 Subject: [PATCH 055/269] removed secondary attitude from attitude topic --- src/modules/uORB/topics/vehicle_attitude.h | 18 ------------------ 1 file changed, 18 deletions(-) diff --git a/src/modules/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h index 77324415a7..019944dc07 100755 --- a/src/modules/uORB/topics/vehicle_attitude.h +++ b/src/modules/uORB/topics/vehicle_attitude.h @@ -79,24 +79,6 @@ struct vehicle_attitude_s { float g_comp[3]; /**< Compensated gravity vector */ bool R_valid; /**< Rotation matrix valid */ bool q_valid; /**< Quaternion valid */ - - // secondary attitude, use for VTOL - float roll_sec; /**< Roll angle (rad, Tait-Bryan, NED) */ - float pitch_sec; /**< Pitch angle (rad, Tait-Bryan, NED) */ - float yaw_sec; /**< Yaw angle (rad, Tait-Bryan, NED) */ - float rollspeed_sec; /**< Roll angular speed (rad/s, Tait-Bryan, NED) */ - float pitchspeed_sec; /**< Pitch angular speed (rad/s, Tait-Bryan, NED) */ - float yawspeed_sec; /**< Yaw angular speed (rad/s, Tait-Bryan, NED) */ - float rollacc_sec; /**< Roll angular accelration (rad/s, Tait-Bryan, NED) */ - float pitchacc_sec; /**< Pitch angular acceleration (rad/s, Tait-Bryan, NED) */ - float yawacc_sec; /**< Yaw angular acceleration (rad/s, Tait-Bryan, NED) */ - float rate_offsets_sec[3]; /**< Offsets of the body angular rates from zero */ - float R_sec[3][3]; /**< Rotation matrix body to world, (Tait-Bryan, NED) */ - float q_sec[4]; /**< Quaternion (NED) */ - float g_comp_sec[3]; /**< Compensated gravity vector */ - bool R_valid_sec; /**< Rotation matrix valid */ - bool q_valid_sec; /**< Quaternion valid */ - }; /** From 59cb0cc3b4d91b4dd56369c47d88b4876e9784a9 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 19 Jan 2015 10:19:18 +0100 Subject: [PATCH 056/269] removed secondary attitude and log quaternion --- .../attitude_estimator_ekf_main.cpp | 38 ------------------- src/modules/sdlog2/sdlog2.c | 16 ++------ src/modules/sdlog2/sdlog2_messages.h | 7 +++- 3 files changed, 9 insertions(+), 52 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index e61204e6c6..b0086676a1 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -595,44 +595,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds memcpy(&att.R[0][0], &R.data[0][0], sizeof(att.R)); att.R_valid = true; - // compute secondary attitude - math::Matrix<3, 3> R_adapted; //modified rotation matrix - R_adapted = R; - - //move z to x - R_adapted(0, 0) = R(0, 2); - R_adapted(1, 0) = R(1, 2); - R_adapted(2, 0) = R(2, 2); - //move x to z - R_adapted(0, 2) = R(0, 0); - R_adapted(1, 2) = R(1, 0); - R_adapted(2, 2) = R(2, 0); - - //change direction of pitch (convert to right handed system) - R_adapted(0, 0) = -R_adapted(0, 0); - R_adapted(1, 0) = -R_adapted(1, 0); - R_adapted(2, 0) = -R_adapted(2, 0); - math::Vector<3> euler_angles_sec; //adapted euler angles for fixed wing operation - euler_angles_sec = R_adapted.to_euler(); - - att.roll_sec = euler_angles_sec(0); - att.pitch_sec = euler_angles_sec(1); - att.yaw_sec = euler_angles_sec(2); - - memcpy(&att.R_sec[0][0], &R_adapted.data[0][0], sizeof(att.R_sec)); - - att.rollspeed_sec = -x_aposteriori[2]; - att.pitchspeed_sec = x_aposteriori[1]; - att.yawspeed_sec = x_aposteriori[0]; - att.rollacc_sec = -x_aposteriori[5]; - att.pitchacc_sec = x_aposteriori[4]; - att.yawacc_sec = x_aposteriori[3]; - - att.g_comp_sec[0] = -raw.accelerometer_m_s2[2] - (-acc(2)); - att.g_comp_sec[1] = raw.accelerometer_m_s2[1] - acc(1); - att.g_comp_sec[2] = raw.accelerometer_m_s2[0] - acc(0); - - if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { // Broadcast orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 0cc193b6c1..a3407e42c4 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1437,6 +1437,10 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- ATTITUDE --- */ if (copy_if_updated(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att)) { log_msg.msg_type = LOG_ATT_MSG; + log_msg.body.log_ATT.q_w = buf.att.q[0]; + log_msg.body.log_ATT.q_x = buf.att.q[1]; + log_msg.body.log_ATT.q_y = buf.att.q[2]; + log_msg.body.log_ATT.q_z = buf.att.q[3]; log_msg.body.log_ATT.roll = buf.att.roll; log_msg.body.log_ATT.pitch = buf.att.pitch; log_msg.body.log_ATT.yaw = buf.att.yaw; @@ -1447,18 +1451,6 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_ATT.gy = buf.att.g_comp[1]; log_msg.body.log_ATT.gz = buf.att.g_comp[2]; LOGBUFFER_WRITE_AND_COUNT(ATT); - // secondary attitude - log_msg.msg_type = LOG_ATT2_MSG; - log_msg.body.log_ATT.roll = buf.att.roll_sec; - log_msg.body.log_ATT.pitch = buf.att.pitch_sec; - log_msg.body.log_ATT.yaw = buf.att.yaw_sec; - log_msg.body.log_ATT.roll_rate = buf.att.rollspeed_sec; - log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed_sec; - log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed_sec; - log_msg.body.log_ATT.gx = buf.att.g_comp_sec[0]; - log_msg.body.log_ATT.gy = buf.att.g_comp_sec[1]; - log_msg.body.log_ATT.gz = buf.att.g_comp_sec[2]; - LOGBUFFER_WRITE_AND_COUNT(ATT); } /* --- ATTITUDE SETPOINT --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index c9221d58a6..7080d9c316 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -50,8 +50,11 @@ #pragma pack(push, 1) /* --- ATT - ATTITUDE --- */ #define LOG_ATT_MSG 2 -#define LOG_ATT2_MSG 41 struct log_ATT_s { + float q_w; + float q_x; + float q_y; + float q_z; float roll; float pitch; float yaw; @@ -460,7 +463,7 @@ struct log_PARM_s { static const struct log_format_s log_formats[] = { /* business-level messages, ID < 0x80 */ LOG_FORMAT_S(ATT, ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), - LOG_FORMAT_S(ATT2, ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), + LOG_FORMAT(ATT, "fffffffffffff", "qw,qx,qy,qz,Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT_S(IMU, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT_S(IMU1, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), From 764f4994052e2e73fe7576c98d222d5d9cd3f352 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 20 Jan 2015 14:06:12 +0100 Subject: [PATCH 057/269] Fix CID 12470 --- src/drivers/px4io/px4io.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 96ebedd83c..ed8f86fae1 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2278,6 +2278,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) case PWM_SERVO_GET_DISABLE_LOCKDOWN: *(unsigned *)arg = _lockdown_override; + break; case PWM_SERVO_SET_FORCE_SAFETY_OFF: /* force safety swith off */ From fc9c67f5bee43127bba75d426ccb5b5ebd903c73 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 20 Jan 2015 14:07:59 +0100 Subject: [PATCH 058/269] Fix CID 12439 --- src/drivers/px4io/px4io.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index ed8f86fae1..73204e2207 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -3070,12 +3070,13 @@ lockdown(int argc, char *argv[]) if (ret > 0) { - read(0, &c, 1); + if (read(0, &c, 1) > 0) { - if (c != 'y') { - exit(0); - } else if (c == 'y') { - break; + if (c != 'y') { + exit(0); + } else if (c == 'y') { + break; + } } } From f1a53f5152da68e48f7dea8bb0fc3a03e4effd12 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 20 Jan 2015 14:12:25 +0100 Subject: [PATCH 059/269] Fixing CID 12528 --- src/drivers/px4io/px4io.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 73204e2207..f98010f688 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1773,12 +1773,12 @@ PX4IO::print_debug() #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 int io_fd = -1; - if (io_fd < 0) { + if (io_fd <= 0) { io_fd = ::open("/dev/ttyS0", O_RDONLY | O_NONBLOCK | O_NOCTTY); } /* read IO's output */ - if (io_fd > 0) { + if (io_fd >= 0) { pollfd fds[1]; fds[0].fd = io_fd; fds[0].events = POLLIN; From c04f8705643962d9099517475c110ebb511b7e8b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 20 Jan 2015 14:14:23 +0100 Subject: [PATCH 060/269] Fix CID 12427 & 12438 --- src/drivers/px4io/px4io.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index f98010f688..82c7bf2c45 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -3007,8 +3007,9 @@ monitor(void) poll(fds, 1, 2000); if (fds[0].revents == POLLIN) { - int c; - read(0, &c, 1); + /* control logic is to cancel with any key */ + char c; + (void)read(0, &c, 1); if (cancels-- == 0) { printf("\033[2J\033[H"); /* move cursor home and clear screen */ From 603b43b6d840bf6a730c56f15a0bb35b2af8ea17 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 20 Jan 2015 14:16:48 +0100 Subject: [PATCH 061/269] Fix CID 12574 & 12553 --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 82c7bf2c45..0e4e38c2fe 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1102,7 +1102,7 @@ PX4IO::io_set_control_state(unsigned group) uint16_t regs[_max_actuators]; /* get controls */ - bool changed; + bool changed = false; switch (group) { case 0: From 81082c4119c715d4b8e22040052115ecbff6cc4d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 20 Jan 2015 14:21:03 +0100 Subject: [PATCH 062/269] Fix CID 12453 --- src/drivers/px4io/px4io.cpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 0e4e38c2fe..5698dbc578 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1144,11 +1144,13 @@ PX4IO::io_set_control_state(unsigned group) break; } - if (!changed) + if (!changed) { return -1; + } - for (unsigned i = 0; i < _max_controls; i++) + for (unsigned i = 0; i < _max_controls; i++) { regs[i] = FLOAT_TO_REG(controls.control[i]); + } /* copy values to registers in IO */ return io_reg_set(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT, regs, _max_controls); @@ -3240,7 +3242,13 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "limit")) { if ((argc > 2)) { - g_dev->set_update_rate(atoi(argv[2])); + int limitrate = atoi(argv[2]); + + if (limitrate > 0) { + g_dev->set_update_rate(limitrate); + } else { + errx(1, "invalid rate: %d", limitrate); + } } else { errx(1, "missing argument (50 - 500 Hz)"); From b84e50a3c601d8afebacad40d17d2db0e5644ce1 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 20 Jan 2015 09:34:03 -0500 Subject: [PATCH 063/269] travis upload s3 artifacts to Firmware subdirectory --- .travis.yml | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/.travis.yml b/.travis.yml index f0008b3b45..52ced6b4fc 100644 --- a/.travis.yml +++ b/.travis.yml @@ -56,9 +56,10 @@ after_script: - git clone git://github.com/PX4/CI-Tools.git - ./CI-Tools/s3cmd-configure # upload newest build for this branch with s3 index - - ./CI-Tools/s3cmd-put Images/px4*.px4 CI-Tools/directory/index.html $TRAVIS_BRANCH/ + - ./CI-Tools/s3cmd-put Images/px4*.px4 CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/ # archive newest build by date with s3 index - - ./CI-Tools/s3cmd-put Firmware.zip CI-Tools/directory/index.html archives/$TRAVIS_BRANCH/`date "+%Y-%m-%d"`-$TRAVIS_BUILD_ID/ + - ./CI-Tools/s3cmd-put Firmware.zip archives/Firmware/$TRAVIS_BRANCH/`date "+%Y-%m-%d"`-$TRAVIS_BUILD_ID/ + - ./CI-Tools/s3cmd-put CI-Tools/directory/index.html archives/Firmware/$TRAVIS_BRANCH/ # upload top level index.html and timestamp.html - ./CI-Tools/s3cmd-put CI-Tools/index.html timestamp.html / From b8b0d1d4ae3c7ced44eb1a6c394ee5877803c309 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Tue, 20 Jan 2015 22:28:58 +0100 Subject: [PATCH 064/269] Remove a noisy debugging statement. --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index adf1b0950a..247a2c5b83 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1457,7 +1457,7 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(subsystem_info), subsys_sub, &info); - warnx("subsystem changed: %d\n", (int)info.subsystem_type); + //warnx("subsystem changed: %d\n", (int)info.subsystem_type); /* mark / unmark as present */ if (info.present) { From 76e84e017f89172d46096101b2cd96620dd432dc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Jan 2015 14:05:09 +0100 Subject: [PATCH 065/269] VA_ARGS fixes from coverity --- src/modules/systemlib/err.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/systemlib/err.c b/src/modules/systemlib/err.c index 998b5ac7d0..a1a8e7ea8e 100644 --- a/src/modules/systemlib/err.c +++ b/src/modules/systemlib/err.c @@ -154,6 +154,7 @@ warn(const char *fmt, ...) va_start(args, fmt); vwarn(fmt, args); + va_end(args); } void @@ -169,6 +170,7 @@ warnc(int errcode, const char *fmt, ...) va_start(args, fmt); vwarnc(errcode, fmt, args); + va_end(args); } void @@ -184,6 +186,7 @@ warnx(const char *fmt, ...) va_start(args, fmt); vwarnx(fmt, args); + va_end(args); } void From ebacd9b5ad1ab0590559d8839a55f286febd0dd2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Jan 2015 14:05:38 +0100 Subject: [PATCH 066/269] GPS fixes from coverity --- src/drivers/gps/gps_helper.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp index 05dfc99b76..8157bc7e68 100644 --- a/src/drivers/gps/gps_helper.cpp +++ b/src/drivers/gps/gps_helper.cpp @@ -88,8 +88,6 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud) case 115200: speed = B115200; break; - warnx("try baudrate: %d\n", speed); - default: warnx("ERR: baudrate: %d\n", baud); return -EINVAL; From 6300338d2a3ebd110c738305a7ea21c1d318dfba Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Jan 2015 14:05:59 +0100 Subject: [PATCH 067/269] FMU driver: Fix resource leak for test routine. --- src/drivers/px4fmu/fmu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index d3fee16263..112c015136 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. + * Copyright (C) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions From 12ce2de7ae0aa62f9501f7f53837772a6c662724 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Jan 2015 14:06:21 +0100 Subject: [PATCH 068/269] IO driver: Fix resource corner case (not a real leak) --- src/drivers/px4io/px4io.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 5698dbc578..3b64809e39 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -3006,7 +3006,9 @@ monitor(void) fds[0].fd = 0; fds[0].events = POLLIN; - poll(fds, 1, 2000); + if (poll(fds, 1, 2000) < 0) { + errx(1, "poll fail"); + } if (fds[0].revents == POLLIN) { /* control logic is to cancel with any key */ From 859b34531473c9c78a4fee04de3cbdd147381913 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Jan 2015 14:07:00 +0100 Subject: [PATCH 069/269] Mag cal: Make error handling more explicit --- src/modules/commander/mag_calibration.cpp | 72 ++++++++++++----------- 1 file changed, 39 insertions(+), 33 deletions(-) diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 9d2abd3ceb..53013fdb97 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -150,54 +150,60 @@ int do_mag_calibration(int mavlink_fd) if (res == OK) { int sub_mag = orb_subscribe(ORB_ID(sensor_mag0)); - struct mag_report mag; - /* limit update rate to get equally spaced measurements over time (in ms) */ - orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); + if (sub_mag < 0) { + mavlink_log_critical(mavlink_fd, "No mag found, abort"); + res = ERROR; + } else { + struct mag_report mag; - /* calibrate offsets */ - uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; - unsigned poll_errcount = 0; + /* limit update rate to get equally spaced measurements over time (in ms) */ + orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); - mavlink_log_info(mavlink_fd, "Turn on all sides: front/back,left/right,up/down"); + /* calibrate offsets */ + uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; + unsigned poll_errcount = 0; - calibration_counter = 0; + mavlink_log_info(mavlink_fd, "Turn on all sides: front/back,left/right,up/down"); - while (hrt_absolute_time() < calibration_deadline && - calibration_counter < calibration_maxcount) { + calibration_counter = 0U; - /* wait blocking for new data */ - struct pollfd fds[1]; - fds[0].fd = sub_mag; - fds[0].events = POLLIN; + while (hrt_absolute_time() < calibration_deadline && + calibration_counter < calibration_maxcount) { - int poll_ret = poll(fds, 1, 1000); + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = sub_mag; + fds[0].events = POLLIN; - if (poll_ret > 0) { - orb_copy(ORB_ID(sensor_mag0), sub_mag, &mag); + int poll_ret = poll(fds, 1, 1000); - x[calibration_counter] = mag.x; - y[calibration_counter] = mag.y; - z[calibration_counter] = mag.z; + if (poll_ret > 0) { + orb_copy(ORB_ID(sensor_mag0), sub_mag, &mag); - calibration_counter++; + x[calibration_counter] = mag.x; + y[calibration_counter] = mag.y; + z[calibration_counter] = mag.z; - if (calibration_counter % (calibration_maxcount / 20) == 0) { - mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount); + calibration_counter++; + + if (calibration_counter % (calibration_maxcount / 20) == 0) { + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount); + } + + } else { + poll_errcount++; } - } else { - poll_errcount++; + if (poll_errcount > 1000) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); + res = ERROR; + break; + } } - if (poll_errcount > 1000) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); - res = ERROR; - break; - } + close(sub_mag); } - - close(sub_mag); } float sphere_x; @@ -205,7 +211,7 @@ int do_mag_calibration(int mavlink_fd) float sphere_z; float sphere_radius; - if (res == OK) { + if (res == OK && calibration_counter > (calibration_maxcount / 2)) { /* sphere fit */ mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70); From a9c8f2331b6cca3ca59debe82df82ace502170d8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Jan 2015 14:07:18 +0100 Subject: [PATCH 070/269] IO firmware: Make error checking more obvious --- src/modules/px4iofirmware/controls.c | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 58c9429b6f..d20f776d62 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -454,6 +454,10 @@ ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len) { bool result = false; + if (!(num_values) || !(values) || !(frame_len)) { + return result; + } + /* avoid racing with PPM updates */ irqstate_t state = irqsave(); @@ -468,7 +472,7 @@ ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len) if (*num_values > PX4IO_RC_INPUT_CHANNELS) *num_values = PX4IO_RC_INPUT_CHANNELS; - for (unsigned i = 0; i < *num_values; i++) { + for (unsigned i = 0; ((i < *num_values) && (i < PPM_MAX_CHANNELS)); i++) { values[i] = ppm_buffer[i]; } @@ -476,8 +480,7 @@ ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len) ppm_last_valid_decode = 0; /* store PPM frame length */ - if (num_values) - *frame_len = ppm_frame_length; + *frame_len = ppm_frame_length; /* good if we got any channels */ result = (*num_values > 0); From 9a40fe4edeb2874ee1ec280b863d43825b92df7a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Jan 2015 14:07:41 +0100 Subject: [PATCH 071/269] Sensors: Fix corner case on FD leak (not a real leak) --- src/modules/sensors/sensors.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 2c798af3bd..793b7c2b65 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -1465,7 +1465,7 @@ Sensors::parameter_update_poll(bool forced) /* this sensor is optional, abort without error */ - if (fd > 0) { + if (fd >= 0) { struct airspeed_scale airscale = { _parameters.diff_pres_offset_pa, 1.0f, From 5f77a57761517b6f7b2113a17ff7cb621376216b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Jan 2015 09:19:15 +0100 Subject: [PATCH 072/269] FMUv1 + FMUv2: Reduce excessively large OS stack sizes. Actual use was 800 bytes hpwork, 400 bytes lpwork, 1700 bytes in running system. This leaves 1K headroom per task --- nuttx-configs/px4fmu-v1/nsh/defconfig | 6 +++--- nuttx-configs/px4fmu-v2/nsh/defconfig | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index fade1f0c69..a467fa605e 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -417,7 +417,7 @@ CONFIG_PREALLOC_TIMERS=50 # Stack and heap information # CONFIG_IDLETHREAD_STACKSIZE=3500 -CONFIG_USERMAIN_STACKSIZE=3000 +CONFIG_USERMAIN_STACKSIZE=2600 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_PTHREAD_STACK_DEFAULT=2048 @@ -719,11 +719,11 @@ CONFIG_SCHED_WORKQUEUE=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_WORKPRIORITY=192 CONFIG_SCHED_WORKPERIOD=5000 -CONFIG_SCHED_WORKSTACKSIZE=2048 +CONFIG_SCHED_WORKSTACKSIZE=1800 CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 CONFIG_SCHED_LPWORKPERIOD=50000 -CONFIG_SCHED_LPWORKSTACKSIZE=2048 +CONFIG_SCHED_LPWORKSTACKSIZE=1800 # CONFIG_LIB_KBDCODEC is not set # CONFIG_LIB_SLCDCODEC is not set diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index ac3dbe5daf..9030a1f022 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -451,7 +451,7 @@ CONFIG_PREALLOC_TIMERS=50 # Stack and heap information # CONFIG_IDLETHREAD_STACKSIZE=3500 -CONFIG_USERMAIN_STACKSIZE=3000 +CONFIG_USERMAIN_STACKSIZE=2600 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_PTHREAD_STACK_DEFAULT=2048 @@ -797,11 +797,11 @@ CONFIG_SCHED_WORKQUEUE=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_WORKPRIORITY=192 CONFIG_SCHED_WORKPERIOD=5000 -CONFIG_SCHED_WORKSTACKSIZE=2000 +CONFIG_SCHED_WORKSTACKSIZE=1800 CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 CONFIG_SCHED_LPWORKPERIOD=50000 -CONFIG_SCHED_LPWORKSTACKSIZE=2000 +CONFIG_SCHED_LPWORKSTACKSIZE=1800 # CONFIG_LIB_KBDCODEC is not set # CONFIG_LIB_SLCDCODEC is not set From 9aa7daa86c7f18dcbba4f20cc1a2b3189117f242 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Jan 2015 09:22:46 +0100 Subject: [PATCH 073/269] PX4IO: Adjust stack size based on real usage of 900 bytes --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 3b64809e39..1c9a5adc9a 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -834,7 +834,7 @@ PX4IO::init() _task = task_spawn_cmd("px4io", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, - 2000, + 1800, (main_t)&PX4IO::task_main_trampoline, nullptr); From 21b88cd8742959585dbe479907ef82e4e0136bbb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Jan 2015 09:24:31 +0100 Subject: [PATCH 074/269] dataman: Reduce stack to 1800, based on real usage of 800 bytes --- src/modules/dataman/dataman.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 705e54be32..68bf120240 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -793,7 +793,7 @@ start(void) sem_init(&g_init_sema, 1, 0); /* start the worker thread */ - if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2000, task_main, NULL)) <= 0) { + if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1800, task_main, NULL)) <= 0) { warn("task start failed"); return -1; } From dad2f754e968493d56a23c29625c2c0c264df4c1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Jan 2015 09:54:31 +0100 Subject: [PATCH 075/269] Logging: Reduce logging on FMUv1 to free system resources --- ROMFS/px4fmu_common/init.d/rc.logging | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index 25f31a7e04..a14eb5247a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -8,7 +8,7 @@ then if ver hwcmp PX4FMU_V1 then echo "Start sdlog2 at 50Hz" - sdlog2 start -r 50 -a -b 4 -t + sdlog2 start -r 40 -a -b 3 -t else echo "Start sdlog2 at 200Hz" sdlog2 start -r 200 -a -b 22 -t From 0632d882bb734ac52ff5f7f5d1dc7930046e4600 Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Wed, 21 Jan 2015 12:22:07 +0300 Subject: [PATCH 076/269] INAV_W_Z_GPS_V defaults to 0 --- .../position_estimator_inav/position_estimator_inav_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 1a1b3d3102..b2771f1a62 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -72,7 +72,7 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f); * @max 10.0 * @group Position Estimator INAV */ -PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_V, 1.0f); +PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_V, 0.0f); /** * Z axis weight for vision From e092705b0d518197987a7a74a2a8cdd4eb740997 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Jan 2015 10:57:41 +0100 Subject: [PATCH 077/269] EKF bugfix: Replace all off-by-one (<=22 and <23) instances by correct check. Replace all badly-written <=21 (correct, but unreadable) code by < n_states --- .../estimator_22states.cpp | 40 +++++++++---------- 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index 6cff4b948a..15d018ab62 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -343,9 +343,9 @@ void AttPosEKF::CovariancePrediction(float dt) } if (!inhibitMagStates) { for (uint8_t i=16; i<=18; i++) processNoise[i] = dt * magEarthSigma; - for (uint8_t i=19; i<=21; i++) processNoise[i] = dt * magBodySigma; + for (uint8_t i=19; i < n_states; i++) processNoise[i] = dt * magBodySigma; } else { - for (uint8_t i=16; i<=21; i++) processNoise[i] = 0; + for (uint8_t i=16; i < n_states; i++) processNoise[i] = 0; } // square all sigmas @@ -1179,7 +1179,7 @@ void AttPosEKF::FuseVelposNED() } // Don't update magnetic field states if inhibited if (inhibitMagStates) { - for (uint8_t i = 16; i<=21; i++) + for (uint8_t i = 16; i < n_states; i++) { Kfusion[i] = 0; } @@ -1360,7 +1360,7 @@ void AttPosEKF::FuseMagnetometer() Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[5] - P[20][18]*SK_MX[4]); Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[3] - P[21][2]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[5] - P[21][18]*SK_MX[4]); } else { - for (uint8_t i=16; i <= 21; i++) { + for (uint8_t i=16; i < n_states; i++) { Kfusion[i] = 0; } } @@ -1538,14 +1538,14 @@ void AttPosEKF::FuseMagnetometer() for (uint8_t j = 4; j <= 15; j++) KH[i][j] = 0.0f; if (!onGround) { - for (uint8_t j = 16; j <= 21; j++) + for (uint8_t j = 16; j < n_states; j++) { KH[i][j] = Kfusion[i] * H_MAG[j]; } } else { - for (uint8_t j = 16; j <= 21; j++) + for (uint8_t j = 16; j < n_states; j++) { KH[i][j] = 0.0f; } @@ -1562,7 +1562,7 @@ void AttPosEKF::FuseMagnetometer() } if (!onGround) { - for (uint8_t k = 16; k<=21; k++) + for (uint8_t k = 16; k < n_states; k++) { KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; } @@ -1664,7 +1664,7 @@ void AttPosEKF::FuseAirspeed() Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][14]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][15]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]); Kfusion[21] = SK_TAS*(P[21][4]*SH_TAS[2] - P[21][14]*SH_TAS[2] + P[21][5]*SH_TAS[1] - P[21][15]*SH_TAS[1] + P[21][6]*vd*SH_TAS[0]); } else { - for (uint8_t i=16; i <= 21; i++) { + for (uint8_t i=16; i < n_states; i++) { Kfusion[i] = 0; } } @@ -1676,7 +1676,7 @@ void AttPosEKF::FuseAirspeed() if ((innovVtas*innovVtas*SK_TAS) < 25.0f) { // correct the state vector - for (uint8_t j=0; j <= 22; j++) + for (uint8_t j=0; j < n_states; j++) { states[j] = states[j] - Kfusion[j] * innovVtas; } @@ -1693,7 +1693,7 @@ void AttPosEKF::FuseAirspeed() // correct the covariance P = (I - K*H)*P // take advantage of the empty columns in H to reduce the // number of operations - for (uint8_t i = 0; i <= 22; i++) + for (uint8_t i = 0; i < n_states; i++) { for (uint8_t j = 0; j <= 3; j++) KH[i][j] = 0.0; for (uint8_t j = 4; j <= 6; j++) @@ -1705,11 +1705,11 @@ void AttPosEKF::FuseAirspeed() { KH[i][j] = Kfusion[i] * H_TAS[j]; } - for (uint8_t j = 16; j <= 22; j++) KH[i][j] = 0.0; + for (uint8_t j = 16; j < n_states; j++) KH[i][j] = 0.0; } - for (uint8_t i = 0; i <= 22; i++) + for (uint8_t i = 0; i < n_states; i++) { - for (uint8_t j = 0; j <= 22; j++) + for (uint8_t j = 0; j < n_states; j++) { KHP[i][j] = 0.0; for (uint8_t k = 4; k <= 6; k++) @@ -1722,9 +1722,9 @@ void AttPosEKF::FuseAirspeed() } } } - for (uint8_t i = 0; i <= 22; i++) + for (uint8_t i = 0; i < n_states; i++) { - for (uint8_t j = 0; j <= 22; j++) + for (uint8_t j = 0; j < n_states; j++) { P[i][j] = P[i][j] - KHP[i][j]; } @@ -1836,7 +1836,7 @@ void AttPosEKF::FuseOptFlow() tempVar[8] = (SK_LOS[4] + q0*tempVar[2]); // calculate observation jacobians for X LOS rate - for (uint8_t i = 0; i < 23; i++) H_LOS[0][i] = 0; + for (uint8_t i = 0; i < n_states; i++) H_LOS[0][i] = 0; H_LOS[0][0] = - SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) - 2*q0*SH_LOS[2]*SH_LOS[3]; H_LOS[0][1] = 2*q1*SH_LOS[2]*SH_LOS[3] - SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn); H_LOS[0][2] = 2*q2*SH_LOS[2]*SH_LOS[3] - SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn); @@ -1877,7 +1877,7 @@ void AttPosEKF::FuseOptFlow() K_LOS[0][20] = -SK_LOS[1]*(P[20][0]*tempVar[8] + P[20][1]*tempVar[7] - P[20][3]*tempVar[6] + P[20][2]*tempVar[5] - P[20][4]*tempVar[4] + P[20][6]*tempVar[3] - P[20][9]*tempVar[1] + P[20][5]*tempVar[0]); K_LOS[0][21] = -SK_LOS[1]*(P[21][0]*tempVar[8] + P[21][1]*tempVar[7] - P[21][3]*tempVar[6] + P[21][2]*tempVar[5] - P[21][4]*tempVar[4] + P[21][6]*tempVar[3] - P[21][9]*tempVar[1] + P[21][5]*tempVar[0]); } else { - for (uint8_t i = 16; i <= 21; i++) { + for (uint8_t i = 16; i < n_states; i++) { K_LOS[0][i] = 0.0f; } } @@ -1898,7 +1898,7 @@ void AttPosEKF::FuseOptFlow() tempVar[8] = SH_LOS[0]*SK_LOS[7]*SK_LOS[8]; // Calculate observation jacobians for Y LOS rate - for (uint8_t i = 0; i < 23; i++) H_LOS[1][i] = 0; + for (uint8_t i = 0; i < n_states; i++) H_LOS[1][i] = 0; H_LOS[1][0] = SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]; H_LOS[1][1] = SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]; H_LOS[1][2] = - SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q2*SH_LOS[1]*SH_LOS[3]; @@ -1939,7 +1939,7 @@ void AttPosEKF::FuseOptFlow() K_LOS[1][20] = SK_LOS[0]*(P[20][0]*tempVar[1] + P[20][1]*tempVar[2] - P[20][2]*tempVar[3] + P[20][3]*tempVar[4] + P[20][5]*tempVar[5] - P[20][6]*tempVar[6] - P[20][9]*tempVar[7] + P[20][4]*tempVar[8]); K_LOS[1][21] = SK_LOS[0]*(P[21][0]*tempVar[1] + P[21][1]*tempVar[2] - P[21][2]*tempVar[3] + P[21][3]*tempVar[4] + P[21][5]*tempVar[5] - P[21][6]*tempVar[6] - P[21][9]*tempVar[7] + P[21][4]*tempVar[8]); } else { - for (uint8_t i = 16; i <= 21; i++) { + for (uint8_t i = 16; i < n_states; i++) { K_LOS[1][i] = 0.0f; } } @@ -1983,7 +1983,7 @@ void AttPosEKF::FuseOptFlow() KH[i][j] = 0.0f; } KH[i][9] = K_LOS[obsIndex][i] * H_LOS[obsIndex][9]; - for (uint8_t j = 10; j <= 21; j++) + for (uint8_t j = 10; j < n_states; j++) { KH[i][j] = 0.0f; } From 87aaf2a959433d2c3df3848ffdcb173d113e807e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Jan 2015 11:01:25 +0100 Subject: [PATCH 078/269] uORB: Remove unused function --- src/modules/uORB/uORB.cpp | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index cc6617aea9..3373aac837 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -817,18 +817,6 @@ uorb_main(int argc, char *argv[]) namespace { -void debug(const char *fmt, ...) -{ - va_list ap; - - va_start(ap, fmt); - vfprintf(stderr, fmt, ap); - va_end(ap); - fprintf(stderr, "\n"); - fflush(stderr); - usleep(100000); -} - /** * Advertise a node; don't consider it an error if the node has * already been advertised. From e8e4a3b5da058bd2ab8575c095dd74a5484333be Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Jan 2015 11:04:56 +0100 Subject: [PATCH 079/269] Allow GCC 4.9.3 --- makefiles/toolchain_gnu-arm-eabi.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index d7e48f03be..fbe378f509 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -50,7 +50,7 @@ OBJDUMP = $(CROSSDEV)objdump # Check if the right version of the toolchain is available # -CROSSDEV_VER_SUPPORTED = 4.7.4 4.7.5 4.7.6 4.8.4 +CROSSDEV_VER_SUPPORTED = 4.7.4 4.7.5 4.7.6 4.8.4 4.9.3 CROSSDEV_VER_FOUND = $(shell $(CC) -dumpversion) ifeq (,$(findstring $(CROSSDEV_VER_FOUND), $(CROSSDEV_VER_SUPPORTED))) From 7c3223b8609ee418b520d19cae7e52d2a7a85e99 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 18 Jan 2015 18:43:45 +0100 Subject: [PATCH 080/269] added a messageplayer prototype for ros --- CMakeLists.txt | 85 +++++---- makefiles/config_px4fmu-v2_multiplatform.mk | 3 +- makefiles/config_px4fmu-v2_test.mk | 2 +- msg/templates/msg.h.template | 74 +++++-- src/examples/publisher/publisher_example.cpp | 19 +- src/examples/publisher/publisher_example.h | 1 + .../subscriber/subscriber_example.cpp | 14 +- src/examples/subscriber/subscriber_example.h | 3 +- .../nuttx/px4_messages/px4_rc_channels.h | 26 +++ src/platforms/px4_includes.h | 4 + src/platforms/px4_message.h | 77 ++++++++ src/platforms/px4_nodehandle.h | 180 ++++++++++++------ src/platforms/px4_publisher.h | 41 +++- src/platforms/px4_subscriber.h | 114 +++++++---- .../ros/px4_messages/px4_rc_channels.h | 25 +++ 15 files changed, 496 insertions(+), 172 deletions(-) create mode 100644 src/platforms/nuttx/px4_messages/px4_rc_channels.h create mode 100644 src/platforms/px4_message.h create mode 100644 src/platforms/ros/px4_messages/px4_rc_channels.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 2de236ff6a..0ac9912df3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -116,6 +116,7 @@ catkin_package( include_directories( ${catkin_INCLUDE_DIRS} src/platforms + src/platforms/ros/px4_messages src/include src/modules src/ @@ -157,52 +158,52 @@ target_link_libraries(subscriber px4 ) -## MC Attitude Control -add_executable(mc_att_control - src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp - src/modules/mc_att_control_multiplatform/mc_att_control.cpp - src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp) -add_dependencies(mc_att_control ${PROJECT_NAME}_generate_messages_cpp_cpp) -target_link_libraries(mc_att_control - ${catkin_LIBRARIES} - px4 -) +# ## MC Attitude Control +# add_executable(mc_att_control + # src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp + # src/modules/mc_att_control_multiplatform/mc_att_control.cpp + # src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp) +# add_dependencies(mc_att_control ${PROJECT_NAME}_generate_messages_cpp_cpp) +# target_link_libraries(mc_att_control + # ${catkin_LIBRARIES} + # px4 +# ) -## Attitude Estimator dummy -add_executable(attitude_estimator - src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp) -add_dependencies(attitude_estimator ${PROJECT_NAME}_generate_messages_cpp_cpp) -target_link_libraries(attitude_estimator - ${catkin_LIBRARIES} - px4 -) +# ## Attitude Estimator dummy +# add_executable(attitude_estimator + # src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp) +# add_dependencies(attitude_estimator ${PROJECT_NAME}_generate_messages_cpp_cpp) +# target_link_libraries(attitude_estimator + # ${catkin_LIBRARIES} + # px4 +# ) -## Manual input -add_executable(manual_input - src/platforms/ros/nodes/manual_input/manual_input.cpp) -add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp) -target_link_libraries(manual_input - ${catkin_LIBRARIES} - px4 -) +# ## Manual input +# add_executable(manual_input + # src/platforms/ros/nodes/manual_input/manual_input.cpp) +# add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp) +# target_link_libraries(manual_input + # ${catkin_LIBRARIES} + # px4 +# ) -## Multicopter Mixer dummy -add_executable(mc_mixer - src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp) -add_dependencies(mc_mixer ${PROJECT_NAME}_generate_messages_cpp_cpp) -target_link_libraries(mc_mixer - ${catkin_LIBRARIES} - px4 -) +# ## Multicopter Mixer dummy +# add_executable(mc_mixer + # src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp) +# add_dependencies(mc_mixer ${PROJECT_NAME}_generate_messages_cpp_cpp) +# target_link_libraries(mc_mixer + # ${catkin_LIBRARIES} + # px4 +# ) -## Commander -add_executable(commander - src/platforms/ros/nodes/commander/commander.cpp) -add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp) -target_link_libraries(commander - ${catkin_LIBRARIES} - px4 -) +# ## Commander +# add_executable(commander + # src/platforms/ros/nodes/commander/commander.cpp) +# add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp) +# target_link_libraries(commander + # ${catkin_LIBRARIES} + # px4 +# ) ############# ## Install ## diff --git a/makefiles/config_px4fmu-v2_multiplatform.mk b/makefiles/config_px4fmu-v2_multiplatform.mk index bed2fa5061..bfa26f83cf 100644 --- a/makefiles/config_px4fmu-v2_multiplatform.mk +++ b/makefiles/config_px4fmu-v2_multiplatform.mk @@ -86,7 +86,8 @@ MODULES += modules/position_estimator_inav #MODULES += modules/fw_pos_control_l1 #MODULES += modules/fw_att_control # MODULES += modules/mc_att_control -MODULES += modules/mc_att_control_multiplatform +# MODULES += modules/mc_att_control_multiplatform +MODULES += examples/publisher MODULES += modules/mc_pos_control MODULES += modules/vtol_att_control diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index fa2c832625..4defe49e75 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -56,7 +56,7 @@ MODULES += systemcmds/ver # Example modules # MODULES += examples/matlab_csv_serial -MODULES += examples/subscriber +#MODULES += examples/subscriber MODULES += examples/publisher # diff --git a/msg/templates/msg.h.template b/msg/templates/msg.h.template index 19068a3a1e..c27daed835 100644 --- a/msg/templates/msg.h.template +++ b/msg/templates/msg.h.template @@ -95,17 +95,17 @@ for field in spec.parsed_fields(): @############################## @{ -type_map = {'int8': 'int8_t', - 'int16': 'int16_t', - 'int32': 'int32_t', - 'int64': 'int64_t', - 'uint8': 'uint8_t', - 'uint16': 'uint16_t', - 'uint32': 'uint32_t', - 'uint64': 'uint64_t', - 'float32': 'float', - 'bool': 'bool', - 'fence_vertex': 'fence_vertex'} +type_map = {'int8': ['int8_t', '0'], + 'int16': ['int16_t', '0'], + 'int32': ['int32_t', '0'], + 'int64': ['int64_t', '0'], + 'uint8': ['uint8_t', '0'], + 'uint16': ['uint16_t', '0'], + 'uint32': ['uint32_t', '0'], + 'uint64': ['uint64_t', '0'], + 'float32': ['float', '0.0f'], + 'bool': ['bool', 'false'], + 'fence_vertex': ['fence_vertex', '']} # Function to print a standard ros type def print_field_def(field): @@ -129,20 +129,70 @@ def print_field_def(field): if type in type_map: # need to add _t: int8 --> int8_t - type_px4 = type_map[type] + type_px4 = type_map[type][0] else: raise Exception("Type {0} not supported, add to to template file!".format(type)) print('\t%s%s%s %s%s;'%(type_prefix, type_px4, type_appendix, field.name, array_size)) +# Function to init fields +def get_field_init(field): + type = field.type + # detect embedded types + sl_pos = type.find('/') + type_appendix = '' + type_prefix = '' + if (sl_pos >= 0): + type = type[sl_pos + 1:] + type_prefix = 'struct ' + type_appendix = '_s' + + # detect arrays + a_pos = type.find('[') + array_size = '' + if (a_pos >= 0): + # field is array + array_size = type[a_pos:] + type = type[:a_pos] + return '\n\t%s{},'%(field.name) + + if type in type_map: + # need to add _t: int8 --> int8_t + type_px4 = type_map[type][0] + init_value = type_map[type][1] + else: + raise Exception("Type {0} not supported, add to to template file!".format(type)) + + return '\n\t%s(%s),'%(field.name, init_value) } +@#ifdef __cplusplus +@#class @(spec.short_name)_s { +@#public: +@#else struct @(spec.short_name)_s { +@#endif @{ # loop over all fields and print the type and name for field in spec.parsed_fields(): if (not field.is_header): print_field_def(field) }@ + +@##ifdef __cplusplus +@#@(spec.short_name)_s() : +@#@{ +@#field_init = '' +@## loop over all fields and init +@#for field in spec.parsed_fields(): +@# if (not field.is_header): +@# field_init += get_field_init(field) +@# +@#print(field_init[:-1]) +@#}@ +@#{} +@#virtual ~@(spec.short_name)_s() {} +@##endif + }; /** diff --git a/src/examples/publisher/publisher_example.cpp b/src/examples/publisher/publisher_example.cpp index 2e5779ebec..e85e42b386 100644 --- a/src/examples/publisher/publisher_example.cpp +++ b/src/examples/publisher/publisher_example.cpp @@ -45,7 +45,8 @@ using namespace px4; PublisherExample::PublisherExample() : _n(), - _rc_channels_pub(PX4_ADVERTISE(_n, rc_channels)) + _rc_channels_pub(_n.advertise()) + // _rc_channels_pub(PX4_ADVERTISE(_n, rc_channels)) { } @@ -55,11 +56,19 @@ int PublisherExample::main() px4::Rate loop_rate(10); while (px4::ok()) { - PX4_TOPIC_T(rc_channels) msg; - msg.timestamp_last_valid = px4::get_time_micros(); - PX4_INFO("%llu", msg.timestamp_last_valid); + // PX4_TOPIC_T(rc_channels) msg; + // msg.timestamp_last_valid = px4::get_time_micros(); + // PX4_INFO("%llu", msg.timestamp_last_valid); + + // _rc_channels_pub->publish(msg); + + //XXX + px4_rc_channels msg2; + msg2.data().timestamp_last_valid = px4::get_time_micros(); + PX4_INFO("%llu", msg2.data().timestamp_last_valid); + // msg2.pub->publish2(); + _rc_channels_pub->publish(msg2); - _rc_channels_pub->publish(msg); _n.spinOnce(); loop_rate.sleep(); diff --git a/src/examples/publisher/publisher_example.h b/src/examples/publisher/publisher_example.h index 304ecef47a..86586d0d34 100644 --- a/src/examples/publisher/publisher_example.h +++ b/src/examples/publisher/publisher_example.h @@ -48,5 +48,6 @@ public: int main(); protected: px4::NodeHandle _n; + // px4::Publisher * _rc_channels_pub; px4::Publisher * _rc_channels_pub; }; diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index 3c80561caf..1c8f4c62bf 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -62,11 +62,13 @@ SubscriberExample::SubscriberExample() : /* Do some subscriptions */ /* Function */ - PX4_SUBSCRIBE(_n, rc_channels, rc_channels_callback_function, _interval); - /* Class Method */ - PX4_SUBSCRIBE(_n, rc_channels, SubscriberExample::rc_channels_callback, this, 1000); - /* No callback */ - _sub_rc_chan = PX4_SUBSCRIBE(_n, rc_channels, 500); + // PX4_SUBSCRIBE(_n, rc_channels, rc_channels_callback_function, _interval); + _n.subscribe(rc_channels_callback_function); + + // [> Class Method <] + // PX4_SUBSCRIBE(_n, rc_channels, SubscriberExample::rc_channels_callback, this, 1000); + // [> No callback <] + // _sub_rc_chan = PX4_SUBSCRIBE(_n, rc_channels, 500); PX4_INFO("subscribed"); } @@ -78,5 +80,5 @@ SubscriberExample::SubscriberExample() : void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) { PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]", msg.timestamp_last_valid, - _sub_rc_chan->get().timestamp_last_valid); + _sub_rc_chan->get().data().timestamp_last_valid); } diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h index eb2c956e09..b52ae991bf 100644 --- a/src/examples/subscriber/subscriber_example.h +++ b/src/examples/subscriber/subscriber_example.h @@ -56,7 +56,8 @@ protected: int32_t _interval; px4_param_t _p_test_float; float _test_float; - px4::PX4_SUBSCRIBER(rc_channels) * _sub_rc_chan; + // px4::PX4_SUBSCRIBER(rc_channels) * _sub_rc_chan; + px4::Subscriber * _sub_rc_chan; void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg); diff --git a/src/platforms/nuttx/px4_messages/px4_rc_channels.h b/src/platforms/nuttx/px4_messages/px4_rc_channels.h new file mode 100644 index 0000000000..bfca484698 --- /dev/null +++ b/src/platforms/nuttx/px4_messages/px4_rc_channels.h @@ -0,0 +1,26 @@ +#include +#include +#include "platforms/px4_message.h" + +#pragma once +namespace px4 +{ + +class px4_rc_channels : + public PX4Message +{ +public: + px4_rc_channels() : + PX4Message() + {} + + px4_rc_channels(rc_channels_s msg) : + PX4Message(msg) + {} + + ~px4_rc_channels() {} + + PX4TopicHandle handle() {return (PX4TopicHandle)ORB_ID(rc_channels);} +}; + +} diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index fc31162c8a..bdb2b8de9b 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -49,6 +49,7 @@ #ifdef __cplusplus #include "ros/ros.h" #include "px4/rc_channels.h" +#include "px4_rc_channels.h" //XXX #include "px4/vehicle_attitude.h" #include #include @@ -71,6 +72,9 @@ #include #include #include +#ifdef __cplusplus +#include //XXX +#endif #include #include #include diff --git a/src/platforms/px4_message.h b/src/platforms/px4_message.h new file mode 100644 index 0000000000..05fcf1140b --- /dev/null +++ b/src/platforms/px4_message.h @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4_message.h + * + * Defines the message base types + */ +#pragma once + +#if defined(__PX4_ROS) +typedef const char* PX4TopicHandle; +#else +#include +typedef const struct orb_metatdata* PX4TopicHandle; +#endif + +namespace px4 +{ + +template +class PX4Message +{ + // friend class NodeHandle; +// #if defined(__PX4_ROS) + // template + // friend class SubscriberROS; +// #endif + +public: + PX4Message() : + _data() + {} + + PX4Message(M msg) : + _data(msg) + {} + + virtual ~PX4Message() {}; + + virtual M& data() {return _data;} + virtual PX4TopicHandle handle() = 0; +private: + M _data; +}; + +} diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 7b14caed97..a405812394 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -48,6 +48,7 @@ #include "ros/ros.h" #include #include +#include #else /* includes when building for NuttX */ #include @@ -77,15 +78,25 @@ public: * @param topic Name of the topic * @param fb Callback, executed on receiving a new message */ - template - Subscriber *subscribe(const char *topic, void(*fp)(const M &)) + // template + // Subscriber *subscribe(const char *topic, void(*fp)(const M &)) + // { + // SubscriberBase *sub = new SubscriberROS(std::bind(fp, std::placeholders::_1)); + // ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, + // (SubscriberROS *)sub); + // ((SubscriberROS *)sub)->set_ros_sub(ros_sub); + // _subs.push_back(sub); + // return (Subscriber *)sub; + // } + template + Subscriber *subscribe(void(*fp)(const typename std::remove_referencedata())>::type &)) { - SubscriberBase *sub = new SubscriberROS(std::bind(fp, std::placeholders::_1)); - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, - (SubscriberROS *)sub); - ((SubscriberROS *)sub)->set_ros_sub(ros_sub); + SubscriberBase *sub = new SubscriberROS(std::bind(fp, std::placeholders::_1)); + ros::Subscriber ros_sub = ros::NodeHandle::subscribe((new T())->handle(), kQueueSizeDefault, + &SubscriberROS::callback, (SubscriberROS *)sub); + ((SubscriberROS *)sub)->set_ros_sub(ros_sub); _subs.push_back(sub); - return (Subscriber *)sub; + return (Subscriber *)sub; } /** @@ -93,40 +104,49 @@ public: * @param topic Name of the topic * @param fb Callback, executed on receiving a new message */ - template - Subscriber *subscribe(const char *topic, void(T::*fp)(const M &), T *obj) - { - SubscriberBase *sub = new SubscriberROS(std::bind(fp, obj, std::placeholders::_1)); - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, - (SubscriberROS *)sub); - ((SubscriberROS *)sub)->set_ros_sub(ros_sub); - _subs.push_back(sub); - return (Subscriber *)sub; - } + // template + // Subscriber *subscribe(const char *topic, void(T::*fp)(const M &), T *obj) + // { + // SubscriberBase *sub = new SubscriberROS(std::bind(fp, obj, std::placeholders::_1)); + // ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, + // (SubscriberROS *)sub); + // ((SubscriberROS *)sub)->set_ros_sub(ros_sub); + // _subs.push_back(sub); + // return (Subscriber *)sub; + // } /** * Subscribe with no callback, just the latest value is stored on updates * @param topic Name of the topic */ - template - Subscriber *subscribe(const char *topic) - { - SubscriberBase *sub = new SubscriberROS(); - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, - (SubscriberROS *)sub); - ((SubscriberROS *)sub)->set_ros_sub(ros_sub); - _subs.push_back(sub); - return (Subscriber *)sub; - } + // template + // Subscriber *subscribe(const char *topic) + // { + // SubscriberBase *sub = new SubscriberROS(); + // ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, + // (SubscriberROS *)sub); + // ((SubscriberROS *)sub)->set_ros_sub(ros_sub); + // _subs.push_back(sub); + // return (Subscriber *)sub; + // } /** * Advertise topic * @param topic Name of the topic */ - template - Publisher *advertise(const char *topic) + // template + // Publisher *advertise(const char *topic) + // { + // ros::Publisher ros_pub = ros::NodeHandle::advertise(topic, kQueueSizeDefault); + // Publisher *pub = new Publisher(ros_pub); + // _pubs.push_back(pub); + // return pub; + // } + template + Publisher* advertise() + // Publisher *advertise() { - ros::Publisher ros_pub = ros::NodeHandle::advertise(topic, kQueueSizeDefault); + ros::Publisher ros_pub = ros::NodeHandle::advertisedata())>::type &>((new T())->handle(), kQueueSizeDefault); Publisher *pub = new Publisher(ros_pub); _pubs.push_back(pub); return pub; @@ -161,7 +181,7 @@ public: ~NodeHandle() { /* Empty subscriptions list */ - uORB::SubscriptionNode *sub = _subs.getHead(); + SubscriberNode *sub = _subs.getHead(); int count = 0; while (sub != nullptr) { @@ -170,13 +190,13 @@ public: break; } - uORB::SubscriptionNode *sib = sub->getSibling(); + SubscriberNode *sib = sub->getSibling(); delete sub; sub = sib; } /* Empty publications list */ - uORB::PublicationNode *pub = _pubs.getHead(); + Publisher *pub = _pubs.getHead(); count = 0; while (pub != nullptr) { @@ -185,7 +205,7 @@ public: break; } - uORB::PublicationNode *sib = pub->getSibling(); + Publisher *sib = pub->getSibling(); delete pub; pub = sib; } @@ -198,19 +218,41 @@ public: * @param interval Minimal interval between calls to callback */ - template - Subscriber *subscribe(const struct orb_metadata *meta, - std::function callback, - unsigned interval) + // template + // Subscriber *subscribe(const struct orb_metadata *meta, + // std::function callback, + // unsigned interval) + // { + // SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(meta, interval, callback, &_subs); + + // [> Check if this is the smallest interval so far and update _sub_min_interval <] + // if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() > sub_px4->getInterval()) { + // _sub_min_interval = sub_px4; + // } + + // return (Subscriber *)sub_px4; + // } + /** + * Subscribe with callback to function + * @param meta Describes the topic which nodehande should subscribe to + * @param callback Callback, executed on receiving a new message + * @param interval Minimal interval between calls to callback + */ + + template + Subscriber *subscribe(std::functiondata())>::type &)> callback, unsigned interval=10) //XXX interval { - SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(meta, interval, callback, &_subs); + const struct orb_metadata * meta = NULL; + uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(meta, interval); + SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(uorb_sub, callback); /* Check if this is the smallest interval so far and update _sub_min_interval */ - if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() > sub_px4->getInterval()) { + if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) { _sub_min_interval = sub_px4; } + _subs.add((SubscriberNode *)sub_px4); - return (Subscriber *)sub_px4; + return (Subscriber *)sub_px4; } /** @@ -219,29 +261,47 @@ public: * @param interval Minimal interval between data fetches from orb */ - template - Subscriber *subscribe(const struct orb_metadata *meta, - unsigned interval) - { - SubscriberUORB *sub_px4 = new SubscriberUORB(meta, interval, &_subs); + // template + // Subscriber *subscribe(const struct orb_metadata *meta, + // unsigned interval) + // { + // SubscriberUORB *sub_px4 = new SubscriberUORB(meta, interval, &_subs); - /* Check if this is the smallest interval so far and update _sub_min_interval */ - if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() > sub_px4->getInterval()) { - _sub_min_interval = sub_px4; - } + // [> Check if this is the smallest interval so far and update _sub_min_interval <] + // if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() > sub_px4->getInterval()) { + // _sub_min_interval = sub_px4; + // } - return (Subscriber *)sub_px4; - } + // return (Subscriber *)sub_px4; + // } /** * Advertise topic * @param meta Describes the topic which is advertised */ - template - Publisher *advertise(const struct orb_metadata *meta) + // template + // Publisher *advertise(const struct orb_metadata *meta) + // { + // //XXX + // Publisher *pub = new Publisher(meta, &_pubs); + // return pub; + // } + + /** + * Advertise topic + * @param meta Describes the topic which is advertised + */ + template + Publisher *advertise() { //XXX - Publisher *pub = new Publisher(meta, &_pubs); + // uORB::PublicationBase * uorb_pub = new uORB::PublicationBase((new T())->handle()); + const struct orb_metadata * meta = NULL; + uORB::PublicationBase * uorb_pub = new uORB::PublicationBase(meta); + Publisher *pub = new Publisher(uorb_pub); + + _pubs.add(pub); + return pub; } @@ -251,7 +311,7 @@ public: void spinOnce() { /* Loop through subscriptions, call callback for updated subscriptions */ - uORB::SubscriptionNode *sub = _subs.getHead(); + SubscriberNode *sub = _subs.getHead(); int count = 0; while (sub != nullptr) { @@ -281,7 +341,7 @@ public: /* Poll fd with smallest interval */ struct pollfd pfd; - pfd.fd = _sub_min_interval->getHandle(); + pfd.fd = _sub_min_interval->getUORBHandle(); pfd.events = POLLIN; poll(&pfd, 1, timeout_ms); spinOnce(); @@ -290,9 +350,9 @@ public: private: static const uint16_t kMaxSubscriptions = 100; static const uint16_t kMaxPublications = 100; - List _subs; /**< Subcriptions of node */ - List _pubs; /**< Publications of node */ - uORB::SubscriptionNode *_sub_min_interval; /**< Points to the sub wtih the smallest interval + List _subs; /**< Subcriptions of node */ + List _pubs; /**< Publications of node */ + SubscriberNode *_sub_min_interval; /**< Points to the sub wtih the smallest interval of all Subscriptions in _subs*/ }; #endif diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index c6f3d6108e..aff045d23b 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -46,6 +46,8 @@ #include #endif +#include + namespace px4 { @@ -58,11 +60,19 @@ public: PublisherBase() {}; ~PublisherBase() {}; + /** Publishes msg + * @param msg the message which is published to the topic + */ + // virtual int publish2(const PX4Message * const msg) = 0; + }; #if defined(__PX4_ROS) +// template class Publisher : public PublisherBase + // public PublisherBase, + // public T { public: /** @@ -71,6 +81,7 @@ public: */ Publisher(ros::Publisher ros_pub) : PublisherBase(), + // T(), _ros_pub(ros_pub) {} @@ -79,15 +90,20 @@ public: /** Publishes msg * @param msg the message which is published to the topic */ - template - int publish(const M &msg) { _ros_pub.publish(msg); return 0; } + // int publish(const M &msg) { _ros_pub.publish(msg); return 0; } + template + int publish(T &msg) { + _ros_pub.publish(msg.data()); + return 0;} private: ros::Publisher _ros_pub; /**< Handle to the ros publisher */ }; #else class Publisher : - public uORB::PublicationNode, - public PublisherBase + // public uORB::PublicationNode, + public PublisherBase, + public ListNode + { public: /** @@ -95,10 +111,14 @@ public: * @param meta orb metadata for the topic which is used * @param list publisher is added to this list */ - Publisher(const struct orb_metadata *meta, - List *list) : - uORB::PublicationNode(meta, list), - PublisherBase() + // Publisher(const struct orb_metadata *meta, + // List *list) : + // uORB::PublicationNode(meta, list), + // PublisherBase() + // {} + Publisher(uORB::PublicationBase * uorb_pub) : + PublisherBase(), + _uorb_pub(uorb_pub) {} ~Publisher() {}; @@ -109,7 +129,7 @@ public: template int publish(const M &msg) { - uORB::PublicationBase::update((void *)&msg); + _uorb_pub->update((void *)&msg); return 0; } @@ -117,6 +137,9 @@ public: * Empty callback for list traversal */ void update() {} ; +private: + uORB::PublicationBase * _uorb_pub; /**< Handle to the publisher */ + }; #endif } diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index aaacf9e484..ef03922ad3 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -39,6 +39,7 @@ #pragma once #include +#include #if defined(__PX4_ROS) /* includes when building for ros */ @@ -67,7 +68,7 @@ public: /** * Subscriber class which is used by nodehandle */ -template +template class Subscriber : public SubscriberBase { @@ -81,7 +82,7 @@ public: /** * Get the last message value */ - virtual M get() = 0; + virtual T get() = 0; /** * Get void pointer to last message value @@ -93,9 +94,9 @@ public: /** * Subscriber class that is templated with the ros n message type */ -template +template class SubscriberROS : - public Subscriber + public Subscriber { friend class NodeHandle; @@ -103,8 +104,8 @@ public: /** * Construct Subscriber by providing a callback function */ - SubscriberROS(std::function cbf) : - Subscriber(), + SubscriberROS(std::functiondata())>::type &)> cbf) : + Subscriber(), _ros_sub(), _cbf(cbf), _msg_current() @@ -114,7 +115,7 @@ public: * Construct Subscriber without a callback function */ SubscriberROS() : - Subscriber(), + Subscriber(), _ros_sub(), _cbf(NULL), _msg_current() @@ -127,7 +128,7 @@ public: /** * Get the last message value */ - M get() { return _msg_current; } + T get() { return _msg_current; } /** * Get void pointer to last message value */ @@ -137,10 +138,10 @@ protected: /** * Called on topic update, saves the current message and then calls the provided callback function */ - void callback(const M &msg) + void callback(const typename std::remove_referencedata())>::type &msg) { /* Store data */ - _msg_current = msg; + _msg_current = (T)msg; /* Call callback */ if (_cbf != NULL) { @@ -158,20 +159,47 @@ protected: } ros::Subscriber _ros_sub; /**< Handle to ros subscriber */ - std::function _cbf; /**< Callback that the user provided on the subscription */ - M _msg_current; /**< Current Message value */ + std::functiondata())>::type &)> _cbf; /**< Callback that the user provided on the subscription */ + T _msg_current; /**< Current Message value */ }; #else // Building for NuttX + +/** + * Because we maintain a list of subscribers we need a node class + */ +class SubscriberNode : + public ListNode +{ +public: + SubscriberNode(unsigned interval) : + ListNode(), + _interval(interval) + {} + + virtual ~SubscriberNode() {} + + virtual void update() = 0; + + virtual int getUORBHandle() = 0; + + unsigned get_interval() { return _interval; } + +protected: + unsigned _interval; + +}; + + /** * Subscriber class that is templated with the uorb subscription message type */ -template +template class SubscriberUORB : - public Subscriber, - public uORB::Subscription + public Subscriber, + public SubscriberNode { public: @@ -181,11 +209,15 @@ public: * @param interval Minimal interval between calls to callback * @param list subscriber is added to this list */ - SubscriberUORB(const struct orb_metadata *meta, - unsigned interval, - List *list) : - Subscriber(), - uORB::Subscription(meta, interval, list) + // SubscriberUORB(const struct orb_metadata *meta, + // unsigned interval, + // List *list) : + // Subscriber(), + // uORB::Subscription(meta, interval, list) + // {} + SubscriberUORB(uORB::SubscriptionBase * uorb_sub, unsigned interval) : + SubscriberNode(interval), + _uorb_sub(uorb_sub) {} ~SubscriberUORB() {}; @@ -196,29 +228,36 @@ public: */ virtual void update() { - if (!uORB::Subscription::updated()) { + if (!_uorb_sub->updated()) { /* Topic not updated */ return; } /* get latest data */ - uORB::Subscription::update(); + _uorb_sub->update(get_void_ptr()); }; /* Accessors*/ /** * Get the last message value */ - M get() { return uORB::Subscription::getData(); } + T get() { return (T)(typename std::remove_referencedata())>::type)*_uorb_sub; } /** * Get void pointer to last message value */ - void *get_void_ptr() { return uORB::Subscription::getDataVoidPtr(); } + void *get_void_ptr() { return (void *)(typename std::remove_referencedata())>::type*)_uorb_sub; } + + int getUORBHandle() { return _uorb_sub->getHandle(); } + +protected: + uORB::SubscriptionBase * _uorb_sub; /**< Handle to the subscription */ + typename std::remove_referencedata())>::type getUORBData() { return (typename std::remove_referencedata())>::type)*_uorb_sub; } }; -template +//XXX reduce to one class with overloaded constructor? +template class SubscriberUORBCallback : - public SubscriberUORB + public SubscriberUORB { public: /** @@ -228,11 +267,16 @@ public: * @param interval Minimal interval between calls to callback * @param list subscriber is added to this list */ - SubscriberUORBCallback(const struct orb_metadata *meta, - unsigned interval, - std::function callback, - List *list) : - SubscriberUORB(meta, interval, list), + // SubscriberUORBCallback(const struct orb_metadata *meta, + // unsigned interval, + // std::function callback, + // List *list) : + // SubscriberUORB(meta, interval, list), + // _callback(callback) + // {} + SubscriberUORBCallback(uORB::SubscriptionBase * uorb_sub, + std::functiondata())>::type &)> callback) : + SubscriberUORB(uorb_sub), _callback(callback) {} @@ -245,13 +289,13 @@ public: */ virtual void update() { - if (!uORB::Subscription::updated()) { + if (!SubscriberUORB::_uorb_sub->updated()) { /* Topic not updated, do not call callback */ return; } /* get latest data */ - uORB::Subscription::update(); + SubscriberUORB::_uorb_sub->update(); /* Check if there is a callback */ @@ -260,12 +304,12 @@ public: } /* Call callback which performs actions based on this data */ - _callback(uORB::Subscription::getData()); + _callback(SubscriberUORB::getUORBData()); }; protected: - std::function _callback; /**< Callback handle, + std::functiondata())>::type &)> _callback; /**< Callback handle, called when new data is available */ }; #endif diff --git a/src/platforms/ros/px4_messages/px4_rc_channels.h b/src/platforms/ros/px4_messages/px4_rc_channels.h new file mode 100644 index 0000000000..a5a5ee2020 --- /dev/null +++ b/src/platforms/ros/px4_messages/px4_rc_channels.h @@ -0,0 +1,25 @@ +#include "px4/rc_channels.h" +#include "platforms/px4_message.h" + +#pragma once +namespace px4 +{ + +class px4_rc_channels : + public PX4Message +{ +public: + px4_rc_channels() : + PX4Message() + {} + + px4_rc_channels(rc_channels msg) : + PX4Message(msg) + {} + + ~px4_rc_channels() {} + + PX4TopicHandle handle() {return "rc_channels";} +}; + +} From cadcad6ffbdfbe9b92a5936f4d894138f912b4ff Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 20 Jan 2015 18:27:05 +0100 Subject: [PATCH 081/269] messagelayer prototype for nuttx --- makefiles/config_px4fmu-v2_test.mk | 2 +- msg/templates/msg.h.template | 7 +- .../subscriber/subscriber_example.cpp | 19 ++-- src/examples/subscriber/subscriber_example.h | 2 +- .../nuttx/px4_messages/px4_rc_channels.h | 2 +- src/platforms/px4_message.h | 3 +- src/platforms/px4_nodehandle.h | 20 ++-- src/platforms/px4_publisher.h | 4 +- src/platforms/px4_subscriber.h | 93 +++++++++---------- 9 files changed, 78 insertions(+), 74 deletions(-) diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 4defe49e75..fa2c832625 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -56,7 +56,7 @@ MODULES += systemcmds/ver # Example modules # MODULES += examples/matlab_csv_serial -#MODULES += examples/subscriber +MODULES += examples/subscriber MODULES += examples/publisher # diff --git a/msg/templates/msg.h.template b/msg/templates/msg.h.template index c27daed835..826418d118 100644 --- a/msg/templates/msg.h.template +++ b/msg/templates/msg.h.template @@ -165,12 +165,13 @@ def get_field_init(field): return '\n\t%s(%s),'%(field.name, init_value) } -@#ifdef __cplusplus +#ifdef __cplusplus @#class @(spec.short_name)_s { +struct __EXPORT @(spec.short_name)_s { @#public: -@#else +#else struct @(spec.short_name)_s { -@#endif +#endif @{ # loop over all fields and print the type and name for field in spec.parsed_fields(): diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index 1c8f4c62bf..c662a07ad2 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -43,8 +43,10 @@ using namespace px4; -void rc_channels_callback_function(const PX4_TOPIC_T(rc_channels) &msg) { - PX4_INFO("I heard: [%llu]", msg.timestamp_last_valid); +// void rc_channels_callback_function(const PX4_TOPIC_T(rc_channels) &msg) { +void rc_channels_callback_function(const px4_rc_channels &msg); +void rc_channels_callback_function(const px4_rc_channels &msg) { + PX4_INFO("I heard: [%llu]", msg.data().timestamp_last_valid); } SubscriberExample::SubscriberExample() : @@ -63,7 +65,8 @@ SubscriberExample::SubscriberExample() : /* Do some subscriptions */ /* Function */ // PX4_SUBSCRIBE(_n, rc_channels, rc_channels_callback_function, _interval); - _n.subscribe(rc_channels_callback_function); + _n.subscribe(rc_channels_callback_function); //ROS version + // _n.subscribe(std::bind(&rc_channels_callback_function, std::placeholders::_1)); UORB version // [> Class Method <] // PX4_SUBSCRIBE(_n, rc_channels, SubscriberExample::rc_channels_callback, this, 1000); @@ -77,8 +80,8 @@ SubscriberExample::SubscriberExample() : * This tutorial demonstrates simple receipt of messages over the PX4 middleware system. * Also the current value of the _sub_rc_chan subscription is printed */ -void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) { - PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]", - msg.timestamp_last_valid, - _sub_rc_chan->get().data().timestamp_last_valid); -} +// void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) { + // PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]", + // msg.timestamp_last_valid, + // _sub_rc_chan->get().data().timestamp_last_valid); +// } diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h index b52ae991bf..43dafe010a 100644 --- a/src/examples/subscriber/subscriber_example.h +++ b/src/examples/subscriber/subscriber_example.h @@ -59,7 +59,7 @@ protected: // px4::PX4_SUBSCRIBER(rc_channels) * _sub_rc_chan; px4::Subscriber * _sub_rc_chan; - void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg); + // void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg); }; diff --git a/src/platforms/nuttx/px4_messages/px4_rc_channels.h b/src/platforms/nuttx/px4_messages/px4_rc_channels.h index bfca484698..cb1135eb98 100644 --- a/src/platforms/nuttx/px4_messages/px4_rc_channels.h +++ b/src/platforms/nuttx/px4_messages/px4_rc_channels.h @@ -6,7 +6,7 @@ namespace px4 { -class px4_rc_channels : +class __EXPORT px4_rc_channels : public PX4Message { public: diff --git a/src/platforms/px4_message.h b/src/platforms/px4_message.h index 05fcf1140b..c908f6fc69 100644 --- a/src/platforms/px4_message.h +++ b/src/platforms/px4_message.h @@ -49,7 +49,7 @@ namespace px4 { template -class PX4Message +class __EXPORT PX4Message { // friend class NodeHandle; // #if defined(__PX4_ROS) @@ -69,6 +69,7 @@ public: virtual ~PX4Message() {}; virtual M& data() {return _data;} + virtual const M& data() const {return _data;} virtual PX4TopicHandle handle() = 0; private: M _data; diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index a405812394..2406a4a775 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -89,7 +89,8 @@ public: // return (Subscriber *)sub; // } template - Subscriber *subscribe(void(*fp)(const typename std::remove_referencedata())>::type &)) + // Subscriber *subscribe(void(*fp)(const typename std::remove_referencedata())>::type &)) + Subscriber *subscribe(void(*fp)(const T &)) { SubscriberBase *sub = new SubscriberROS(std::bind(fp, std::placeholders::_1)); ros::Subscriber ros_sub = ros::NodeHandle::subscribe((new T())->handle(), kQueueSizeDefault, @@ -240,17 +241,20 @@ public: */ template - Subscriber *subscribe(std::functiondata())>::type &)> callback, unsigned interval=10) //XXX interval + // Subscriber *subscribe(std::functiondata())>::type &)> callback, unsigned interval=10) //XXX interval + // Subscriber *subscribe(void(*fp)(const typename std::remove_referencedata())>::type &), unsigned interval=10) //XXX interval + Subscriber *subscribe(void(*fp)(const T &), unsigned interval=10) //XXX interval { const struct orb_metadata * meta = NULL; uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(meta, interval); - SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(uorb_sub, callback); + // SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(uorb_sub, interval, callback); + SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(uorb_sub, interval, std::bind(fp, std::placeholders::_1)); - /* Check if this is the smallest interval so far and update _sub_min_interval */ - if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) { - _sub_min_interval = sub_px4; - } - _subs.add((SubscriberNode *)sub_px4); + // [> Check if this is the smallest interval so far and update _sub_min_interval <] + // if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) { + // _sub_min_interval = sub_px4; + // } + // _subs.add((SubscriberNode *)sub_px4); return (Subscriber *)sub_px4; } diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index aff045d23b..6d75e28fcc 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -54,7 +54,7 @@ namespace px4 /** * Untemplated publisher base class * */ -class PublisherBase +class __EXPORT PublisherBase { public: PublisherBase() {}; @@ -99,7 +99,7 @@ private: ros::Publisher _ros_pub; /**< Handle to the ros publisher */ }; #else -class Publisher : +class __EXPORT Publisher : // public uORB::PublicationNode, public PublisherBase, public ListNode diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index ef03922ad3..88cc86ab80 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -57,11 +57,11 @@ namespace px4 /** * Untemplated subscriber base class * */ -class SubscriberBase +class __EXPORT SubscriberBase { public: SubscriberBase() {}; - ~SubscriberBase() {}; + virtual ~SubscriberBase() {}; }; @@ -69,25 +69,25 @@ public: * Subscriber class which is used by nodehandle */ template -class Subscriber : +class __EXPORT Subscriber : public SubscriberBase { public: Subscriber() : - SubscriberBase() + SubscriberBase(), + _msg_current() {}; - ~Subscriber() {}; + + virtual ~Subscriber() {} /* Accessors*/ /** * Get the last message value */ - virtual T get() = 0; + virtual T get() {return _msg_current;} - /** - * Get void pointer to last message value - */ - virtual void *get_void_ptr() = 0; +protected: + T _msg_current; /**< Current Message value */ }; #if defined(__PX4_ROS) @@ -104,11 +104,11 @@ public: /** * Construct Subscriber by providing a callback function */ - SubscriberROS(std::functiondata())>::type &)> cbf) : + // SubscriberROS(std::functiondata())>::type &)> cbf) : + SubscriberROS(std::function cbf) : Subscriber(), _ros_sub(), - _cbf(cbf), - _msg_current() + _cbf(cbf) {} /** @@ -117,35 +117,26 @@ public: SubscriberROS() : Subscriber(), _ros_sub(), - _cbf(NULL), - _msg_current() + _cbf(NULL) {} - ~SubscriberROS() {}; - - /* Accessors*/ - /** - * Get the last message value - */ - T get() { return _msg_current; } - /** - * Get void pointer to last message value - */ - void *get_void_ptr() { return (void *)&_msg_current; } + virtual ~SubscriberROS() {}; protected: /** * Called on topic update, saves the current message and then calls the provided callback function + * needs to use the native type as it is called by ROS */ void callback(const typename std::remove_referencedata())>::type &msg) { /* Store data */ - _msg_current = (T)msg; + this->_msg_current = (T)msg; /* Call callback */ if (_cbf != NULL) { - _cbf(msg); + // _cbf(_msg_current); + _cbf(this->get()); } } @@ -159,8 +150,7 @@ protected: } ros::Subscriber _ros_sub; /**< Handle to ros subscriber */ - std::functiondata())>::type &)> _cbf; /**< Callback that the user provided on the subscription */ - T _msg_current; /**< Current Message value */ + std::function _cbf; /**< Callback that the user provided on the subscription */ }; @@ -170,7 +160,7 @@ protected: /** * Because we maintain a list of subscribers we need a node class */ -class SubscriberNode : +class __EXPORT SubscriberNode : public ListNode { public: @@ -197,7 +187,7 @@ protected: * Subscriber class that is templated with the uorb subscription message type */ template -class SubscriberUORB : +class __EXPORT SubscriberUORB : public Subscriber, public SubscriberNode { @@ -220,7 +210,7 @@ public: _uorb_sub(uorb_sub) {} - ~SubscriberUORB() {}; + virtual ~SubscriberUORB() {}; /** * Update Subscription @@ -229,11 +219,10 @@ public: virtual void update() { if (!_uorb_sub->updated()) { - /* Topic not updated */ + /* Topic not updated, do not call callback */ return; } - /* get latest data */ _uorb_sub->update(get_void_ptr()); }; @@ -241,11 +230,16 @@ public: /** * Get the last message value */ - T get() { return (T)(typename std::remove_referencedata())>::type)*_uorb_sub; } + // T get() { return (T)(typename std::remove_referencedata())>::type)*_uorb_sub; } + // T get() { + // typename std::remove_referencedata())>::type msg = (typename std::remove_referencedata())>::type)*_uorb_sub; + // return (T)msg; + // } + /** * Get void pointer to last message value */ - void *get_void_ptr() { return (void *)(typename std::remove_referencedata())>::type*)_uorb_sub; } + void *get_void_ptr() { return (void *)&(this->_msg_current.data()); } int getUORBHandle() { return _uorb_sub->getHandle(); } @@ -256,7 +250,7 @@ protected: //XXX reduce to one class with overloaded constructor? template -class SubscriberUORBCallback : +class __EXPORT SubscriberUORBCallback : public SubscriberUORB { public: @@ -275,12 +269,13 @@ public: // _callback(callback) // {} SubscriberUORBCallback(uORB::SubscriptionBase * uorb_sub, - std::functiondata())>::type &)> callback) : - SubscriberUORB(uorb_sub), - _callback(callback) + unsigned interval, + std::function cbf) : + SubscriberUORB(uorb_sub, interval), + _cbf(cbf) {} - ~SubscriberUORBCallback() {}; + virtual ~SubscriberUORBCallback() {}; /** * Update Subscription @@ -289,28 +284,28 @@ public: */ virtual void update() { - if (!SubscriberUORB::_uorb_sub->updated()) { + if (!this->_uorb_sub->updated()) { /* Topic not updated, do not call callback */ return; - } /* get latest data */ - SubscriberUORB::_uorb_sub->update(); + this->_uorb_sub->update(this->get_void_ptr()); /* Check if there is a callback */ - if (_callback == nullptr) { + if (_cbf == nullptr) { return; } /* Call callback which performs actions based on this data */ - _callback(SubscriberUORB::getUORBData()); + _cbf(Subscriber::get()); + } }; protected: - std::functiondata())>::type &)> _callback; /**< Callback handle, - called when new data is available */ + // std::functiondata())>::type &)> _callback; [>*< Callback handle, called when new data is available */ + std::function _cbf; /**< Callback that the user provided on the subscription */ }; #endif From 632a0866ef16723dd6e1f0a2f7c575706b9e10cc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 21 Jan 2015 14:34:05 +0100 Subject: [PATCH 082/269] remove unneeded functionality in template --- msg/templates/msg.h.template | 69 +++++++----------------------------- 1 file changed, 12 insertions(+), 57 deletions(-) diff --git a/msg/templates/msg.h.template b/msg/templates/msg.h.template index 826418d118..cc128c1ea2 100644 --- a/msg/templates/msg.h.template +++ b/msg/templates/msg.h.template @@ -95,17 +95,17 @@ for field in spec.parsed_fields(): @############################## @{ -type_map = {'int8': ['int8_t', '0'], - 'int16': ['int16_t', '0'], - 'int32': ['int32_t', '0'], - 'int64': ['int64_t', '0'], - 'uint8': ['uint8_t', '0'], - 'uint16': ['uint16_t', '0'], - 'uint32': ['uint32_t', '0'], - 'uint64': ['uint64_t', '0'], - 'float32': ['float', '0.0f'], - 'bool': ['bool', 'false'], - 'fence_vertex': ['fence_vertex', '']} +type_map = {'int8': 'int8_t', + 'int16': 'int16_t', + 'int32': 'int32_t', + 'int64': 'int64_t', + 'uint8': 'uint8_t', + 'uint16': 'uint16_t', + 'uint32': 'uint32_t', + 'uint64': 'uint64_t', + 'float32': 'float', + 'bool': 'bool', + 'fence_vertex': 'fence_vertex'} # Function to print a standard ros type def print_field_def(field): @@ -129,41 +129,12 @@ def print_field_def(field): if type in type_map: # need to add _t: int8 --> int8_t - type_px4 = type_map[type][0] + type_px4 = type_map[type] else: raise Exception("Type {0} not supported, add to to template file!".format(type)) print('\t%s%s%s %s%s;'%(type_prefix, type_px4, type_appendix, field.name, array_size)) -# Function to init fields -def get_field_init(field): - type = field.type - # detect embedded types - sl_pos = type.find('/') - type_appendix = '' - type_prefix = '' - if (sl_pos >= 0): - type = type[sl_pos + 1:] - type_prefix = 'struct ' - type_appendix = '_s' - - # detect arrays - a_pos = type.find('[') - array_size = '' - if (a_pos >= 0): - # field is array - array_size = type[a_pos:] - type = type[:a_pos] - return '\n\t%s{},'%(field.name) - - if type in type_map: - # need to add _t: int8 --> int8_t - type_px4 = type_map[type][0] - init_value = type_map[type][1] - else: - raise Exception("Type {0} not supported, add to to template file!".format(type)) - - return '\n\t%s(%s),'%(field.name, init_value) } #ifdef __cplusplus @#class @(spec.short_name)_s { @@ -178,22 +149,6 @@ for field in spec.parsed_fields(): if (not field.is_header): print_field_def(field) }@ - -@##ifdef __cplusplus -@#@(spec.short_name)_s() : -@#@{ -@#field_init = '' -@## loop over all fields and init -@#for field in spec.parsed_fields(): -@# if (not field.is_header): -@# field_init += get_field_init(field) -@# -@#print(field_init[:-1]) -@#}@ -@#{} -@#virtual ~@(spec.short_name)_s() {} -@##endif - }; /** From 1f706eeb2fd210e7059e2b58f14432c987d76abf Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 21 Jan 2015 14:34:34 +0100 Subject: [PATCH 083/269] small cleanup --- src/examples/publisher/publisher_example.h | 1 - src/examples/subscriber/subscriber_example.cpp | 3 +-- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/src/examples/publisher/publisher_example.h b/src/examples/publisher/publisher_example.h index 86586d0d34..304ecef47a 100644 --- a/src/examples/publisher/publisher_example.h +++ b/src/examples/publisher/publisher_example.h @@ -48,6 +48,5 @@ public: int main(); protected: px4::NodeHandle _n; - // px4::Publisher * _rc_channels_pub; px4::Publisher * _rc_channels_pub; }; diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index c662a07ad2..fd462cd2d3 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -66,8 +66,7 @@ SubscriberExample::SubscriberExample() : /* Function */ // PX4_SUBSCRIBE(_n, rc_channels, rc_channels_callback_function, _interval); _n.subscribe(rc_channels_callback_function); //ROS version - // _n.subscribe(std::bind(&rc_channels_callback_function, std::placeholders::_1)); UORB version - + // [> Class Method <] // PX4_SUBSCRIBE(_n, rc_channels, SubscriberExample::rc_channels_callback, this, 1000); // [> No callback <] From 3a38b0fe359296aa19ec43ab82743aebeadb335c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 21 Jan 2015 14:35:36 +0100 Subject: [PATCH 084/269] define __EXPORT for ROS --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 0ac9912df3..5488dbe9db 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 2.8.3) project(px4) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") add_definitions(-D__PX4_ROS) +add_definitions(-D__EXPORT=) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) From eea3c801f4be7efa306af01a8153e8bbee4d43ce Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 10 Jan 2015 22:26:49 +0300 Subject: [PATCH 085/269] UAVCAN perf counters --- src/modules/uavcan/uavcan_main.cpp | 30 ++++++++++++++++++++++++++++++ src/modules/uavcan/uavcan_main.hpp | 9 +++++++-- 2 files changed, 37 insertions(+), 2 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 60901e0c75..af5f2ec96f 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -81,6 +81,22 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys if (res < 0) { std::abort(); } + + if (_perfcnt_node_spin_elapsed == nullptr) { + errx(1, "uavcan: couldn't allocate _perfcnt_node_spin_elapsed"); + } + + if (_perfcnt_esc_mixer_output_elapsed == nullptr) { + errx(1, "uavcan: couldn't allocate _perfcnt_esc_mixer_output_elapsed"); + } + + if (_perfcnt_esc_mixer_total_elapsed == nullptr) { + errx(1, "uavcan: couldn't allocate _perfcnt_esc_mixer_total_elapsed"); + } + + if (_perfcnt_esc_mixer_subscriptions == nullptr) { + errx(1, "uavcan: couldn't allocate _perfcnt_esc_mixer_subscriptions"); + } } UavcanNode::~UavcanNode() @@ -118,6 +134,11 @@ UavcanNode::~UavcanNode() } _instance = nullptr; + + perf_free(_perfcnt_node_spin_elapsed); + perf_free(_perfcnt_esc_mixer_output_elapsed); + perf_free(_perfcnt_esc_mixer_total_elapsed); + perf_free(_perfcnt_esc_mixer_subscriptions); } int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) @@ -265,10 +286,12 @@ int UavcanNode::init(uavcan::NodeID node_id) void UavcanNode::node_spin_once() { + perf_begin(_perfcnt_node_spin_elapsed); const int spin_res = _node.spin(uavcan::MonotonicTime()); if (spin_res < 0) { warnx("node spin error %i", spin_res); } + perf_end(_perfcnt_node_spin_elapsed); } /* @@ -344,8 +367,12 @@ int UavcanNode::run() // Mutex is unlocked while the thread is blocked on IO multiplexing (void)pthread_mutex_unlock(&_node_mutex); + perf_end(_perfcnt_esc_mixer_total_elapsed); // end goes first, it's not a mistake + const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs); + perf_begin(_perfcnt_esc_mixer_total_elapsed); + (void)pthread_mutex_lock(&_node_mutex); node_spin_once(); // Non-blocking @@ -430,7 +457,9 @@ int UavcanNode::run() } // Output to the bus _outputs.timestamp = hrt_absolute_time(); + perf_begin(_perfcnt_esc_mixer_output_elapsed); _esc_controller.update_outputs(_outputs.output, _outputs.noutputs); + perf_end(_perfcnt_esc_mixer_output_elapsed); } @@ -498,6 +527,7 @@ UavcanNode::arm_actuators(bool arm) void UavcanNode::subscribe() { + perf_count(_perfcnt_esc_mixer_subscriptions); // Subscribe/unsubscribe to required actuator control groups uint32_t sub_groups = _groups_required & ~_groups_subscribed; uint32_t unsub_groups = _groups_subscribed & ~_groups_required; diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 98f2e5ad4b..8a0993f15d 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -34,9 +34,9 @@ #pragma once #include - #include #include +#include #include #include @@ -66,7 +66,7 @@ */ class UavcanNode : public device::CDev { - static constexpr unsigned MemPoolSize = 10752; + static constexpr unsigned MemPoolSize = 10752; ///< Refer to the libuavcan manual to learn why static constexpr unsigned RxQueueLenPerIface = 64; static constexpr unsigned StackSize = 3000; @@ -142,4 +142,9 @@ private: // index into _poll_fds for each _control_subs handle uint8_t _poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; + + perf_counter_t _perfcnt_node_spin_elapsed = perf_alloc(PC_ELAPSED, "uavcan_node_spin_elapsed"); + perf_counter_t _perfcnt_esc_mixer_output_elapsed = perf_alloc(PC_ELAPSED, "uavcan_esc_mixer_output_elapsed"); + perf_counter_t _perfcnt_esc_mixer_total_elapsed = perf_alloc(PC_ELAPSED, "uavcan_esc_mixer_total_elapsed"); + perf_counter_t _perfcnt_esc_mixer_subscriptions = perf_alloc(PC_COUNT, "uavcan_esc_mixer_subscriptions"); }; From 7f0f4b3072008239f4481c79769ab1b00f92e707 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 11 Jan 2015 00:11:48 +0300 Subject: [PATCH 086/269] UavcanEscController broadcasting percounter --- src/modules/uavcan/actuators/esc.cpp | 14 ++++++++++++++ src/modules/uavcan/actuators/esc.hpp | 1 + 2 files changed, 15 insertions(+) diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 9f682c7e16..995c8987c0 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -49,12 +49,24 @@ UavcanEscController::UavcanEscController(uavcan::INode &node) : _uavcan_sub_status(node), _orb_timer(node) { + if (_perfcnt_invalid_input == nullptr) { + errx(1, "uavcan: couldn't allocate _perfcnt_invalid_input"); + } + + if (_perfcnt_scaling_error == nullptr) { + errx(1, "uavcan: couldn't allocate _perfcnt_scaling_error"); + } + + if (_perfcnt_broadcast_elapsed == nullptr) { + errx(1, "uavcan: couldn't allocate _perfcnt_broadcast_elapsed"); + } } UavcanEscController::~UavcanEscController() { perf_free(_perfcnt_invalid_input); perf_free(_perfcnt_scaling_error); + perf_free(_perfcnt_broadcast_elapsed); } int UavcanEscController::init() @@ -129,7 +141,9 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) * Publish the command message to the bus * Note that for a quadrotor it takes one CAN frame */ + perf_begin(_perfcnt_broadcast_elapsed); (void)_uavcan_pub_raw_cmd.broadcast(msg); + perf_end(_perfcnt_broadcast_elapsed); } void UavcanEscController::arm_all_escs(bool arm) diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp index 12c0355422..498fb9dd8b 100644 --- a/src/modules/uavcan/actuators/esc.hpp +++ b/src/modules/uavcan/actuators/esc.hpp @@ -109,4 +109,5 @@ private: */ perf_counter_t _perfcnt_invalid_input = perf_alloc(PC_COUNT, "uavcan_esc_invalid_input"); perf_counter_t _perfcnt_scaling_error = perf_alloc(PC_COUNT, "uavcan_esc_scaling_error"); + perf_counter_t _perfcnt_broadcast_elapsed = perf_alloc(PC_ELAPSED, "uavcan_esc_broadcast_elapsed"); }; From 0901d999924012b482a5379854c24eb9f2c6f345 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 11 Jan 2015 04:38:20 +0300 Subject: [PATCH 087/269] UAVCAN update (speed optimizations) --- src/lib/uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/uavcan b/src/lib/uavcan index c4c45b995f..20439bb397 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit c4c45b995f5c8192c7a36c4293c201711ceac74d +Subproject commit 20439bb3975c34a24c3c337a1f231fbf973a41e8 From 9215a8d8dafb3807852e10a3dab150c1fc6fc6f4 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 15 Jan 2015 00:11:55 +0300 Subject: [PATCH 088/269] Nuttx.py fixes: Python 2.7 support (required for gcc-arm-toolchain from Terry Guo), fixed int parsing, show mybt by name --- Debug/Nuttx.py | 51 ++++++++++++++++++++++++++++++-------------------- 1 file changed, 31 insertions(+), 20 deletions(-) diff --git a/Debug/Nuttx.py b/Debug/Nuttx.py index cf86dd668f..f915d3bcbb 100644 --- a/Debug/Nuttx.py +++ b/Debug/Nuttx.py @@ -1,7 +1,10 @@ # GDB/Python functions for dealing with NuttX +from __future__ import print_function import gdb, gdb.types +parse_int = lambda x: int(str(x), 0) + class NX_register_set(object): """Copy of the registers for a given context""" @@ -155,7 +158,7 @@ class NX_task(object): result = [] for i in range(pidhash_type.range()[0],pidhash_type.range()[1]): entry = pidhash_value[i] - pid = int(entry['pid']) + pid = parse_int(entry['pid']) if pid is not -1: result.append(pid) return result @@ -184,7 +187,7 @@ class NX_task(object): self.__dict__['stack_used'] = 0 else: stack_limit = self._tcb['adj_stack_size'] - for offset in range(0, int(stack_limit)): + for offset in range(0, parse_int(stack_limit)): if stack_base[offset] != 0xff: break self.__dict__['stack_used'] = stack_limit - offset @@ -244,7 +247,7 @@ class NX_task(object): filearray = filelist['fl_files'] result = dict() for i in range(filearray.type.range()[0],filearray.type.range()[1]): - inode = int(filearray[i]['f_inode']) + inode = parse_int(filearray[i]['f_inode']) if inode != 0: result[i] = inode return result @@ -299,7 +302,7 @@ class NX_show_task (gdb.Command): super(NX_show_task, self).__init__("show task", gdb.COMMAND_USER) def invoke(self, arg, from_tty): - t = NX_task.for_pid(int(arg)) + t = NX_task.for_pid(parse_int(arg)) if t is not None: my_fmt = 'PID:{pid} name:{name} state:{state}\n' my_fmt += ' stack used {stack_used} of {stack_limit}\n' @@ -435,12 +438,12 @@ class NX_tcb(object): first_tcb = tcb_ptr.dereference() tcb_list.append(first_tcb); next_tcb = first_tcb['flink'].dereference() - while not self.is_in(int(next_tcb['pid']),[int(t['pid']) for t in tcb_list]): + while not self.is_in(parse_int(next_tcb['pid']),[parse_int(t['pid']) for t in tcb_list]): tcb_list.append(next_tcb); old_tcb = next_tcb; next_tcb = old_tcb['flink'].dereference() - return [t for t in tcb_list if int(t['pid'])<2000] + return [t for t in tcb_list if parse_int(t['pid'])<2000] def getTCB(self): list_of_listsnames = ['g_pendingtasks','g_readytorun','g_waitingforsemaphore','g_waitingforsignal','g_inactivetasks'] @@ -469,12 +472,12 @@ class NX_check_stack_order(gdb.Command): first_tcb = tcb_ptr.dereference() tcb_list.append(first_tcb); next_tcb = first_tcb['flink'].dereference() - while not self.is_in(int(next_tcb['pid']),[int(t['pid']) for t in tcb_list]): + while not self.is_in(parse_int(next_tcb['pid']),[parse_int(t['pid']) for t in tcb_list]): tcb_list.append(next_tcb); old_tcb = next_tcb; next_tcb = old_tcb['flink'].dereference() - return [t for t in tcb_list if int(t['pid'])<2000] + return [t for t in tcb_list if parse_int(t['pid'])<2000] def getTCB(self): list_of_listsnames = ['g_pendingtasks','g_readytorun','g_waitingforsemaphore','g_waitingforsignal','g_inactivetasks'] @@ -488,7 +491,7 @@ class NX_check_stack_order(gdb.Command): def getSPfromTask(self,tcb): regmap = NX_register_set.v7em_regmap a =tcb['xcp']['regs'] - return int(a[regmap['SP']]) + return parse_int(a[regmap['SP']]) def find_closest(self,list,val): tmp_list = [abs(i-val) for i in list] @@ -525,8 +528,8 @@ class NX_check_stack_order(gdb.Command): for t in tcb: p = []; #print(t.name,t._tcb['stack_alloc_ptr']) - p.append(int(t['stack_alloc_ptr'])) - p.append(int(t['adj_stack_ptr'])) + p.append(parse_int(t['stack_alloc_ptr'])) + p.append(parse_int(t['adj_stack_ptr'])) p.append(self.getSPfromTask(t)) stackadresses[str(t['name'])] = p; address = int("0x30000000",0) @@ -594,12 +597,12 @@ class NX_search_tcb(gdb.Command): first_tcb = tcb_ptr.dereference() tcb_list.append(first_tcb); next_tcb = first_tcb['flink'].dereference() - while not self.is_in(int(next_tcb['pid']),[int(t['pid']) for t in tcb_list]): + while not self.is_in(parse_int(next_tcb['pid']),[parse_int(t['pid']) for t in tcb_list]): tcb_list.append(next_tcb); old_tcb = next_tcb; next_tcb = old_tcb['flink'].dereference() - return [t for t in tcb_list if int(t['pid'])<2000] + return [t for t in tcb_list if parse_int(t['pid'])<2000] def invoke(self,args,sth): list_of_listsnames = ['g_pendingtasks','g_readytorun','g_waitingforsemaphore','g_waitingforsignal','g_inactivetasks'] @@ -612,7 +615,7 @@ class NX_search_tcb(gdb.Command): # filter for tasks that are listed twice tasks_filt = {} for t in tasks: - pid = int(t['pid']); + pid = parse_int(t['pid']); if not pid in tasks_filt.keys(): tasks_filt[pid] = t['name']; print('{num_t} Tasks found:'.format(num_t = len(tasks_filt))) @@ -687,13 +690,21 @@ class NX_my_bt(gdb.Command): def invoke(self,args,sth): - addr_dec = int(args[2:],16) - _tcb = self.get_tcb_from_address(addr_dec) + try: + addr_dec = parse_int(args) # Trying to interpret the input as TCB address + except ValueError: + for task in NX_task.tasks(): # Interpreting as a task name + if task.name == args: + _tcb = task._tcb + break + else: + _tcb = self.get_tcb_from_address(addr_dec) + print("found task with PID: ",_tcb["pid"]) - up_stack = int(_tcb['adj_stack_ptr']) - curr_sp = int(_tcb['xcp']['regs'][0]) #curr stack pointer - other_sp = int(_tcb['xcp']['regs'][8]) # other stack pointer - stacksize = int(_tcb['adj_stack_size']) # other stack pointer + up_stack = parse_int(_tcb['adj_stack_ptr']) + curr_sp = parse_int(_tcb['xcp']['regs'][0]) #curr stack pointer + other_sp = parse_int(_tcb['xcp']['regs'][8]) # other stack pointer + stacksize = parse_int(_tcb['adj_stack_size']) # other stack pointer print("tasks current SP = ",hex(curr_sp),"stack max ptr is at ",hex(up_stack)) From e41bf13ac57081342a94f48a468e3f6f24404f8b Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 15 Jan 2015 00:49:32 +0300 Subject: [PATCH 089/269] Nuttx.py: 'show mybt' outputs in GDB-like format for ease of parsing --- Debug/Nuttx.py | 37 ++++++++----------------------------- 1 file changed, 8 insertions(+), 29 deletions(-) diff --git a/Debug/Nuttx.py b/Debug/Nuttx.py index f915d3bcbb..826e12c97c 100644 --- a/Debug/Nuttx.py +++ b/Debug/Nuttx.py @@ -656,38 +656,20 @@ class NX_my_bt(gdb.Command): tcb_ptr = addr_value.cast(gdb.lookup_type('struct tcb_s').pointer()) return tcb_ptr.dereference() - def print_instruction_at(self,addr,stack_percentage): + def resolve_file_line_func(self,addr,stack_percentage): gdb.write(str(round(stack_percentage,2))+":") str_to_eval = "info line *"+hex(addr) #gdb.execute(str_to_eval) res = gdb.execute(str_to_eval,to_string = True) # get information from results string: words = res.split() - valid = False - if words[0] == 'No': - #no line info... - pass - else: - valid = True + if words[0] != 'No': line = int(words[1]) - idx = words[3].rfind("/"); #find first backslash - if idx>0: - name = words[3][idx+1:]; - path = words[3][:idx]; - else: - name = words[3]; - path = ""; block = gdb.block_for_pc(addr) func = block.function if str(func) == "None": func = block.superblock.function - - if valid: - print("Line: ",line," in ",path,"/",name,"in ",func) - return name,path,line,func - - - + return words[3].strip('"'), line, func def invoke(self,args,sth): try: @@ -708,18 +690,15 @@ class NX_my_bt(gdb.Command): print("tasks current SP = ",hex(curr_sp),"stack max ptr is at ",hex(up_stack)) - if curr_sp == up_stack: - sp = other_sp - else: - sp = curr_sp; - - while(sp < up_stack): + item = 0 + for sp in range(other_sp if curr_sp == up_stack else curr_sp, up_stack, 4): mem = self.readmem(sp) #print(hex(sp)," : ",hex(mem)) if self.is_in_bounds(mem): # this is a potential instruction ptr stack_percentage = (up_stack-sp)/stacksize - name,path,line,func = self.print_instruction_at(mem,stack_percentage) - sp = sp + 4; # jump up one word + filename,line,func = self.resolve_file_line_func(mem, stack_percentage) + print('#%-2d ' % item, '0x%08x in ' % mem, func, ' at ', filename, ':', line, sep='') + item += 1 NX_my_bt() From dd3fa2532e63118bfd076e1027d74af2d06b9e05 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 15 Jan 2015 02:44:54 +0300 Subject: [PATCH 090/269] Poor man's sampling profiler for NuttX --- Debug/poor-mans-profiler.sh | 66 +++++++++++++++++++++++++++++++++++++ 1 file changed, 66 insertions(+) create mode 100755 Debug/poor-mans-profiler.sh diff --git a/Debug/poor-mans-profiler.sh b/Debug/poor-mans-profiler.sh new file mode 100755 index 0000000000..f706fad853 --- /dev/null +++ b/Debug/poor-mans-profiler.sh @@ -0,0 +1,66 @@ +#!/bin/bash +# +# Poor man's sampling profiler for NuttX. +# +# The stack folding script was inspired by stackcollapse-gdb.pl from the FlameGraph project. +# +# Usage: Install flamegraph.pl in your PATH, define the variables below, configure your .gdbinit, run the script and go +# have a coffee. When you're back, you'll see the flamegraph. Note that frequent calls to GDB significantly +# interfere with normal operation of the target, which means that you can't profile real-time tasks with it. +# +# Requirements: ARM GDB with Python support +# + +root=$(dirname $0)/.. + +nsamples=100 +sleeptime=0.01 # Doctors recommend 7-8 hours a day +taskname=uavcan +exe=$root/Build/px4fmu-v2_default.build/firmware.elf + +set -e + +stacksfile=/tmp/$taskname-stacks.log + +cd $root +rm -f $stacksfile +echo "Sampling..." + +for x in $(seq 1 $nsamples) +do + arm-none-eabi-gdb $exe --batch -ex "set print asm-demangle on" \ + -ex "source $root/Debug/Nuttx.py" \ + -ex "show mybt $taskname" \ + 2> /dev/null \ + | sed -n 's/0\.0:\(#.*\)/\1/p' \ + >> $stacksfile + echo -e '\n\n' >> $stacksfile + echo -ne "\r$x/$nsamples" + sleep $sleeptime +done + +echo +echo "Stacks saved to $stacksfile" + +cat $stacksfile | perl -e 'my $current = ""; +my %stacks; +while(<>) { + if(m/^#[0-9]*\s*0x[a-zA-Z0-9]*\s*in (.*) at/) { + if ($1 ne "None") { + if ($current eq "") { $current = $1; } + else { $current = $1 . ";" . $current; } + } + } elsif(!($current eq "")) { + $stacks{$current} += 1; + $current = ""; + } +} +foreach my $k (sort { $a cmp $b } keys %stacks) { + print "$k $stacks{$k}\n"; +}' > /tmp/$taskname-folded.txt + +echo "Folded stacks saved to /tmp/$taskname-folded.txt" + +cat /tmp/$taskname-folded.txt | flamegraph.pl --fontsize=8 --width=1800 > /tmp/$taskname-flamegraph.svg + +xdg-open /tmp/$taskname-flamegraph.svg From 9293950cdb17af28e344f710f9121e606710a006 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 16 Jan 2015 20:47:29 +0300 Subject: [PATCH 091/269] NuttX profiler improvements --- Debug/poor-mans-profiler.sh | 136 +++++++++++++++++++++++++++--------- 1 file changed, 102 insertions(+), 34 deletions(-) diff --git a/Debug/poor-mans-profiler.sh b/Debug/poor-mans-profiler.sh index f706fad853..1d76eaa445 100755 --- a/Debug/poor-mans-profiler.sh +++ b/Debug/poor-mans-profiler.sh @@ -11,45 +11,111 @@ # Requirements: ARM GDB with Python support # +set -e root=$(dirname $0)/.. -nsamples=100 -sleeptime=0.01 # Doctors recommend 7-8 hours a day -taskname=uavcan -exe=$root/Build/px4fmu-v2_default.build/firmware.elf +# +# Parsing the arguments. Read this section for usage info. +# +nsamples=0 +sleeptime=0.1 # Doctors recommend 7-8 hours a day +taskname= +elf=$root/Build/px4fmu-v2_default.build/firmware.elf +append=0 +fgfontsize=5 +fgwidth=1900 -set -e +function usage() +{ + echo "Invalid usage. Supported options:" + cat $0 | sed -n 's/^\s*--\([^)\*]*\).*/\1/p' # Don't try this at home. + exit 1 +} -stacksfile=/tmp/$taskname-stacks.log - -cd $root -rm -f $stacksfile -echo "Sampling..." - -for x in $(seq 1 $nsamples) +for i in "$@" do - arm-none-eabi-gdb $exe --batch -ex "set print asm-demangle on" \ - -ex "source $root/Debug/Nuttx.py" \ - -ex "show mybt $taskname" \ - 2> /dev/null \ - | sed -n 's/0\.0:\(#.*\)/\1/p' \ - >> $stacksfile - echo -e '\n\n' >> $stacksfile - echo -ne "\r$x/$nsamples" - sleep $sleeptime + case $i in + --nsamples=*) + nsamples="${i#*=}" + ;; + --sleeptime=*) + sleeptime="${i#*=}" + ;; + --taskname=*) + taskname="${i#*=}" + ;; + --elf=*) + elf="${i#*=}" + ;; + --append) + append=1 + ;; + --fgfontsize=*) + fgfontsize="${i#*=}" + ;; + --fgwidth=*) + fgwidth="${i#*=}" + ;; + *) + usage + ;; + esac + shift done -echo -echo "Stacks saved to $stacksfile" +# +# Temporary files +# +stacksfile=/tmp/pmpn-stacks.log +foldfile=/tmp/pmpn-folded.txt +graphfile=/tmp/pmpn-flamegraph.svg -cat $stacksfile | perl -e 'my $current = ""; +# +# Sampling if requested. Note that if $append is true, the stack file will not be rewritten. +# +cd $root + +if [[ $nsamples > 0 && "$taskname" != "" ]] +then + [[ $append = 0 ]] && (rm -f $stacksfile; echo "Old stacks removed") + + echo "Sampling..." + + for x in $(seq 1 $nsamples) + do + arm-none-eabi-gdb $exe --batch -ex "set print asm-demangle on" \ + -ex "source $root/Debug/Nuttx.py" \ + -ex "show mybt $taskname" \ + 2> /dev/null \ + | sed -n 's/0\.0:\(#.*\)/\1/p' \ + >> $stacksfile + echo -e '\n\n' >> $stacksfile + echo -ne "\r$x/$nsamples" + sleep $sleeptime + done + + echo + echo "Stacks saved to $stacksfile" +else + echo "Sampling skipped - set 'nsamples' and 'taskname' to re-sample." +fi + +# +# Folding the stacks. +# +if [ ! -f $stacksfile ]; then + echo "Where are the stack samples?" + exit 1 +fi + +cat $stacksfile | perl -e 'use File::Basename; +my $current = ""; my %stacks; while(<>) { - if(m/^#[0-9]*\s*0x[a-zA-Z0-9]*\s*in (.*) at/) { - if ($1 ne "None") { - if ($current eq "") { $current = $1; } - else { $current = $1 . ";" . $current; } - } + if(m/^#[0-9]*\s*0x[a-zA-Z0-9]*\s*in (.*) at (.*)/) { + my $x = $1 eq "None" ? basename($2) : $1; + if ($current eq "") { $current = $x; } + else { $current = $x . ";" . $current; } } elsif(!($current eq "")) { $stacks{$current} += 1; $current = ""; @@ -57,10 +123,12 @@ while(<>) { } foreach my $k (sort { $a cmp $b } keys %stacks) { print "$k $stacks{$k}\n"; -}' > /tmp/$taskname-folded.txt +}' > $foldfile -echo "Folded stacks saved to /tmp/$taskname-folded.txt" +echo "Folded stacks saved to $foldfile" -cat /tmp/$taskname-folded.txt | flamegraph.pl --fontsize=8 --width=1800 > /tmp/$taskname-flamegraph.svg - -xdg-open /tmp/$taskname-flamegraph.svg +# +# Graphing. +# +cat $foldfile | flamegraph.pl --fontsize=$fgfontsize --width=$fgwidth > $graphfile +xdg-open $graphfile From c0937ec8cac523d4c8fad028584e2b87956f3019 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 16 Jan 2015 21:00:48 +0300 Subject: [PATCH 092/269] Profiler fixes --- Debug/poor-mans-profiler.sh | 27 ++++++++++++++++----------- 1 file changed, 16 insertions(+), 11 deletions(-) diff --git a/Debug/poor-mans-profiler.sh b/Debug/poor-mans-profiler.sh index 1d76eaa445..d2393b296e 100755 --- a/Debug/poor-mans-profiler.sh +++ b/Debug/poor-mans-profiler.sh @@ -14,6 +14,21 @@ set -e root=$(dirname $0)/.. +function die() +{ + echo "$@" + exit 1 +} + +function usage() +{ + echo "Invalid usage. Supported options:" + cat $0 | sed -n 's/^\s*--\([^)\*]*\).*/\1/p' # Don't try this at home. + exit 1 +} + +which flamegraph.pl > /dev/null || die "Install flamegraph.pl first" + # # Parsing the arguments. Read this section for usage info. # @@ -25,13 +40,6 @@ append=0 fgfontsize=5 fgwidth=1900 -function usage() -{ - echo "Invalid usage. Supported options:" - cat $0 | sed -n 's/^\s*--\([^)\*]*\).*/\1/p' # Don't try this at home. - exit 1 -} - for i in "$@" do case $i in @@ -103,10 +111,7 @@ fi # # Folding the stacks. # -if [ ! -f $stacksfile ]; then - echo "Where are the stack samples?" - exit 1 -fi +[ -f $stacksfile ] || die "Where are the stack samples?" cat $stacksfile | perl -e 'use File::Basename; my $current = ""; From c0d71529bce816596dc3574e876e0f4c69bc9b9f Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 16 Jan 2015 21:13:10 +0300 Subject: [PATCH 093/269] Profiler fixes --- Debug/poor-mans-profiler.sh | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/Debug/poor-mans-profiler.sh b/Debug/poor-mans-profiler.sh index d2393b296e..eede0072d7 100755 --- a/Debug/poor-mans-profiler.sh +++ b/Debug/poor-mans-profiler.sh @@ -77,6 +77,7 @@ done stacksfile=/tmp/pmpn-stacks.log foldfile=/tmp/pmpn-folded.txt graphfile=/tmp/pmpn-flamegraph.svg +gdberrfile=/tmp/pmpn-gdberr.log # # Sampling if requested. Note that if $append is true, the stack file will not be rewritten. @@ -87,14 +88,14 @@ if [[ $nsamples > 0 && "$taskname" != "" ]] then [[ $append = 0 ]] && (rm -f $stacksfile; echo "Old stacks removed") - echo "Sampling..." + echo "Sampling the task '$taskname'..." for x in $(seq 1 $nsamples) do - arm-none-eabi-gdb $exe --batch -ex "set print asm-demangle on" \ - -ex "source $root/Debug/Nuttx.py" \ - -ex "show mybt $taskname" \ - 2> /dev/null \ + arm-none-eabi-gdb $elf --batch -ex "set print asm-demangle on" \ + -ex "source $root/Debug/Nuttx.py" \ + -ex "show mybt $taskname" \ + 2> $gdberrfile \ | sed -n 's/0\.0:\(#.*\)/\1/p' \ >> $stacksfile echo -e '\n\n' >> $stacksfile From ff7c33a4b04e0545dc78a324cbc8f9a945f97519 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 16 Jan 2015 21:40:14 +0300 Subject: [PATCH 094/269] Profiler: xdg-open work-around --- Debug/poor-mans-profiler.sh | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/Debug/poor-mans-profiler.sh b/Debug/poor-mans-profiler.sh index eede0072d7..2b25337565 100755 --- a/Debug/poor-mans-profiler.sh +++ b/Debug/poor-mans-profiler.sh @@ -37,7 +37,7 @@ sleeptime=0.1 # Doctors recommend 7-8 hours a day taskname= elf=$root/Build/px4fmu-v2_default.build/firmware.elf append=0 -fgfontsize=5 +fgfontsize=10 fgwidth=1900 for i in "$@" @@ -137,4 +137,12 @@ echo "Folded stacks saved to $foldfile" # Graphing. # cat $foldfile | flamegraph.pl --fontsize=$fgfontsize --width=$fgwidth > $graphfile -xdg-open $graphfile +echo "FlameGraph saved to $graphfile" + +# On KDE, xdg-open prefers Gwenview by default, which doesn't handle interactive SVGs, so we need a browser. +# The current implementation is hackish and stupid. Somebody, please do something about it. +opener=xdg-open +which firefox > /dev/null && opener=firefox +which google-chrome > /dev/null && opener=google-chrome + +$opener $graphfile From 1898b51c740dba2b3dd979980775f25c982dea1e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 16 Jan 2015 22:08:46 +0300 Subject: [PATCH 095/269] Profiler: reporting function position in flame graphs --- Debug/poor-mans-profiler.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Debug/poor-mans-profiler.sh b/Debug/poor-mans-profiler.sh index 2b25337565..460b28d0ad 100755 --- a/Debug/poor-mans-profiler.sh +++ b/Debug/poor-mans-profiler.sh @@ -119,7 +119,7 @@ my $current = ""; my %stacks; while(<>) { if(m/^#[0-9]*\s*0x[a-zA-Z0-9]*\s*in (.*) at (.*)/) { - my $x = $1 eq "None" ? basename($2) : $1; + my $x = $1 eq "None" ? basename($2) : ("$1 at " . basename($2)); if ($current eq "") { $current = $x; } else { $current = $x . ";" . $current; } } elsif(!($current eq "")) { From d1abf9c133bfe865f9648a88610d92a78f787f21 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Jan 2015 02:25:26 +0300 Subject: [PATCH 096/269] Poor man's profiler - proper stack folder that handles generic C++ types --- Debug/poor-mans-profiler.sh | 137 +++++++++++++++++++++++++++++------- 1 file changed, 113 insertions(+), 24 deletions(-) diff --git a/Debug/poor-mans-profiler.sh b/Debug/poor-mans-profiler.sh index 460b28d0ad..7657e4dc7d 100755 --- a/Debug/poor-mans-profiler.sh +++ b/Debug/poor-mans-profiler.sh @@ -84,7 +84,7 @@ gdberrfile=/tmp/pmpn-gdberr.log # cd $root -if [[ $nsamples > 0 && "$taskname" != "" ]] +if [[ $nsamples > 0 ]] then [[ $append = 0 ]] && (rm -f $stacksfile; echo "Old stacks removed") @@ -92,12 +92,20 @@ then for x in $(seq 1 $nsamples) do - arm-none-eabi-gdb $elf --batch -ex "set print asm-demangle on" \ - -ex "source $root/Debug/Nuttx.py" \ - -ex "show mybt $taskname" \ - 2> $gdberrfile \ - | sed -n 's/0\.0:\(#.*\)/\1/p' \ - >> $stacksfile + if [[ "$taskname" = "" ]] + then + arm-none-eabi-gdb $elf --batch -ex "set print asm-demangle on" -ex bt \ + 2> $gdberrfile \ + | sed -n 's/\(#.*\)/\1/p' \ + >> $stacksfile + else + arm-none-eabi-gdb $elf --batch -ex "set print asm-demangle on" \ + -ex "source $root/Debug/Nuttx.py" \ + -ex "show mybt $taskname" \ + 2> $gdberrfile \ + | sed -n 's/0\.0:\(#.*\)/\1/p' \ + >> $stacksfile + fi echo -e '\n\n' >> $stacksfile echo -ne "\r$x/$nsamples" sleep $sleeptime @@ -106,7 +114,7 @@ then echo echo "Stacks saved to $stacksfile" else - echo "Sampling skipped - set 'nsamples' and 'taskname' to re-sample." + echo "Sampling skipped - set 'nsamples' to re-sample." fi # @@ -114,22 +122,103 @@ fi # [ -f $stacksfile ] || die "Where are the stack samples?" -cat $stacksfile | perl -e 'use File::Basename; -my $current = ""; -my %stacks; -while(<>) { - if(m/^#[0-9]*\s*0x[a-zA-Z0-9]*\s*in (.*) at (.*)/) { - my $x = $1 eq "None" ? basename($2) : ("$1 at " . basename($2)); - if ($current eq "") { $current = $x; } - else { $current = $x . ";" . $current; } - } elsif(!($current eq "")) { - $stacks{$current} += 1; - $current = ""; - } -} -foreach my $k (sort { $a cmp $b } keys %stacks) { - print "$k $stacks{$k}\n"; -}' > $foldfile +cat $stacksfile | python -c " +# +# This stack folder correctly handles C++ types. +# + +from __future__ import print_function, division +import fileinput, collections, os + +def enforce(x, msg='Invalid input'): + if not x: + raise Exception(msg) + +def split_first_part_with_parens(line): + LBRACES = {'(':'()', '<':'<>', '[':'[]', '{':'{}'} + RBRACES = {')':'()', '>':'<>', ']':'[]', '}':'{}'} + braces = collections.defaultdict(int) + out = '' + for ch in line: + out += ch + # special cases + if out.endswith('operator>') or out.endswith('operator->'): # gotta love c++ + braces['<>'] += 1 + if out.endswith('operator<'): + braces['<>'] -= 1 + # counting parens + if ch in LBRACES.keys(): + braces[LBRACES[ch]] += 1 + if ch in RBRACES.keys(): + braces[RBRACES[ch]] -= 1 + # sanity check + for v in braces.values(): + enforce(v >= 0, 'Unaligned braces: ' + str(dict(braces))) + # termination condition + if ch == ' ' and sum(braces.values()) == 0: + break + out = out.strip() + return out, line[len(out):] + +def parse(line): + def take_path(line, output): + line = line.strip() + if line.startswith('at '): + line = line[3:].strip() + if line: + output['file_full_path'] = line.rsplit(':', 1)[0].strip() + output['file_base_name'] = os.path.basename(output['file_full_path']) + output['line'] = int(line.rsplit(':', 1)[1]) + return output + + def take_args(line, output): + line = line.lstrip() + if line[0] == '(': + output['args'], line = split_first_part_with_parens(line) + return take_path(line.lstrip(), output) + + def take_function(line, output): + output['function'], line = split_first_part_with_parens(line.lstrip()) + return take_args(line.lstrip(), output) + + def take_mem_loc(line, output): + line = line.lstrip() + if line.startswith('0x'): + end = line.find(' ') + num = line[:end] + output['memloc'] = int(num, 16) + line = line[end:].lstrip() + end = line.find(' ') + enforce(line[:end] == 'in') + line = line[end:].lstrip() + return take_function(line, output) + + def take_frame_num(line, output): + line = line.lstrip() + enforce(line[0] == '#') + end = line.find(' ') + num = line[1:end] + output['frame_num'] = int(num) + return take_mem_loc(line[end:], output) + + return take_frame_num(line, {}) + +stacks = collections.defaultdict(int) +current = '' + +for line in fileinput.input(): + line = line.strip() + if line: + inf = parse(line) + fun = inf['function'] + current = (fun + ';' + current) if current else fun + elif current: + stacks[current] += 1 + current = '' + +for s, f in sorted(stacks.items(), key=lambda (s, f): s): + print(s, f) +" > $foldfile echo "Folded stacks saved to $foldfile" From f158c8270b7572b3f3384a52cb6f6c51ecd7ec57 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Jan 2015 02:58:07 +0300 Subject: [PATCH 097/269] Rich man's profiler - handling quotes --- Debug/poor-mans-profiler.sh | 49 +++++++++++++++++++++++-------------- 1 file changed, 31 insertions(+), 18 deletions(-) diff --git a/Debug/poor-mans-profiler.sh b/Debug/poor-mans-profiler.sh index 7657e4dc7d..89ff4f424b 100755 --- a/Debug/poor-mans-profiler.sh +++ b/Debug/poor-mans-profiler.sh @@ -122,13 +122,12 @@ fi # [ -f $stacksfile ] || die "Where are the stack samples?" -cat $stacksfile | python -c " +cat << 'EOF' > /tmp/pmpn-folder.py # # This stack folder correctly handles C++ types. # - from __future__ import print_function, division -import fileinput, collections, os +import fileinput, collections, os, sys def enforce(x, msg='Invalid input'): if not x: @@ -137,20 +136,29 @@ def enforce(x, msg='Invalid input'): def split_first_part_with_parens(line): LBRACES = {'(':'()', '<':'<>', '[':'[]', '{':'{}'} RBRACES = {')':'()', '>':'<>', ']':'[]', '}':'{}'} + QUOTES = set(['"', "'"]) + quotes = collections.defaultdict(bool) braces = collections.defaultdict(int) out = '' for ch in line: out += ch + # escape character cancels further processing + if ch == '\\': + continue # special cases if out.endswith('operator>') or out.endswith('operator->'): # gotta love c++ braces['<>'] += 1 if out.endswith('operator<'): braces['<>'] -= 1 - # counting parens - if ch in LBRACES.keys(): - braces[LBRACES[ch]] += 1 - if ch in RBRACES.keys(): - braces[RBRACES[ch]] -= 1 + # switching quotes + if ch in QUOTES: + quotes[ch] = not quotes[ch] + # counting parens only when outside quotes + if sum(quotes.values()) == 0: + if ch in LBRACES.keys(): + braces[LBRACES[ch]] += 1 + if ch in RBRACES.keys(): + braces[RBRACES[ch]] -= 1 # sanity check for v in braces.values(): enforce(v >= 0, 'Unaligned braces: ' + str(dict(braces))) @@ -206,19 +214,24 @@ def parse(line): stacks = collections.defaultdict(int) current = '' -for line in fileinput.input(): - line = line.strip() - if line: - inf = parse(line) - fun = inf['function'] - current = (fun + ';' + current) if current else fun - elif current: - stacks[current] += 1 - current = '' +for idx,line in enumerate(fileinput.input()): + try: + line = line.strip() + if line: + inf = parse(line) + fun = inf['function'] + current = (fun + ';' + current) if current else fun + elif current: + stacks[current] += 1 + current = '' + except Exception, ex: + print('ERROR (line %d):' % (idx + 1), ex, file=sys.stderr) for s, f in sorted(stacks.items(), key=lambda (s, f): s): print(s, f) -" > $foldfile +EOF + +cat $stacksfile | python /tmp/pmpn-folder.py > $foldfile echo "Folded stacks saved to $foldfile" From 647163d6fa4761b0799e073bae8ea368a986b4fe Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Jan 2015 03:06:41 +0300 Subject: [PATCH 098/269] Profiler - header comment fix --- Debug/poor-mans-profiler.sh | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/Debug/poor-mans-profiler.sh b/Debug/poor-mans-profiler.sh index 89ff4f424b..63dba562a8 100755 --- a/Debug/poor-mans-profiler.sh +++ b/Debug/poor-mans-profiler.sh @@ -2,9 +2,7 @@ # # Poor man's sampling profiler for NuttX. # -# The stack folding script was inspired by stackcollapse-gdb.pl from the FlameGraph project. -# -# Usage: Install flamegraph.pl in your PATH, define the variables below, configure your .gdbinit, run the script and go +# Usage: Install flamegraph.pl in your PATH, configure your .gdbinit, run the script with proper arguments and go # have a coffee. When you're back, you'll see the flamegraph. Note that frequent calls to GDB significantly # interfere with normal operation of the target, which means that you can't profile real-time tasks with it. # From 543cb231282fdb899557f42003f53ac2f7a7dcc7 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Jan 2015 03:57:46 +0300 Subject: [PATCH 099/269] Profiler: computing stack top distribution --- Debug/poor-mans-profiler.sh | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/Debug/poor-mans-profiler.sh b/Debug/poor-mans-profiler.sh index 63dba562a8..d907d8749b 100755 --- a/Debug/poor-mans-profiler.sh +++ b/Debug/poor-mans-profiler.sh @@ -212,6 +212,9 @@ def parse(line): stacks = collections.defaultdict(int) current = '' +stack_tops = collections.defaultdict(int) +num_stack_frames = 0 + for idx,line in enumerate(fileinput.input()): try: line = line.strip() @@ -219,6 +222,10 @@ for idx,line in enumerate(fileinput.input()): inf = parse(line) fun = inf['function'] current = (fun + ';' + current) if current else fun + + if inf['frame_num'] == 0: + num_stack_frames += 1 + stack_tops[fun] += 1 elif current: stacks[current] += 1 current = '' @@ -227,6 +234,11 @@ for idx,line in enumerate(fileinput.input()): for s, f in sorted(stacks.items(), key=lambda (s, f): s): print(s, f) + +print('Total stack frames:', num_stack_frames, file=sys.stderr) +print('Top consumers (distribution of the stack tops):', file=sys.stderr) +for name,num in sorted(stack_tops.items(), key=lambda (name, num): num, reverse=True)[:10]: + print('% 5.1f%% ' % (100 * num / num_stack_frames), name, file=sys.stderr) EOF cat $stacksfile | python /tmp/pmpn-folder.py > $foldfile From 6bbacc4271680cc262094af7f9a34807ff5c34ed Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Jan 2015 04:04:31 +0300 Subject: [PATCH 100/269] Intrusive changes made for UAVCAN profiling. Will be reverted in the next commit (this one is needed to keep the changes in history) --- ROMFS/px4fmu_common/init.d/rcS | 5 ++++- makefiles/toolchain_gnu-arm-eabi.mk | 2 +- src/modules/uavcan/actuators/esc.cpp | 10 +++++----- src/modules/uavcan/uavcan_main.cpp | 10 ++++++++++ 4 files changed, 20 insertions(+), 7 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 2c9387ff06..02f4649d5f 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -8,7 +8,7 @@ # # Default to auto-start mode. # -set MODE autostart +set MODE none set FRC /fs/microsd/etc/rc.txt set FCONFIG /fs/microsd/etc/config.txt @@ -656,6 +656,9 @@ then # End of autostart fi +param set UAVCAN_ENABLE 1 +sh /etc/init.d/rc.uavcan + # There is no further processing, so we can free some RAM # XXX potentially unset all script variables. unset TUNE_ERR diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index fbe378f509..1bbc5437d5 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -105,7 +105,7 @@ ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) # optimisation flags # ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ - -g \ + -g3 \ -fno-strict-aliasing \ -fno-strength-reduce \ -fomit-frame-pointer \ diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 995c8987c0..f7adbed760 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -98,11 +98,11 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) /* * Rate limiting - we don't want to congest the bus */ - const auto timestamp = _node.getMonotonicTime(); - if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) { - return; - } - _prev_cmd_pub = timestamp; +// const auto timestamp = _node.getMonotonicTime(); +// if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) { +// return; +// } +// _prev_cmd_pub = timestamp; /* * Fill the command message diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index af5f2ec96f..c29cd83230 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -357,6 +357,16 @@ int UavcanNode::run() _actuator_direct_poll_fd_num = add_poll_fd(_actuator_direct_sub); } + _esc_controller.arm_all_escs(true); + while (true) { + for (int i = 0; i < 1000; i++) { + node_spin_once(); + _outputs.noutputs = 8; + _esc_controller.update_outputs(_outputs.output, _outputs.noutputs); + } + ::usleep(1000); + } + while (!_task_should_exit) { // update actuator controls subscriptions if needed if (_groups_subscribed != _groups_required) { From d87bb4dfcb59601bc51d46332bbe0db78d11294a Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Jan 2015 04:04:48 +0300 Subject: [PATCH 101/269] Revert "Intrusive changes made for UAVCAN profiling. Will be reverted in the next commit (this one is needed to keep the changes in history)" This reverts commit 4c301d9dcf180e39186fa6753c7a3d3215b3cfa7. --- ROMFS/px4fmu_common/init.d/rcS | 5 +---- makefiles/toolchain_gnu-arm-eabi.mk | 2 +- src/modules/uavcan/actuators/esc.cpp | 10 +++++----- src/modules/uavcan/uavcan_main.cpp | 10 ---------- 4 files changed, 7 insertions(+), 20 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 02f4649d5f..2c9387ff06 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -8,7 +8,7 @@ # # Default to auto-start mode. # -set MODE none +set MODE autostart set FRC /fs/microsd/etc/rc.txt set FCONFIG /fs/microsd/etc/config.txt @@ -656,9 +656,6 @@ then # End of autostart fi -param set UAVCAN_ENABLE 1 -sh /etc/init.d/rc.uavcan - # There is no further processing, so we can free some RAM # XXX potentially unset all script variables. unset TUNE_ERR diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 1bbc5437d5..fbe378f509 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -105,7 +105,7 @@ ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) # optimisation flags # ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ - -g3 \ + -g \ -fno-strict-aliasing \ -fno-strength-reduce \ -fomit-frame-pointer \ diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index f7adbed760..995c8987c0 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -98,11 +98,11 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) /* * Rate limiting - we don't want to congest the bus */ -// const auto timestamp = _node.getMonotonicTime(); -// if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) { -// return; -// } -// _prev_cmd_pub = timestamp; + const auto timestamp = _node.getMonotonicTime(); + if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) { + return; + } + _prev_cmd_pub = timestamp; /* * Fill the command message diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index c29cd83230..af5f2ec96f 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -357,16 +357,6 @@ int UavcanNode::run() _actuator_direct_poll_fd_num = add_poll_fd(_actuator_direct_sub); } - _esc_controller.arm_all_escs(true); - while (true) { - for (int i = 0; i < 1000; i++) { - node_spin_once(); - _outputs.noutputs = 8; - _esc_controller.update_outputs(_outputs.output, _outputs.noutputs); - } - ::usleep(1000); - } - while (!_task_should_exit) { // update actuator controls subscriptions if needed if (_groups_subscribed != _groups_required) { From 569c3b7d37f4be5c9a635fe3e4633ecebbe8f9b5 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Jan 2015 05:10:43 +0300 Subject: [PATCH 102/269] Using -g3 flag instead of -g --- makefiles/toolchain_gnu-arm-eabi.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index fbe378f509..1bbc5437d5 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -105,7 +105,7 @@ ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) # optimisation flags # ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ - -g \ + -g3 \ -fno-strict-aliasing \ -fno-strength-reduce \ -fomit-frame-pointer \ From 885077a1c3cfa9980001e8dce76615e1f7552788 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Jan 2015 18:32:29 +0300 Subject: [PATCH 103/269] Profiler: folder fix - more special cases for operator<< and operator>> --- Debug/poor-mans-profiler.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Debug/poor-mans-profiler.sh b/Debug/poor-mans-profiler.sh index d907d8749b..ab06a1b66a 100755 --- a/Debug/poor-mans-profiler.sh +++ b/Debug/poor-mans-profiler.sh @@ -144,9 +144,9 @@ def split_first_part_with_parens(line): if ch == '\\': continue # special cases - if out.endswith('operator>') or out.endswith('operator->'): # gotta love c++ + if out.endswith('operator>') or out.endswith('operator>>') or out.endswith('operator->'): # gotta love c++ braces['<>'] += 1 - if out.endswith('operator<'): + if out.endswith('operator<') or out.endswith('operator<<'): braces['<>'] -= 1 # switching quotes if ch in QUOTES: From c2bc298409585aadce4b60dff5e6fada87c6c436 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Jan 2015 20:40:09 +0300 Subject: [PATCH 104/269] Disable instrumentation for the uavcan module --- makefiles/toolchain_gnu-arm-eabi.mk | 2 +- src/modules/uavcan/module.mk | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 1bbc5437d5..5a7f8390ff 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -116,7 +116,7 @@ ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ # enable precise stack overflow tracking # note - requires corresponding support in NuttX -INSTRUMENTATIONDEFINES = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH)) +INSTRUMENTATIONDEFINES ?= $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH)) # Language-specific flags # diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index e5d30f6c47..64e60bd186 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -40,6 +40,8 @@ MODULE_COMMAND = uavcan MAXOPTIMIZATION = -Os +INSTRUMENTATIONDEFINES = -fno-instrument-functions + # Main SRCS += uavcan_main.cpp \ uavcan_clock.cpp \ From f49f183f74be7d602fee6a1192538273d3d11cac Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Jan 2015 22:25:33 +0300 Subject: [PATCH 105/269] UAVCAN module: -O3 instead of -Os; fixed instrumentation defines --- src/modules/uavcan/module.mk | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 64e60bd186..4d1b7156b8 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -38,9 +38,9 @@ MODULE_COMMAND = uavcan -MAXOPTIMIZATION = -Os +MAXOPTIMIZATION = -O3 -INSTRUMENTATIONDEFINES = -fno-instrument-functions +INSTRUMENTATIONDEFINES = -fno-instrument-functions -ffixed-r10 # Main SRCS += uavcan_main.cpp \ From 1d13edcf9221cfeabfe23eeff046dd7324f532b8 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Jan 2015 22:35:59 +0300 Subject: [PATCH 106/269] Stack checks made optional: ENABLE_STACK_CHECKS --- makefiles/toolchain_gnu-arm-eabi.mk | 16 ++++++++++++---- src/modules/uavcan/module.mk | 3 ++- 2 files changed, 14 insertions(+), 5 deletions(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 5a7f8390ff..b38f40eb4c 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -80,14 +80,22 @@ ARCHCPUFLAGS_CORTEXM3 = -mcpu=cortex-m3 \ -march=armv7-m \ -mfloat-abi=soft -ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -finstrument-functions \ - -ffixed-r10 +ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -ffixed-r10 -ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -finstrument-functions \ - -ffixed-r10 +ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -ffixed-r10 ARCHINSTRUMENTATIONDEFINES_CORTEXM3 = +# Enabling stack checks if requested +# +ENABLE_STACK_CHECKS ?= 0 +ifneq ($(ENABLE_STACK_CHECKS),0) +$(info Stack checks enabled) +ARCHINSTRUMENTATIONDEFINES_CORTEXM4F += -finstrument-functions +ARCHINSTRUMENTATIONDEFINES_CORTEXM4 += -finstrument-functions +ARCHINSTRUMENTATIONDEFINES_CORTEXM3 += +endif + # Pick the right set of flags for the architecture. # ARCHCPUFLAGS = $(ARCHCPUFLAGS_$(CONFIG_ARCH)) diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 4d1b7156b8..a3c6e46a0a 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -40,7 +40,8 @@ MODULE_COMMAND = uavcan MAXOPTIMIZATION = -O3 -INSTRUMENTATIONDEFINES = -fno-instrument-functions -ffixed-r10 +# Instrumentation makes the CPU load about 3 times higher, see https://github.com/PX4/Firmware/issues/1417 +INSTRUMENTATIONDEFINES += -fno-instrument-functions # Main SRCS += uavcan_main.cpp \ From 4b1782174c334a9846096145a7156a015df3edae Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Jan 2015 22:40:03 +0300 Subject: [PATCH 107/269] INSTRUMENTATIONDEFINES assignment made non-optional --- makefiles/toolchain_gnu-arm-eabi.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index b38f40eb4c..7e1f510e6d 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -124,7 +124,7 @@ ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ # enable precise stack overflow tracking # note - requires corresponding support in NuttX -INSTRUMENTATIONDEFINES ?= $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH)) +INSTRUMENTATIONDEFINES = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH)) # Language-specific flags # From ed27e583e01bee74a5bf46561b9aea7bd630846b Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 18 Jan 2015 01:30:46 +0300 Subject: [PATCH 108/269] UAVCAN update --- src/lib/uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/uavcan b/src/lib/uavcan index 20439bb397..c9227cf6d2 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit 20439bb3975c34a24c3c337a1f231fbf973a41e8 +Subproject commit c9227cf6d24e0bafee3ada499dea183446aa35b1 From c9eae96cf67ebdf6d202dc7ecac55a5b4a670a50 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 18 Jan 2015 04:05:50 +0300 Subject: [PATCH 109/269] Frame size fix fix per Lorenz's suggestion --- src/modules/attitude_estimator_ekf/module.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk index 1158536e1d..d158d7a49c 100644 --- a/src/modules/attitude_estimator_ekf/module.mk +++ b/src/modules/attitude_estimator_ekf/module.mk @@ -43,6 +43,6 @@ SRCS = attitude_estimator_ekf_main.cpp \ MODULE_STACKSIZE = 1200 -EXTRACFLAGS = -Wno-float-equal -Wframe-larger-than=3600 +EXTRACFLAGS = -Wno-float-equal -Wframe-larger-than=3700 EXTRACXXFLAGS = -Wframe-larger-than=2400 From 2ebd7099de83c603b01bedd278c38a4eb6b77b2b Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 18 Jan 2015 16:09:46 +0300 Subject: [PATCH 110/269] Globally configurable stack checks, R10 is always fixed --- makefiles/toolchain_gnu-arm-eabi.mk | 16 +++++++--------- nuttx-configs/px4fmu-v2/nsh/Make.defs | 6 ++++++ src/modules/uavcan/module.mk | 3 --- 3 files changed, 13 insertions(+), 12 deletions(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 7e1f510e6d..7055137ca5 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -80,20 +80,18 @@ ARCHCPUFLAGS_CORTEXM3 = -mcpu=cortex-m3 \ -march=armv7-m \ -mfloat-abi=soft -ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -ffixed-r10 - -ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -ffixed-r10 - -ARCHINSTRUMENTATIONDEFINES_CORTEXM3 = - # Enabling stack checks if requested # ENABLE_STACK_CHECKS ?= 0 ifneq ($(ENABLE_STACK_CHECKS),0) $(info Stack checks enabled) -ARCHINSTRUMENTATIONDEFINES_CORTEXM4F += -finstrument-functions -ARCHINSTRUMENTATIONDEFINES_CORTEXM4 += -finstrument-functions -ARCHINSTRUMENTATIONDEFINES_CORTEXM3 += +ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -finstrument-functions -ffixed-r10 +ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -finstrument-functions -ffixed-r10 +ARCHINSTRUMENTATIONDEFINES_CORTEXM3 = +else +ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -ffixed-r10 +ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -ffixed-r10 +ARCHINSTRUMENTATIONDEFINES_CORTEXM3 = endif # Pick the right set of flags for the architecture. diff --git a/nuttx-configs/px4fmu-v2/nsh/Make.defs b/nuttx-configs/px4fmu-v2/nsh/Make.defs index 99f3b3140d..798d58572d 100644 --- a/nuttx-configs/px4fmu-v2/nsh/Make.defs +++ b/nuttx-configs/px4fmu-v2/nsh/Make.defs @@ -62,8 +62,14 @@ ARCHCPUFLAGS = -mcpu=cortex-m4 \ # enable precise stack overflow tracking +ENABLE_STACK_CHECKS ?= 0 +ifneq ($(ENABLE_STACK_CHECKS),0) +$(info Stack checks enabled) INSTRUMENTATIONDEFINES = -finstrument-functions \ -ffixed-r10 +else +INSTRUMENTATIONDEFINES = -ffixed-r10 +endif # pull in *just* libm from the toolchain ... this is grody LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index a3c6e46a0a..924b730a23 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -40,9 +40,6 @@ MODULE_COMMAND = uavcan MAXOPTIMIZATION = -O3 -# Instrumentation makes the CPU load about 3 times higher, see https://github.com/PX4/Firmware/issues/1417 -INSTRUMENTATIONDEFINES += -fno-instrument-functions - # Main SRCS += uavcan_main.cpp \ uavcan_clock.cpp \ From f6786d0be91659bfe6cca393d512edd5aa7a407e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 18 Jan 2015 22:40:22 +0300 Subject: [PATCH 111/269] Removing -ffixed-r10 when stack checks aren't enabled --- makefiles/toolchain_gnu-arm-eabi.mk | 4 ++-- nuttx-configs/px4fmu-v2/nsh/Make.defs | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 7055137ca5..0d681cacc8 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -89,8 +89,8 @@ ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -finstrument-functions -ffixed-r10 ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -finstrument-functions -ffixed-r10 ARCHINSTRUMENTATIONDEFINES_CORTEXM3 = else -ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -ffixed-r10 -ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -ffixed-r10 +ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = +ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = ARCHINSTRUMENTATIONDEFINES_CORTEXM3 = endif diff --git a/nuttx-configs/px4fmu-v2/nsh/Make.defs b/nuttx-configs/px4fmu-v2/nsh/Make.defs index 798d58572d..b2f05293d1 100644 --- a/nuttx-configs/px4fmu-v2/nsh/Make.defs +++ b/nuttx-configs/px4fmu-v2/nsh/Make.defs @@ -68,7 +68,7 @@ $(info Stack checks enabled) INSTRUMENTATIONDEFINES = -finstrument-functions \ -ffixed-r10 else -INSTRUMENTATIONDEFINES = -ffixed-r10 +INSTRUMENTATIONDEFINES = endif # pull in *just* libm from the toolchain ... this is grody From 91362223ea5dc5badc34870210c356eb47ebd59d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 19 Jan 2015 15:49:18 +0300 Subject: [PATCH 112/269] Fixed uninitialized fields of UavcanNode --- src/modules/uavcan/uavcan_main.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 8a0993f15d..583c621eb2 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -107,11 +107,11 @@ private: int _task = -1; ///< handle to the OS task bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver int _armed_sub = -1; ///< uORB subscription of the arming status - actuator_armed_s _armed; ///< the arming request of the system + actuator_armed_s _armed = {}; ///< the arming request of the system bool _is_armed = false; ///< the arming status of the actuators on the bus int _test_motor_sub = -1; ///< uORB subscription of the test_motor status - test_motor_s _test_motor; + test_motor_s _test_motor = {}; bool _test_in_progress = false; unsigned _output_count = 0; ///< number of actuators currently available @@ -135,10 +135,10 @@ private: unsigned _poll_fds_num = 0; int _actuator_direct_sub = -1; ///< uORB subscription of the actuator_direct topic - uint8_t _actuator_direct_poll_fd_num; - actuator_direct_s _actuator_direct; + uint8_t _actuator_direct_poll_fd_num = 0; + actuator_direct_s _actuator_direct = {}; - actuator_outputs_s _outputs; + actuator_outputs_s _outputs = {}; // index into _poll_fds for each _control_subs handle uint8_t _poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; From 4baf4a032fa87a01863ae475d4ad27496c90db86 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 19 Jan 2015 15:54:14 +0300 Subject: [PATCH 113/269] Fixed: Passing this->_armed_sub to close, which cannot accept a negative number. --- src/modules/uavcan/uavcan_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index af5f2ec96f..5b961e2efc 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -513,7 +513,7 @@ UavcanNode::teardown() _control_subs[i] = -1; } } - return ::close(_armed_sub); + return (_armed_sub >= 0) ? ::close(_armed_sub) : 0; } int From f6dc2af3986ba823822525d9865c101d91aa67c3 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 19 Jan 2015 16:24:07 +0300 Subject: [PATCH 114/269] UAVCAN update --- src/lib/uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/uavcan b/src/lib/uavcan index c9227cf6d2..8966f97013 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit c9227cf6d24e0bafee3ada499dea183446aa35b1 +Subproject commit 8966f970135fb421db31886d6f99ac918594a780 From 4b8feb03cfca89b18ca88a19079e796b44f6d216 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 20 Jan 2015 17:36:55 -1000 Subject: [PATCH 115/269] Match the OS build's CONFIG_ARMV7M_STACKCHECK setting by using actual setting in the exported nuttx config.h file to control each board build setting of ENABLE_STACK_CHECKS in toolchain_gnu-arm-eabi.mk --- makefiles/setup.mk | 1 + makefiles/toolchain_gnu-arm-eabi.mk | 9 +++++---- nuttx-configs/aerocore/nsh/Make.defs | 5 +++-- nuttx-configs/px4fmu-v1/nsh/Make.defs | 5 +++-- nuttx-configs/px4fmu-v2/nsh/Make.defs | 9 ++------- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- nuttx-configs/px4io-v1/nsh/Make.defs | 5 +++++ nuttx-configs/px4io-v2/nsh/Make.defs | 5 +++++ 8 files changed, 25 insertions(+), 16 deletions(-) mode change 100644 => 100755 makefiles/setup.mk mode change 100644 => 100755 makefiles/toolchain_gnu-arm-eabi.mk mode change 100644 => 100755 nuttx-configs/aerocore/nsh/Make.defs mode change 100644 => 100755 nuttx-configs/px4fmu-v1/nsh/Make.defs mode change 100644 => 100755 nuttx-configs/px4fmu-v2/nsh/Make.defs mode change 100644 => 100755 nuttx-configs/px4fmu-v2/nsh/defconfig mode change 100644 => 100755 nuttx-configs/px4io-v1/nsh/Make.defs mode change 100644 => 100755 nuttx-configs/px4io-v2/nsh/Make.defs diff --git a/makefiles/setup.mk b/makefiles/setup.mk old mode 100644 new mode 100755 index 4bfa7a0876..c932a67586 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -80,6 +80,7 @@ export ECHO = echo export UNZIP_CMD = unzip export PYTHON = python export OPENOCD = openocd +export GREP = grep # # Host-specific paths, hacks and fixups diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk old mode 100644 new mode 100755 index 0d681cacc8..d4d73fb843 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -80,11 +80,12 @@ ARCHCPUFLAGS_CORTEXM3 = -mcpu=cortex-m3 \ -march=armv7-m \ -mfloat-abi=soft -# Enabling stack checks if requested +# Enabling stack checks if OS was build with them # -ENABLE_STACK_CHECKS ?= 0 -ifneq ($(ENABLE_STACK_CHECKS),0) -$(info Stack checks enabled) +TEST_FILE_STACKCHECK=$(WORK_DIR)nuttx-export/include/nuttx/config.h +TEST_VALUE_STACKCHECK=CONFIG_ARMV7M_STACKCHECK\ 1 +ENABLE_STACK_CHECKS=$(shell $(GREP) -q "$(TEST_VALUE_STACKCHECK)" $(TEST_FILE_STACKCHECK); echo $$?;) +ifeq ("$(ENABLE_STACK_CHECKS)","0") ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -finstrument-functions -ffixed-r10 ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -finstrument-functions -ffixed-r10 ARCHINSTRUMENTATIONDEFINES_CORTEXM3 = diff --git a/nuttx-configs/aerocore/nsh/Make.defs b/nuttx-configs/aerocore/nsh/Make.defs old mode 100644 new mode 100755 index c1f5a8ac42..3808fc1cfd --- a/nuttx-configs/aerocore/nsh/Make.defs +++ b/nuttx-configs/aerocore/nsh/Make.defs @@ -62,8 +62,9 @@ ARCHCPUFLAGS = -mcpu=cortex-m4 \ # enable precise stack overflow tracking -INSTRUMENTATIONDEFINES = -finstrument-functions \ - -ffixed-r10 +ifeq ($(CONFIG_ARMV7M_STACKCHECK),y) +INSTRUMENTATIONDEFINES = -finstrument-functions -ffixed-r10 +endif # pull in *just* libm from the toolchain ... this is grody LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" diff --git a/nuttx-configs/px4fmu-v1/nsh/Make.defs b/nuttx-configs/px4fmu-v1/nsh/Make.defs old mode 100644 new mode 100755 index 5e28f2473d..4e08d28a29 --- a/nuttx-configs/px4fmu-v1/nsh/Make.defs +++ b/nuttx-configs/px4fmu-v1/nsh/Make.defs @@ -62,8 +62,9 @@ ARCHCPUFLAGS = -mcpu=cortex-m4 \ # enable precise stack overflow tracking -INSTRUMENTATIONDEFINES = -finstrument-functions \ - -ffixed-r10 +ifeq ($(CONFIG_ARMV7M_STACKCHECK),y) +INSTRUMENTATIONDEFINES = -finstrument-functions -ffixed-r10 +endif # pull in *just* libm from the toolchain ... this is grody LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" diff --git a/nuttx-configs/px4fmu-v2/nsh/Make.defs b/nuttx-configs/px4fmu-v2/nsh/Make.defs old mode 100644 new mode 100755 index b2f05293d1..5a1d5af2c4 --- a/nuttx-configs/px4fmu-v2/nsh/Make.defs +++ b/nuttx-configs/px4fmu-v2/nsh/Make.defs @@ -62,13 +62,8 @@ ARCHCPUFLAGS = -mcpu=cortex-m4 \ # enable precise stack overflow tracking -ENABLE_STACK_CHECKS ?= 0 -ifneq ($(ENABLE_STACK_CHECKS),0) -$(info Stack checks enabled) -INSTRUMENTATIONDEFINES = -finstrument-functions \ - -ffixed-r10 -else -INSTRUMENTATIONDEFINES = +ifeq ($(CONFIG_ARMV7M_STACKCHECK),y) +INSTRUMENTATIONDEFINES = -finstrument-functions -ffixed-r10 endif # pull in *just* libm from the toolchain ... this is grody diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig old mode 100644 new mode 100755 index 9030a1f022..dedebdfa03 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -117,7 +117,7 @@ CONFIG_ARCH_HAVE_MPU=y # # CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y -CONFIG_ARMV7M_STACKCHECK=y +CONFIG_ARMV7M_STACKCHECK=n CONFIG_SERIAL_TERMIOS=y CONFIG_SDIO_DMA=y CONFIG_SDIO_DMAPRIO=0x00010000 diff --git a/nuttx-configs/px4io-v1/nsh/Make.defs b/nuttx-configs/px4io-v1/nsh/Make.defs old mode 100644 new mode 100755 index b4f5577aed..74f38c0cb4 --- a/nuttx-configs/px4io-v1/nsh/Make.defs +++ b/nuttx-configs/px4io-v1/nsh/Make.defs @@ -58,6 +58,11 @@ ARCHCPUFLAGS = -mcpu=cortex-m3 \ -mthumb \ -march=armv7-m +# enable precise stack overflow tracking +ifeq ($(CONFIG_ARMV7M_STACKCHECK),y) +INSTRUMENTATIONDEFINES = -finstrument-functions -ffixed-r10 +endif + # use our linker script LDSCRIPT = ld.script diff --git a/nuttx-configs/px4io-v2/nsh/Make.defs b/nuttx-configs/px4io-v2/nsh/Make.defs old mode 100644 new mode 100755 index 51420eb23d..287466db60 --- a/nuttx-configs/px4io-v2/nsh/Make.defs +++ b/nuttx-configs/px4io-v2/nsh/Make.defs @@ -58,6 +58,11 @@ ARCHCPUFLAGS = -mcpu=cortex-m3 \ -mthumb \ -march=armv7-m +# enable precise stack overflow tracking +ifeq ($(CONFIG_ARMV7M_STACKCHECK),y) +INSTRUMENTATIONDEFINES = -finstrument-functions -ffixed-r10 +endif + # use our linker script LDSCRIPT = ld.script From ae0e2d720926702f5dc1f97aeaa893d30c61fe29 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 21 Jan 2015 10:39:14 +0300 Subject: [PATCH 116/269] Removing extra UAVCAN perfcounters --- src/modules/uavcan/actuators/esc.cpp | 7 ------- src/modules/uavcan/actuators/esc.hpp | 1 - src/modules/uavcan/uavcan_main.cpp | 6 ------ src/modules/uavcan/uavcan_main.hpp | 1 - 4 files changed, 15 deletions(-) diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 995c8987c0..51589e43eb 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -56,17 +56,12 @@ UavcanEscController::UavcanEscController(uavcan::INode &node) : if (_perfcnt_scaling_error == nullptr) { errx(1, "uavcan: couldn't allocate _perfcnt_scaling_error"); } - - if (_perfcnt_broadcast_elapsed == nullptr) { - errx(1, "uavcan: couldn't allocate _perfcnt_broadcast_elapsed"); - } } UavcanEscController::~UavcanEscController() { perf_free(_perfcnt_invalid_input); perf_free(_perfcnt_scaling_error); - perf_free(_perfcnt_broadcast_elapsed); } int UavcanEscController::init() @@ -141,9 +136,7 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) * Publish the command message to the bus * Note that for a quadrotor it takes one CAN frame */ - perf_begin(_perfcnt_broadcast_elapsed); (void)_uavcan_pub_raw_cmd.broadcast(msg); - perf_end(_perfcnt_broadcast_elapsed); } void UavcanEscController::arm_all_escs(bool arm) diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp index 498fb9dd8b..12c0355422 100644 --- a/src/modules/uavcan/actuators/esc.hpp +++ b/src/modules/uavcan/actuators/esc.hpp @@ -109,5 +109,4 @@ private: */ perf_counter_t _perfcnt_invalid_input = perf_alloc(PC_COUNT, "uavcan_esc_invalid_input"); perf_counter_t _perfcnt_scaling_error = perf_alloc(PC_COUNT, "uavcan_esc_scaling_error"); - perf_counter_t _perfcnt_broadcast_elapsed = perf_alloc(PC_ELAPSED, "uavcan_esc_broadcast_elapsed"); }; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 5b961e2efc..4dc03b61b7 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -93,10 +93,6 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys if (_perfcnt_esc_mixer_total_elapsed == nullptr) { errx(1, "uavcan: couldn't allocate _perfcnt_esc_mixer_total_elapsed"); } - - if (_perfcnt_esc_mixer_subscriptions == nullptr) { - errx(1, "uavcan: couldn't allocate _perfcnt_esc_mixer_subscriptions"); - } } UavcanNode::~UavcanNode() @@ -138,7 +134,6 @@ UavcanNode::~UavcanNode() perf_free(_perfcnt_node_spin_elapsed); perf_free(_perfcnt_esc_mixer_output_elapsed); perf_free(_perfcnt_esc_mixer_total_elapsed); - perf_free(_perfcnt_esc_mixer_subscriptions); } int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) @@ -527,7 +522,6 @@ UavcanNode::arm_actuators(bool arm) void UavcanNode::subscribe() { - perf_count(_perfcnt_esc_mixer_subscriptions); // Subscribe/unsubscribe to required actuator control groups uint32_t sub_groups = _groups_required & ~_groups_subscribed; uint32_t unsub_groups = _groups_subscribed & ~_groups_required; diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 583c621eb2..19b9b4b484 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -146,5 +146,4 @@ private: perf_counter_t _perfcnt_node_spin_elapsed = perf_alloc(PC_ELAPSED, "uavcan_node_spin_elapsed"); perf_counter_t _perfcnt_esc_mixer_output_elapsed = perf_alloc(PC_ELAPSED, "uavcan_esc_mixer_output_elapsed"); perf_counter_t _perfcnt_esc_mixer_total_elapsed = perf_alloc(PC_ELAPSED, "uavcan_esc_mixer_total_elapsed"); - perf_counter_t _perfcnt_esc_mixer_subscriptions = perf_alloc(PC_COUNT, "uavcan_esc_mixer_subscriptions"); }; From e62c8d73678f87b9f6cab1ad3a33c8911277a8a8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Jan 2015 09:29:47 +0100 Subject: [PATCH 117/269] FMUv1: Disable stack checking --- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index a467fa605e..539634e3da 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -92,7 +92,7 @@ CONFIG_ARCH_HAVE_MPU=y # # CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y -CONFIG_ARMV7M_STACKCHECK=y +CONFIG_ARMV7M_STACKCHECK=n CONFIG_SERIAL_TERMIOS=y # From 517e1e8d4879c7a2d359b92e2eff89eddb944a16 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 21 Jan 2015 03:07:15 -1000 Subject: [PATCH 118/269] Fixed permissions --- makefiles/setup.mk | 0 makefiles/toolchain_gnu-arm-eabi.mk | 0 nuttx-configs/aerocore/nsh/Make.defs | 0 nuttx-configs/px4fmu-v1/nsh/Make.defs | 0 nuttx-configs/px4fmu-v2/nsh/Make.defs | 0 nuttx-configs/px4fmu-v2/nsh/defconfig | 0 nuttx-configs/px4io-v1/nsh/Make.defs | 0 nuttx-configs/px4io-v2/nsh/Make.defs | 0 8 files changed, 0 insertions(+), 0 deletions(-) mode change 100755 => 100644 makefiles/setup.mk mode change 100755 => 100644 makefiles/toolchain_gnu-arm-eabi.mk mode change 100755 => 100644 nuttx-configs/aerocore/nsh/Make.defs mode change 100755 => 100644 nuttx-configs/px4fmu-v1/nsh/Make.defs mode change 100755 => 100644 nuttx-configs/px4fmu-v2/nsh/Make.defs mode change 100755 => 100644 nuttx-configs/px4fmu-v2/nsh/defconfig mode change 100755 => 100644 nuttx-configs/px4io-v1/nsh/Make.defs mode change 100755 => 100644 nuttx-configs/px4io-v2/nsh/Make.defs diff --git a/makefiles/setup.mk b/makefiles/setup.mk old mode 100755 new mode 100644 diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk old mode 100755 new mode 100644 diff --git a/nuttx-configs/aerocore/nsh/Make.defs b/nuttx-configs/aerocore/nsh/Make.defs old mode 100755 new mode 100644 diff --git a/nuttx-configs/px4fmu-v1/nsh/Make.defs b/nuttx-configs/px4fmu-v1/nsh/Make.defs old mode 100755 new mode 100644 diff --git a/nuttx-configs/px4fmu-v2/nsh/Make.defs b/nuttx-configs/px4fmu-v2/nsh/Make.defs old mode 100755 new mode 100644 diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig old mode 100755 new mode 100644 diff --git a/nuttx-configs/px4io-v1/nsh/Make.defs b/nuttx-configs/px4io-v1/nsh/Make.defs old mode 100755 new mode 100644 diff --git a/nuttx-configs/px4io-v2/nsh/Make.defs b/nuttx-configs/px4io-v2/nsh/Make.defs old mode 100755 new mode 100644 From 4b27e4029d15e5ae4936252f224ee4275f83cab0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Jan 2015 15:32:38 +0100 Subject: [PATCH 119/269] Disabled stack checking on aerocore --- nuttx-configs/aerocore/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/aerocore/nsh/defconfig b/nuttx-configs/aerocore/nsh/defconfig index 317194f685..c44b074f31 100644 --- a/nuttx-configs/aerocore/nsh/defconfig +++ b/nuttx-configs/aerocore/nsh/defconfig @@ -94,7 +94,7 @@ CONFIG_ARCH_HAVE_MPU=y # CONFIG_ARMV7M_TOOLCHAIN_CODEREDL is not set # CONFIG_ARMV7M_TOOLCHAIN_CODESOURCERYL is not set CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y -CONFIG_ARMV7M_STACKCHECK=y +CONFIG_ARMV7M_STACKCHECK=n CONFIG_SERIAL_TERMIOS=y # From 779400aad3f3a97057e4377d51ff362756938241 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Jan 2015 13:39:13 +0100 Subject: [PATCH 120/269] Add hackery on NuttX header, to be removed during rebase -i Conflicts: NuttX --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index 574bac488f..3d8171f6ea 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 574bac488f384ddaa344378e25653c27124a2b69 +Subproject commit 3d8171f6ea88297d8595525c8222d61e9cf20fd0 From b3b5a2de0329aa42ab842bd568244aaa9fd8d40d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Jan 2015 15:59:15 +0100 Subject: [PATCH 121/269] Set NuttX version to one which always colors the stack --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index 574bac488f..e4c914e261 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 574bac488f384ddaa344378e25653c27124a2b69 +Subproject commit e4c914e261d2647e44d05222afa7aa3cc90d3c67 From 1511fd7b2dd19e0ae4c63553cbaf00e2a753148f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 21 Jan 2015 16:33:19 +0100 Subject: [PATCH 122/269] make handle() static --- src/platforms/nuttx/px4_messages/px4_rc_channels.h | 2 +- src/platforms/px4_message.h | 3 +-- src/platforms/px4_nodehandle.h | 10 ++++------ src/platforms/ros/px4_messages/px4_rc_channels.h | 2 +- 4 files changed, 7 insertions(+), 10 deletions(-) diff --git a/src/platforms/nuttx/px4_messages/px4_rc_channels.h b/src/platforms/nuttx/px4_messages/px4_rc_channels.h index cb1135eb98..2ce7bfc80a 100644 --- a/src/platforms/nuttx/px4_messages/px4_rc_channels.h +++ b/src/platforms/nuttx/px4_messages/px4_rc_channels.h @@ -20,7 +20,7 @@ public: ~px4_rc_channels() {} - PX4TopicHandle handle() {return (PX4TopicHandle)ORB_ID(rc_channels);} + static PX4TopicHandle handle() {return ORB_ID(rc_channels);} }; } diff --git a/src/platforms/px4_message.h b/src/platforms/px4_message.h index c908f6fc69..bff7aa3133 100644 --- a/src/platforms/px4_message.h +++ b/src/platforms/px4_message.h @@ -42,7 +42,7 @@ typedef const char* PX4TopicHandle; #else #include -typedef const struct orb_metatdata* PX4TopicHandle; +typedef orb_id_t PX4TopicHandle; #endif namespace px4 @@ -70,7 +70,6 @@ public: virtual M& data() {return _data;} virtual const M& data() const {return _data;} - virtual PX4TopicHandle handle() = 0; private: M _data; }; diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 2406a4a775..5ae4f9325c 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -93,7 +93,7 @@ public: Subscriber *subscribe(void(*fp)(const T &)) { SubscriberBase *sub = new SubscriberROS(std::bind(fp, std::placeholders::_1)); - ros::Subscriber ros_sub = ros::NodeHandle::subscribe((new T())->handle(), kQueueSizeDefault, + ros::Subscriber ros_sub = ros::NodeHandle::subscribe(T::handle(), kQueueSizeDefault, &SubscriberROS::callback, (SubscriberROS *)sub); ((SubscriberROS *)sub)->set_ros_sub(ros_sub); _subs.push_back(sub); @@ -147,7 +147,7 @@ public: Publisher* advertise() // Publisher *advertise() { - ros::Publisher ros_pub = ros::NodeHandle::advertisedata())>::type &>((new T())->handle(), kQueueSizeDefault); + ros::Publisher ros_pub = ros::NodeHandle::advertisedata())>::type &>(T::handle(), kQueueSizeDefault); Publisher *pub = new Publisher(ros_pub); _pubs.push_back(pub); return pub; @@ -245,8 +245,7 @@ public: // Subscriber *subscribe(void(*fp)(const typename std::remove_referencedata())>::type &), unsigned interval=10) //XXX interval Subscriber *subscribe(void(*fp)(const T &), unsigned interval=10) //XXX interval { - const struct orb_metadata * meta = NULL; - uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(meta, interval); + uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(T::handle(), interval); // SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(uorb_sub, interval, callback); SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(uorb_sub, interval, std::bind(fp, std::placeholders::_1)); @@ -300,8 +299,7 @@ public: { //XXX // uORB::PublicationBase * uorb_pub = new uORB::PublicationBase((new T())->handle()); - const struct orb_metadata * meta = NULL; - uORB::PublicationBase * uorb_pub = new uORB::PublicationBase(meta); + uORB::PublicationBase * uorb_pub = new uORB::PublicationBase(T::handle()); Publisher *pub = new Publisher(uorb_pub); _pubs.add(pub); diff --git a/src/platforms/ros/px4_messages/px4_rc_channels.h b/src/platforms/ros/px4_messages/px4_rc_channels.h index a5a5ee2020..5e9dc72cda 100644 --- a/src/platforms/ros/px4_messages/px4_rc_channels.h +++ b/src/platforms/ros/px4_messages/px4_rc_channels.h @@ -19,7 +19,7 @@ public: ~px4_rc_channels() {} - PX4TopicHandle handle() {return "rc_channels";} + static PX4TopicHandle handle() {return "rc_channels";} }; } From 2a2594a1717a5067ac1f209974851f9512e46415 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 21 Jan 2015 17:40:42 +0100 Subject: [PATCH 123/269] re-enable adding of subscriber to list --- src/platforms/px4_nodehandle.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 5ae4f9325c..4a1e700755 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -249,11 +249,11 @@ public: // SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(uorb_sub, interval, callback); SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(uorb_sub, interval, std::bind(fp, std::placeholders::_1)); - // [> Check if this is the smallest interval so far and update _sub_min_interval <] - // if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) { - // _sub_min_interval = sub_px4; - // } - // _subs.add((SubscriberNode *)sub_px4); + /* Check if this is the smallest interval so far and update _sub_min_interval */ + if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) { + _sub_min_interval = sub_px4; + } + _subs.add((SubscriberNode *)sub_px4); return (Subscriber *)sub_px4; } From 02fdd48a477853a2f54c7954478a3ce5b5b3f497 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 21 Jan 2015 17:41:13 +0100 Subject: [PATCH 124/269] publisher: use wrapper message type --- src/platforms/px4_publisher.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index 6d75e28fcc..0c8dd62ba1 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -126,10 +126,10 @@ public: /** Publishes msg * @param msg the message which is published to the topic */ - template - int publish(const M &msg) + template + int publish(const T &msg) { - _uorb_pub->update((void *)&msg); + _uorb_pub->update((void *)&(msg.data())); return 0; } From b04fcad525c8f76419d468f43a4b159bc5200fbe Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 21 Jan 2015 17:41:56 +0100 Subject: [PATCH 125/269] fix bracket position --- src/platforms/px4_subscriber.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 88cc86ab80..8e8b786b17 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -287,6 +287,7 @@ public: if (!this->_uorb_sub->updated()) { /* Topic not updated, do not call callback */ return; + } /* get latest data */ this->_uorb_sub->update(this->get_void_ptr()); @@ -299,7 +300,6 @@ public: /* Call callback which performs actions based on this data */ _cbf(Subscriber::get()); - } }; From ed526173bb4f6a809e725aeb8214cf24042edd76 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 21 Jan 2015 17:57:37 +0100 Subject: [PATCH 126/269] clean up publisher example --- src/examples/publisher/publisher_example.cpp | 19 ++++--------------- 1 file changed, 4 insertions(+), 15 deletions(-) diff --git a/src/examples/publisher/publisher_example.cpp b/src/examples/publisher/publisher_example.cpp index e85e42b386..6b30c2fe3d 100644 --- a/src/examples/publisher/publisher_example.cpp +++ b/src/examples/publisher/publisher_example.cpp @@ -46,9 +46,7 @@ using namespace px4; PublisherExample::PublisherExample() : _n(), _rc_channels_pub(_n.advertise()) - // _rc_channels_pub(PX4_ADVERTISE(_n, rc_channels)) { - } int PublisherExample::main() @@ -56,19 +54,10 @@ int PublisherExample::main() px4::Rate loop_rate(10); while (px4::ok()) { - // PX4_TOPIC_T(rc_channels) msg; - // msg.timestamp_last_valid = px4::get_time_micros(); - // PX4_INFO("%llu", msg.timestamp_last_valid); - - // _rc_channels_pub->publish(msg); - - //XXX - px4_rc_channels msg2; - msg2.data().timestamp_last_valid = px4::get_time_micros(); - PX4_INFO("%llu", msg2.data().timestamp_last_valid); - // msg2.pub->publish2(); - _rc_channels_pub->publish(msg2); - + px4_rc_channels msg; + msg.data().timestamp_last_valid = px4::get_time_micros(); + PX4_INFO("%llu", msg.data().timestamp_last_valid); + _rc_channels_pub->publish(msg); _n.spinOnce(); loop_rate.sleep(); From f60e65b38fd051595b465f619a19effa9c13efee Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 21 Jan 2015 18:05:31 +0100 Subject: [PATCH 127/269] clean up subscriber example --- src/examples/subscriber/subscriber_example.cpp | 5 +---- src/examples/subscriber/subscriber_example.h | 3 +-- 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index fd462cd2d3..18f1a9eb9b 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -43,8 +43,6 @@ using namespace px4; -// void rc_channels_callback_function(const PX4_TOPIC_T(rc_channels) &msg) { -void rc_channels_callback_function(const px4_rc_channels &msg); void rc_channels_callback_function(const px4_rc_channels &msg) { PX4_INFO("I heard: [%llu]", msg.data().timestamp_last_valid); } @@ -64,9 +62,8 @@ SubscriberExample::SubscriberExample() : /* Do some subscriptions */ /* Function */ - // PX4_SUBSCRIBE(_n, rc_channels, rc_channels_callback_function, _interval); _n.subscribe(rc_channels_callback_function); //ROS version - + // [> Class Method <] // PX4_SUBSCRIBE(_n, rc_channels, SubscriberExample::rc_channels_callback, this, 1000); // [> No callback <] diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h index 43dafe010a..883d83be7b 100644 --- a/src/examples/subscriber/subscriber_example.h +++ b/src/examples/subscriber/subscriber_example.h @@ -41,7 +41,7 @@ using namespace px4; -void rc_channels_callback_function(const PX4_TOPIC_T(rc_channels) &msg); +void rc_channels_callback_function(const px4_rc_channels &msg); class SubscriberExample { public: @@ -56,7 +56,6 @@ protected: int32_t _interval; px4_param_t _p_test_float; float _test_float; - // px4::PX4_SUBSCRIBER(rc_channels) * _sub_rc_chan; px4::Subscriber * _sub_rc_chan; // void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg); From a761df4ffa0e77e1bd34a6e02ba621ca71523389 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 21 Jan 2015 18:14:20 +0100 Subject: [PATCH 128/269] clean up px4_publisher --- src/platforms/px4_publisher.h | 19 ------------------- 1 file changed, 19 deletions(-) diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index 0c8dd62ba1..9115545030 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -59,20 +59,11 @@ class __EXPORT PublisherBase public: PublisherBase() {}; ~PublisherBase() {}; - - /** Publishes msg - * @param msg the message which is published to the topic - */ - // virtual int publish2(const PX4Message * const msg) = 0; - }; #if defined(__PX4_ROS) -// template class Publisher : public PublisherBase - // public PublisherBase, - // public T { public: /** @@ -81,7 +72,6 @@ public: */ Publisher(ros::Publisher ros_pub) : PublisherBase(), - // T(), _ros_pub(ros_pub) {} @@ -90,7 +80,6 @@ public: /** Publishes msg * @param msg the message which is published to the topic */ - // int publish(const M &msg) { _ros_pub.publish(msg); return 0; } template int publish(T &msg) { _ros_pub.publish(msg.data()); @@ -100,7 +89,6 @@ private: }; #else class __EXPORT Publisher : - // public uORB::PublicationNode, public PublisherBase, public ListNode @@ -108,14 +96,7 @@ class __EXPORT Publisher : public: /** * Construct Publisher by providing orb meta data - * @param meta orb metadata for the topic which is used - * @param list publisher is added to this list */ - // Publisher(const struct orb_metadata *meta, - // List *list) : - // uORB::PublicationNode(meta, list), - // PublisherBase() - // {} Publisher(uORB::PublicationBase * uorb_pub) : PublisherBase(), _uorb_pub(uorb_pub) From 358c91932575c191c7717d9c083611d651721476 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 21 Jan 2015 18:23:18 +0100 Subject: [PATCH 129/269] clean up px4_subscriber --- src/platforms/px4_subscriber.h | 43 ++++++++-------------------------- 1 file changed, 10 insertions(+), 33 deletions(-) diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 8e8b786b17..2b289771bb 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -104,7 +104,6 @@ public: /** * Construct Subscriber by providing a callback function */ - // SubscriberROS(std::functiondata())>::type &)> cbf) : SubscriberROS(std::function cbf) : Subscriber(), _ros_sub(), @@ -195,16 +194,8 @@ public: /** * Construct SubscriberUORB by providing orb meta data without callback - * @param meta orb metadata for the topic which is used * @param interval Minimal interval between calls to callback - * @param list subscriber is added to this list */ - // SubscriberUORB(const struct orb_metadata *meta, - // unsigned interval, - // List *list) : - // Subscriber(), - // uORB::Subscription(meta, interval, list) - // {} SubscriberUORB(uORB::SubscriptionBase * uorb_sub, unsigned interval) : SubscriberNode(interval), _uorb_sub(uorb_sub) @@ -227,25 +218,21 @@ public: }; /* Accessors*/ - /** - * Get the last message value - */ - // T get() { return (T)(typename std::remove_referencedata())>::type)*_uorb_sub; } - // T get() { - // typename std::remove_referencedata())>::type msg = (typename std::remove_referencedata())>::type)*_uorb_sub; - // return (T)msg; - // } + int getUORBHandle() { return _uorb_sub->getHandle(); } + +protected: + uORB::SubscriptionBase * _uorb_sub; /**< Handle to the subscription */ + + typename std::remove_referencedata())>::type getUORBData() + { + return (typename std::remove_referencedata())>::type)*_uorb_sub; + } /** * Get void pointer to last message value */ void *get_void_ptr() { return (void *)&(this->_msg_current.data()); } - int getUORBHandle() { return _uorb_sub->getHandle(); } - -protected: - uORB::SubscriptionBase * _uorb_sub; /**< Handle to the subscription */ - typename std::remove_referencedata())>::type getUORBData() { return (typename std::remove_referencedata())>::type)*_uorb_sub; } }; //XXX reduce to one class with overloaded constructor? @@ -256,18 +243,9 @@ class __EXPORT SubscriberUORBCallback : public: /** * Construct SubscriberUORBCallback by providing orb meta data - * @param meta orb metadata for the topic which is used - * @param callback Callback, executed on receiving a new message + * @param cbf Callback, executed on receiving a new message * @param interval Minimal interval between calls to callback - * @param list subscriber is added to this list */ - // SubscriberUORBCallback(const struct orb_metadata *meta, - // unsigned interval, - // std::function callback, - // List *list) : - // SubscriberUORB(meta, interval, list), - // _callback(callback) - // {} SubscriberUORBCallback(uORB::SubscriptionBase * uorb_sub, unsigned interval, std::function cbf) : @@ -304,7 +282,6 @@ public: }; protected: - // std::functiondata())>::type &)> _callback; [>*< Callback handle, called when new data is available */ std::function _cbf; /**< Callback that the user provided on the subscription */ }; #endif From 2b103d319c8547d0d6e9ae14a6413a787051e205 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 21 Jan 2015 18:29:22 +0100 Subject: [PATCH 130/269] clean up px4_nodehandle --- src/platforms/px4_nodehandle.h | 61 +--------------------------------- 1 file changed, 1 insertion(+), 60 deletions(-) diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 4a1e700755..634e5e5db6 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -78,18 +78,7 @@ public: * @param topic Name of the topic * @param fb Callback, executed on receiving a new message */ - // template - // Subscriber *subscribe(const char *topic, void(*fp)(const M &)) - // { - // SubscriberBase *sub = new SubscriberROS(std::bind(fp, std::placeholders::_1)); - // ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, - // (SubscriberROS *)sub); - // ((SubscriberROS *)sub)->set_ros_sub(ros_sub); - // _subs.push_back(sub); - // return (Subscriber *)sub; - // } template - // Subscriber *subscribe(void(*fp)(const typename std::remove_referencedata())>::type &)) Subscriber *subscribe(void(*fp)(const T &)) { SubscriberBase *sub = new SubscriberROS(std::bind(fp, std::placeholders::_1)); @@ -133,19 +122,9 @@ public: /** * Advertise topic - * @param topic Name of the topic */ - // template - // Publisher *advertise(const char *topic) - // { - // ros::Publisher ros_pub = ros::NodeHandle::advertise(topic, kQueueSizeDefault); - // Publisher *pub = new Publisher(ros_pub); - // _pubs.push_back(pub); - // return pub; - // } template Publisher* advertise() - // Publisher *advertise() { ros::Publisher ros_pub = ros::NodeHandle::advertisedata())>::type &>(T::handle(), kQueueSizeDefault); Publisher *pub = new Publisher(ros_pub); @@ -214,39 +193,14 @@ public: /** * Subscribe with callback to function - * @param meta Describes the topic which nodehande should subscribe to - * @param callback Callback, executed on receiving a new message - * @param interval Minimal interval between calls to callback - */ - - // template - // Subscriber *subscribe(const struct orb_metadata *meta, - // std::function callback, - // unsigned interval) - // { - // SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(meta, interval, callback, &_subs); - - // [> Check if this is the smallest interval so far and update _sub_min_interval <] - // if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() > sub_px4->getInterval()) { - // _sub_min_interval = sub_px4; - // } - - // return (Subscriber *)sub_px4; - // } - /** - * Subscribe with callback to function - * @param meta Describes the topic which nodehande should subscribe to - * @param callback Callback, executed on receiving a new message + * @param fp Callback, executed on receiving a new message * @param interval Minimal interval between calls to callback */ template - // Subscriber *subscribe(std::functiondata())>::type &)> callback, unsigned interval=10) //XXX interval - // Subscriber *subscribe(void(*fp)(const typename std::remove_referencedata())>::type &), unsigned interval=10) //XXX interval Subscriber *subscribe(void(*fp)(const T &), unsigned interval=10) //XXX interval { uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(T::handle(), interval); - // SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(uorb_sub, interval, callback); SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(uorb_sub, interval, std::bind(fp, std::placeholders::_1)); /* Check if this is the smallest interval so far and update _sub_min_interval */ @@ -280,19 +234,6 @@ public: /** * Advertise topic - * @param meta Describes the topic which is advertised - */ - // template - // Publisher *advertise(const struct orb_metadata *meta) - // { - // //XXX - // Publisher *pub = new Publisher(meta, &_pubs); - // return pub; - // } - - /** - * Advertise topic - * @param meta Describes the topic which is advertised */ template Publisher *advertise() From 0943bd9a31896996db7a030715eb9d0e8626b0b4 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 21 Jan 2015 09:54:49 -1000 Subject: [PATCH 131/269] Added Prefix to message to identify it as PX4IO related --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 1c9a5adc9a..8aecbb4693 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2856,7 +2856,7 @@ checkcrc(int argc, char *argv[]) } if (ret != OK) { - printf("check CRC failed - %d\n", ret); + printf("[PX4IO::checkcrc] check CRC failed - %d\n", ret); exit(1); } printf("CRCs match\n"); From e037b7ae9b361812e99c5985a9c543c2ba503de0 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 21 Jan 2015 10:02:22 -1000 Subject: [PATCH 132/269] Added delay to ensure the the px4io is in loop waiting for a characterit loop. --- src/drivers/px4io/px4io_uploader.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index fb16f891f6..02e527695c 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -621,6 +621,7 @@ int PX4IO_Uploader::reboot() { send(PROTO_REBOOT); + up_udelay(100*1000); // Ensure the farend is in wait for char. send(PROTO_EOC); return OK; From 2a00948c7ad71f849223337d566c41b56f7e1e08 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 21 Jan 2015 10:19:24 -1000 Subject: [PATCH 133/269] Added prefix to Match Message also --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 8aecbb4693..556eebab6d 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2859,7 +2859,7 @@ checkcrc(int argc, char *argv[]) printf("[PX4IO::checkcrc] check CRC failed - %d\n", ret); exit(1); } - printf("CRCs match\n"); + printf("[PX4IO::checkcrc] CRCs match\n"); exit(0); } From d80a00fad105a2dc98ab6ec415e28fa86f1ceed6 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 21 Jan 2015 16:46:04 +0900 Subject: [PATCH 134/269] batt_smbus: disable if no batt 10seconds after startup --- src/drivers/batt_smbus/batt_smbus.cpp | 24 ++++++++++++++++++++++-- 1 file changed, 22 insertions(+), 2 deletions(-) diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 2b5fef4d77..92b752a282 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -91,6 +91,7 @@ #define BATT_SMBUS_MANUFACTURE_INFO 0x25 ///< cell voltage register #define BATT_SMBUS_CURRENT 0x2a ///< current register #define BATT_SMBUS_MEASUREMENT_INTERVAL_MS (1000000 / 10) ///< time in microseconds, measure at 10hz +#define BATT_SMBUS_TIMEOUT_MS 10000000 ///< timeout looking for battery 10seconds after startup #define BATT_SMBUS_PEC_POLYNOMIAL 0x07 ///< Polynomial for calculating PEC @@ -171,11 +172,13 @@ private: uint8_t get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const; // internal variables + bool _enabled; ///< true if we have successfully connected to battery work_s _work; ///< work queue for scheduling reads RingBuffer *_reports; ///< buffer of recorded voltages, currents struct battery_status_s _last_report; ///< last published report, used for test() orb_advert_t _batt_topic; ///< uORB battery topic orb_id_t _batt_orb_id; ///< uORB battery topic ID + uint64_t _start_time; ///< system time we first attempt to communicate with battery }; namespace @@ -189,13 +192,18 @@ extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[]); BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) : I2C("batt_smbus", BATT_SMBUS_DEVICE_PATH, bus, batt_smbus_addr, 400000), + _enabled(false), _work{}, _reports(nullptr), _batt_topic(-1), - _batt_orb_id(nullptr) + _batt_orb_id(nullptr), + _start_time(0) { // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); + + // capture startup time + _start_time = hrt_absolute_time(); } BATT_SMBUS::~BATT_SMBUS() @@ -330,11 +338,20 @@ BATT_SMBUS::cycle_trampoline(void *arg) void BATT_SMBUS::cycle() { + // get current time + uint64_t now = hrt_absolute_time(); + + // exit without rescheduling if we have failed to find a battery after 10 seconds + if (!_enabled && (now - _start_time > BATT_SMBUS_TIMEOUT_MS)) { + warnx("did not find smart battery"); + return; + } + // read data from sensor struct battery_status_s new_report; // set time of reading - new_report.timestamp = hrt_absolute_time(); + new_report.timestamp = now; // read voltage uint16_t tmp; @@ -375,6 +392,9 @@ BATT_SMBUS::cycle() // notify anyone waiting for data poll_notify(POLLIN); + + // record we are working + _enabled = true; } // schedule a fresh cycle call when the measurement is done From 30e32b004d6e9d0a4b13abf6ef5f93bfe7bb4754 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 21 Jan 2015 17:18:55 -0500 Subject: [PATCH 135/269] travis fix s3 index.html and timestamp.html upload these files were being uploaded as "/index.html" and "/timestamp.html" --- .travis.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 52ced6b4fc..8677bf91bf 100644 --- a/.travis.yml +++ b/.travis.yml @@ -61,7 +61,8 @@ after_script: - ./CI-Tools/s3cmd-put Firmware.zip archives/Firmware/$TRAVIS_BRANCH/`date "+%Y-%m-%d"`-$TRAVIS_BUILD_ID/ - ./CI-Tools/s3cmd-put CI-Tools/directory/index.html archives/Firmware/$TRAVIS_BRANCH/ # upload top level index.html and timestamp.html - - ./CI-Tools/s3cmd-put CI-Tools/index.html timestamp.html / + - ./CI-Tools/s3cmd-put CI-Tools/index.html index.html + - ./CI-Tools/s3cmd-put CI-Tools/timestamp.html timestamp.html deploy: provider: releases From 38ae6bcc5fd5e9ab1a7304140ec6b36ff027625f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 22 Jan 2015 07:24:22 +0100 Subject: [PATCH 136/269] Added block diagram --- Documentation/px4_block_diagram.odg | Bin 0 -> 42200 bytes 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 Documentation/px4_block_diagram.odg diff --git a/Documentation/px4_block_diagram.odg b/Documentation/px4_block_diagram.odg new file mode 100644 index 0000000000000000000000000000000000000000..95b6f600b87aedcb06535d5b330afbdb76d91c41 GIT binary patch literal 42200 zcmb5V1yo#3v#<-n-QC^Y-Q5Z9?(R--cXtm2hd^+54-6U{f(LiMgg5!l`=4{~zs}`Z zGi!Etbyam$?VjCxHi|MJpr}AVkU&6~Uzo)6K1oeUi;ERCHN{||+SmGd12pg=&s zU+)S{T@2|xY;AZ2-rfk^Or4zHRPqrs(K8Ygn%WuLn^@YJ^AW46h|_Tr3-Ckp+MAhK z8k=(ewL-~HjXGqg2z=5{gW{+t5Mk&WrUjT+in+Wv3V{FZ&6>Aw}2JDHfgIr2Y~^5!!O zgE@nVp^G7%o298c$zRX^naBTqwp{;hwkA%7?*G>jOpFY_Wrzv?(a!y^LS-iA|67&6 z)5p*Io(VVMs{|SKc@f7%0E{B zbACJjE`DbhOJlIE*8Ns&u68bb#GD)~#0>nrf0D`lPx${H{YS-n^FON)BqZef zvi7!;{pSz%?US>q%kNk^|6Yl9wAP)s*b)6s^%Kso3VaDepR9U8?I43;oP)muE$FQW zcYMOuypVK)eDx(WR-Rnk7%l^3jP;S3%d9tPRk;gP`92=d5mMB2gPApK&dOIzLMg`G zr%&Qcnyggm@Z@C*tky5VdMn;$V1NmlihWA93g=V;d?~B3=g1BRaEpA1;uZrAZ@oIC z1#QhxTgTUp8<|&MgZ#_@XHI0Q;l8@#a(7~8F)Tz$NopJvg=9Ci z^!XYyXVX(D`Fj8OKDy4m=sNhi*Qjr*d=AF_`E|kq$v`<-Ca%hZ7BXex<3&yl;((A6 zCZC)6oOQ*`vR_jD>Yhc5yLi>%A=9K(eTz1c>nl)}(#Qk(`Df%Ne zHe4|c|AAVcICcu_wOi*-qXNA7txtorF6I0N^~gh^d$Lz_|p9#t2k z4KO>X0@wxP1ePe`P9i!?iY=&3(FIqE6;`2{s3W5#8 z$W^qVHSoIE5@AL6l6+SEqcuo4xV51gOMMsB#sX2LzX&NK=h zIu%6|)Q#TY8TqMiwsy$a_kPf0utH}Kv!kMrkaS$6UqP53{LRLG$u%>Mca73{L5M4( zq4fg_Q1*2Co>v7C(pRYPhPvlWF8r@ZQ) z`7DnWxTknbxO;2<^r?^$vL3+3dEO(0{zNj;EEWr|h+a`+lZxzkIeC68pfSY3bF?3! zac-6AT@I#wl29#+q_mG7e61f1robwKk6OpIz2#<2c?x&><;yj|NHGR&AQ7G zjTnxp{*I#9U8!opkCfhHgulGCl(F*`$Oj5+{ZJ)oPwW zJ;+7NW_y$sqSFC!0mCfg(~6)OQt75eOPry7U0OIYL!rR1<}FKj8hdgh_!$1$>p?;@ zOydu3&#Z%LzZS4m$dtETjHJinUtx#MlZ#;hOlsGVlA^feE~T33l6rk<)sBQ>J*dqy zR=}gY<2c{ULZvSzxS%nLC?Rdg7C00A;t+e7k8hZ(+A+~R6jbG(rvu9)d>eioAJ`T~ z)S8OnNn=J_G0P~K@y-o^i3T!#z$#IFme4ne{!Zhg>rZknx*QCJGj%vD0{V-U-e|wW z7ei!2YQN&4nA(6bl#tZhO4ToQ$`-zvXT6}=Q=gsf62ndG^2rMTz+2*zAdZ2IxO9Aq*Cecy3{wrYAO^4D_qhhL|j5HDABlbpjFF7B7N=; z74ShJ6UzvJ`GJay7e!xx+0Gu2%_@75`POiO697Z32yy0D1O#UxFm{>U__{NH^}k=; z3wzx6WX*~zO**ai=&dfCHsS2RSFr19dpOu8Fm}IgTX{0Bi#KEb7C< z03#p#Bphx{UTnQ?+A2zyel8yv@aOvTVDU!puG5FE%M(Z>&FQY2)x5C#sB1Cn`?T2m zcsFx;-x^(cPG6a|v%8;e?Cq}O(oe4W^5OVcd9moh2v4l}vDkWD?%n(Puz-CL;Dtxu z7N+B0kE7`Fad&Of;PrbvoGVqxOH^Ran6%S(JbkWkaMRt~d}tjiUGi-{^t!Wh5$I*e z>e_beFl!|fX_(pL@PwH*5S#t(dw-Qw^ z9cOCWe^y93YQR$lj^!7KH8bXAYes1D&GHtGRT?aML>sT{>N;dn1l$?IcfgA;T%9kE zuX+Y%^7+`^aN;Tfj__*>whphQp`?V&U#xT) zJ+x%&_Taa>i7yvk6YHh!uuAgkX4Z9e1uiN~dnSOyCom|bJ-PgFS7NL6=GMJq9(23iuTXNG(q3imoFedU#?q)~(z>rrnt-hqT&Snm9gHq2M5`W~)-`BNA|Irm3 zFD#xC_w(+Ksh2X;2@$q8d{AbdH-rlx&g|&gN_LyJvsz>Fp#lu!CYBQ?;0rxUa81$V-cY|l z-Dai0zQx(d8$4%6eV$aX9EgT}Ij(s-9;`Gq?#*KXDL~O6wGMcuJ-7Gg0Wi@;&6HUQ zKle?O2J_;K7!s&o$&}qmMC)^M;a2XrU)_Nj55k5q@J@PTc5z@RxMUD+i ziL7nf*vvFSxWGFWPigi^GK?8m`a9daSU@ywvAmd__0^-7hR!dX#ErxrcLc2;JuD;b zC2s95M31Sq(rVe3A2^(`C#hKb(-^szp$~RHYZ{GA2VWu2n`;xX;gw0C%*beDOQ_&D z9yvB3BA~2Feu;8ZXi{IV?6kZWIUhlBzGj_E?!DiC3ers!-r z8{6hlco(Ky85`0uv=B^PVmx(6pmxbCtJsc99Lmq;K(Bs@Ns(QRX#H5gb3F#d_^{;A z!p-7~3p)f;mT2}m>zeN&KwxB)QUiTRbGqpbp=Bmd+T;RedOZXv>}N)Bx~0R6K`i1(VTX*I~QF)$2 zc|5n$H0K{FJRl70X4Ng8Mop}4H#Sp%0GFQyey_|N4jJYA3 z)ZKn|Xuip)N&?%9nkLb?VC|t#@x59#U}}#N$=V@PR=qY|s#h=}VDZCTiLKY$n`y~T z<35)9I=yWHBzACmLHBi9?g^4ut3PkE)BlHCPZ}NuYS{H1zQWeRARsI29+KupVcIIb zt5!(;IBPt&9g6`-Z|nYUL1_nSg~0 zXV+bnPcYG$E(EC~tb^O+O2ywp3$jSFh%yE4OD;^vhEa__9w0(4-&fvNX~fL7XU3d=q>5PR z->s42@2$2R|40h^yvtU~{BAAq;5~rCT>x~{%B_bs zl=sYpixcHHXW&3Rdt!TiJA!%g8Sa7kgGb4z#Q8&q_Qq*dIXH%N&>T(5;And0)>spl z0fY8*?wrC4lX8`As)F*y$X`^9tV6O!g8F7W%_< zI3s0KG-S{pijWKyFEEk7{~-P*pTsL3Jk8QesFlw}-H+XmEnyzR{-v}Zo6k4~R~;7X zg^Zu*9Zw>D#|DW}uyd$7@OK8MHG|xQTL|aN@^r-hE2ITEihnZ{uF$%HlBweN#YdP% z1UV1!jdt3+?>?6@J1F&kvZ$A;V8Q)Rav{|TKzWxy%)`x6u=w`Y1hw=hrwn5htQk3X z!G9=*;V)=8NgJbR81@#p`&o}6Q6RR~#XXb%c zAcH>Fp%^0mft46GvsC`yfwCZe07C+w{!k)=WQ}Nzo5N<&dJk479!q19;J$_OMpD`i zN~WFPTm4iDz2oRIpD(q8(T~jv760zs&tMq0I>Z->?Ag;>r8^-1v) zB~$g^Ds1LAqIXmE5p7WeiAHC}5r|RWNX8&V&Ghzb2^N+iz6nyK_ogPIi@3Y41D6x* zQkl<>v3=~tHVKW#x|ly?KL*=UM?FePhU4rIt5N>GFgxK2eW|B~~j-HSq)=nn^hikxy`|2hs55hWUlV;`5N0gS*h#De3$$sI&KX!a*fDvR&*=i3CuYYwNLR=O90Z`cNHu?v1+rt&LAckOdY@Nu zcB8a`f!CV@PFm{aE96$kl?yk=f27K-A1I9VZ74b56^-%35&sYkW+)R{>w5Nmx-_^O zvz^a#Oa!+?QEJHtABjQq9|gDEPT&P;8&WoR)`IhwN7?!fVRrTS{PoXOG>+5gIU!b2A)SeTeoUeME zF3@Q*Xeacs8j7`RG1BHAxMPu^%?I|yDdjqV@9P^$3^b#An%G22*PgA;tD}aJj;2Wo zI;9yPdh-O}L(ej|kEU6DLwcUKR}HFGc;!N)N@qdhHetWZ4pI)vRiYhUuvnmX<*^Dtcw(Vh{*G?{u}c#v`D#)=hy$VmIZnEtcc1D7E* zpIiW8V=XU@Kc5=AgMbdb%+vVJ%qi+Ri*F-s^Gnaq8%u{M`MRqYM3fw%kj__;UC)YJ zt#u4NYqS9B4u(aZBF%VNd15hfeZHED$%X``c#d3NxxC$ba8ElY)oRl z?c9wvU2_>W0GmuFXZB?vG_Q{XU}Fo~BI`pvcrN3%$Qt@uG(p#B*UDwHbMsCbc~$ZK z4>~cl%zH=0O3qUICPfh4-(0#=277SF3#XwKS>keHPKJuIB5~%QTj8bVF=(f$dFg%N z`;Jb=wpT5fHKO5bze?L>iCc-H+uwY|9*fAOm`R*in-8@#wwYb=DXUVFx93j4Y+CxF zi;c5$=8d55(Wcv(r>O@ods?zlvqc!vL;cONEro&uk@lv-pXbsc<)>@w14k`_l&<~f zYI7X0qbp8mG#S;##=uM`b%}Cctp`?#v@1|RI(v4;!p)@SoO|)1eth|<W1n9B-(oMF&{}^y&#OR3(FnSg}gzHp#_g)XXT8=XSkhff#P6WPs!*q*}~Di&B#7 z6_U3No8>qZZDu0D&FqbA3ZmXdU6cL3F=Lj@_BBUUHfHu;#H*&E=Dm2ehk+5Y6v7EScAFgjNSfvb&s$?yrOZT}3 z#TCglp(}|qlGw`R*v>E=oTZmLvo~$dU*B#t-J+>jav~F#LE{yj1@y)yA5R9j$|hAy z#IV9@MKq#Ho$6?=laF+Xu>@r9YpNAFh0XK@%f>|yy~dT(}hG|t;0j(k%>8Z ze1-<2xuL`eK*Ntxy~;!52=1);r`Za8Us^{aKxUp-M0p$;J8whNY?kDgi!{IWu-+4v z6!zICfCvAS?)31py5E+v(Qt~^X`FRo7Yj`II)x&%3qqfd+sThtMLAjh;Hz~GL zO|s+s$K1Ju^>l9e8g6;2BVYBQ5jKm$CL{Cf16kERFz+(&64+5UtzONV#ml<(GbJ9& z_V2ws1Y7?28sZ$JgC1gTt`Gixtq`#Cw-~THiUB)r zK?h$vJUD<3p8Q{@UOQ3^cZqPMPGC(dGeLeOErl^4oS|1Vt+e)PcCsbEl%5?f>zT!= zqd|07c_}~+4hh8Beg2AM$BgEd2W{@4UHsTHvNHI^;%?2Qy%~kP$o^vej^^D4Z4n$aWG9F}7h|1-sI&8w;B*51K&+BNVACVTEm=Sy0x>mo_|zyZ0@mxt%Qo?j0D1NO+Z z@lf}-+da)5nKARr&3>8^Q`H3bUJ@-_tzu_F1H(Tac!a(qP*yI9d`{$eyMxC2)tM>R z$EW=jSwC)fdwFCA;p?Oqv_80d_wl}*7j6oTE6;X@FTiPZcUy5KbL&lP%=*U8^>Cg)#FeJqj*wVj|pdMti1#vY})9CZ1%{L z_u=Z1%1)Y~PST+-RUqy3l~W`C(ve*Y_7uN#TWYZ@m!)YZkl`L?X0~X_0x0IT4F~J_ zM7+lz?scI}*WVMj0Oi8c32%GwDrn@`tA^WR?tH?7H7E;>g2&?Tp$#?6s@SaT;N>DelFbQbGR=;%;2s{_%FlzI{LJ+8o1fgEHAa?&a8utoJpA zoX9+^d&0|GWIs{i76do_2wuYzgkkNh>+62k)KJp;(+N2L)2`!FB+tzR$1(wNBtpOs z@mF0Z5`9gQiJL6bbF*={*S+=CaEnLZa*uVRhs6zc-F7;YoS`;*)&U#<=@q#9k5LX# zA^WZg)sBtHHXk`Q_qAaeUy2mj8}a?sg7YCx*GirCY7pRWBqNA0Qp)qF` zOzomOXV`u=KaXCUm(0tcse^kLQj!T3tj~^HO=4E<(OH6yKl5=TlAg`g`@Q!$}T(aF2 zA;D*C^4`WWLFgLqChwlgyp&oG2CrjvNg%@A^X4(m2vpZ!(x5q} zfI02A9@q3=;e~|O^gv!;6p8~-0{@`~DSHPg+ckK@6EFKw9`R}7mH;sqvIpv&Dcl}= zm@b9XY@Kv|V#=Z5`b;QwDD^2+b%oryXphV>D|DzM`Y-pwiYds2P^wU>3=pbuxrlIO zR!&gUAEI?f!|)aPSqrEO4nqnLr!NbDRc+}Bv`7nsT=ttzshl&I)1dO8@}|)9teMju zw;S1p&X#T)y*x%&7mUI=xbG?F6tRJiqjLI}Im_qzdK>7~`E(|?{Nh~J zoe9p$S%GE{6b@dqI4(%apU0nZEN+mXn-8IYnaVkklA;DJ1J66dEqSzGI$xI~>+w8E zZ^rTEN56AhlB%ElnQ&Zq)hSevM!iU_WkqiP8(LO(X0@xPVi3qZ*Dk z)+JW(eUUo+y~|QAw&o)g4o4OfOods6-f-m3ER&HLaMX&3v#qa{E%c$HNFigk;m|yK zi8Sp+ThPO#YrJGX1#*VE9h91k-V`?Sp>vAZmc_PZ9Tww?ldofr#NII_O_5U&65+)p zvhB~nw8DqF%O`;l@a?xPwl$IaDc6sdOoal~v!P>RbU=d9m!w0xAwz;r)8kTUViU|! z$5h9z2w7@`;pc=xh0lXm0&(V2Wdyu#VMJhw;#?UR*Am?vR)HH=+{DgIJF`a3Ipja# zG+5zk%|_PDj&kf`q&X6*47L9K$6h=R@kI-I?GK2=~5KthT(0#2X9x<;xM# z*GW6@qT(5prA@>|&OgnFRmk4x&`_z;mvrJNuNQ5L^k|7+V&(3J*<&fQLat&^S{FS6 zrzNs%`HmhQ5})u!UDvS2k+3C;E{HBATExKoF0+9kmMWKMLsnL#JmQtTM#p)e#V!eZ zbP9qU3?3Dp^9cz;7iknLOOJLBR$844`_|h|9u5tvc~ne(OQEMFT(vm(d%_Kpg$pw? z{|t8L>?3CG#&?3=%_Zjf-a%ogCvGGekoAhHm`z}<0r4Sx{>!VhB=~Z&7}YbGK6Q!8 z5}soIu{I<=RWXs`pIlz`I(r8r?(D9i`w{Pak@2$H-du=aL&Nv8_n3t1wShGf zBScNxu*R&x0nH31EmxO@UrTlO4~D$=A&QyrJOnCg?J2g7jI!YsHORFeKz?qjRura` zduKIHbDmF|ERxKsMI4o92V1Fe3;8-=n>;E?bq6=tv<@B>1EJT7q|BdS{l$-!TZ^_O zX1UTi^mG*Z5%7p0di)Q3`8Su)x_9s*82&i=ai57CnA$!U5cQ9qB_}89Ko9J9v$D*6Ll$3&gRW;PxxT1 zU(@N2#SFQHwcl8l5n7%Aol>P3CUAx7U)f>r<8!bqKj;$)lo}{Hcb;g+3^aUa^J}nS zN|2!|C?|s1h1~e@egWSMnlFAG4>}pW0vD_6r#MI^n;E@OCX`c&s()E2}QIwg&|S6E#j43>CP!Z@_x4)>Uj6*Z%Z@Qy1e;MUE*4@H;%y;~nm8cjb^Gp&a9o@0XtlX0< z#<**Q1J>ZuE}k2=nzTGWenx@D5QMeU?i;(_bxNQb*Q4cVbK!!!>j`oCC2uj9$MvMP zmPS|J=)uhnr)JFKKbv7m*tSuXxAt_GkRJKY;gKPA#$A;x*?8h;dAhXPd4kob#7xaS zq@SC(R3#P}RNi*+^`;&V7rG=>7lsi;K|wqTS_grwcrumPtmqrFFjj}qftvV%nhS_4 zV!5+n0|w`;6tLv)baKM(da|XvSAoOK2N>^)T=jW$N-}X0Ba*+v4}oZx$z^;=*v`XZ zserhvD~pM_p*KIh4kQN^_zw(_fnxlkRBl6*x4{NW*q{6{6~z16dTfNwWZqeZxZ{&g zPHdC?AQ%3zTqk~pTzKcg^2y0I!<5cRNjisSGL-b|7nY+jxzDxnK3^mCol0?|nvJ6Q ztjQO3$3u+ZjMARt=jYQ;a%ak;ZssLv6lcE3E*f41?#l-vCd=7$Ss5lTwMUwjw81%MeS)mNnPz@Vy?DSO^VIVl!4kokl5 z2kkkq#ZJ!$-1%pq{v4EftgY2&&KfZ*+>K~Az8ts_!F3x%j9G*SF z-THGGfW>@_u#%F@KY0v%$OFIA*8yMlb@{A@$m8Sky}i9wGJ5i1KUh9`gR7K$1u4)b z#g03RX6@j!b4D*VM~))yvkSg?wMJiF3KBAZgPS$JstH#>gy@%Q(A`_qbhD{mnh1hP z2?N?T4PPG5Dk>9%0Ly{a_%uEZJ@WH;bFy!#!&)ZvBXH6#ISY|CbU~k)x8Y38@*@)r z_E{D}`GyFEpZceytePQ;s`KzWYZkF(5g8&UoYxgCcxb~?oK44Pwn2|6iYP$_cZ0G-K&K`s+`JZ@84{-3A?t_KyTI;X zHm-ySW%PjRW6kSJG#W889LeZ=LZ-609K$C~NY_#vxvnEM1Is0k50sZ<`v7}%? z7;rJF0z!(>Y)sSYXmX;;|DW>+7%cfvc7U3iG$$6rRSX z9%h%%#*euaab&6Gf$7E{XM1@s3EA#kF8qc<5tLvbSPv%>q#GEQ_?vZ$y%qaAiEX8k ztRuE~ZST-7zr!d9VwC{XeKf2t@y0x_HdzBZAN{Nh*I(f%-URP7H+3E{=q6RpJN?Tb z-Q+uIPLD}YM}*!k=~#Vp%^_l~A&5meJ# z9d1_T-PJEgj3rJ4$Hw5QieExy8>y(pcQX~hm|^=bMNBTgmV;Rk%gcCtW1qAWhrQPy zd6j-yc)8biCoHN|_+T9vJwC?FY6hM5ecu-!r{%k@;Wly;bhv6SNxLT)ZOf2o);iT_ zJ2#J|^uUx{=6%fCqv*CYatz8JFv2Cc3eQQTlPuarurUHHvj5O7$lHq?L!gu?x{Wu} z3=rM^3)t3qw8c~S3@P?${2)gL?%#INZ5j(!(QS!j@@WrsR`?Zi+G)-|G>?lEq5kO# zW1WQkPlrc<2PXx#Kp|_yAWyO5PWWKNA7J{6$foa*N~BoVh+;5E(ErOwgz9fayIcT| z+^M+6z}ULS>inR?M1GI4?Q%93{ zW=Z(d?fT2q`iu7|Y0+wDVnUUg-b0_kx8^8(@TR0`Bi?AigVrwoL<^JW`7dp^yC@NA zC1mGOg*~ceE_PdCGPB==T)VaIDZfs=U7|5J$E_)tM+G%utq1(NwstIE!PlnQsdQ+z z<#Ds5`DT2JlXkS%i#>IK>`WhZ4*@Cvwv&E-bRR#c=D_lJ-{kvnuEdN@dELPcpEJ6xKK32ivgllZ8f!;`3K1>UO-y z&?EUQ58NDH`#*QZ)VQhFth*%|cOLp3^{p*U58yf{SeT8(lZ#}+S~3@R*@?6!qroli zPflDM)iG9a?2%7&di6z7?e=f|WvAG}=v+QTiq@|I*7?5z;3Usl=TX{AiY}lP$W_Pr z2M;c?OpW%BhR9I_G+o(}e`(J~k|SOF!Z?-Dk8`5`og5I)fFZZe08sok&iZsP5l6r? z0;yAQ`cUhhslY^{n0P2sj_`%Q_#yuAqqI>{@q-K1xU-tC*U;8|wBWy7K*ss&f;i#7 zT!@JI;{qK?yhipP+Ozw`Y2RFk7>G+jk|dS?gPyz*3G%5*`kw3-Ln0Do@+uFBV&gcZ z&!sPi3~2S5z3(E=KxsN+ndBwWL>McK=J+WIL-U~j#Kcz;{^YoIQ{?f zH3jisaeDLhPn|ZpMO1RmB}gKhQSp#ph_bSbn7FhuScM~*!TS8=-m@K7CE|o!QTAz$=A(`<@Ieo8E_9k}Vk}LIZLHBR%Gzk7JXKxR+Kl)H z+ggY^=(W=o(aUywz!kO?+boC4f1lL{pk05uJO9o*mP2jyyn)7J`kDD2l_ud_WPr>Q zF%4nr$!ry7lsF)^OV`BVC9bGv%ku4a0Ju+aZ~JPibKPLO@^nXoZOQ6QkSFrhWS5UK za#QLYY>l5_S8bA5yQ~tQrrG_Hva*BOG$@2KDho?cNSu0HGxno1U4byiDi-|xZq<0m z+6Fhm&TZIp2HXKlcMAt5OwD1rEX{dw7o~X;Z;w1`@Gg-&?aG%2m>8~~0`L!jRG(~wa=+Kh?c$nyk5u>ab?4X)40 z%b5i~Ht=Z|>E+4o3=|Dm31K^nYSs$Q{9t5g=#k<4+Je6`r~eaO(cFPLoRmbQ*8Gxf zUy!cl*!Y%Ms0OD(^LR_7SZ|R2%Z$!IZ*4zSb!CIHhJuCs8*4JGZ;Nd&UA9BQJ9R_N z!`6mMCLO3=a{!pI>vSp&Wv9N&;4idhnf*JAD9|SI8NB0`Pkqbo)B5E3$rAtVrwa?8 z3nCQm-{b*vM^&A*g9OkNdC7pv6SSHeD3Bp_kZV`vyIMcwmI!zVsVEj9pf-*sM`lWwKRF zP1!%D7nZAYhm{LM3eVw|YmpLG5f-+ zk{#J(&V}TcH^`I!kld_P5l*U>E|V_v8Z0ZfR#IgbRHf4i*;Z}-Q0|o`p}Op(7SinX zeDkcpT2zQ#kdg}3r0iAQPV7Ve#t9k!f^7I{+^HG?zqSigT#cCfSQPeXnTinTWE~xct?corwADV_3xY6>cQpJ=&EfO1(`*6imxhrBDR;` zPX*d3hG@CWpst{Pnv zGuP@wV}~;pdON3=%npS?i&XzpXA(`=SGQ2p4I`ORs!vUov&s~qyb>iP&wKTi8l&VF ziuNjv*T?O`c=!3c+Gk)B-(TLhtV+2Wx|vl1^p;T!f&M5mF^XLHiB|SizbS;zBbcIznW+`OR-O_%YVGm!=;1W4VgRp$fD^%7qC3MVU1Ps ztzFG9C^d}ie5v*~yd``TU>Ssdt6T+__j=;7;^?Iap#cx8_*)kh@Pau<0vo>2`t zhKCoX{HLuq?FmP%{PV>Zb>wqn8Ym|girKi|j|`IHBWOI8)f@N0Fw$1cAGDOeGnUjWPxd~VfuGUJ0+o2TEDj|hco%oT;k;`+b%5MPov$Si$ zK;u(y8>_bu_PY!WYpAFv&aoUwJ1Bq1BVTigjb5i*105f}8W_mhs0u#RZ;wzvu1t$t zO>Tm8q=4mcR8rcMH7s)y&ylLAJf4RXO7Z|ehs=lZMRPdjm0d=12rXapdVGuwzUmq+ zp)%p0Hb=Mrp}WHp!EqjWANoH$HEM^t3(8giQz6?!`CY^J`Bnls3UJ$UK;3)0zE+s_ zfe^ZHG6297_nK(3!XNxIJpfqDeqTLMp!YAfAU_@<&hdJpdE1hpPAR ze&%vuD&q8?|gYbyCi(-zxyIp?E^i>(C{8g1+N;6wO-QGL}0p^lQ z?UB5#;3}UnFP~9=gu8^1K(o{&CpXTu6-Ct(MupD;OGJua;Nt#05lnB1*jsSG67lqt z?+P8m9F2m#?6G#m+WmO_xbN0~S7Hhqaq$<0uhE;oXK4z%Ll%0!QAJNOPBFerRG$B8hK&bD~1Dwhq#NP8+>2LMX6P!rojBG>xIP&?Mtv20zw%0bL={#FuGam zJw3KiBl2q4fCYP|cz{(`C;PB$oXs3~eH6Q3)vqf##h)Mzq||Ud?3=bJ7E@y}JAiPl zgxB4-ivf618 zmLz4eSTm8|PpB?^3T_ka->jH%&X?d1CcaBG{6%bMY)AaXh=^pun85MvYlBv$c&WZy zT)AIZVsywr)Gz|!mXr2;lgRGlr?f>4MxRKNJJiW@a$2Oip9xNv8sQFv8wSnX2x7dc z(2+*;!V$1!X9H(&%v;#jA!OtqKQx-w1>3XVBNIrg^cUzE&^%}au&Pi_YkX`L?^!tF z!r!ojDh;dP@gdcrM0T!Z&JANoanK72MULEPk3m(KTD}5WhZN}Bentd?SSKSxxAd?N ztsiJB=L1=&FZEL`~!GJfDzL6 z9Xb#w0ML@WN9RWyRNtF6tiTvPVFYhGtc4nXYuD>`$85K7_1;w6F38!Ih#FHeAmT~9NHGj2xy0Tydq#|z37EYghZ#VE<<1Mjy_YGw7iRYD zVX)J{NmOpUT((WxWD6hME14K~D-ReJ1c>j;8&c@iLkzuk5NuZ-98LEhaM{mHFxBo( zvZl_UX?;S4s2!R&qr9G~5KGSdED(dM2(d!I_)!-X4Qi1U5v?e7DIuHERRadNAJ1sx z7_!m31@7!nzreB$>l6xu2?N8ZpE-y%L(23P1dDGks-77|Y+&bF^?jUpaPdMU4e$G) z1r%&uH>oH?E7U%QR%Q=!P}P>Mt>!2!N?oLXXja>fp1I^CK4+c>5Gj}&<3$6#@qAt42lV3 zauP&gAE^2cikc>r5)s#e-yks?=P5j$7dHI3x5(5>;YOo72=OBpv)n$3Mm+pMx32Vr zXFk!d6RUm9a4@AMV}jrZsj#Xc;_b~ZjEp5=nc9M^yL^d1YfQBv(ISlx(G8YyKWkb* zWPYk0wX}`Q1XC9e_y#0!5g8(l+Fq`zWrWdWD8&olkZ$ErLjlM)ACsHPfG~@V+I#66 zL$A=$p=av}O`%bsQ)M@M&qHBsICq7{fSQGBIgmDxeP-{F^g$AM`Mv?)A-l|{ zFi9WqN$T-Iw`%82J|GeS>W)IXDWD*+Bx;qx6(3W876ALT5I_csPa-o24rLN?(?|Bu z=X|yZF<3hclWiN`3-(I_4IE1dLPImKS;}zL)_~6d{H+kWMFLhg+9A6Uz5{zS_j_VE zteHqZA>WyPiUkT&-1u;Iq)4bNDbc+k(;bFDFIWjzR+9=wk1DCc8w0Ei;N(RD=dw!2 z*Necx{DOPMw||yj*liw*P3A;*)!P?pHx?~~X@zFTMCT5xS0%KKq09nD?rM0Z#gLq; z(*rT2Yk1YGjYH9*#CYiVwPu)9){Blc9Bdp4QzfMcqZ2Nfy%DQT0|*bIK?MRbf}5|< ztr@Z>yFd-EK})FQzEj`VcaxR4@9LLFv#7d^Bt zWxZsLGuj{#C4P4p=DE<@|H#BGN@*!wBsxM=X&fw%6EnI8(wKvCd@2(IWlv>7*voLM z-oFI}k%y@$+4_!@k7Qd|0OH+k5DmP9y(}M?0(#tKDH`>IgP_--y8wuqei$E$y&HFM z*rBqZ0GwH3P;5@=Ep&q@>5lN)AlJxv^b$Bsn7+)crWQsvWO$nRl{8@nHbI-9c!EbQ zKYMPNJ=mj-BewO5N$_m*3`B|twQ$9qt-QgC6tEjQl<`O?JKQueGRoJ^;Ccu@Xd_+A zg*~Ifee?iGiog*UBNBRhAaA%9Z#q)9Lkum5!|8$Yptufp3uz!D;(Y>7*AKuZ1Ccl| z$SHWiclqW31D*ekLhZ4@+BN8*y2U&9i)?a@q?B8v{Xv#PE*D7I7trqDruq%7T4_!} z%$-Wd#RWkSL~!Wr6lp958@XGMkPliyHV=8I0f;^9624rjglLJZtaw`J-%Dw)c!IR> z4_Afw!oNcgiH8U?nu85>(+4r{iy@zK7(^3qgHYy!BdiV@c6G189FHiXNceuR1&!bX z<$-d_5e^(mq9sf=0M8#1!PpL$$t5WJ(P+t^a8}+ClDszOqlR-Jysc>sjS5s{0_4W2 z{HaeAF@^IJ&h{r@-cJ%aABK=9k|!Go@=+R~P(+Yc_=G5cD#r4|gO~_0VQyeVs0D8Z z#o>Nnu|mhHz>PLvK(c=l!!Tv;%pG4Yw9*vEY$UX}aoA2)+NO*N6y8kGY z13d!j@(nT*8WBsYt)#{T62y05{2yK%Ykb_om{BHc|ZoR@rnm6-3t3h(8a)?z98Bl z9dWh-&N$I~RMFavdK^;)M9Cw92u2Ez=x{U1ip^p{#wZn&j}LLn!nr+Q0rt?}KZXBo8`>Sa>+o z2u5pwKezFaBZWZsN#J&st=|ZNl~-cjeY+U+aZ`Hx`eJSW7?uy683W@d5R2O4$PJ#C zwk1oJGRSE+9^4v9K*ABjyAsw>R`?rl;O17OJ0!V6B9CxvB&4e-inkB!EzA zdPv*QgJcnyrck95MB9v@=|E5)O%V@vOQfxr)cT=7v^m?Jv6*LqqacWp(j~OOG)(ST zsIAYf9=-}8LL{fa7-}t;=}H^(SKB@{r=pZSTd-*_wazVhv4w4kq9JFr?y(YpKH5N# z!NWRV#`1oK#~ut)7-zdjPpWCBB3}_*XMNm+Gr8^>GPMFJ`T_Tfi#K^VWRq4q)(Wje zgfJQ%m%xaxej4)w9>ml><9<*QJ8sqlryRtPS$G&zcI*tJn&NqoHDC&Q%9bB~+rape z(W?3Xq3RugEQhwR?Y3?AwC$d@ZEIT7wr$(CF>TwnZQHiKp1Jq_|M%5X=Ts`$*-6em zl}grH@4KP5k};cg*TIM|7)UNG3FrHR*Jfj}_$Xl(bc~A~Vi<7*B#L%?bq4nUgdQPw zd>up8A`LLxYfL{11EUKf4i+jd^BT`}fCr5fy40XqAU(A~I z42UvBhe)EhDv~Qblq{>!P43lcGCxxN8m=}2cFGRRfXmVA31DOcdl2Q~^yE-?sN|}U zd<$o^MYFjZlSPXSj`O*unHCr6k!wfWE60&mVYGbkjYu%Gb$+L}-y9%=CY&*ou0lkz z(};D#ZHv1>kEY2v4x&F|U9bS7mn_4#z@{-oNSa6)j5){^I^E%Dph@G_hvpg+)^sO= zDN*+uqjSiH7>sOp^Gk>A5PY)<-jiGn)IUG8x1{c&>#kEq@Zlg9gBHtObhjLH>>sDb6FmBFz|;6a=u!wo ziALJu=~+W4oTwr7jKC1hm^n}kGY6qa{i=sSAfP8jT9*(3S5>-ZFVsf}UN&v5Wkl6V zwX|o{Q3YFAW-=|H-$eN(MrD-9Z{8g=9x+Tht`|jfZ$$rNc4E5>hFu6f+ZHUB$Ttvx z^cB&UKww;?+^>h4ue?{l!ouQ;nOKinASYaLCpd@XUK{@%VjVnF*hfakKQ5X>G9V0& z1rbaY(FlhnO*CA$q}LAqhWJ;hVNxpbm0PbCY6dgWAssmRaWfM` zlTN)`AHX@F2vd5D-VE(-wm&3?@M;u2adr#1eZA8^Dd!lNw|xQi0(K!ZAnu@ZJc*-> z!fCm)N?(tQMfC)-k+I%=G>RGY_Ms9Djv=5P!~!SJwqLZc;-}G>;r0+TpeHc`!najn zKKbXJxPAOY$@#d`{0U5n`rsCb*6LAY?~UNMr6f(e$_n`;gESfd+~txqeS7uyL9qfc zu_IiGzKc4f48c3rZel; z+dG)?K~lo^u%Q&E=Rpk#miq5YrppZwa81oI0I%YArwb1Yr1VY0ck|?^Lu7<7(_-(c zvk?v`u=61UN&C#_1|@_iz>tas#L8mi%khuD>f{MO*J){_LLG6!-1&27=!7%CHb<>y zz(ZDI>@kC_F0fTG?>Xj=<~02>OyF{`UfC{@A@bx>LoHr>?pEy~1%~D9&eN(xkLcNM z4N@Q$lp0ZhI|sxDBITd?&7S6;g#LcOS4_H{*QZM&x5tRV1p~=U{Rk%wuC(m8R^D0S zv@gk;$)?O}UFy4Jv8GP1Knie}S|FmesmI0jWu8ShKr%#*#G3Q!Yt=MLm-C{){5$MW z(s1(8uycNL!AN7`!p0DpK=nJur|=!)18P7KSb!;22Q$M_o9OL^C*t}SU^;!tGsBL? z0kf6FYthsVD^-8SDfP>gDNYkqHpLAY5%Yy4{$Gr5vRIwhTAjs5S*sz;`i~7{+2(=n zf>E**#S}V-&=ya&oMy-8^$z^ccuC0o|1Z`TK>E!kiNKPJwX>V~8G2*DW#=zk%}NgM z9UIBl`6$N#b~>7o%XaJF1DnsjN?|lOkMlhpwlJRc=-{95ycra30gv?-3_ILAAHTLe zn~rJkjypB9wn8rlGO=6vpLnAWzx7PB+?`)Lt~S|^cz(|Rc#`yUP7HQlz}x(g-~Ono zyD_O<2I{lK`vfcAbGoH3U9$=Ok@b&HE&}Z%0_~Q9g+7=Hl(at!Xs!V$-W`&1(9UkH|llI1Y+Kf2~XRtUzTAH>40SEnt#TDZA# zEu(u}vK61UllLu0oY#Ihl3fyn8|@+vDlM8udDu0#{{5}Na%~ESsyfPS=k&$}M24T=67?OreRI+?pK{<@INCK1 zlSJxT;7(`cbs3Weeb686hKqZY@rz}>DglIsvk*^T^WT|cc;Nk~vF{v+3|aWf z{il}_vM#X&>3^vlFv{#PS^;F~O3%Z5N*>W;Zo|~PM-lHe{N~`U=U_E+(e*>1u+9FA zy4z@=_S~p04i6|nyjGI$CBW73814M^^dWu6284qS;MwthrwIQGViFU-yk?`={&h@kL z%rNY)wb%sfnOKzj)G*5nC|t-VsqNG#kT|{CiDCC>ZgTYjJrS&qY;Q^RD%AiM$lJ=oUhV=L|hyb_DvfK2#*HP7MBaf2dQYpO*xEkdoO zT5J*=A+@fj^c4;WxLu+a;hXE<6>xh48-gGToq`%3&yv&P4yvUZs!Wi|UNi`f0S$bM zVG^80{!xAw(e#QQ3u@@+Lpi1a{bBuKM}1+LJ?>sKF%P=IuDG~8Z?P)3Z+#vNUd{LI z)&kDoJTVD8p{Ju2A@w*(him+y#E`H2FD@v0MSG;cMNJmpeQ*%{cck;bpr;+)Y zjn)+1-TEs?XX7=n4{izYQp^)VX6PWtDbK;#B@YY+x);lIFA)oK}Oc&7tCp!I!e*vdm+Cwf1SIOUstE@XR<1V#w`&p)0 z5zN$+l~`TG)F4T?HvYQ;eI;(yECG5kc36ntz>j}GA$Eu56jyDHE_;0sX@a69_&NET zC#2q#mjgNFw*$E`FR|nlxIaZ*EqGkCgHxg*243yyk%!K6Z)ecXq6*aU-?z5UGymsz zDB@CoR56LrkPW`~uLtJlEYSN?y!|gWZEA=u$bZnmY~76wl)@#2(-;yu`Com3RfODi zh%9aFy{)Y{@I-%)Y2A#c0J-cxw!jm-Ya)cQi!-4gs;YjekX}7i{SIwhy|QL1Tw_A& z;CN`gL&Xd=^i5XRASsq({H5DM@TO`=Tz_Nq#(HEBDj<(TTMskVXkYB50xkecb*C^1C2LVVI|@r z{ztnM54HQqs7J_Bvzug%*Ilj|Px;zvVSjK*vJ}%N^>XIz=W=fY% zf$I~8H->b|L1)*&CP;f%#1+fB_$8Rm;1EhoULp?Nq=+jyhh{;%IXY9YBUStP3srmS z^q!6Kr5m`QRTmd7s%O6jFh)Od^B)yEWlLC;5!l6z@T*{`zC~2b^RXA7r-{y3!##xn~2C8)Z z(R`q7g?&W&O$)7`W9D3g(#yL?+Nv;28s6KkItcH-=D&YqHd4?; zwX!`#El1sy6}1_SFPhTgrku3)uotF+$A^kBq31DpyAGZ+f7Wx3rET*s?F_UzPW1kA zu?-1E%Qr{gnc7RPP3M~Yf9d1IVKlkaGqrQ)E?Nh}+e!x$N*fuRiw1o-=p?-OlOQb1 z?rFylKiu!Jli2Dn%MDgd|3p%is}!@<$(Yl$Ps54u86aPE*T!wykq<;}cD7KKi6|69 zSS_2B;U&Hkh-wM;Qmo61_M<`V1j9H#vpehX2>0M&Kw*}M?Iel}6@5oy3E^aq5IuOUSck7U zb0tX8?A!4=)dzZEcHr>%3BtE?t?>4Ul&Dd?d?>5cT;HvVgTiV`oiHYcdWTq4kgkF_ zx-X!v!l8ZdeI*xvnygQ`$! zRMcF3t9(I8+XK_>+cWo)dlzi9`=JW4H4sqX^N?$Nul(KUA@gFdiCxe%v9Ud{W2n4> zvUhMBUK=c=b-pZ3#Ol?B%IeV*WNL?r*AK6uvmDTyozP(F`!OdY0jN?97jemivrt?W z_T7tyw~T<@sSqpRp~C>8-X!J|Xyg>Fo%6!(3DYe;?>6=FkC2ME%^*?AT*z8e`1G9v+tG4Y>%R|T3 zPQBP9fd&zhiT1c{0l8-3pL&J0h;L^4d5Z{*71|LkP@M>a4Q}5yx9EV{aw?swgI1Sw z8aMr}Q|;AQUS_1wbR_guK-ys%9S0S+PV*-t+H}(9dhY+_O}~a)HeiXXI6Vr*ot0@7UqlGovw} zBloPkkH#A%`~=dBo9iS(nwH-AfwmE4S?&d0FWQ@`~(-NN@y%GG;@@_t9%!Czaz zlIx@ynX1csFr}JtwK5KlO~=xSk_wx@4wxY6dcq6t;zzjhAdpi<4e&DaT^-zt6bX9! zl`e|rN`8ev0;Z@A>A9UbnW5?Tvfh1W;K)YiNnNX#4$8XV>5}Go z{FnE+{?y6X8*I=t8@r0lB%hhKB{L*bzouEUbS(QcHDo$)W|z3{#IFF>V8g9e6ZHR+ z`#ra)D?!xQm8SAj!+&JhBt~-Az9CI4&H}fVh>iJF{Z3PB9=(q5N^_QZ(B6>7x`eT! zjuVHm#-<7x&Az0F7d`Qy?n&oh#NL(ol+6ZHEMSUT^=K1HLv&#;}eH z<-T%3mgo5|pQgLg#>3AOyV8sd=zq7ig}B6vy|)ac8}!15u`bZsPo-aY;+*z&}TzG`YGYe5FjMjWP4fa6oS-2a zBY*9M8h&V_SEHn!dVi0AQCZLW5>|3Rd)Z*H@E3ELC*qb5G+fAM&1_!8?~VzH9^Tqt zBSzf#>5gQ@i`MZC*h=tFpEdmq>N_TUS$zNMMuZo<+<>sbzHEjvm^Ai^b+4mI&@o>7 zJjvQLX5^NxjOGxxk_Ehlp8rP?FA?Ms0qc{%zY zSG6luRGQANn$8=$Zs;l#!P#E#`cEmJ!BgQY0HJYwng8ej&(S_>IDB~dX+)+|CS;xA zoL-$LztW7#8aj2h2QKPyzi{^bfW?vbLZ{RU`ylb|NFQh8E(kbji z&x`~qb9)e^;UdkrfK7t9(^H<6F3~Mp6aM?z_NT5*K+G}z&{H^~D}2b? zUtYUCTN1@@cUog8kfY9!k{!!X#T%o(jwB!cMXo374c z$j$9OdTi#-u#QMz9yWfhc_#lTalZWN3Q8vl&3*tir)sm3prj2MgW<|;~-X}0VSa@wEVHd9_&ydhook4dy7%@vH z_M00I-I~l%bEyu(?}lQcssrrPe1Up4DTTPG4cqc448hfklSb;Yq~4!~Bo?fjtLFNAO|Rb8;7S@^Q%pwy}D(K0rd;^_01afH@B0tKV!EBOEy)%l0@kn)hjwyO1VVUJBAOE z>zxZHv}y%rPX^pai|qt)^34nqv}QJU!XDw89778Y&S@Vfl5RNT%Fi3&2BgVwOWj_) z*Z9<&Hgd6Ozr2Yl4xFLYY2KUB;@9*IhiTS`m8{SVgd#H-sRVbm`KfoMC?CrMTl%z+ zZQ4lcH1rzGt4CEdf_-ZVB|d?&T(TzgkwfbVj}{k#X?strw7(4f{=9DUxPhN8&RN{k zr`4~$ofVBVZvgZ8zj~?HB|V)u!7>$gRNa=fS=y;NRoRVl62@BmW^t=))Hog*eNNWg zwJfYgh0{n;EY;UPb$Qc{r4epKSy3>M^h9l(kIJlKtnQF}NOYgo*%(ayrD0Zgr%W{A ze4s3JU7DcyQ6B&no~yhEu^TaH@_DRnEQU<})ce@}w@BSsMjMu0n~sQP2L_K=qnbmx zQ9ZdPqRQ>F`iOwA9sKBq0W%DLvK=V+Ci*?S6J_dLyqd@NB|8nhblkcZp4jjL!ie5?aP++ zG^j95kI~14dilAxAzs0sV*M|HRrzHR26U zvuY+cHq^&P(bR)TrsJvuf>Akup92CxVPm|EKcVHwIJ7{9Dk0h)+ao+ zsY~!%>uW=sLd#Krs+o-Tkuy(IN(CqBfxQ+AkSJ;d?35<$VhLGF8TG+d=KO9WfWQ|T z#T`Wo76`!o{bL;xfX9Rb_yT-Z2@wXT8DKCNAXl&q3Iher2ZaO02NObT5KvYZgJ0tm z16!q^(t~eXaXAdkyYIIP5{$(&l1o@HT<*4l2hzNFMnQ?@f+i=0Q!N$@73c(LUw`R) z4$xhcAo^p3_z>c6B540nELfjooV^ry`4y!};JDCo+=vu^WQpoJ!J?p=e4%q9Vw?oF z3{Y2pX4GB+0c{K>xU|3?6MoVF0a)QhNgXouy=Q-0FaYR1CfrycY&bk%eepDmRh|J} z5V0sCh*!W}a(_YaWX>OAx-xSre+VVzsNAc82zO#IDYRwXF&C=Z>S$wH2yFGo; z%2uLV<{w_{-Cj6dY&o^dhv-wekF-5)X6e6ycsEWG$twTK*CYEDcQMJIQefLH;kj3 z%1PG`dlC`w{x5pW;WwdU_V_*_mEd*e#cTJ6MqD_7@*L6EM@{}@$Cvau!8a2qub*zO z!fv-WH=mLo+-cF)WDNAnCXTPlf|;V$sW5eG=<-m31}qB`S)WL&nMrYzrkFu@{rUb< zkOvd16hi7?rkDkW_wr-QpoNXF2}v2Pv`t*#=r1&Rm^~E#I63G&;Rh41FfuzuQJg`I zdW3*x7X?Q{R^Xtco(l;4%f~?P+f4x$aFZ1G%4Gt@xaG1`GBh4$X7(TC$)tBewGu6R z{lKYJ=@fDip?`R#9lCQgHWCrhgJkqz6!? zgw_w&qtFx$f(1kgnpAWL3x^yCO&LaJM~A<~7U9f)MytgNF{LtkT){xmkWKVPQhWE& zn&Iax^#gfvNc+hVcSHk212uIzVM(ca*dsBSHha-s$3Z|YgQ^}RL`SV zUODRB7bF5Cf*Wj5J$NLEW+9DTN+NX_=3Xpuj>!=Y9#;eVSU5R2EMp;cr7TU<0s}g* z5(*JAV;_5mSO*m{1DD#Mfb0E{x|YonylBlf@Xp&tqBNXM5$8ylF*u%x2vRH(`3gh; zX+7FL-7umqN}Ae6g8HfEkS%p=5baa2L|8S{PWQGZoSO+z2IpIVCaHEwxHt<~YX}}C z;$T2&RB4xg>#?sS^I5Ve8zA(Lc}{`VIG)wv8+qT5(NP?H%w#g+Au335wgSYmTKjg- zAH=CL_n@s1cNZiwi;uoZJ}g`|D5}lnpp)EDLTd29UT*g_f{vIS?po6 z*(=s-$(j|3-H?CKZFh}RDD-^?rs>LLGZ+xSeq=46d0E&x_+4Tv5hE&*AxfyY%^b#S zdb&Pq@l5rxH-3)Tf5FR@$w}^xyM38SOSm7a6>D6S$XC=SXt5^Mmoy~VK{9t@t>wSH z-R!lyso{f+n_~|p6Je^QNEGc^r7Nwe9PorpKou&F`OsrbtupSgTP5jju+DJM0m*PsMaYYoT+y)p ziNVaBPKQd9FU|I}7eWw``sF$4V+|LjI&6?{S)H~SoC2n@jC4{Cfv;4FN3j}pH23mX zy*U)V(oEaHc;|@fG6uA1bk_Xp2_bT6?cRw5o|5*p4iF&O9fClsJ;EI+W#Q{RNV+^X z3d3mOvqeIMyn);npukA4x@DxU^SRla^*H2d%Uxx~s=Rrg6dztdHqsOo$OMQyi7!+3;&HQUGL`Z!-mp*pSBgzb4@u?)UCQ@9)1q9!mK+js#s2irYH2I*Tp2DoP{$VO* z8pW)NVA5pFZyx(z)*!c?WefPBtYmzykZvm+L9yKNb9BRnrJXnGM%%Z+b&ne#uab3R zjstJau8PsuKB-N*par5&iyUxo5UVc$Rg;-@ogXOb+)0mFuilG6Jvyf&gk$1RqD4rT zoC>Kju1`A%fKsa|Z$a_|Aen`s79yE$3Prg}bpl2Dq#F=7BO)WLC+?IGdb*f+6*R5!QLiAmMkl(#x1?n?gFC- zrQ|1&fYvt~k}B6ysLM)jCyYr2@dECaEwkE&G~$}%kf@xx^$+94gww}{E}H3=y#yf4 zf4T7e!_sSuav@{2m6`@=Ssx1}k`cGQ+|H6uL(YuF%n6Dm3289T2r7#1dHk|1njQDz z>~fg`6pyvF3go9wTDxMsXnKOnibQRuOkYgXeZvS)!CvAN&>Yb0DX_lDFI`>QQwg?) z>A*Hp7{|VqIUnR+$hD^}rggueYiH(zwWW0(mlO2OpP^ALL&mR0>JTlgaC9n+o(hPx zW9x-5O-uE$Bu83jYud~eOpCgNaE28_TIgSIT(K$Z&D_5V8(q(ngpZw;l1SE7L2MWU zbwdHD3>pXZTCi>(>vL%wH)|a<>Nr$sHV*H~5=~Ug>xvWX<~Xc>X_RoXk9iEqRhYFK zZa2o%u96xS+YL*tX&-e+m(ql=aHMSPIjz2 z2nEbPX4JhCt;Nhg9!!+2+DO`(5m~HMwkLInrp~AaQ@L0;Xz38JX^=zb!XSZpBd!A? z|8f)_X*C8CNV;r7c}p$j_33D&kwU^qIfu%qJ<3DBI3k2i@m=+4GRyYY?`sNExk)O* z4pvUtT{TGg0y-SG%rAO;WwH^k9Ys@FopfCy6w*EH+n*~@t|7Tt9XWV8v`!jomhB8|4?M=#u$g z9}A@Fsa%z%mwDm=W&gpmZ-FM2Jkmy4g?fNIVr-}Mh&H-xJgzIz<}VL=whcTQGS#Hb z>Zm)ibM=g`r4)y@j*SiIJ(T}2+tP8A6tE_8R92FTNTTg(!eR=%2bdU!@uzB_|#Z9bc2aWvTg>d?uhYv z^w-0mi^xN=*4GzEShEo%t1_e9m2+z5Y>0`vhr}^ILF37n*Y=&tQy8Si);W$P;{#mj zNsAXG@TC@>z5^3GE0rTpWAfM66gz*`9Z0I71pB}qQ8Ejb#~#;R2UQN)?0dAd95FZO z6Plx(CT9P^*J>Lk69W=nY_wAxW{F&=>0{11aQA>nX8zdf*y!3@cd~y;W zJpL9Gzoqq`pziC|%yZ`K;7C-!H0??ZLmI;q4x>YVV_=YH;g!#h=wMm7d{bU*isCtF zBwu1${F@c$heqY?WRgXd;wBbey7g-ydTnCHINro>E36y8m>R>{P(##~;G_e4cxfv$ zR;*7yyH@ahITB40%?>%u1J{t{t$ef6MM6eb{dJT`+pW}5SBh#0JUJj`poLF=vlZah zEi~qghS5XR(Bf66p|bbYfytv=aZD47M0aO{yLM+J;j)Cyv@prehFuT#i`&_DWIIJF zAlGkRzPdW>FD%!V7q5R4yqkh!7Z@+DLo@TG0dWtQR@=ulF0*anh}t@qs`>8U7koUp z2eqaMMv18x;7WMvs-Y_&1&7bpEtaoILEnn!IRn=gD)2c0&DO;rnPbN8$y)&_Hl%?| z!!LsbKn1DtJvm(vJCeRQXhD-`=Df`F?5s`#Ml ze7yNI6iVgdt0}_9n^yu*^(fn*YJ#eM@n~97;9!jC{0xk!>jwDS5>!nVrTpYRz-qQp ziI7W$o{HI<;RlTv^sHZfk?3lcjwOYGOSul(e~%V8=?$AtgSN!wDa}VS7(ND>c z%+iU)%(5@0G5@XlHdqR_%Ftm0iytjXLE5Ug6w&OA*OD{Ha!48PapQK(>KvEba$w*N zU!v8Bj{8$}O!)D!@}2h2+Pd~)l&vdWcu}j_`={w?>&?BHS|Zg_#k@us0C#1lho%S9 zQ&-N5dxxOF#mUZ@27B^{9cCicn4yDdi5q2T(m;Eeeod>(=f>&XYtw*5$EMbHplXW# z6Ma@3NlqbEbKELBZQ@}yhz?{oQc3~IHS?>VMO*Lkr8icju37=Zqpav0#*JQ#=xKy1 zo!5t{*PHtx1LvE=GKNp*e@ID@tZaI?Bt*N)jvCa%lG$BaGmq0UqPx62bk5$Ia&qEI zfg$xD(YY0;Zz+5ewZOj%8Tr#jp+)31D_}4gg#40x_1k!>;_%Aa2!I{{1^~EQ5#ajv zaq%lHxs2Hpz6s*QrqD()WjPh8MPYZNCs|?Eg)^vZVZ$gU*4m@)!{~;6c#-K`o0e&+ z;BBdGbn&#z)Sj8{HhLZ7U?vYk-+O=GI$b8CLnlj9Zl1Q-5`~C@@`+v^b5TP{DNG{r zM@ioVVk>cBz7AY?e?b`|W3X_26ZC6Xh+D|8IZ={r;|t3}{EUlN&jxGNqeFR1G0q&# zvs**EhPJgp;8Ir~j2|Wl6NJ$;%6QI=OSwMYS*$CyNJ>bhaoPFMTHDHL-@*g+8$|>R zHQ2Y)Vf8by+WHz9xgMHxmC5`ia=pGg$nW+SOj`bH^mkD2qQ0%65m~fAqtL*$y>|nd z1@h;Z`TehzebOe8_BO>hd{Gpo>-kz&nmii~vhmp7=cS9bgN4(N8U+knJE0nZKfX}x zSy(3wn#x@sUs$ae|8N0nbrHHB#pvK>cDibrX4`&xzNN~7p%D`48 z6}zq?XXH-mK1KvlAr)VTJ}0=vnj&ZiHorJg>tVMVstwpi3*rayhX9)y!4TRb;Tu(H zAkmml;wf%C<~Ivpn8P=u59zFAS4vh`;E^B;t3?w}1(!IL%QF#FDAc+Kze-l%l&f+s zwpz(;%_m(UQd;lmNU!*wc5NZ?t7X#KI;S=FA7nYL@Iegh*1r_WIPGJhDv>v|%zdS_ zlspF-a{#4_?s<|u?qe?XhyBpk2lzM4P(127mv3UV=pwSUJ0+lZLkrkRP-zEK+{?<9 zGFN2B&dLVftCE6@ruY8?ayU3IuC-|BYp2WIoi}i!4~-7=GEif18LSB3#aUU~_b9Wt zGqq`Nu1^ALbfZ;+S^%zrS%}^cyR3gEb!cmDY!YGsOas*e(C}%1Uy#!Vcz3+<$`+j; zEi61-@zKaZ&jU5^S#D@>0-1p7>F3axK(G2WY6V?DoCNSvCtX#d{r7;#eisL5u&kWd zLBR`5@KerPVU&xT-m4#MJlYjkMrB;>Hf>iG;C*jp3+`X~nqJTJU0I0=8pb=1>19)& z*V^4nFTsWQV@+)h0j!+7p6c(ZUD*{!tW7OsqZUo>WP3xuRcGsIseWnLzJx9UUo04i zpbOlTZZi7UGJW5d&_5MC)98|%=geC2p*B}M$RUViCUBa$PR9<0WV1h>xZWH);cQ&q zioO=TJ=&Ed-N&C?seWt@YgKqevZj{B+B`pqjfM1q`N9HWgRl%EB6p?(KkYcD5%2G4 zvvaFJQ-+Y3;j*-P-<5o>FY-?U{&ZG#t~l)CM)O892s<5djKpv!cRXT}UHpq+ z*1L79$e@zR_}y1?F!OQzF4$4QaQ0)N-mN$Bq-+vScCn2P4K-=(bs*1gqblaJ&;xHl zPtNn_Qk|Tcz_0-HtcrR?{`++-^iQ#GYY6kCYzblf;-kT20 zK9_DEFU@MQ$5oxb7&Km1ygbQ@p-TNqb58Z;9*VuJG)imzkg`~`ZK0Fqz%41|5(7~l zVJ4)J3_dbMF=R%?$^b-s5Op$WG2mY%;8+EZkEl!?;Ee)*)}G4KM$921Ca&^Uy>S{C z2{!%7?D9vMT6UCSe5N2L8ciH47-<+6Yt4usF1UM&G88%8?%l>rPOVSf-$`Qneplx) zqF|#AFm@z;cL$Dte-k%U*dO(7oD{FiuurD4vbU15dhgmY^UD@m)l9WHOL-*PNxIvX zW>`0}$a&Haq1!qm+Ni$m5SV3D5o1{t9p%I4|A(|ZKJ3kgqAoI5f{4)RaH<;aiF+B( zVJ55uF*E9BQ)hgol4$ST@6vH9=r77x_sHHrs>C=|tVwfnTKk*c{@u~p3+fz}6%0zFWmb#XE(d#$mY2kuf1`z1*r*d7o~~5h%6Ff zT{4`M0bRorB3VScuF_gIff%8^(?!$QM$>DZ%uQ3+2kItxU%U}h6_?5m+%m-?bjH1x zxSt{zKmQD@om_0q%H_@aO{2NlgkD*|Q|_Jp z@zqu5{mja6L5;pn|JBDNGF@kpjfTiG8?&eoZ#QXEk>3xY+Puv*@!pai)AV$Xh=>zX z>#oUf4YG%uEVT%>F!2U7^DbpE#x8gOj2WrlxBBAN(wRkLboOrblQ$t&gtZCsvAdJv1XM|IsA+Gm!PoVW zzYZK1hjq?C7nkYhK+L$ew7iG z+G?R+v>Qy--<0#}KHmuUp?42f@mEPtQAr0|j~{gC@}u~^o2{?A^sNy8Tv>YD#q-o7 z5Wtm*2*gm@?axxsDu@eKU--6o{QVpcUsMS#xi{g96$p|j2Eh#ZkC8zU!?;QL_+bIo zh^tVOxW}_aS&eTs#?2wx^goQzeoD38I=0RE6TdJqt1~h7Q;AU1HODB7=^S#N5%Lr7SHgUomJn&_T_Nv}ke21sq>oS-vi3Y6X>qqiucHio^aY1aaQq%T={>?Ebkqge zaEvz#FfABEM$Ocx&nc{-MU)?BEGE)dTrI&k`zaC+jL9z&bK@|=HvT9r%-Rsvto94v zXiS(>z3SNh7s8;LJUPuU7J+!rDu`mwJhGA57^Q}1PHBdvf-*b0zH;fZx#FPtlbWT| z#sk;Y@#q+BR+r1&XhHf7(5ynJA2xx2Up7ddugMR$xDXzM(c^POFO1cB;8g+Aq?wi{cU#g$frm6=07~(D<^?lh)tKFdozjb- zDrKD&ZUegC`7-9iuoQETIDa_zgA(3gUns`ZTzk@PK1}paqKJ_PB87C#i)`B)m`K7w zmCLKubZRpE24*~>Su&eLIGug)EMxe@pDoufx2H*J&-{+K~hK3c74#t(nE!3E;ck>0cxP0C|&_U?s zgrNq2<;o^bN* zHw2J^tAdA7_WZJW6k_KDtPI+e2l|d4{0kynj4cR#t7*%K*_xp${mQl}n^%*|N@y2; z^7iB$L2YKVTQ9|QFc?BB=y3rIgFN=?iD zn(qI^CVCcZ_td_-``7Dfu`RU@sv4+@udhzla(TDBKcC!fSR{UM{0_$7U*lbJu8S7cY#FNnYKM$Y#^)KYGTsW1zFSLwgS|?+ z`YJ!y*7p8nyOX$GrPA~MdguK+Lvhq^m>g3V0Dy5wsQ=DT3;+S}Z{~NGTuUo01OR}) zAB^uAij_&VY!`0fO&*jsDlZlh*ZDR6? zYWrgH^8C5uD*feieB8^{&TbY#sKR5$85kl@RtzwtlmPTcX%LD}F_r>Ap4s0OHI&__ zr<5K?mzR}xu$;(6@*uI$WkAhh_PI#ba~d!0??p9tufL%j5GqiST^d=|QiU$$I~H+(ZgaUmO7Xh7xRB#lR#k!jmQC>E2aJi3W~4}~ zZ*FEwm3lsc$LAq0T&mPkr1iQ#l(c2b{ABz3BIJF)v*ta&xDd7`hy|=ej*te=V-0oO ztoq&6>HXDt;a8?wMQPmm^~u^8UNqweZ47&d<7Eg)1OEojWj z^0LeOvu#4@=grXXuM@+Bw2*bBQh@S4Baz9wqp8F@AIa-?I=cIK-U+&|&^*2Yg%Y6i z^K%z1?}Jg)XTR-gq!~I#3IL>`@NPj&C`Javy zKGpduDk>~2Ec=8n2;>956H)^wMn0@XZd0wh%;)>6ObBqQN}Lp72H3@oSrrr`Qse?1QOL9a3-EHIt;mB9 zV#}X=ElCtaKr?fD8xi1cxq^fIi{uAmRs*3OTWl@)uc!ngMkQE#<3 ztD&I*)yCQ=Pm;IX0zjZDZ%eez06nMHhozInJP)}BzL$Cj1Gm>yjG_hF# zv!nOeMci94519)l;i_Ygr=SrLpxLXttr)wotD)F=71d{rH-Kok?No>!(U`kKe%hB1 zX8tD-dAF9GjWw=LO~6n6?mcl9dP+VP-sbQSVBu)-0&N zIIfTg$mh29yhG0n&5hwl;K^lTmguscJ9Kyud4R(OOa%m5-9DEjIW(wg() zV|%o)ERY$WT)g^4-k}kYQ_teU4*pnw%~8P7Yd{5ya6L}_^hl9j4X`uG-9tD$0W zE+DW}zK&r2{GuP%9#@I}Me&w5fNtYWMK1Nvn-9t3q3!7mA4{+Qr?TsSYGPa0p?5-) zBE2IZO+=d1fI#T|C@qEnq4%oNB3%$d2~E0yQk9M%0R;sFq*nnc5|mz~`@*@;$0?rs z?)|f7WhRsF+k5^!v)BLqd+j~GvIio#SJ!kmIfB(rS?_Rd*fD=U2$(wFnQJDl8L-GT zT6}c}H8KVrd`x`ajb=2_0iqYw=JLo1axXzZTpR=|vi9S24r2uZfQPyf1^3HHaScP_ zy&@iYSQK5B4`sk(a?G|UB#?-(=gpW3-F$F!$!N-vO`L7#x z`@kQ)hx^a3Mb_nFqD09FMK}}$wPWNsFUr0d-r8cY#G7FV7#Dn`y6-u4vB@83OoD{n&j1 zj%Q1BSxa^*Md*OE8V{CkFtdc1b*vg+{}LyIF2pOED+E~VHV*XIzX_OnUps)f%}H$Xi${`|8{$Ul8iNFy!|S%xEKe44V$~YqBN0{lha)v)Jdc0I~)OG z4IETr$4TRyhQmY8wvmLaxiAOu&^dOR_emRhZIk#&Oz)Hj)MyB6Dze zYwe2Y_))TFpEucX#Kz?T8jC>9Yf+}%Vxcw~lTkLf*oK#vx%JBV=o4#5gmJYJt{Gyf z8;5 z&u^y&v69v%KvhMc+9#ueeteHMixU(|%!N|o_g{DY9E014gk%yzA65HiGj>K$Yh*RK z^EXUC2CuqY4)HfmZeb&=f`G>D1h16z;O9Hggjb>x{AMpIUQ^ z^9cH-zZC;SuFWU+nhnX2rBW_*4tKu~sb3)}TBC8U)l^(KIPmxI5O1&}pQ~1r(~e|w z!7=M**0k*jt8JAz*pd30_Eurdt~^x_SJtK{(w)^|gr%#r_QjR*Y4uhh_}lsDX*Dp*Ui(MoZj|)cm%1DmA8!*b(9Ya&beoat6v!CD z?4BI%^tniKD^j}SNtY*C=C62lN8xfZi4+XU;*NuMW}5w2*&o@;IBnnyDlLP|y^-Qt zfZ|#j%XUCD@>X~xkbN5%$P)4VO|f2%Zo-WsWoIY7R`o8%5e)BeIQBX73+vHAcnlJV zceFAcHm7vtYG+(xg7FP}p$;KCwKKaTa*aR^t(K6f`PzHg%)Kb4K_c|~Qec9WDnbF+ zp^0d;5SC?Ij21&Buq(2$vNDm5OScgsKBOQYC5lDjYsaqK%#wxiwC7h6_aKRfJlGZ8 zKE0agg;|O=-9O$hzf({C-92x1GV>85G@MyUv}h!nonWxwY=#!1siS^#XM_g{yv4kY z4kwC?jI1-PU>3Zcrq@8_rG{#aZMw%MqaxB6*ByDt{PN|?1?<3E@UnjZu_VJgpp6>J zgTd!yh07{DK;K)8n+sb73xS)$nM@OmJK3b1+pTpwT8W-FpVfFU3AGzaqw^lE8k0PG zU`FnZHsS;rNYUrf+}HirsTO>3BVNPhk9w7Ipli2_g2&0b@SkN2N}&CTjY9;Y3rz=a z=C6QI?ats;>xZ*eswG4m`Z#xalPuVhOD3&oAQE0)4w65_$TB`Cs)B``d!=};y5K8G zP?wPShT&MG{x*-UI}@_uMCI<)$_{$*4<5e%1nlL>xxc)@MwI`?{xZ>8RiHaLAjr3A^3ze z>Y@2ODbg5i^_$k5ebA_>K>cBC8I(%2PXQuHXacZpYoK1}#GDK5Rq~g5bjMvENg@iA zK4}QGZ;EaUj*u)@&BEo08q^Pc5~CEd=HvI0*QfH!%FDNayPxNW$@!_5Yp=#=+bzjA zvzJIPtr z#;B6ucS7y-!Z~OlKCiu(@rFO*^Y&$zU8yqy8 ze!5Z^gdcGUl31VwqRP&i3l9%BU#pt0z_SWvz&<1~xG-*aHAW;jb5~SUG`8P4_c8F0 zsh_?{>Wc7zfc@7DVQS7p4{Ru}5q7FZ^@1zzw7I2a%}@^cKxFHrV>_$UnqqKChptkx zW50Sk=H*1NLB5h&QK$iR0~fj2gShq&dJ#T9N-;8;47{W~MWLBAqxcID>1eSqV(cwB zP7G+kIY7VWD&sDDX=bY88~vyqE;^?*1=HZey^huuySCIw$>T6+pESyTBZRvv8MhuW zFjviC7J2Y32{6^1((bMYm9dHrl*ocENG#&yBfo%&J{wH>YAy> zj1hi_(r~4)My17XP1nvl>5sIK`Bgd(5vqB;TwJ;}9HHdev;{Gx&j3lwIyE@27UekE zLTsjO>)AR6b3N_1$oqEqVrTGj$AKeIJFz1_qf%5Dln=$7gx1lzzL`P9Ii+ZcU7S_u zb;wK}Ttr_A_ZEQR_*SA1IoyVvHNpHLxrM4{&Las4^v5EpofxT5S?ZE2P_^+862Gv8 zI=ly3yHDi0W9pja9)X$9ytTD@_Bb_6!0&vI-B zxpN|cE}+svGMwNHZYII^hrGoHMG_~itp_?0Mlo|8g(>*Z=Zad_B#e`Gz`9#t2s7fV zL@2RJ=pXg=-vs!demDb`P>TkCds@bOo;}c1(*;$i*o5hW@$l&YKT{ItXMP$xc1P9C z)edg&;|cMGySaKvh{9bU_AoC|6}Yzx#NA8upTU=Z0d{wD_i=ZDx%&Kd3i&;eZ1~MJpT=j3e{Q5-RmRTQ z4dM;^>9Su{<~$GO_mxav^+aisQ#*}f09PL7^^ zK3d}v4qSIcenYrh-4*?Qy02Mx=fxv(DHI~hR^^!?%%^8{!|3_c3@SlUo^LuJSHhmF zW%ck>xjzpsz?CXe5tyTR+LOKepy6z}APP!9_#02ViHgzEvSSB)y=_r4hiTr#jdv)W zL#D_6il&2t(Y&^;F;&THvb-Ek|KKG_v1YZEaF&nl1x^-^iQ&6Q=?<3ygX*kcCi z+TyPKuZvw-&DHLziMF6>Rv|VS@J3}IxadXtm-4Z%XMy7kp7V!8nBk$p?zyVL9@8YJ za)Die;s*2e!~3&BKD5s?%#QWPq^?-}`86x$u*?q@}xhigLKNjW4QK(T(5^E4pcaw;1)wqkn0-7nXn6Cw^HZMw?AX4SH z-&$+0l6kkHC(F?gvv;e8eua-ZyomMw zLqf~F2ST0eVd!k2las-l?KtG4J7yk&0f|POM@C%F#iO+58ZDHZ&wNW19}=@XrRj}4 zFw5*PR2#xhxfxeryjR6{9jwvRHgbw-sIeEZMO?u#epO+9%58qyXU-`d$In1v&foF) z%@%W;tbCY{c~{H=f&n_eVB%&mLF@YMxhxQgb9Q}`YPJ8Rzdz~fd+K2%tuLb@#k;3R zUh%Ayg#8n980wOtKK*crSX?_{2^CMVz31|mgY6|qVOsy}ka@7xm*KODHMK0}?nd)j zf~^^g7@x_gNe4Ogk(Yb%9k=2bbd;TBTN)Uj@G2$j4d^*!wjb4L`!@E;V`%4H zTs0bhsjMdF_w81tcPyII5k$VFj32Vidq>4qV|s^=O5L{tgMP$mU{}+A8AqH`Y5BsC ztDktXlS?p8aDF0bm$cS7CBm_YeS5E^GEfFAA$-JoO{LiroI(;_&MBG z`{5kWT?$p&)?MVePrx3lo<4|OV0$b^*hWA?ixXtRi%zwE;BuQL#xyB4;*9vlm%cB_ z58GuV~kdAku{Ij<1* z@?BvGmrBKNg$uw=Hm8QI(t$#ivBgQxyTRrU%QNH=Mlq$sEFUs(`4;yvvRyZ{Edp26_zm>4eQH^u@uA4 zKm9m6&go7l%XGGU??i@b{?P|tb+~4`VH#4VxAf$oy>Bp>KO({&`fh`0BPs7A6H~@3a73p)RA43H6R#?RJ=Ebq z}Z2ZQ-?9ZrvNT@}=4oZK6AzRZ_!O$nc#> zALeTb5cY88DL9fQ9+{hj({(iERTtW|Vhq|#?BX%0kp?bHHF$2LplZVg8xzQHTrN?! zwW@Cm&*U$_NH=F(hFlOk`!`NBKhBClmsTav549A!pY^zsIiN|SlAc~0w*B5@Q~%0( ze#=<79HW9sU$sj5VB$oyv^;vKBe0oSIVLMUJg%}fXA=9S(-wU*cXQG*nI2?XSx8|N z8{>f^JttWA-l_r!G%6H^C zt$~(P(Bwne=$8aM{57d>Glw7Gkm!AI>LlA64@TEnu^{H4 zfu+ZpCcfbI)>%V3)3Auso8=;9ZbzGiwl4~K+9N-WB?#Xt5glhCaPcO5;1{ne$H;rD zlIEFJLSbN@mFcJ#x?rd@D6eYUu9W%ut~4r4l}#b-BO>V@@9WNhTI6D>+TE%h0@^~p*>PqRyjMRNj z$W*RW{t^;gyBL-YLBdWtg^l7@_eehr=8S~H!o^V;hAmZk5*%t8b*I#fM8WS=lO_mK z2nuEh-1418=|1z#3(@(9$Iu12_kXApjiC>Med_(DzaCJ2Y(h+vB(#RiToY9W&{0AP z{DWLxOFQVBRWhucI7KqI45% zO?%c~%WsWMTDtXYQ>I((=$TbwXN_S4z7h^U?QjwR;7Imo!1t@r?GiS0(@`^4zN)FK zF6shth16nbG_|6lqMl%P2&^(bHk5@k5CnjYDWb3r2@fvT+ zt;9WplMhH-3cG&W08Y#Q?LZhM>a)%H%w1AeyjndH&+t{5e`MO6_FchBTpcCjdm4H~ zqipcWy@`fMsIPTNoKBbsCs2BtQ(TvVNCJn7Y%l-^%LH?Q*m(V02fLr2T$aU z5Z0aN+wb8v_WQzI;4U!l0C(75QLAO6EjIW7!1?xPlmQlu#X!zu>Yu2eO*m_VN61D1 z05D^f8~H^c*u60 Date: Thu, 22 Jan 2015 08:10:08 +0100 Subject: [PATCH 137/269] multiplatform: introduce PublisherNode class for uorb for more consistency --- src/platforms/px4_nodehandle.h | 6 +++--- src/platforms/px4_publisher.h | 18 +++++++++++++++++- src/platforms/px4_subscriber.h | 3 --- 3 files changed, 20 insertions(+), 7 deletions(-) diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 634e5e5db6..8fafa168a2 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -176,7 +176,7 @@ public: } /* Empty publications list */ - Publisher *pub = _pubs.getHead(); + PublisherNode *pub = _pubs.getHead(); count = 0; while (pub != nullptr) { @@ -185,7 +185,7 @@ public: break; } - Publisher *sib = pub->getSibling(); + PublisherNode *sib = pub->getSibling(); delete pub; pub = sib; } @@ -294,7 +294,7 @@ private: static const uint16_t kMaxSubscriptions = 100; static const uint16_t kMaxPublications = 100; List _subs; /**< Subcriptions of node */ - List _pubs; /**< Publications of node */ + List _pubs; /**< Publications of node */ SubscriberNode *_sub_min_interval; /**< Points to the sub wtih the smallest interval of all Subscriptions in _subs*/ }; diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index 9115545030..7195777f5b 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -88,9 +88,25 @@ private: ros::Publisher _ros_pub; /**< Handle to the ros publisher */ }; #else +/** + * Because we maintain a list of publishers we need a node class + */ +class __EXPORT PublisherNode : + public ListNode +{ +public: + PublisherNode() : + ListNode() + {} + + virtual ~PublisherNode() {} + + virtual void update() = 0; +}; + class __EXPORT Publisher : public PublisherBase, - public ListNode + public PublisherNode { public: diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 2b289771bb..d03b3edee7 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -154,8 +154,6 @@ protected: }; #else // Building for NuttX - - /** * Because we maintain a list of subscribers we need a node class */ @@ -181,7 +179,6 @@ protected: }; - /** * Subscriber class that is templated with the uorb subscription message type */ From 7faef870c8201696372661e063336069d91123b9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 22 Jan 2015 09:24:13 +0100 Subject: [PATCH 138/269] Fix UAVCAN dependency generation issue --- src/lib/uavcan | 2 +- src/modules/uavcan/module.mk | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/lib/uavcan b/src/lib/uavcan index 8966f97013..7719f7692a 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit 8966f970135fb421db31886d6f99ac918594a780 +Subproject commit 7719f7692aba67f01b6321773bb7be13f23d2f68 diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 924b730a23..600cb47f39 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -57,7 +57,7 @@ SRCS += sensors/sensor_bridge.cpp \ # # libuavcan # -include $(PX4_LIB_DIR)/uavcan/libuavcan/include.mk +include $(PX4_LIB_DIR)uavcan/libuavcan/include.mk SRCS += $(LIBUAVCAN_SRC) INCLUDE_DIRS += $(LIBUAVCAN_INC) # Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile @@ -67,7 +67,7 @@ override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAV # # libuavcan drivers for STM32 # -include $(PX4_LIB_DIR)/uavcan/libuavcan_drivers/stm32/driver/include.mk +include $(PX4_LIB_DIR)uavcan/libuavcan_drivers/stm32/driver/include.mk SRCS += $(LIBUAVCAN_STM32_SRC) INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC) override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=2 From 8c4fce3654bbf4cb31314f1fb596f4fd17772589 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 22 Jan 2015 09:30:43 +0100 Subject: [PATCH 139/269] multiplatform: better publisher base class --- makefiles/toolchain_gnu-arm-eabi.mk | 40 ++++++++++---------- src/examples/publisher/publisher_example.h | 3 +- src/platforms/px4_nodehandle.h | 14 +++---- src/platforms/px4_publisher.h | 44 +++++++++++++++------- 4 files changed, 59 insertions(+), 42 deletions(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 3969804535..d38ef645f6 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -125,26 +125,26 @@ ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics # Generic warnings # -ARCHWARNINGS = -Wall \ - -Wextra \ - -Werror \ - -Wdouble-promotion \ - -Wshadow \ - -Wfloat-equal \ - -Wframe-larger-than=1024 \ - -Wpointer-arith \ - -Wlogical-op \ - -Wmissing-declarations \ - -Wpacked \ - -Wno-unused-parameter \ - -Werror=format-security \ - -Werror=array-bounds \ - -Wfatal-errors \ - -Wformat=1 \ - -Werror=unused-but-set-variable \ - -Werror=unused-variable \ - -Werror=double-promotion \ - -Werror=reorder +# ARCHWARNINGS = -Wall \ + # -Wextra \ + # -Werror \ + # -Wdouble-promotion \ + # -Wshadow \ + # -Wfloat-equal \ + # -Wframe-larger-than=1024 \ + # -Wpointer-arith \ + # -Wlogical-op \ + # -Wmissing-declarations \ + # -Wpacked \ + # -Wno-unused-parameter \ + # -Werror=format-security \ + # -Werror=array-bounds \ + # -Wfatal-errors \ + # -Wformat=1 \ + # -Werror=unused-but-set-variable \ + # -Werror=unused-variable \ + # -Werror=double-promotion \ + # -Werror=reorder # -Wcast-qual - generates spurious noreturn attribute warnings, try again later # -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code # -Wcast-align - would help catch bad casts in some cases, but generates too many false positives diff --git a/src/examples/publisher/publisher_example.h b/src/examples/publisher/publisher_example.h index 304ecef47a..4ff59a226d 100644 --- a/src/examples/publisher/publisher_example.h +++ b/src/examples/publisher/publisher_example.h @@ -37,6 +37,7 @@ * * @author Thomas Gubler */ +#pragma once #include class PublisherExample { @@ -48,5 +49,5 @@ public: int main(); protected: px4::NodeHandle _n; - px4::Publisher * _rc_channels_pub; + px4::Publisher * _rc_channels_pub; }; diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 8fafa168a2..a25d578455 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -124,12 +124,12 @@ public: * Advertise topic */ template - Publisher* advertise() + Publisher* advertise() { ros::Publisher ros_pub = ros::NodeHandle::advertisedata())>::type &>(T::handle(), kQueueSizeDefault); - Publisher *pub = new Publisher(ros_pub); - _pubs.push_back(pub); - return pub; + PublisherROS *pub = new PublisherROS(ros_pub); + _pubs.push_back((PublisherBase*)pub); + return (Publisher*)pub; } /** @@ -236,16 +236,16 @@ public: * Advertise topic */ template - Publisher *advertise() + Publisher *advertise() { //XXX // uORB::PublicationBase * uorb_pub = new uORB::PublicationBase((new T())->handle()); uORB::PublicationBase * uorb_pub = new uORB::PublicationBase(T::handle()); - Publisher *pub = new Publisher(uorb_pub); + PublisherUORB *pub = new PublisherUORB(uorb_pub); _pubs.add(pub); - return pub; + return (Publisher*)pub; } /** diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index 7195777f5b..afedbbee77 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -61,29 +61,44 @@ public: ~PublisherBase() {}; }; +/** + * Publisher base class, templated with the message type + * */ +template +class __EXPORT Publisher +{ +public: + Publisher() {}; + ~Publisher() {}; + + virtual int publish(const T &msg) = 0; +}; + #if defined(__PX4_ROS) -class Publisher : - public PublisherBase +template +class PublisherROS : + public Publisher { public: /** * Construct Publisher by providing a ros::Publisher * @param ros_pub the ros publisher which will be used to perform the publications */ - Publisher(ros::Publisher ros_pub) : - PublisherBase(), + PublisherROS(ros::Publisher ros_pub) : + Publisher(), _ros_pub(ros_pub) {} - ~Publisher() {}; + ~PublisherROS() {}; /** Publishes msg * @param msg the message which is published to the topic */ - template - int publish(T &msg) { + int publish(const T &msg) + { _ros_pub.publish(msg.data()); - return 0;} + return 0; + } private: ros::Publisher _ros_pub; /**< Handle to the ros publisher */ }; @@ -104,8 +119,9 @@ public: virtual void update() = 0; }; -class __EXPORT Publisher : - public PublisherBase, +template +class __EXPORT PublisherUORB : + public Publisher, public PublisherNode { @@ -113,17 +129,17 @@ public: /** * Construct Publisher by providing orb meta data */ - Publisher(uORB::PublicationBase * uorb_pub) : - PublisherBase(), + PublisherUORB(uORB::PublicationBase * uorb_pub) : + Publisher(), + PublisherNode(), _uorb_pub(uorb_pub) {} - ~Publisher() {}; + ~PublisherUORB() {}; /** Publishes msg * @param msg the message which is published to the topic */ - template int publish(const T &msg) { _uorb_pub->update((void *)&(msg.data())); From 20a8b26ac8c34ccf906b7775804a59afa1311f19 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 20 Jan 2015 20:40:57 -0500 Subject: [PATCH 140/269] Fixed coverity CID #12538 --- src/modules/navigator/geofence.cpp | 32 +++++++++++++++--------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 4482fb36b7..4fa5806781 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -257,6 +257,7 @@ Geofence::loadFromFile(const char *filename) int pointCounter = 0; bool gotVertical = false; const char commentChar = '#'; + int rc = ERROR; /* Make sure no data is left in the datamanager */ clearDm(); @@ -269,10 +270,10 @@ Geofence::loadFromFile(const char *filename) /* create geofence points from valid lines and store in DM */ for (;;) { - /* get a line, bail on error/EOF */ - if (fgets(line, sizeof(line), fp) == NULL) + if (fgets(line, sizeof(line), fp) == NULL) { break; + } /* Trim leading whitespace */ size_t textStart = 0; @@ -299,7 +300,7 @@ Geofence::loadFromFile(const char *filename) if (sscanf(line, "DMS %f %f %f %f %f %f", &lat_d, &lat_m, &lat_s, &lon_d, &lon_m, &lon_s) != 6) { warnx("Scanf to parse DMS geofence vertex failed."); - return ERROR; + goto error; } // warnx("Geofence DMS: %.5f %.5f %.5f ; %.5f %.5f %.5f", (double)lat_d, (double)lat_m, (double)lat_s, (double)lon_d, (double)lon_m, (double)lon_s); @@ -311,43 +312,42 @@ Geofence::loadFromFile(const char *filename) /* Handle decimal degree format */ if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2) { warnx("Scanf to parse geofence vertex failed."); - return ERROR; + goto error; } } - if (dm_write(DM_KEY_FENCE_POINTS, pointCounter, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) != sizeof(vertex)) - return ERROR; + if (dm_write(DM_KEY_FENCE_POINTS, pointCounter, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) != sizeof(vertex)) { + goto error; + } warnx("Geofence: point: %d, lat %.5f: lon: %.5f", pointCounter, (double)vertex.lat, (double)vertex.lon); pointCounter++; } else { /* Parse the line as the vertical limits */ - if (sscanf(line, "%f %f", &_altitude_min, &_altitude_max) != 2) - return ERROR; - + if (sscanf(line, "%f %f", &_altitude_min, &_altitude_max) != 2) { + goto error; + } warnx("Geofence: alt min: %.4f, alt_max: %.4f", (double)_altitude_min, (double)_altitude_max); gotVertical = true; } - - } - fclose(fp); - /* Check if import was successful */ - if(gotVertical && pointCounter > 0) - { + if (gotVertical && pointCounter > 0) { _verticesCount = pointCounter; warnx("Geofence: imported successfully"); mavlink_log_info(_mavlinkFd, "Geofence imported"); + rc = OK; } else { warnx("Geofence: import error"); mavlink_log_critical(_mavlinkFd, "#audio: Geofence import error"); } - return ERROR; +error: + fclose(fp); + return rc; } int Geofence::clearDm() From ce11cc9f32c1d86e0ebdab3d114517995add2595 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 20 Jan 2015 20:42:01 -0500 Subject: [PATCH 141/269] geofence.cpp format --- src/modules/navigator/geofence.cpp | 84 +++++++++++++++++++----------- 1 file changed, 54 insertions(+), 30 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 4fa5806781..9bc9be245c 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -58,17 +58,17 @@ static const int ERROR = -1; Geofence::Geofence() : - SuperBlock(NULL, "GF"), - _fence_pub(-1), - _altitude_min(0), - _altitude_max(0), - _verticesCount(0), - _param_geofence_on(this, "ON"), - _param_altitude_mode(this, "ALTMODE"), - _param_source(this, "SOURCE"), - _param_counter_threshold(this, "COUNT"), - _outside_counter(0), - _mavlinkFd(-1) + SuperBlock(NULL, "GF"), + _fence_pub(-1), + _altitude_min(0), + _altitude_max(0), + _verticesCount(0), + _param_geofence_on(this, "ON"), + _param_altitude_mode(this, "ALTMODE"), + _param_source(this, "SOURCE"), + _param_counter_threshold(this, "COUNT"), + _outside_counter(0), + _mavlinkFd(-1) { /* Load initial params */ updateParams(); @@ -92,22 +92,26 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position, f bool Geofence::inside(const struct vehicle_global_position_s &global_position, - const struct vehicle_gps_position_s &gps_position,float baro_altitude_amsl) { + const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl) +{ updateParams(); if (getAltitudeMode() == Geofence::GF_ALT_MODE_WGS84) { if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) { return inside(global_position); + } else { return inside((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, - (double)gps_position.alt * 1.0e-3); + (double)gps_position.alt * 1.0e-3); } + } else { if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) { return inside(global_position, baro_altitude_amsl); + } else { return inside((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, - baro_altitude_amsl); + baro_altitude_amsl); } } } @@ -120,9 +124,12 @@ bool Geofence::inside(double lat, double lon, float altitude) _outside_counter = 0; return inside_fence; } { + _outside_counter++; - if(_outside_counter > _param_counter_threshold.get()) { + + if (_outside_counter > _param_counter_threshold.get()) { return inside_fence; + } else { return true; } @@ -133,8 +140,9 @@ bool Geofence::inside(double lat, double lon, float altitude) bool Geofence::inside_polygon(double lat, double lon, float altitude) { /* Return true if geofence is disabled */ - if (_param_geofence_on.get() != 1) + if (_param_geofence_on.get() != 1) { return true; + } if (valid()) { @@ -159,20 +167,22 @@ bool Geofence::inside_polygon(double lat, double lon, float altitude) if (dm_read(DM_KEY_FENCE_POINTS, i, &temp_vertex_i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) { break; } + if (dm_read(DM_KEY_FENCE_POINTS, j, &temp_vertex_j, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) { break; } // skip vertex 0 (return point) if (((double)temp_vertex_i.lon >= lon) != ((double)temp_vertex_j.lon >= lon) && - (lat <= (double)(temp_vertex_j.lat - temp_vertex_i.lat) * (lon - (double)temp_vertex_i.lon) / - (double)(temp_vertex_j.lon - temp_vertex_i.lon) + (double)temp_vertex_i.lat)) { - c = !c; + (lat <= (double)(temp_vertex_j.lat - temp_vertex_i.lat) * (lon - (double)temp_vertex_i.lon) / + (double)(temp_vertex_j.lon - temp_vertex_i.lon) + (double)temp_vertex_i.lat)) { + c = !c; } } return c; + } else { /* Empty fence --> accept all points */ return true; @@ -188,8 +198,9 @@ bool Geofence::valid() { // NULL fence is valid - if (isEmpty()) + if (isEmpty()) { return true; + } // Otherwise if ((_verticesCount < 4) || (_verticesCount > GEOFENCE_MAX_VERTICES)) { @@ -214,26 +225,33 @@ Geofence::addPoint(int argc, char *argv[]) return; } - if (argc < 3) + if (argc < 3) { errx(1, "Specify: -clear | sequence latitude longitude [-publish]"); + } ix = atoi(argv[0]); - if (ix >= DM_KEY_FENCE_POINTS_MAX) + + if (ix >= DM_KEY_FENCE_POINTS_MAX) { errx(1, "Sequence must be less than %d", DM_KEY_FENCE_POINTS_MAX); + } lat = strtod(argv[1], &end); lon = strtod(argv[2], &end); last = 0; - if ((argc > 3) && (strcmp(argv[3], "-publish") == 0)) + + if ((argc > 3) && (strcmp(argv[3], "-publish") == 0)) { last = 1; + } vertex.lat = (float)lat; vertex.lon = (float)lon; if (dm_write(DM_KEY_FENCE_POINTS, ix, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) == sizeof(vertex)) { - if (last) + if (last) { publishFence((unsigned)ix + 1); + } + return; } @@ -243,10 +261,12 @@ Geofence::addPoint(int argc, char *argv[]) void Geofence::publishFence(unsigned vertices) { - if (_fence_pub == -1) + if (_fence_pub == -1) { _fence_pub = orb_advertise(ORB_ID(fence), &vertices); - else + + } else { orb_publish(ORB_ID(fence), _fence_pub, &vertices); + } } int @@ -264,6 +284,7 @@ Geofence::loadFromFile(const char *filename) /* open the mixer definition file */ fp = fopen(GEOFENCE_FILENAME, "r"); + if (fp == NULL) { return ERROR; } @@ -277,7 +298,8 @@ Geofence::loadFromFile(const char *filename) /* Trim leading whitespace */ size_t textStart = 0; - while((textStart < sizeof(line)/sizeof(char)) && isspace(line[textStart])) textStart++; + + while ((textStart < sizeof(line) / sizeof(char)) && isspace(line[textStart])) { textStart++; } /* if the line starts with #, skip */ if (line[textStart] == commentChar) { @@ -305,8 +327,8 @@ Geofence::loadFromFile(const char *filename) // warnx("Geofence DMS: %.5f %.5f %.5f ; %.5f %.5f %.5f", (double)lat_d, (double)lat_m, (double)lat_s, (double)lon_d, (double)lon_m, (double)lon_s); - vertex.lat = lat_d + lat_m/60.0f + lat_s/3600.0f; - vertex.lon = lon_d + lon_m/60.0f + lon_s/3600.0f; + vertex.lat = lat_d + lat_m / 60.0f + lat_s / 3600.0f; + vertex.lon = lon_d + lon_m / 60.0f + lon_s / 3600.0f; } else { /* Handle decimal degree format */ @@ -320,9 +342,10 @@ Geofence::loadFromFile(const char *filename) goto error; } - warnx("Geofence: point: %d, lat %.5f: lon: %.5f", pointCounter, (double)vertex.lat, (double)vertex.lon); + warnx("Geofence: point: %d, lat %.5f: lon: %.5f", pointCounter, (double)vertex.lat, (double)vertex.lon); pointCounter++; + } else { /* Parse the line as the vertical limits */ if (sscanf(line, "%f %f", &_altitude_min, &_altitude_max) != 2) { @@ -340,6 +363,7 @@ Geofence::loadFromFile(const char *filename) warnx("Geofence: imported successfully"); mavlink_log_info(_mavlinkFd, "Geofence imported"); rc = OK; + } else { warnx("Geofence: import error"); mavlink_log_critical(_mavlinkFd, "#audio: Geofence import error"); From 9f32a85409034b2601e42959706a6e0c1a144ce0 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 20 Jan 2015 20:46:56 -0500 Subject: [PATCH 142/269] Fixed coverity CID #12530 --- src/drivers/hmc5883/hmc5883.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 95fbed0ba3..a06be72d9a 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1351,13 +1351,15 @@ start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation) } int fd = open(bus.devpath, O_RDONLY); - if (fd < 0) + if (fd < 0) { return false; + } if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { close(fd); errx(1,"Failed to setup poll rate"); } + close(fd); return true; } From e10d6bf603e8728061465271957486b727387d1f Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 20 Jan 2015 20:56:18 -0500 Subject: [PATCH 143/269] Fixed coverity CID #12521 --- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index dfbf00b664..e5a21651d2 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -435,7 +435,7 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) /* Use distance value for range finder report */ struct range_finder_report r; - memset(&r, 0, sizeof(f)); + memset(&r, 0, sizeof(r)); r.timestamp = hrt_absolute_time(); r.error_count = 0; From 49d41773fc990a6b878543fac2c5cda328bf7d78 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 22 Jan 2015 16:47:28 +0100 Subject: [PATCH 144/269] ros wrapper port callback to methods and subscriber without callback --- .../subscriber/subscriber_example.cpp | 20 +++++---- src/examples/subscriber/subscriber_example.h | 2 +- src/platforms/px4_nodehandle.h | 43 +++++++++---------- src/platforms/px4_subscriber.h | 3 +- 4 files changed, 34 insertions(+), 34 deletions(-) diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index 18f1a9eb9b..217e4d48fe 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -64,10 +64,11 @@ SubscriberExample::SubscriberExample() : /* Function */ _n.subscribe(rc_channels_callback_function); //ROS version - // [> Class Method <] - // PX4_SUBSCRIBE(_n, rc_channels, SubscriberExample::rc_channels_callback, this, 1000); - // [> No callback <] - // _sub_rc_chan = PX4_SUBSCRIBE(_n, rc_channels, 500); + /* No callback */ + _sub_rc_chan = _n.subscribe(); + + /* Class Method */ + _n.subscribe(&SubscriberExample::rc_channels_callback, this); PX4_INFO("subscribed"); } @@ -76,8 +77,9 @@ SubscriberExample::SubscriberExample() : * This tutorial demonstrates simple receipt of messages over the PX4 middleware system. * Also the current value of the _sub_rc_chan subscription is printed */ -// void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) { - // PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]", - // msg.timestamp_last_valid, - // _sub_rc_chan->get().data().timestamp_last_valid); -// } +void SubscriberExample::rc_channels_callback(const px4_rc_channels &msg) { + PX4_INFO("Callback (method): [%llu]", + msg.data().timestamp_last_valid); + PX4_INFO("Callback (method): value of _sub_rc_chan: [%llu]", + _sub_rc_chan->get().data().timestamp_last_valid); +} diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h index 883d83be7b..8da3df4387 100644 --- a/src/examples/subscriber/subscriber_example.h +++ b/src/examples/subscriber/subscriber_example.h @@ -58,7 +58,7 @@ protected: float _test_float; px4::Subscriber * _sub_rc_chan; - // void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg); + void rc_channels_callback(const px4_rc_channels &msg); }; diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index a25d578455..e619623b9d 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -91,34 +91,33 @@ public: /** * Subscribe with callback to class method - * @param topic Name of the topic * @param fb Callback, executed on receiving a new message + * @param obj pointer class instance */ - // template - // Subscriber *subscribe(const char *topic, void(T::*fp)(const M &), T *obj) - // { - // SubscriberBase *sub = new SubscriberROS(std::bind(fp, obj, std::placeholders::_1)); - // ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, - // (SubscriberROS *)sub); - // ((SubscriberROS *)sub)->set_ros_sub(ros_sub); - // _subs.push_back(sub); - // return (Subscriber *)sub; - // } + template + Subscriber *subscribe(void(C::*fp)(const T &), C *obj) + { + SubscriberBase *sub = new SubscriberROS(std::bind(fp, obj, std::placeholders::_1)); + ros::Subscriber ros_sub = ros::NodeHandle::subscribe(T::handle(), kQueueSizeDefault, + &SubscriberROS::callback, (SubscriberROS *)sub); + ((SubscriberROS *)sub)->set_ros_sub(ros_sub); + _subs.push_back(sub); + return (Subscriber *)sub; + } /** * Subscribe with no callback, just the latest value is stored on updates - * @param topic Name of the topic */ - // template - // Subscriber *subscribe(const char *topic) - // { - // SubscriberBase *sub = new SubscriberROS(); - // ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, - // (SubscriberROS *)sub); - // ((SubscriberROS *)sub)->set_ros_sub(ros_sub); - // _subs.push_back(sub); - // return (Subscriber *)sub; - // } + template + Subscriber *subscribe() + { + SubscriberBase *sub = new SubscriberROS(); + ros::Subscriber ros_sub = ros::NodeHandle::subscribe(T::handle(), kQueueSizeDefault, + &SubscriberROS::callback, (SubscriberROS *)sub); + ((SubscriberROS *)sub)->set_ros_sub(ros_sub); + _subs.push_back(sub); + return (Subscriber *)sub; + } /** * Advertise topic diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index d03b3edee7..c499712a96 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -130,11 +130,10 @@ protected: void callback(const typename std::remove_referencedata())>::type &msg) { /* Store data */ - this->_msg_current = (T)msg; + this->_msg_current.data() = msg; /* Call callback */ if (_cbf != NULL) { - // _cbf(_msg_current); _cbf(this->get()); } From b2a911b88d6a541218ef6e633be0d6693de31c8e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 22 Jan 2015 17:10:20 +0100 Subject: [PATCH 145/269] uorb wrapper port callback to methods and subscriber without callback --- src/platforms/px4_nodehandle.h | 53 ++++++++++++++++++++++++---------- 1 file changed, 37 insertions(+), 16 deletions(-) diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index e619623b9d..d78c865de6 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -81,8 +81,7 @@ public: template Subscriber *subscribe(void(*fp)(const T &)) { - SubscriberBase *sub = new SubscriberROS(std::bind(fp, std::placeholders::_1)); - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(T::handle(), kQueueSizeDefault, + SubscriberBase *sub = new SubscriberROS(std::bind(fp, std::placeholders::_1)); ros::Subscriber ros_sub = ros::NodeHandle::subscribe(T::handle(), kQueueSizeDefault, &SubscriberROS::callback, (SubscriberROS *)sub); ((SubscriberROS *)sub)->set_ros_sub(ros_sub); _subs.push_back(sub); @@ -99,7 +98,8 @@ public: { SubscriberBase *sub = new SubscriberROS(std::bind(fp, obj, std::placeholders::_1)); ros::Subscriber ros_sub = ros::NodeHandle::subscribe(T::handle(), kQueueSizeDefault, - &SubscriberROS::callback, (SubscriberROS *)sub); + &SubscriberROS::callback, (SubscriberROS *)sub);//XXX needs cleanup in destructor ore move into class + ((SubscriberROS *)sub)->set_ros_sub(ros_sub); _subs.push_back(sub); return (Subscriber *)sub; @@ -199,7 +199,7 @@ public: template Subscriber *subscribe(void(*fp)(const T &), unsigned interval=10) //XXX interval { - uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(T::handle(), interval); + uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(T::handle(), interval);//XXX needs cleanup in destructor ore move into class SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(uorb_sub, interval, std::bind(fp, std::placeholders::_1)); /* Check if this is the smallest interval so far and update _sub_min_interval */ @@ -211,25 +211,46 @@ public: return (Subscriber *)sub_px4; } + /** + * Subscribe with callback to class method + * @param fb Callback, executed on receiving a new message + * @param obj pointer class instance + */ + template + Subscriber *subscribe(void(C::*fp)(const T &), C *obj, unsigned interval=10) + { + uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(T::handle(), interval);//XXX needs cleanup in destructor ore move into class + SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(uorb_sub, interval, std::bind(fp, obj, std::placeholders::_1)); + + /* Check if this is the smallest interval so far and update _sub_min_interval */ + if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) { + _sub_min_interval = sub_px4; + } + _subs.add((SubscriberNode *)sub_px4); + + return (Subscriber *)sub_px4; + } + /** * Subscribe without callback to function - * @param meta Describes the topic which nodehande should subscribe to * @param interval Minimal interval between data fetches from orb */ - // template - // Subscriber *subscribe(const struct orb_metadata *meta, - // unsigned interval) - // { - // SubscriberUORB *sub_px4 = new SubscriberUORB(meta, interval, &_subs); + template + Subscriber *subscribe(unsigned interval=10) //XXX interval + { + uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(T::handle(), interval);//XXX needs cleanup in destructor ore move into class - // [> Check if this is the smallest interval so far and update _sub_min_interval <] - // if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() > sub_px4->getInterval()) { - // _sub_min_interval = sub_px4; - // } + SubscriberUORB *sub_px4 = new SubscriberUORB(uorb_sub, interval); - // return (Subscriber *)sub_px4; - // } + /* Check if this is the smallest interval so far and update _sub_min_interval */ + if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) { + _sub_min_interval = sub_px4; + } + _subs.add((SubscriberNode *)sub_px4); + + return (Subscriber *)sub_px4; + } /** * Advertise topic From 1e504478a00f08c4d7ab381aa9bec5cdbee5513f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 07:06:02 +0100 Subject: [PATCH 146/269] move ros::subscriber into constructor of subscriber --- src/platforms/px4_nodehandle.h | 15 +++------------ src/platforms/px4_subscriber.h | 29 ++++++++++------------------- 2 files changed, 13 insertions(+), 31 deletions(-) diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index d78c865de6..40604aa86e 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -81,9 +81,7 @@ public: template Subscriber *subscribe(void(*fp)(const T &)) { - SubscriberBase *sub = new SubscriberROS(std::bind(fp, std::placeholders::_1)); ros::Subscriber ros_sub = ros::NodeHandle::subscribe(T::handle(), kQueueSizeDefault, - &SubscriberROS::callback, (SubscriberROS *)sub); - ((SubscriberROS *)sub)->set_ros_sub(ros_sub); + SubscriberBase *sub = new SubscriberROS((ros::NodeHandle*)this, std::bind(fp, std::placeholders::_1)); _subs.push_back(sub); return (Subscriber *)sub; } @@ -96,11 +94,7 @@ public: template Subscriber *subscribe(void(C::*fp)(const T &), C *obj) { - SubscriberBase *sub = new SubscriberROS(std::bind(fp, obj, std::placeholders::_1)); - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(T::handle(), kQueueSizeDefault, - &SubscriberROS::callback, (SubscriberROS *)sub);//XXX needs cleanup in destructor ore move into class - - ((SubscriberROS *)sub)->set_ros_sub(ros_sub); + SubscriberBase *sub = new SubscriberROS((ros::NodeHandle*)this, std::bind(fp, obj, std::placeholders::_1)); _subs.push_back(sub); return (Subscriber *)sub; } @@ -111,10 +105,7 @@ public: template Subscriber *subscribe() { - SubscriberBase *sub = new SubscriberROS(); - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(T::handle(), kQueueSizeDefault, - &SubscriberROS::callback, (SubscriberROS *)sub); - ((SubscriberROS *)sub)->set_ros_sub(ros_sub); + SubscriberBase *sub = new SubscriberROS((ros::NodeHandle*)this); _subs.push_back(sub); return (Subscriber *)sub; } diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index c499712a96..a54b8eb087 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -102,24 +102,23 @@ class SubscriberROS : public: /** - * Construct Subscriber by providing a callback function + * Construct Subscriber without a callback function */ - SubscriberROS(std::function cbf) : - Subscriber(), - _ros_sub(), - _cbf(cbf) + SubscriberROS(ros::NodeHandle *rnh) : + px4::Subscriber(), + _cbf(NULL), + _ros_sub(rnh->subscribe(T::handle(), 1, &SubscriberROS::callback, this)) {} /** - * Construct Subscriber without a callback function + * Construct Subscriber by providing a callback function */ - SubscriberROS() : - Subscriber(), - _ros_sub(), - _cbf(NULL) + //XXX queue default + SubscriberROS(ros::NodeHandle *rnh, std::function cbf) : + _cbf(cbf), + _ros_sub(rnh->subscribe(T::handle(), 1, &SubscriberROS::callback, this)) {} - virtual ~SubscriberROS() {}; protected: @@ -139,14 +138,6 @@ protected: } - /** - * Saves the ros subscriber to keep ros subscription alive - */ - void set_ros_sub(ros::Subscriber ros_sub) - { - _ros_sub = ros_sub; - } - ros::Subscriber _ros_sub; /**< Handle to ros subscriber */ std::function _cbf; /**< Callback that the user provided on the subscription */ From 1ad6e00234e990d44f2a9ca93bcc50696c4c530c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 07:16:07 +0100 Subject: [PATCH 147/269] re-enable warnings/errors --- makefiles/toolchain_gnu-arm-eabi.mk | 40 ++++++++++++++--------------- 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index d38ef645f6..3969804535 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -125,26 +125,26 @@ ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics # Generic warnings # -# ARCHWARNINGS = -Wall \ - # -Wextra \ - # -Werror \ - # -Wdouble-promotion \ - # -Wshadow \ - # -Wfloat-equal \ - # -Wframe-larger-than=1024 \ - # -Wpointer-arith \ - # -Wlogical-op \ - # -Wmissing-declarations \ - # -Wpacked \ - # -Wno-unused-parameter \ - # -Werror=format-security \ - # -Werror=array-bounds \ - # -Wfatal-errors \ - # -Wformat=1 \ - # -Werror=unused-but-set-variable \ - # -Werror=unused-variable \ - # -Werror=double-promotion \ - # -Werror=reorder +ARCHWARNINGS = -Wall \ + -Wextra \ + -Werror \ + -Wdouble-promotion \ + -Wshadow \ + -Wfloat-equal \ + -Wframe-larger-than=1024 \ + -Wpointer-arith \ + -Wlogical-op \ + -Wmissing-declarations \ + -Wpacked \ + -Wno-unused-parameter \ + -Werror=format-security \ + -Werror=array-bounds \ + -Wfatal-errors \ + -Wformat=1 \ + -Werror=unused-but-set-variable \ + -Werror=unused-variable \ + -Werror=double-promotion \ + -Werror=reorder # -Wcast-qual - generates spurious noreturn attribute warnings, try again later # -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code # -Wcast-align - would help catch bad casts in some cases, but generates too many false positives From 2dfd30c25e2839b762ca127fd4af696284df34b9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 07:17:06 +0100 Subject: [PATCH 148/269] move uorb::subscriptionbase into constructor of subscriber --- src/platforms/px4_nodehandle.h | 10 +++------- src/platforms/px4_subscriber.h | 13 +++++++------ 2 files changed, 10 insertions(+), 13 deletions(-) diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 40604aa86e..f2d09c15cb 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -190,8 +190,7 @@ public: template Subscriber *subscribe(void(*fp)(const T &), unsigned interval=10) //XXX interval { - uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(T::handle(), interval);//XXX needs cleanup in destructor ore move into class - SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(uorb_sub, interval, std::bind(fp, std::placeholders::_1)); + SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(interval, std::bind(fp, std::placeholders::_1)); /* Check if this is the smallest interval so far and update _sub_min_interval */ if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) { @@ -210,8 +209,7 @@ public: template Subscriber *subscribe(void(C::*fp)(const T &), C *obj, unsigned interval=10) { - uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(T::handle(), interval);//XXX needs cleanup in destructor ore move into class - SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(uorb_sub, interval, std::bind(fp, obj, std::placeholders::_1)); + SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(interval, std::bind(fp, obj, std::placeholders::_1)); /* Check if this is the smallest interval so far and update _sub_min_interval */ if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) { @@ -230,9 +228,7 @@ public: template Subscriber *subscribe(unsigned interval=10) //XXX interval { - uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(T::handle(), interval);//XXX needs cleanup in destructor ore move into class - - SubscriberUORB *sub_px4 = new SubscriberUORB(uorb_sub, interval); + SubscriberUORB *sub_px4 = new SubscriberUORB(interval); /* Check if this is the smallest interval so far and update _sub_min_interval */ if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) { diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index a54b8eb087..e086cd4c26 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -183,12 +183,14 @@ public: * Construct SubscriberUORB by providing orb meta data without callback * @param interval Minimal interval between calls to callback */ - SubscriberUORB(uORB::SubscriptionBase * uorb_sub, unsigned interval) : + SubscriberUORB(unsigned interval) : SubscriberNode(interval), - _uorb_sub(uorb_sub) + _uorb_sub(new uORB::SubscriptionBase(T::handle(), interval)) {} - virtual ~SubscriberUORB() {}; + virtual ~SubscriberUORB() { + delete _uorb_sub; + }; /** * Update Subscription @@ -233,10 +235,9 @@ public: * @param cbf Callback, executed on receiving a new message * @param interval Minimal interval between calls to callback */ - SubscriberUORBCallback(uORB::SubscriptionBase * uorb_sub, - unsigned interval, + SubscriberUORBCallback(unsigned interval, std::function cbf) : - SubscriberUORB(uorb_sub, interval), + SubscriberUORB(interval), _cbf(cbf) {} From 59f05a7195ae4a2d57e276c31e12bcf6af477408 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 08:03:07 +0100 Subject: [PATCH 149/269] move ros::publisher into constructor of publisher --- src/platforms/px4_nodehandle.h | 4 +--- src/platforms/px4_publisher.h | 7 ++++--- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index f2d09c15cb..c85b8118b3 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -116,8 +116,7 @@ public: template Publisher* advertise() { - ros::Publisher ros_pub = ros::NodeHandle::advertisedata())>::type &>(T::handle(), kQueueSizeDefault); - PublisherROS *pub = new PublisherROS(ros_pub); + PublisherROS *pub = new PublisherROS((ros::NodeHandle*)this); _pubs.push_back((PublisherBase*)pub); return (Publisher*)pub; } @@ -134,7 +133,6 @@ public: private: - static const uint32_t kQueueSizeDefault = 1; /**< Size of queue for ROS */ std::list _subs; /**< Subcriptions of node */ std::list _pubs; /**< Publications of node */ }; diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index afedbbee77..ea675e67be 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -84,9 +84,9 @@ public: * Construct Publisher by providing a ros::Publisher * @param ros_pub the ros publisher which will be used to perform the publications */ - PublisherROS(ros::Publisher ros_pub) : + PublisherROS(ros::NodeHandle *rnh) : Publisher(), - _ros_pub(ros_pub) + _ros_pub(rnh->advertisedata())>::type &>(T::handle(), kQueueSizeDefault)) {} ~PublisherROS() {}; @@ -99,7 +99,8 @@ public: _ros_pub.publish(msg.data()); return 0; } -private: +protected: + static const uint32_t kQueueSizeDefault = 1; /**< Size of queue for ROS */ ros::Publisher _ros_pub; /**< Handle to the ros publisher */ }; #else From 16c85c6d1870f6f410cf75fc0fe3cb386eb9f6ee Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 08:03:26 +0100 Subject: [PATCH 150/269] move uorb::publisherbase into constructor of publisher --- src/platforms/px4_nodehandle.h | 7 +------ src/platforms/px4_publisher.h | 4 ++-- src/platforms/px4_subscriber.h | 7 ++++--- 3 files changed, 7 insertions(+), 11 deletions(-) diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index c85b8118b3..fd1647051a 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -243,13 +243,8 @@ public: template Publisher *advertise() { - //XXX - // uORB::PublicationBase * uorb_pub = new uORB::PublicationBase((new T())->handle()); - uORB::PublicationBase * uorb_pub = new uORB::PublicationBase(T::handle()); - PublisherUORB *pub = new PublisherUORB(uorb_pub); - + PublisherUORB *pub = new PublisherUORB(); _pubs.add(pub); - return (Publisher*)pub; } diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index ea675e67be..a9e40e8d67 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -130,10 +130,10 @@ public: /** * Construct Publisher by providing orb meta data */ - PublisherUORB(uORB::PublicationBase * uorb_pub) : + PublisherUORB() : Publisher(), PublisherNode(), - _uorb_pub(uorb_pub) + _uorb_pub(new uORB::PublicationBase(T::handle())) {} ~PublisherUORB() {}; diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index e086cd4c26..17a95f12d8 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -107,21 +107,22 @@ public: SubscriberROS(ros::NodeHandle *rnh) : px4::Subscriber(), _cbf(NULL), - _ros_sub(rnh->subscribe(T::handle(), 1, &SubscriberROS::callback, this)) + _ros_sub(rnh->subscribe(T::handle(), kQueueSizeDefault, &SubscriberROS::callback, this)) {} /** * Construct Subscriber by providing a callback function */ - //XXX queue default SubscriberROS(ros::NodeHandle *rnh, std::function cbf) : _cbf(cbf), - _ros_sub(rnh->subscribe(T::handle(), 1, &SubscriberROS::callback, this)) + _ros_sub(rnh->subscribe(T::handle(), kQueueSizeDefault, &SubscriberROS::callback, this)) {} virtual ~SubscriberROS() {}; protected: + static const uint32_t kQueueSizeDefault = 1; /**< Size of queue for ROS */ + /** * Called on topic update, saves the current message and then calls the provided callback function * needs to use the native type as it is called by ROS From 57569482ad5739bb4471826014818c92bdb33c6d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 08:16:07 +0100 Subject: [PATCH 151/269] remove unneeded friend --- src/platforms/px4_subscriber.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 17a95f12d8..6a81ef8d22 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -98,8 +98,6 @@ template class SubscriberROS : public Subscriber { - friend class NodeHandle; - public: /** * Construct Subscriber without a callback function From af943cf16adb79ab33097bd560a85fee203a5215 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 08:27:06 +0100 Subject: [PATCH 152/269] add update_sub_min_interval function --- src/platforms/px4_nodehandle.h | 36 +++++++++++++++------------------- 1 file changed, 16 insertions(+), 20 deletions(-) diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index fd1647051a..2f92ead5a1 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -132,7 +132,7 @@ public: void spin() { ros::spin(); } -private: +protected: std::list _subs; /**< Subcriptions of node */ std::list _pubs; /**< Publications of node */ }; @@ -189,13 +189,8 @@ public: Subscriber *subscribe(void(*fp)(const T &), unsigned interval=10) //XXX interval { SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(interval, std::bind(fp, std::placeholders::_1)); - - /* Check if this is the smallest interval so far and update _sub_min_interval */ - if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) { - _sub_min_interval = sub_px4; - } + update_sub_min_interval(interval, sub_px4); _subs.add((SubscriberNode *)sub_px4); - return (Subscriber *)sub_px4; } @@ -208,13 +203,8 @@ public: Subscriber *subscribe(void(C::*fp)(const T &), C *obj, unsigned interval=10) { SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(interval, std::bind(fp, obj, std::placeholders::_1)); - - /* Check if this is the smallest interval so far and update _sub_min_interval */ - if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) { - _sub_min_interval = sub_px4; - } + update_sub_min_interval(interval, sub_px4); _subs.add((SubscriberNode *)sub_px4); - return (Subscriber *)sub_px4; } @@ -227,13 +217,8 @@ public: Subscriber *subscribe(unsigned interval=10) //XXX interval { SubscriberUORB *sub_px4 = new SubscriberUORB(interval); - - /* Check if this is the smallest interval so far and update _sub_min_interval */ - if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) { - _sub_min_interval = sub_px4; - } + update_sub_min_interval(interval, sub_px4); _subs.add((SubscriberNode *)sub_px4); - return (Subscriber *)sub_px4; } @@ -290,13 +275,24 @@ public: spinOnce(); } } -private: +protected: static const uint16_t kMaxSubscriptions = 100; static const uint16_t kMaxPublications = 100; List _subs; /**< Subcriptions of node */ List _pubs; /**< Publications of node */ SubscriberNode *_sub_min_interval; /**< Points to the sub wtih the smallest interval of all Subscriptions in _subs*/ + + /** + * Check if this is the smallest interval so far and update _sub_min_interval + */ + template + void update_sub_min_interval(unsigned interval, SubscriberUORB *sub) + { + if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) { + _sub_min_interval = sub; + } + } }; #endif } From da7ad9a3329db43144b5ca3e60c6c56d22fdc3d4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 09:02:04 +0100 Subject: [PATCH 153/269] multiplatform: re-enable interval functionality (for uorb) --- src/examples/subscriber/subscriber_example.cpp | 6 +++--- src/platforms/px4_nodehandle.h | 15 +++++++++------ 2 files changed, 12 insertions(+), 9 deletions(-) diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index 217e4d48fe..215336c174 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -62,13 +62,13 @@ SubscriberExample::SubscriberExample() : /* Do some subscriptions */ /* Function */ - _n.subscribe(rc_channels_callback_function); //ROS version + _n.subscribe(rc_channels_callback_function, _interval); /* No callback */ - _sub_rc_chan = _n.subscribe(); + _sub_rc_chan = _n.subscribe(500); /* Class Method */ - _n.subscribe(&SubscriberExample::rc_channels_callback, this); + _n.subscribe(&SubscriberExample::rc_channels_callback, this, 1000); PX4_INFO("subscribed"); } diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 2f92ead5a1..80be9ec527 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -79,7 +79,7 @@ public: * @param fb Callback, executed on receiving a new message */ template - Subscriber *subscribe(void(*fp)(const T &)) + Subscriber *subscribe(void(*fp)(const T &), unsigned interval) { SubscriberBase *sub = new SubscriberROS((ros::NodeHandle*)this, std::bind(fp, std::placeholders::_1)); _subs.push_back(sub); @@ -92,7 +92,7 @@ public: * @param obj pointer class instance */ template - Subscriber *subscribe(void(C::*fp)(const T &), C *obj) + Subscriber *subscribe(void(C::*fp)(const T &), C *obj, unsigned interval) { SubscriberBase *sub = new SubscriberROS((ros::NodeHandle*)this, std::bind(fp, obj, std::placeholders::_1)); _subs.push_back(sub); @@ -103,7 +103,7 @@ public: * Subscribe with no callback, just the latest value is stored on updates */ template - Subscriber *subscribe() + Subscriber *subscribe(unsigned interval) { SubscriberBase *sub = new SubscriberROS((ros::NodeHandle*)this); _subs.push_back(sub); @@ -186,8 +186,9 @@ public: */ template - Subscriber *subscribe(void(*fp)(const T &), unsigned interval=10) //XXX interval + Subscriber *subscribe(void(*fp)(const T &), unsigned interval) { + (void)interval; SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(interval, std::bind(fp, std::placeholders::_1)); update_sub_min_interval(interval, sub_px4); _subs.add((SubscriberNode *)sub_px4); @@ -200,8 +201,9 @@ public: * @param obj pointer class instance */ template - Subscriber *subscribe(void(C::*fp)(const T &), C *obj, unsigned interval=10) + Subscriber *subscribe(void(C::*fp)(const T &), C *obj, unsigned interval) { + (void)interval; SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(interval, std::bind(fp, obj, std::placeholders::_1)); update_sub_min_interval(interval, sub_px4); _subs.add((SubscriberNode *)sub_px4); @@ -214,8 +216,9 @@ public: */ template - Subscriber *subscribe(unsigned interval=10) //XXX interval + Subscriber *subscribe(unsigned interval) { + (void)interval; SubscriberUORB *sub_px4 = new SubscriberUORB(interval); update_sub_min_interval(interval, sub_px4); _subs.add((SubscriberNode *)sub_px4); From c33173cd5de81a9fb32757c3a3c662716f76b300 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 09:23:29 +0100 Subject: [PATCH 154/269] uorb pub: cleanup objects --- src/platforms/px4_publisher.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index a9e40e8d67..d9cd7a3c19 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -136,7 +136,9 @@ public: _uorb_pub(new uORB::PublicationBase(T::handle())) {} - ~PublisherUORB() {}; + ~PublisherUORB() { + delete _uorb_pub; + }; /** Publishes msg * @param msg the message which is published to the topic From 6f7fa3b4e7e6fd387530a60c800992c5d7bab87b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 10:33:18 +0100 Subject: [PATCH 155/269] header generation script: add option to set output filename prefix --- Tools/px_generate_uorb_topic_headers.py | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/Tools/px_generate_uorb_topic_headers.py b/Tools/px_generate_uorb_topic_headers.py index 2ddbd69847..54430cc38d 100755 --- a/Tools/px_generate_uorb_topic_headers.py +++ b/Tools/px_generate_uorb_topic_headers.py @@ -60,7 +60,7 @@ def convert_file(filename, outputdir, templatedir, includepath): """ Converts a single .msg file to a uorb header """ - print("Generating uORB headers from {0}".format(filename)) + print("Generating headers from {0}".format(filename)) genmsg.template_tools.generate_from_file(filename, package, outputdir, @@ -85,7 +85,7 @@ def convert_dir(inputdir, outputdir, templatedir): includepath) -def copy_changed(inputdir, outputdir): +def copy_changed(inputdir, outputdir, prefix=''): """ Copies files from inputdir to outputdir if they don't exist in ouputdir or if their content changed @@ -94,7 +94,7 @@ def copy_changed(inputdir, outputdir): fni = os.path.join(inputdir, f) if os.path.isfile(fni): # Check if f exists in outpoutdir, copy the file if not - fno = os.path.join(outputdir, f) + fno = os.path.join(outputdir, prefix + f) if not os.path.isfile(fno): shutil.copy(fni, fno) print("{0}: new header file".format(f)) @@ -108,7 +108,8 @@ def copy_changed(inputdir, outputdir): print("{0}: unchanged".format(f)) -def convert_dir_save(inputdir, outputdir, templatedir, temporarydir): + +def convert_dir_save(inputdir, outputdir, templatedir, temporarydir, prefix): """ Converts all .msg files in inputdir to uORB header files Unchanged existing files are not overwritten. @@ -117,7 +118,7 @@ def convert_dir_save(inputdir, outputdir, templatedir, temporarydir): convert_dir(inputdir, temporarydir, templatedir) # Copy changed headers from temporary dir to output dir - copy_changed(temporarydir, outputdir) + copy_changed(temporarydir, outputdir, prefix) if __name__ == "__main__": parser = argparse.ArgumentParser( @@ -132,6 +133,9 @@ if __name__ == "__main__": help='output directory for header files') parser.add_argument('-t', dest='temporarydir', help='temporary directory') + parser.add_argument('-p', dest='prefix', default='', + help='string added as prefix to the output file ' + ' name when converting directories') args = parser.parse_args() if args.file is not None: @@ -146,4 +150,5 @@ if __name__ == "__main__": args.dir, args.outputdir, args.templatedir, - args.temporarydir) + args.temporarydir, + args.prefix) From 738f65a705c5dd2e664fedf54fd1064f685ff5c7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 10:34:04 +0100 Subject: [PATCH 156/269] generate wrapper headers for uorb --- .gitignore | 1 + Makefile | 15 +- msg/templates/px4/msg.h.template | 95 +++++++++++ msg/templates/uorb/msg.h.template | 159 ++++++++++++++++++ .../nuttx/px4_messages/px4_rc_channels.h | 26 --- 5 files changed, 266 insertions(+), 30 deletions(-) create mode 100644 msg/templates/px4/msg.h.template create mode 100644 msg/templates/uorb/msg.h.template delete mode 100644 src/platforms/nuttx/px4_messages/px4_rc_channels.h diff --git a/.gitignore b/.gitignore index 1c4f49fdea..764be0029f 100644 --- a/.gitignore +++ b/.gitignore @@ -40,6 +40,7 @@ tags .ropeproject *.orig src/modules/uORB/topics/* +src/platforms/nuttx/px4_messages/* Firmware.zip unittests/build *.generated.h diff --git a/Makefile b/Makefile index 6480286d2d..bffba69aed 100644 --- a/Makefile +++ b/Makefile @@ -225,9 +225,12 @@ updatesubmodules: $(Q) (git submodule update) MSG_DIR = $(PX4_BASE)msg -MSG_TEMPLATE_DIR = $(PX4_BASE)msg/templates +UORB_TEMPLATE_DIR = $(PX4_BASE)msg/templates/uorb +MULTIPLATFORM_TEMPLATE_DIR = $(PX4_BASE)msg/templates/px4 TOPICS_DIR = $(PX4_BASE)src/modules/uORB/topics -TOPICS_TEMPORARY_DIR = $(BUILD_DIR)topics_temporary +MULTIPLATFORM_HEADER_DIR = $(PX4_BASE)src/platforms/nuttx/px4_messages +MULTIPLATFORM_PREFIX = px4_ +TOPICHEADER_TEMP_DIR = $(BUILD_DIR)topics_temporary GENMSG_PYTHONPATH = $(PX4_BASE)/Tools/genmsg/src GENCPP_PYTHONPATH = $(PX4_BASE)/Tools/gencpp/src @@ -236,9 +239,13 @@ generateuorbtopicheaders: @$(ECHO) "Generating uORB topic headers" $(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH) $(PYTHON) \ $(PX4_BASE)Tools/px_generate_uorb_topic_headers.py \ - -d $(MSG_DIR) -o $(TOPICS_DIR) -e $(MSG_TEMPLATE_DIR) -t $(TOPICS_TEMPORARY_DIR)) + -d $(MSG_DIR) -o $(TOPICS_DIR) -e $(UORB_TEMPLATE_DIR) -t $(TOPICHEADER_TEMP_DIR)) + @$(ECHO) "Generating multiplatform uORB topic wrapper headers" + $(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH) $(PYTHON) \ + $(PX4_BASE)Tools/px_generate_uorb_topic_headers.py \ + -d $(MSG_DIR) -o $(MULTIPLATFORM_HEADER_DIR) -e $(MULTIPLATFORM_TEMPLATE_DIR) -t $(TOPICHEADER_TEMP_DIR) -p $(MULTIPLATFORM_PREFIX)) # clean up temporary files - $(Q) (rm -r $(TOPICS_TEMPORARY_DIR)) + $(Q) (rm -r $(TOPICHEADER_TEMP_DIR)) # # Testing targets diff --git a/msg/templates/px4/msg.h.template b/msg/templates/px4/msg.h.template new file mode 100644 index 0000000000..ba0fbd4bfe --- /dev/null +++ b/msg/templates/px4/msg.h.template @@ -0,0 +1,95 @@ +@############################################### +@# +@# PX4 ROS compatible message source code +@# generation for C++ +@# +@# This file generates the multiplatform wrapper +@# +@# EmPy template for generating .h files +@# Based on the original template for ROS +@# +@############################################### +@# Start of Template +@# +@# Context: +@# - file_name_in (String) Source file +@# - spec (msggen.MsgSpec) Parsed specification of the .msg file +@# - md5sum (String) MD5Sum of the .msg specification +@############################################### +/**************************************************************************** + * + * Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* Auto-generated by genmsg_cpp from file @file_name_in */ + +@{ +import genmsg.msgs +import gencpp + +cpp_class = 'px4_%s'%spec.short_name +uorb_struct = '%s_s'%spec.short_name +topic_name = spec.short_name +}@ + +#pragma once + +@############################## +@# Generic Includes +@############################## +#include +#include "platforms/px4_message.h" + +@############################## +@# Class +@############################## + +namespace px4 +{ + +class __EXPORT @(cpp_class) : + public PX4Message<@(uorb_struct)> +{ +public: + @(cpp_class)() : + PX4Message<@(uorb_struct)>() + {} + + @(cpp_class)(@(uorb_struct) msg) : + PX4Message<@(uorb_struct)>(msg) + {} + + ~@(cpp_class)() {} + + static PX4TopicHandle handle() {return ORB_ID(@(topic_name));} +}; + +}; diff --git a/msg/templates/uorb/msg.h.template b/msg/templates/uorb/msg.h.template new file mode 100644 index 0000000000..11cb1ea2ae --- /dev/null +++ b/msg/templates/uorb/msg.h.template @@ -0,0 +1,159 @@ +@############################################### +@# +@# PX4 ROS compatible message source code +@# generation for C++ +@# +@# EmPy template for generating .h files +@# Based on the original template for ROS +@# +@############################################### +@# Start of Template +@# +@# Context: +@# - file_name_in (String) Source file +@# - spec (msggen.MsgSpec) Parsed specification of the .msg file +@# - md5sum (String) MD5Sum of the .msg specification +@############################################### +/**************************************************************************** + * + * Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + /* Auto-generated by genmsg_cpp from file @file_name_in */ + +@{ +import genmsg.msgs +import gencpp + +cpp_namespace = '::%s::'%(spec.package) # TODO handle nested namespace +cpp_class = '%s_'%spec.short_name +cpp_full_name = '%s%s'%(cpp_namespace,cpp_class) +cpp_full_name_with_alloc = '%s'%(cpp_full_name) +cpp_msg_definition = gencpp.escape_message_definition(msg_definition) +}@ + +#pragma once + +@############################## +@# Generic Includes +@############################## +#include +#include + +@############################## +@# Includes for dependencies +@############################## +@{ +for field in spec.parsed_fields(): + if (not field.is_builtin): + if (not field.is_header): + (package, name) = genmsg.names.package_resource_name(field.base_type) + package = package or spec.package # convert '' to package + print('#include '%(name)) + +}@ +@# Constants +@[for constant in spec.constants]@ +#define @(constant.name) @(int(constant.val)) +@[end for] + +/** + * @@addtogroup topics + * @@{ + */ + +@############################## +@# Main struct of message +@############################## +@{ + +type_map = {'int8': 'int8_t', + 'int16': 'int16_t', + 'int32': 'int32_t', + 'int64': 'int64_t', + 'uint8': 'uint8_t', + 'uint16': 'uint16_t', + 'uint32': 'uint32_t', + 'uint64': 'uint64_t', + 'float32': 'float', + 'bool': 'bool', + 'fence_vertex': 'fence_vertex'} + +# Function to print a standard ros type +def print_field_def(field): + type = field.type + # detect embedded types + sl_pos = type.find('/') + type_appendix = '' + type_prefix = '' + if (sl_pos >= 0): + type = type[sl_pos + 1:] + type_prefix = 'struct ' + type_appendix = '_s' + + # detect arrays + a_pos = type.find('[') + array_size = '' + if (a_pos >= 0): + # field is array + array_size = type[a_pos:] + type = type[:a_pos] + + if type in type_map: + # need to add _t: int8 --> int8_t + type_px4 = type_map[type] + else: + raise Exception("Type {0} not supported, add to to template file!".format(type)) + + print('\t%s%s%s %s%s;'%(type_prefix, type_px4, type_appendix, field.name, array_size)) + +} +#ifdef __cplusplus +@#class @(spec.short_name)_s { +struct __EXPORT @(spec.short_name)_s { +@#public: +#else +struct @(spec.short_name)_s { +#endif +@{ +# loop over all fields and print the type and name +for field in spec.parsed_fields(): + if (not field.is_header): + print_field_def(field) +}@ +}; + +/** + * @@} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(@(spec.short_name)); diff --git a/src/platforms/nuttx/px4_messages/px4_rc_channels.h b/src/platforms/nuttx/px4_messages/px4_rc_channels.h deleted file mode 100644 index 2ce7bfc80a..0000000000 --- a/src/platforms/nuttx/px4_messages/px4_rc_channels.h +++ /dev/null @@ -1,26 +0,0 @@ -#include -#include -#include "platforms/px4_message.h" - -#pragma once -namespace px4 -{ - -class __EXPORT px4_rc_channels : - public PX4Message -{ -public: - px4_rc_channels() : - PX4Message() - {} - - px4_rc_channels(rc_channels_s msg) : - PX4Message(msg) - {} - - ~px4_rc_channels() {} - - static PX4TopicHandle handle() {return ORB_ID(rc_channels);} -}; - -} From 5e9bef6f4b710aae319bd60068107bff41003fd8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 10:37:17 +0100 Subject: [PATCH 157/269] cleanup template for normal uorb headers --- msg/templates/uorb/msg.h.template | 17 +++++++---------- 1 file changed, 7 insertions(+), 10 deletions(-) diff --git a/msg/templates/uorb/msg.h.template b/msg/templates/uorb/msg.h.template index 11cb1ea2ae..622641617f 100644 --- a/msg/templates/uorb/msg.h.template +++ b/msg/templates/uorb/msg.h.template @@ -47,17 +47,14 @@ * ****************************************************************************/ - /* Auto-generated by genmsg_cpp from file @file_name_in */ +/* Auto-generated by genmsg_cpp from file @file_name_in */ @{ import genmsg.msgs import gencpp -cpp_namespace = '::%s::'%(spec.package) # TODO handle nested namespace -cpp_class = '%s_'%spec.short_name -cpp_full_name = '%s%s'%(cpp_namespace,cpp_class) -cpp_full_name_with_alloc = '%s'%(cpp_full_name) -cpp_msg_definition = gencpp.escape_message_definition(msg_definition) +uorb_struct = '%s_s'%spec.short_name +topic_name = spec.short_name }@ #pragma once @@ -137,11 +134,11 @@ def print_field_def(field): } #ifdef __cplusplus -@#class @(spec.short_name)_s { -struct __EXPORT @(spec.short_name)_s { +@#class @(uorb_struct) { +struct __EXPORT @(uorb_struct) { @#public: #else -struct @(spec.short_name)_s { +struct @(uorb_struct) { #endif @{ # loop over all fields and print the type and name @@ -156,4 +153,4 @@ for field in spec.parsed_fields(): */ /* register this as object request broker structure */ -ORB_DECLARE(@(spec.short_name)); +ORB_DECLARE(@(topic_name)); From 4ba57ad285884a2b01ebf8aac2c710ed63f7ffd3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 11:39:10 +0100 Subject: [PATCH 158/269] get rid of unnecessary defines --- src/platforms/px4_defines.h | 1 - src/platforms/px4_middleware.h | 4 ---- 2 files changed, 5 deletions(-) diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 6b6a03893b..325832f0f1 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -50,7 +50,6 @@ /* * Building for running within the ROS environment */ -#define __EXPORT #define noreturn_function #ifdef __cplusplus #include "ros/ros.h" diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h index cd52fd650b..735d34234a 100644 --- a/src/platforms/px4_middleware.h +++ b/src/platforms/px4_middleware.h @@ -42,10 +42,6 @@ #include #include -#if defined(__PX4_ROS) -#define __EXPORT -#endif - namespace px4 { From d7e57061c976ba18c69d8be9949660e85f645126 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 11:40:23 +0100 Subject: [PATCH 159/269] generate message wrapper headers on ros --- .gitignore | 1 + CMakeLists.txt | 16 +++- Makefile | 6 +- msg/templates/px4/ros/msg.h.template | 95 +++++++++++++++++++ msg/templates/px4/{ => uorb}/msg.h.template | 10 +- .../ros/px4_messages/px4_rc_channels.h | 25 ----- 6 files changed, 117 insertions(+), 36 deletions(-) create mode 100644 msg/templates/px4/ros/msg.h.template rename msg/templates/px4/{ => uorb}/msg.h.template (94%) delete mode 100644 src/platforms/ros/px4_messages/px4_rc_channels.h diff --git a/.gitignore b/.gitignore index 764be0029f..1c3f8fe047 100644 --- a/.gitignore +++ b/.gitignore @@ -41,6 +41,7 @@ tags *.orig src/modules/uORB/topics/* src/platforms/nuttx/px4_messages/* +src/platforms/ros/px4_messages/* Firmware.zip unittests/build *.generated.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 5488dbe9db..25822d7190 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -125,6 +125,16 @@ include_directories( ${EIGEN_INCLUDE_DIRS} ) +## generate multiplatform wrapper headers +## note that the message header files are generated as in any ROS project with generate_messages() +set(MULTIPLATFORM_HEADER_DIR ${CMAKE_CURRENT_SOURCE_DIR}/src/platforms/ros/px4_messages) +set(MULTIPLATFORM_TEMPLATE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/msg/templates/px4/ros) +set(TOPICHEADER_TEMP_DIR ${CMAKE_BINARY_DIR}/topics_temporary) +set(MULTIPLATFORM_PREFIX px4_) +add_custom_target(multiplatform_message_headers ALL ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/Tools/px_generate_uorb_topic_headers.py + -d ${CMAKE_CURRENT_SOURCE_DIR}/msg -o ${MULTIPLATFORM_HEADER_DIR} -e ${MULTIPLATFORM_TEMPLATE_DIR} + -t ${TOPICHEADER_TEMP_DIR} -p ${MULTIPLATFORM_PREFIX}) + ## Declare a cpp library add_library(px4 src/platforms/ros/px4_ros_impl.cpp @@ -133,7 +143,7 @@ add_library(px4 src/lib/mathlib/math/Limits.cpp src/modules/systemlib/circuit_breaker.cpp ) -add_dependencies(px4 ${PROJECT_NAME}_generate_messages_cpp) +add_dependencies(px4 ${PROJECT_NAME}_generate_messages_cpp multiplatform_message_headers) target_link_libraries(px4 ${catkin_LIBRARIES} @@ -143,7 +153,7 @@ target_link_libraries(px4 add_executable(publisher src/examples/publisher/publisher_main.cpp src/examples/publisher/publisher_example.cpp) -add_dependencies(publisher ${PROJECT_NAME}_generate_messages_cpp) +add_dependencies(publisher ${PROJECT_NAME}_generate_messages_cpp multiplatform_message_headers) target_link_libraries(publisher ${catkin_LIBRARIES} px4 @@ -153,7 +163,7 @@ target_link_libraries(publisher add_executable(subscriber src/examples/subscriber/subscriber_main.cpp src/examples/subscriber/subscriber_example.cpp) -add_dependencies(subscriber ${PROJECT_NAME}_generate_messages_cpp) +add_dependencies(subscriber ${PROJECT_NAME}_generate_messages_cpp multiplatform_message_headers) target_link_libraries(subscriber ${catkin_LIBRARIES} px4 diff --git a/Makefile b/Makefile index bffba69aed..c727c9a697 100644 --- a/Makefile +++ b/Makefile @@ -226,13 +226,13 @@ updatesubmodules: MSG_DIR = $(PX4_BASE)msg UORB_TEMPLATE_DIR = $(PX4_BASE)msg/templates/uorb -MULTIPLATFORM_TEMPLATE_DIR = $(PX4_BASE)msg/templates/px4 +MULTIPLATFORM_TEMPLATE_DIR = $(PX4_BASE)msg/templates/px4/uorb TOPICS_DIR = $(PX4_BASE)src/modules/uORB/topics MULTIPLATFORM_HEADER_DIR = $(PX4_BASE)src/platforms/nuttx/px4_messages MULTIPLATFORM_PREFIX = px4_ TOPICHEADER_TEMP_DIR = $(BUILD_DIR)topics_temporary -GENMSG_PYTHONPATH = $(PX4_BASE)/Tools/genmsg/src -GENCPP_PYTHONPATH = $(PX4_BASE)/Tools/gencpp/src +GENMSG_PYTHONPATH = $(PX4_BASE)Tools/genmsg/src +GENCPP_PYTHONPATH = $(PX4_BASE)Tools/gencpp/src .PHONY: generateuorbtopicheaders generateuorbtopicheaders: diff --git a/msg/templates/px4/ros/msg.h.template b/msg/templates/px4/ros/msg.h.template new file mode 100644 index 0000000000..176be0d091 --- /dev/null +++ b/msg/templates/px4/ros/msg.h.template @@ -0,0 +1,95 @@ +@############################################### +@# +@# PX4 ROS compatible message source code +@# generation for C++ +@# +@# This file generates the multiplatform wrapper +@# +@# EmPy template for generating .h files +@# Based on the original template for ROS +@# +@############################################### +@# Start of Template +@# +@# Context: +@# - file_name_in (String) Source file +@# - spec (msggen.MsgSpec) Parsed specification of the .msg file +@# - md5sum (String) MD5Sum of the .msg specification +@############################################### +/**************************************************************************** + * + * Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* Auto-generated by genmsg_cpp from file @file_name_in */ + +@{ +import genmsg.msgs +import gencpp + +cpp_class = 'px4_%s'%spec.short_name +native_type = spec.short_name +topic_name = spec.short_name +}@ + +#pragma once + +@############################## +@# Generic Includes +@############################## +#include +#include "platforms/px4_message.h" + +@############################## +@# Class +@############################## + +namespace px4 +{ + +class @(cpp_class) : + public PX4Message<@(native_type)> +{ +public: + @(cpp_class)() : + PX4Message<@(native_type)>() + {} + + @(cpp_class)(@(native_type) msg) : + PX4Message<@(native_type)>(msg) + {} + + ~@(cpp_class)() {} + + static PX4TopicHandle handle() {return "@(topic_name)";} +}; + +}; diff --git a/msg/templates/px4/msg.h.template b/msg/templates/px4/uorb/msg.h.template similarity index 94% rename from msg/templates/px4/msg.h.template rename to msg/templates/px4/uorb/msg.h.template index ba0fbd4bfe..2d42511074 100644 --- a/msg/templates/px4/msg.h.template +++ b/msg/templates/px4/uorb/msg.h.template @@ -56,7 +56,7 @@ import genmsg.msgs import gencpp cpp_class = 'px4_%s'%spec.short_name -uorb_struct = '%s_s'%spec.short_name +native_type = '%s_s'%spec.short_name topic_name = spec.short_name }@ @@ -76,15 +76,15 @@ namespace px4 { class __EXPORT @(cpp_class) : - public PX4Message<@(uorb_struct)> + public PX4Message<@(native_type)> { public: @(cpp_class)() : - PX4Message<@(uorb_struct)>() + PX4Message<@(native_type)>() {} - @(cpp_class)(@(uorb_struct) msg) : - PX4Message<@(uorb_struct)>(msg) + @(cpp_class)(@(native_type) msg) : + PX4Message<@(native_type)>(msg) {} ~@(cpp_class)() {} diff --git a/src/platforms/ros/px4_messages/px4_rc_channels.h b/src/platforms/ros/px4_messages/px4_rc_channels.h deleted file mode 100644 index 5e9dc72cda..0000000000 --- a/src/platforms/ros/px4_messages/px4_rc_channels.h +++ /dev/null @@ -1,25 +0,0 @@ -#include "px4/rc_channels.h" -#include "platforms/px4_message.h" - -#pragma once -namespace px4 -{ - -class px4_rc_channels : - public PX4Message -{ -public: - px4_rc_channels() : - PX4Message() - {} - - px4_rc_channels(rc_channels msg) : - PX4Message(msg) - {} - - ~px4_rc_channels() {} - - static PX4TopicHandle handle() {return "rc_channels";} -}; - -} From f55832a37066c2cb0e6db64d95710b6fa0017fb1 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 14:38:39 +0100 Subject: [PATCH 160/269] multiplatform: update topic includes --- src/platforms/px4_includes.h | 49 ++++++++++++++++++------------------ 1 file changed, 24 insertions(+), 25 deletions(-) diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index bdb2b8de9b..90ec789960 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -48,21 +48,20 @@ #ifdef __cplusplus #include "ros/ros.h" -#include "px4/rc_channels.h" -#include "px4_rc_channels.h" //XXX -#include "px4/vehicle_attitude.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #endif #else @@ -73,17 +72,17 @@ #include #include #ifdef __cplusplus -#include //XXX +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #endif -#include -#include -#include -#include -#include -#include -#include -#include -#include #include #include #include From 1ab9ec5811a5b945ca49a8e6aeef59557dd2970c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 14:38:53 +0100 Subject: [PATCH 161/269] fix ros header template --- msg/templates/px4/ros/msg.h.template | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/msg/templates/px4/ros/msg.h.template b/msg/templates/px4/ros/msg.h.template index 176be0d091..76abab163d 100644 --- a/msg/templates/px4/ros/msg.h.template +++ b/msg/templates/px4/ros/msg.h.template @@ -65,7 +65,7 @@ topic_name = spec.short_name @############################## @# Generic Includes @############################## -#include +#include "px4/@(topic_name).h" #include "platforms/px4_message.h" @############################## From 193a210b517c958ebce3aaae8cc9b5709ff9b52b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 Jan 2015 14:32:29 +0100 Subject: [PATCH 162/269] Multi sonar support by jverbeke --- src/drivers/drv_range_finder.h | 3 + src/drivers/mb12xx/mb12xx.cpp | 189 ++++++++++++++++++++++++++------- 2 files changed, 152 insertions(+), 40 deletions(-) diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h index 0f8362f588..12d51aeaad 100644 --- a/src/drivers/drv_range_finder.h +++ b/src/drivers/drv_range_finder.h @@ -45,6 +45,7 @@ #include "drv_orb_dev.h" #define RANGE_FINDER_DEVICE_PATH "/dev/range_finder" +#define MB12XX_MAX_RANGEFINDERS 12 //Maximum number of RangeFinders that can be connected enum RANGE_FINDER_TYPE { RANGE_FINDER_TYPE_LASER = 0, @@ -67,6 +68,8 @@ struct range_finder_report { float minimum_distance; /**< minimum distance the sensor can measure */ float maximum_distance; /**< maximum distance the sensor can measure */ uint8_t valid; /**< 1 == within sensor range, 0 = outside sensor range */ + float distance_vector[MB12XX_MAX_RANGEFINDERS]; /** in meters */ + uint8_t just_updated; /** number of the most recent measurement sensor */ }; /** diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index 9cf5686583..90f1791bd5 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -54,6 +54,7 @@ #include #include #include +#include #include #include @@ -72,7 +73,7 @@ #include /* Configuration Constants */ -#define MB12XX_BUS PX4_I2C_BUS_EXPANSION +#define MB12XX_BUS PX4_I2C_BUS_EXPANSION #define MB12XX_BASEADDR 0x70 /* 7-bit address. 8-bit address is 0xE0 */ #define MB12XX_DEVICE_PATH "/dev/mb12xx" @@ -83,10 +84,12 @@ #define MB12XX_SET_ADDRESS_2 0xA5 /* Change address 2 Register */ /* Device limits */ -#define MB12XX_MIN_DISTANCE (0.20f) -#define MB12XX_MAX_DISTANCE (7.65f) +#define MB12XX_MIN_DISTANCE (0.20f) +#define MB12XX_MAX_DISTANCE (7.65f) + +#define MB12XX_CONVERSION_INTERVAL 100000 /* 60ms for one sonar */ +#define TICKS_BETWEEN_SUCCESIVE_FIRES 100000 /* 30ms between each sonar measurement (watch out for interference!) */ -#define MB12XX_CONVERSION_INTERVAL 60000 /* 60ms */ /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -133,6 +136,14 @@ private: perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; + uint8_t _cycle_counter; /* counter in cycle to change i2c adresses */ + int _cycling_rate; /* */ + uint8_t _index_counter; /* temporary sonar i2c address */ + std::vector addr_ind; /* temp sonar i2c address vector */ + std::vector + _latest_sonar_measurements; /* vector to store latest sonar measurements in before writing to report */ + + /** * Test whether the device supported by the driver is present at a * specific address. @@ -140,7 +151,7 @@ private: * @param address The I2C bus address to probe. * @return True if the device is present. */ - int probe_address(uint8_t address); + int probe_address(uint8_t address); /** * Initialise the automatic measurement state machine and start it. @@ -170,15 +181,15 @@ private: * and start a new one. */ void cycle(); - int measure(); - int collect(); + int measure(); + int collect(); /** * Static trampoline from the workq context; because we don't have a * generic workq wrapper yet. * * @param arg Instance pointer for the driver that is polling. */ - static void cycle_trampoline(void *arg); + static void cycle_trampoline(void *arg); }; @@ -200,12 +211,16 @@ MB12XX::MB12XX(int bus, int address) : _range_finder_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")), _comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")), - _buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")) + _buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")), + _cycle_counter(0), /* initialising counter for cycling function to zero */ + _cycling_rate(0), /* initialising cycling rate (which can differ depending on one sonar or multiple) */ + _index_counter(0) /* initialising temp sonar i2c address to zero */ + { - // enable debug() calls + /* enable debug() calls */ _debug_enabled = false; - // work_cancel in the dtor will explode if we don't do this... + /* work_cancel in the dtor will explode if we don't do this... */ memset(&_work, 0, sizeof(_work)); } @@ -223,7 +238,7 @@ MB12XX::~MB12XX() unregister_class_devname(RANGE_FINDER_DEVICE_PATH, _class_instance); } - // free perf counters + /* free perf counters */ perf_free(_sample_perf); perf_free(_comms_errors); perf_free(_buffer_overflows); @@ -242,6 +257,9 @@ MB12XX::init() /* allocate basic report buffers */ _reports = new RingBuffer(2, sizeof(range_finder_report)); + _index_counter = MB12XX_BASEADDR; /* set temp sonar i2c address to base adress */ + set_address(_index_counter); /* set I2c port to temp sonar i2c adress */ + if (_reports == nullptr) { goto out; } @@ -250,16 +268,53 @@ MB12XX::init() if (_class_instance == CLASS_DEVICE_PRIMARY) { /* get a publish handle on the range finder topic */ - struct range_finder_report rf_report; - measure(); - _reports->get(&rf_report); + struct range_finder_report rf_report = {}; + _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); if (_range_finder_topic < 0) { - debug("failed to create sensor_range_finder object. Did you start uOrb?"); + log("failed advert - uORB started?"); } } + /* + * check for connected rangefinders on each i2c port: + * We start from i2c base address (0x70 = 112) and count downwards. + * So second iteration it uses i2c address 111, third iteration 110 and so on + */ + for (int counter = 0; counter <= MB12XX_MAX_RANGEFINDERS; counter++) { + /* set temp sonar i2c address to base adress - counter */ + _index_counter = MB12XX_BASEADDR - counter; + /* set I2c port to temp sonar i2c adress */ + set_address(_index_counter); + int ret2 = measure(); + + if (ret2 == 0) { /* sonar is present -> store address_index in array */ + addr_ind.push_back(_index_counter); + debug("sonar added"); + _latest_sonar_measurements.push_back(200); + } + } + + _index_counter = MB12XX_BASEADDR; + /* set i2c port back to base adress for rest of driver */ + set_address(_index_counter); + + /* if only one sonar detected, no special timing is required between firing, so use default */ + if (addr_ind.size() == 1) { + _cycling_rate = MB12XX_CONVERSION_INTERVAL; + + } else { + _cycling_rate = TICKS_BETWEEN_SUCCESIVE_FIRES; + } + + /* show the connected sonars in terminal */ + for (int i = 0; i < addr_ind.size(); i++) { + log("sonar %d with address %d added", (i + 1), addr_ind[i]); + } + + debug("Number of sonars connected: %d", addr_ind.size()); + ret = OK; /* sensor is ok, but we don't really know if it is within range */ _sensor_ok = true; @@ -325,11 +380,12 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) bool want_start = (_measure_ticks == 0); /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(MB12XX_CONVERSION_INTERVAL); + _measure_ticks = USEC2TICK(_cycling_rate); /* if we need to start the poll state machine, do it */ if (want_start) { start(); + } return OK; @@ -344,7 +400,7 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) unsigned ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ - if (ticks < USEC2TICK(MB12XX_CONVERSION_INTERVAL)) { + if (ticks < USEC2TICK(_cycling_rate)) { return -EINVAL; } @@ -414,6 +470,7 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) ssize_t MB12XX::read(struct file *filp, char *buffer, size_t buflen) { + unsigned count = buflen / sizeof(struct range_finder_report); struct range_finder_report *rbuf = reinterpret_cast(buffer); int ret = 0; @@ -453,7 +510,7 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen) } /* wait for it to complete */ - usleep(MB12XX_CONVERSION_INTERVAL); + usleep(_cycling_rate * 2); /* run the collection phase */ if (OK != collect()) { @@ -474,17 +531,19 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen) int MB12XX::measure() { + int ret; /* * Send the command to begin a measurement. */ + uint8_t cmd = MB12XX_TAKE_RANGE_REG; ret = transfer(&cmd, 1, nullptr, 0); if (OK != ret) { perf_count(_comms_errors); - log("i2c::transfer returned %d", ret); + debug("i2c::transfer returned %d", ret); return ret; } @@ -506,7 +565,7 @@ MB12XX::collect() ret = transfer(nullptr, 0, &val[0], 2); if (ret < 0) { - log("error reading from sensor: %d", ret); + debug("error reading from sensor: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); return ret; @@ -519,10 +578,35 @@ MB12XX::collect() /* this should be fairly close to the end of the measurement, so the best approximation of the time */ report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); - report.distance = si_units; + + /* assign the first measurement to the plain distance field */ + report.distance = _latest_sonar_measurements[0]; report.minimum_distance = get_minimum_distance(); report.maximum_distance = get_maximum_distance(); - report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; + report.valid = ((_latest_sonar_measurements[0] > get_minimum_distance()) + && (_latest_sonar_measurements[0] < get_maximum_distance())) ? 1 : 0; + + /* intermediate vector _latest_sonar_measurements is used to store the measurements as every cycle the other sonar values + * of the report are thrown away and/or filled in with garbage. We don't want this. We want the report to give the latest + * value for each connected sonar + */ + _latest_sonar_measurements[_cycle_counter] = si_units; + + for (unsigned i = 0; i < (_latest_sonar_measurements.size()); i++) { + if (i < addr_ind.size()) { + /* set data of connected sensor */ + report.distance_vector[i] = _latest_sonar_measurements[i]; + + } else { + /* set unconnected slots to zero */ + report.distance_vector[i] = 0; + } + } + + /* a just_updated variable is added to indicate to higher level software which sonar has most recently been + * collected as this could be of use for Kalman filters + */ + report.just_updated = _index_counter; /* publish it, if we are the primary */ if (_range_finder_topic >= 0) { @@ -545,12 +629,13 @@ MB12XX::collect() void MB12XX::start() { + /* reset the report ring and state machine */ _collect_phase = false; _reports->flush(); /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 1); + work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 5); /* notify about state change */ struct subsystem_info_s info = { @@ -564,8 +649,10 @@ MB12XX::start() if (pub > 0) { orb_publish(ORB_ID(subsystem_info), pub, &info); + } else { pub = orb_advertise(ORB_ID(subsystem_info), &info); + } } @@ -578,21 +665,25 @@ MB12XX::stop() void MB12XX::cycle_trampoline(void *arg) { + MB12XX *dev = (MB12XX *)arg; dev->cycle(); + } void MB12XX::cycle() { - /* collection phase? */ if (_collect_phase) { + /*sonar from previous iteration collect is now read out */ + _index_counter = addr_ind[_cycle_counter]; + set_address(_index_counter); /* perform collection */ if (OK != collect()) { - log("collection error"); - /* restart the measurement state machine */ + debug("collection error"); + /* if error restart the measurement state machine */ start(); return; } @@ -600,25 +691,37 @@ MB12XX::cycle() /* next phase is measurement */ _collect_phase = false; - /* - * Is there a collect->measure gap? - */ - if (_measure_ticks > USEC2TICK(MB12XX_CONVERSION_INTERVAL)) { + /* change i2c adress to next sonar */ + _cycle_counter = _cycle_counter + 1; + + if (_cycle_counter >= addr_ind.size()) { + _cycle_counter = 0; + } + + /* Is there a collect->measure gap? Yes, and the timing is set equal to the cycling_rate + Otherwise the next sonar would fire without the first one having received its reflected sonar pulse */ + + if (_measure_ticks > USEC2TICK(_cycling_rate)) { /* schedule a fresh cycle call when we are ready to measure again */ work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, - _measure_ticks - USEC2TICK(MB12XX_CONVERSION_INTERVAL)); - + _measure_ticks - USEC2TICK(_cycling_rate)); return; } } - /* measurement phase */ + /* Measurement (firing) phase */ + + /* ensure sonar i2c adress is still correct */ + _index_counter = addr_ind[_cycle_counter]; + set_address(_index_counter); + + /* Perform measurement */ if (OK != measure()) { - log("measure error"); + debug("ERR ADDR: %d", _index_counter); } /* next phase is collection */ @@ -629,7 +732,8 @@ MB12XX::cycle() &_work, (worker_t)&MB12XX::cycle_trampoline, this, - USEC2TICK(MB12XX_CONVERSION_INTERVAL)); + USEC2TICK(_cycling_rate)); + } void @@ -750,7 +854,7 @@ test() } warnx("single read"); - warnx("measurement: %0.2f m", (double)report.distance); + warnx("measurement: %0.2f of sonar %d", (double)report.distance_vector[report.just_updated], report.just_updated); warnx("time: %lld", report.timestamp); /* start the sensor polling at 2Hz */ @@ -779,7 +883,12 @@ test() } warnx("periodic read %u", i); - warnx("measurement: %0.3f", (double)report.distance); + + /* Print the sonar rangefinder report sonar distance vector */ + for (uint8_t count = 0; count < MB12XX_MAX_RANGEFINDERS; count++) { + warnx("measurement: %0.3f of sonar %u", (double)report.distance_vector[count], count + 1); + } + warnx("time: %lld", report.timestamp); } @@ -830,7 +939,7 @@ info() exit(0); } -} // namespace +} /* namespace */ int mb12xx_main(int argc, char *argv[]) From 73f342616e275ca726d70807212a733f9971bf66 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 15:16:13 +0100 Subject: [PATCH 163/269] add better accessor --- src/examples/subscriber/subscriber_example.cpp | 2 +- src/platforms/px4_subscriber.h | 10 +++++++++- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index 215336c174..781dde486c 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -81,5 +81,5 @@ void SubscriberExample::rc_channels_callback(const px4_rc_channels &msg) { PX4_INFO("Callback (method): [%llu]", msg.data().timestamp_last_valid); PX4_INFO("Callback (method): value of _sub_rc_chan: [%llu]", - _sub_rc_chan->get().data().timestamp_last_valid); + _sub_rc_chan->data().timestamp_last_valid); } diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 6a81ef8d22..6ca35b173f 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -84,7 +84,15 @@ public: /** * Get the last message value */ - virtual T get() {return _msg_current;} + virtual T& get() {return _msg_current;} + + /** + * Get the last native message value + */ + virtual decltype(((T*)nullptr)->data()) data() + { + return _msg_current.data(); + } protected: T _msg_current; /**< Current Message value */ From 6c0b5cf1259a0560a5adc966c4ca306f998c2c86 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 Jan 2015 15:24:18 +0100 Subject: [PATCH 164/269] Update NuttX version as required --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index 3d8171f6ea..e4c914e261 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 3d8171f6ea88297d8595525c8222d61e9cf20fd0 +Subproject commit e4c914e261d2647e44d05222afa7aa3cc90d3c67 From 6357fa75976a1d215d263b316d7fe495b82cfff4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 15:39:11 +0100 Subject: [PATCH 165/269] header generation script: create dir if it does not exist --- Tools/px_generate_uorb_topic_headers.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/Tools/px_generate_uorb_topic_headers.py b/Tools/px_generate_uorb_topic_headers.py index 54430cc38d..4bcab4d54c 100755 --- a/Tools/px_generate_uorb_topic_headers.py +++ b/Tools/px_generate_uorb_topic_headers.py @@ -90,6 +90,11 @@ def copy_changed(inputdir, outputdir, prefix=''): Copies files from inputdir to outputdir if they don't exist in ouputdir or if their content changed """ + + # Make sure output directory exists: + if not os.path.isdir(outputdir): + os.makedirs(outputdir) + for f in os.listdir(inputdir): fni = os.path.join(inputdir, f) if os.path.isfile(fni): From a23c39eec414ffb9d6808e3402d21f322e14930f Mon Sep 17 00:00:00 2001 From: Jon Verbeke Date: Tue, 20 Jan 2015 16:38:59 +0100 Subject: [PATCH 166/269] Sonar driver by jverbeke --- src/drivers/mb12xx/mb12xx.cpp | 99 +++++++++++++++++------------------ 1 file changed, 49 insertions(+), 50 deletions(-) diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index 90f1791bd5..0ef81e4319 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -34,6 +34,7 @@ /** * @file mb12xx.cpp * @author Greg Hulands + * @author Jon Verbeke * * Driver for the Maxbotix sonar range finders connected via I2C. */ @@ -136,12 +137,11 @@ private: perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; - uint8_t _cycle_counter; /* counter in cycle to change i2c adresses */ - int _cycling_rate; /* */ - uint8_t _index_counter; /* temporary sonar i2c address */ + uint8_t _cycle_counter; /* counter in cycle to change i2c adresses */ + int _cycling_rate; /* */ + uint8_t _index_counter; /* temporary sonar i2c address */ std::vector addr_ind; /* temp sonar i2c address vector */ - std::vector - _latest_sonar_measurements; /* vector to store latest sonar measurements in before writing to report */ + std::vector _latest_sonar_measurements; /* vector to store latest sonar measurements in before writing to report */ /** @@ -149,9 +149,9 @@ private: * specific address. * * @param address The I2C bus address to probe. - * @return True if the device is present. + * @return True if the device is present. */ - int probe_address(uint8_t address); + int probe_address(uint8_t address); /** * Initialise the automatic measurement state machine and start it. @@ -181,8 +181,8 @@ private: * and start a new one. */ void cycle(); - int measure(); - int collect(); + int measure(); + int collect(); /** * Static trampoline from the workq context; because we don't have a * generic workq wrapper yet. @@ -273,20 +273,16 @@ MB12XX::init() _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); if (_range_finder_topic < 0) { - log("failed advert - uORB started?"); + log("failed to create sensor_range_finder object. Did you start uOrb?"); } } - /* - * check for connected rangefinders on each i2c port: - * We start from i2c base address (0x70 = 112) and count downwards. - * So second iteration it uses i2c address 111, third iteration 110 and so on - */ - for (int counter = 0; counter <= MB12XX_MAX_RANGEFINDERS; counter++) { - /* set temp sonar i2c address to base adress - counter */ - _index_counter = MB12XX_BASEADDR - counter; - /* set I2c port to temp sonar i2c adress */ - set_address(_index_counter); + /* check for connected rangefinders on each i2c port: + We start from i2c base address (0x70 = 112) and count downwards + So second iteration it uses i2c address 111, third iteration 110 and so on*/ + for (unsigned counter = 0; counter <= MB12XX_MAX_RANGEFINDERS; counter++) { + _index_counter = MB12XX_BASEADDR - counter; /* set temp sonar i2c address to base adress - counter */ + set_address(_index_counter); /* set I2c port to temp sonar i2c adress */ int ret2 = measure(); if (ret2 == 0) { /* sonar is present -> store address_index in array */ @@ -297,8 +293,7 @@ MB12XX::init() } _index_counter = MB12XX_BASEADDR; - /* set i2c port back to base adress for rest of driver */ - set_address(_index_counter); + set_address(_index_counter); /* set i2c port back to base adress for rest of driver */ /* if only one sonar detected, no special timing is required between firing, so use default */ if (addr_ind.size() == 1) { @@ -309,7 +304,7 @@ MB12XX::init() } /* show the connected sonars in terminal */ - for (int i = 0; i < addr_ind.size(); i++) { + for (unsigned i = 0; i < addr_ind.size(); i++) { log("sonar %d with address %d added", (i + 1), addr_ind[i]); } @@ -397,7 +392,7 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) bool want_start = (_measure_ticks == 0); /* convert hz to tick interval via microseconds */ - unsigned ticks = USEC2TICK(1000000 / arg); + int ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ if (ticks < USEC2TICK(_cycling_rate)) { @@ -579,34 +574,39 @@ MB12XX::collect() report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); - /* assign the first measurement to the plain distance field */ - report.distance = _latest_sonar_measurements[0]; - report.minimum_distance = get_minimum_distance(); - report.maximum_distance = get_maximum_distance(); - report.valid = ((_latest_sonar_measurements[0] > get_minimum_distance()) - && (_latest_sonar_measurements[0] < get_maximum_distance())) ? 1 : 0; + /* if only one sonar, write it to the original distance parameter so that it's still used as altitude sonar */ + if (addr_ind.size() == 1) { + report.distance = si_units; - /* intermediate vector _latest_sonar_measurements is used to store the measurements as every cycle the other sonar values - * of the report are thrown away and/or filled in with garbage. We don't want this. We want the report to give the latest - * value for each connected sonar - */ - _latest_sonar_measurements[_cycle_counter] = si_units; - - for (unsigned i = 0; i < (_latest_sonar_measurements.size()); i++) { - if (i < addr_ind.size()) { - /* set data of connected sensor */ - report.distance_vector[i] = _latest_sonar_measurements[i]; - - } else { - /* set unconnected slots to zero */ + for (unsigned i = 0; i < (MB12XX_MAX_RANGEFINDERS); i++) { report.distance_vector[i] = 0; } + + report.just_updated = 0; + + } else { + /* for multiple sonars connected */ + + /* don't use the orginial single sonar variable */ + report.distance = 0; + + /* intermediate vector _latest_sonar_measurements is used to store the measurements as every cycle the other sonar values of the report are thrown away and/or filled in with garbage. We don't want this. We want the report to give the latest value for each connected sonar */ + _latest_sonar_measurements[_cycle_counter] = si_units; + + for (unsigned i = 0; i < (_latest_sonar_measurements.size()); i++) { + report.distance_vector[i] = _latest_sonar_measurements[i]; + } + + /* a just_updated variable is added to indicate to autopilot (ardupilot or whatever) which sonar has most recently been collected as this could be of use for Kalman filters */ + report.just_updated = _index_counter; + + /* Make sure all elements of the distance vector for which no sonar is connected are zero to prevent strange numbers */ + for (unsigned i = 0; i < (MB12XX_MAX_RANGEFINDERS - addr_ind.size()); i++) { + report.distance_vector[addr_ind.size() + i] = 0; + } } - /* a just_updated variable is added to indicate to higher level software which sonar has most recently been - * collected as this could be of use for Kalman filters - */ - report.just_updated = _index_counter; + report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; /* publish it, if we are the primary */ if (_range_finder_topic >= 0) { @@ -676,8 +676,7 @@ void MB12XX::cycle() { if (_collect_phase) { - /*sonar from previous iteration collect is now read out */ - _index_counter = addr_ind[_cycle_counter]; + _index_counter = addr_ind[_cycle_counter]; /*sonar from previous iteration collect is now read out */ set_address(_index_counter); /* perform collection */ @@ -721,7 +720,7 @@ MB12XX::cycle() /* Perform measurement */ if (OK != measure()) { - debug("ERR ADDR: %d", _index_counter); + debug("measure error sonar adress %d", _index_counter); } /* next phase is collection */ From b14e849fc4bcf81bbffd47064e2850a929652f6d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 Jan 2015 16:33:24 +0100 Subject: [PATCH 167/269] Sonar: Set min/max distance properly, add usleep for detection --- src/drivers/mb12xx/mb12xx.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index 0ef81e4319..a4fb7bcc20 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -277,6 +277,9 @@ MB12XX::init() } } + // XXX we should find out why we need to wait 200 ms here + usleep(200000); + /* check for connected rangefinders on each i2c port: We start from i2c base address (0x70 = 112) and count downwards So second iteration it uses i2c address 111, third iteration 110 and so on*/ @@ -606,6 +609,8 @@ MB12XX::collect() } } + report.minimum_distance = get_minimum_distance(); + report.maximum_distance = get_maximum_distance(); report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; /* publish it, if we are the primary */ From dfd078651f44533b2f5998b0edce781b11507275 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 18:00:59 +0100 Subject: [PATCH 168/269] subscriber: move nuttx startup handling --- src/examples/subscriber/module.mk | 1 + src/examples/subscriber/subscriber_main.cpp | 63 +----------- .../subscriber/subscriber_start_nuttx.cpp | 99 +++++++++++++++++++ 3 files changed, 102 insertions(+), 61 deletions(-) create mode 100644 src/examples/subscriber/subscriber_start_nuttx.cpp diff --git a/src/examples/subscriber/module.mk b/src/examples/subscriber/module.mk index 3156ebb30a..0e02c65b45 100644 --- a/src/examples/subscriber/module.mk +++ b/src/examples/subscriber/module.mk @@ -38,6 +38,7 @@ MODULE_COMMAND = subscriber SRCS = subscriber_main.cpp \ + subscriber_start_nuttx.cpp \ subscriber_example.cpp \ subscriber_params.c diff --git a/src/examples/subscriber/subscriber_main.cpp b/src/examples/subscriber/subscriber_main.cpp index 906921e019..9752bc3cc8 100644 --- a/src/examples/subscriber/subscriber_main.cpp +++ b/src/examples/subscriber/subscriber_main.cpp @@ -37,70 +37,11 @@ * * @author Thomas Gubler */ -#include -#include #include "subscriber_example.h" +bool thread_running = false; /**< Deamon status flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int daemon_task; /**< Handle of deamon task / thread */ -namespace px4 -{ -bool task_should_exit = false; -} -using namespace px4; -PX4_MAIN_FUNCTION(subscriber); - -#if !defined(__PX4_ROS) -extern "C" __EXPORT int subscriber_main(int argc, char *argv[]); -int subscriber_main(int argc, char *argv[]) -{ - if (argc < 1) { - errx(1, "usage: subscriber {start|stop|status}"); - } - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - warnx("already running"); - /* this is not an error */ - exit(0); - } - - task_should_exit = false; - - daemon_task = task_spawn_cmd("subscriber", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - subscriber_task_main, - (argv) ? (char* const*)&argv[2] : (char* const*)NULL); - - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - task_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("is running"); - - } else { - warnx("not started"); - } - - exit(0); - } - - warnx("unrecognized command"); - return 1; -} -#endif - -PX4_MAIN_FUNCTION(subscriber) +int main(int argc, char **argv) { px4::init(argc, argv, "subscriber"); diff --git a/src/examples/subscriber/subscriber_start_nuttx.cpp b/src/examples/subscriber/subscriber_start_nuttx.cpp new file mode 100644 index 0000000000..6129b19acc --- /dev/null +++ b/src/examples/subscriber/subscriber_start_nuttx.cpp @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file subscriber_start_nuttx.cpp + * + * @author Thomas Gubler + */ +#include +#include +#include +#include + +extern bool thread_running; +int daemon_task; /**< Handle of deamon task / thread */ +namespace px4 +{ +bool task_should_exit = false; +} +using namespace px4; + +extern int main(int argc, char **argv); + +extern "C" __EXPORT int subscriber_main(int argc, char *argv[]); +int subscriber_main(int argc, char *argv[]) +{ + if (argc < 1) { + errx(1, "usage: subscriber {start|stop|status}"); + } + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("already running"); + /* this is not an error */ + exit(0); + } + + task_should_exit = false; + + daemon_task = task_spawn_cmd("subscriber", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + main, + (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + task_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("is running"); + + } else { + warnx("not started"); + } + + exit(0); + } + + warnx("unrecognized command"); + return 1; +} From a36d0dce85868f35ad9c4e097f5b788e84d7d18f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 18:01:16 +0100 Subject: [PATCH 169/269] publisher: move nuttx startup handling --- src/examples/publisher/module.mk | 1 + src/examples/publisher/publisher_main.cpp | 61 +----------- .../publisher/publisher_start_nuttx.cpp | 99 +++++++++++++++++++ 3 files changed, 102 insertions(+), 59 deletions(-) create mode 100644 src/examples/publisher/publisher_start_nuttx.cpp diff --git a/src/examples/publisher/module.mk b/src/examples/publisher/module.mk index 4f941cd439..62a5f8dd1f 100644 --- a/src/examples/publisher/module.mk +++ b/src/examples/publisher/module.mk @@ -38,6 +38,7 @@ MODULE_COMMAND = publisher SRCS = publisher_main.cpp \ + publisher_start_nuttx.cpp \ publisher_example.cpp MODULE_STACKSIZE = 1200 diff --git a/src/examples/publisher/publisher_main.cpp b/src/examples/publisher/publisher_main.cpp index 81439a8be2..85d24d4ef2 100644 --- a/src/examples/publisher/publisher_main.cpp +++ b/src/examples/publisher/publisher_main.cpp @@ -41,66 +41,9 @@ #include #include "publisher_example.h" -static bool thread_running = false; /**< Deamon status flag */ -static int daemon_task; /**< Handle of deamon task / thread */ -namespace px4 -{ -bool task_should_exit = false; -} -using namespace px4; +bool thread_running = false; /**< Deamon status flag */ -PX4_MAIN_FUNCTION(publisher); - -#if !defined(__PX4_ROS) -extern "C" __EXPORT int publisher_main(int argc, char *argv[]); -int publisher_main(int argc, char *argv[]) -{ - if (argc < 1) { - errx(1, "usage: publisher {start|stop|status}"); - } - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - warnx("already running"); - /* this is not an error */ - exit(0); - } - - task_should_exit = false; - - daemon_task = task_spawn_cmd("publisher", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - publisher_task_main, - (argv) ? (char* const*)&argv[2] : (char* const*)NULL); - - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - task_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("is running"); - - } else { - warnx("not started"); - } - - exit(0); - } - - warnx("unrecognized command"); - return 1; -} -#endif - -PX4_MAIN_FUNCTION(publisher) +int main(int argc, char **argv) { px4::init(argc, argv, "publisher"); diff --git a/src/examples/publisher/publisher_start_nuttx.cpp b/src/examples/publisher/publisher_start_nuttx.cpp new file mode 100644 index 0000000000..db9d85269b --- /dev/null +++ b/src/examples/publisher/publisher_start_nuttx.cpp @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file publisher_start_nuttx.cpp + * + * @author Thomas Gubler + */ +#include +#include +#include +#include + +extern bool thread_running; +int daemon_task; /**< Handle of deamon task / thread */ +namespace px4 +{ +bool task_should_exit = false; +} +using namespace px4; + +extern int main(int argc, char **argv); + +extern "C" __EXPORT int publisher_main(int argc, char *argv[]); +int publisher_main(int argc, char *argv[]) +{ + if (argc < 1) { + errx(1, "usage: publisher {start|stop|status}"); + } + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("already running"); + /* this is not an error */ + exit(0); + } + + task_should_exit = false; + + daemon_task = task_spawn_cmd("publisher", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + main, + (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + task_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("is running"); + + } else { + warnx("not started"); + } + + exit(0); + } + + warnx("unrecognized command"); + return 1; +} From aa7e5ddb388e5d382b3dc214de6a1581c2a2fe71 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 18:11:44 +0100 Subject: [PATCH 170/269] move position of spin_once in publisher example --- src/examples/publisher/publisher_example.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/examples/publisher/publisher_example.cpp b/src/examples/publisher/publisher_example.cpp index 6b30c2fe3d..9952a16e4b 100644 --- a/src/examples/publisher/publisher_example.cpp +++ b/src/examples/publisher/publisher_example.cpp @@ -54,13 +54,14 @@ int PublisherExample::main() px4::Rate loop_rate(10); while (px4::ok()) { + loop_rate.sleep(); + _n.spinOnce(); + px4_rc_channels msg; msg.data().timestamp_last_valid = px4::get_time_micros(); PX4_INFO("%llu", msg.data().timestamp_last_valid); _rc_channels_pub->publish(msg); - _n.spinOnce(); - loop_rate.sleep(); } return 0; From 67a35eb0b509b2394f82773c460a6492471efd37 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 18:15:55 +0100 Subject: [PATCH 171/269] remove unneeded files --- src/platforms/nuttx/px4_nodehandle.cpp | 44 -------------------------- src/platforms/nuttx/px4_publisher.cpp | 41 ------------------------ src/platforms/nuttx/px4_subscriber.cpp | 40 ----------------------- src/platforms/ros/px4_subscriber.cpp | 41 ------------------------ 4 files changed, 166 deletions(-) delete mode 100644 src/platforms/nuttx/px4_nodehandle.cpp delete mode 100644 src/platforms/nuttx/px4_publisher.cpp delete mode 100644 src/platforms/nuttx/px4_subscriber.cpp delete mode 100644 src/platforms/ros/px4_subscriber.cpp diff --git a/src/platforms/nuttx/px4_nodehandle.cpp b/src/platforms/nuttx/px4_nodehandle.cpp deleted file mode 100644 index ec557e8aa9..0000000000 --- a/src/platforms/nuttx/px4_nodehandle.cpp +++ /dev/null @@ -1,44 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file px4_nodehandle.cpp - * - * PX4 Middleware Wrapper Nodehandle - */ -#include - -namespace px4 -{ - -} diff --git a/src/platforms/nuttx/px4_publisher.cpp b/src/platforms/nuttx/px4_publisher.cpp deleted file mode 100644 index 3bd70272f1..0000000000 --- a/src/platforms/nuttx/px4_publisher.cpp +++ /dev/null @@ -1,41 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file px4_publisher.cpp - * - * PX4 Middleware Wrapper for Publisher - */ -#include - - diff --git a/src/platforms/nuttx/px4_subscriber.cpp b/src/platforms/nuttx/px4_subscriber.cpp deleted file mode 100644 index 426e646c9f..0000000000 --- a/src/platforms/nuttx/px4_subscriber.cpp +++ /dev/null @@ -1,40 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file px4_subscriber.cpp - * - * PX4 Middleware Wrapper Subscriber - */ -#include - diff --git a/src/platforms/ros/px4_subscriber.cpp b/src/platforms/ros/px4_subscriber.cpp deleted file mode 100644 index d040b860d2..0000000000 --- a/src/platforms/ros/px4_subscriber.cpp +++ /dev/null @@ -1,41 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file px4_subscriber.cpp - * - * PX4 Middleware Wrapper Subscriber - */ - -#include - From 4c3a24ddeeee4187a527a1d750b406b96232f66b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 23 Jan 2015 18:30:02 +0100 Subject: [PATCH 172/269] tiny cleanup --- src/examples/publisher/publisher_main.cpp | 2 -- src/examples/subscriber/subscriber_main.cpp | 1 - 2 files changed, 3 deletions(-) diff --git a/src/examples/publisher/publisher_main.cpp b/src/examples/publisher/publisher_main.cpp index 85d24d4ef2..11439acffc 100644 --- a/src/examples/publisher/publisher_main.cpp +++ b/src/examples/publisher/publisher_main.cpp @@ -37,8 +37,6 @@ * * @author Thomas Gubler */ -#include -#include #include "publisher_example.h" bool thread_running = false; /**< Deamon status flag */ diff --git a/src/examples/subscriber/subscriber_main.cpp b/src/examples/subscriber/subscriber_main.cpp index 9752bc3cc8..798c741637 100644 --- a/src/examples/subscriber/subscriber_main.cpp +++ b/src/examples/subscriber/subscriber_main.cpp @@ -40,7 +40,6 @@ #include "subscriber_example.h" bool thread_running = false; /**< Deamon status flag */ - int main(int argc, char **argv) { px4::init(argc, argv, "subscriber"); From f5a1680fd6eb17c4fd1b7663cb16ffe126289cc0 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sat, 24 Jan 2015 14:18:28 +0100 Subject: [PATCH 173/269] Add landing detector for V1 boards as well. --- makefiles/config_px4fmu-v1_default.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 18be47065d..852edb788c 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -62,6 +62,7 @@ MODULES += modules/commander MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/gpio_led +MODULES += modules/land_detector # # Estimation modules (EKF / other filters) From cbf4a5af1611ee0969ea0b8d265441308e2dbc91 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Fri, 23 Jan 2015 22:25:48 -0500 Subject: [PATCH 174/269] Simplified i2c handling for flow. --- src/drivers/px4flow/i2c_frame.h | 82 ++++++++++++++++++++++++++++++ src/drivers/px4flow/px4flow.cpp | 88 +++++---------------------------- 2 files changed, 95 insertions(+), 75 deletions(-) create mode 100644 src/drivers/px4flow/i2c_frame.h diff --git a/src/drivers/px4flow/i2c_frame.h b/src/drivers/px4flow/i2c_frame.h new file mode 100644 index 0000000000..b391b1f6a8 --- /dev/null +++ b/src/drivers/px4flow/i2c_frame.h @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file i2c_frame.h + * Definition of i2c frames. + * @author Thomas Boehm + * @author James Goppert + */ + +#ifndef I2C_FRAME_H_ +#define I2C_FRAME_H_ +#include + + +typedef struct i2c_frame +{ + uint16_t frame_count; + int16_t pixel_flow_x_sum; + int16_t pixel_flow_y_sum; + int16_t flow_comp_m_x; + int16_t flow_comp_m_y; + int16_t qual; + int16_t gyro_x_rate; + int16_t gyro_y_rate; + int16_t gyro_z_rate; + uint8_t gyro_range; + uint8_t sonar_timestamp; + int16_t ground_distance; +} i2c_frame; + +#define I2C_FRAME_SIZE (sizeof(i2c_frame)) + + +typedef struct i2c_integral_frame +{ + uint16_t frame_count_since_last_readout; + int16_t pixel_flow_x_integral; + int16_t pixel_flow_y_integral; + int16_t gyro_x_rate_integral; + int16_t gyro_y_rate_integral; + int16_t gyro_z_rate_integral; + uint32_t integration_timespan; + uint32_t sonar_timestamp; + uint16_t ground_distance; + int16_t gyro_temperature; + uint8_t qual; +} i2c_integral_frame; + +#define I2C_INTEGRAL_FRAME_SIZE (sizeof(i2c_integral_frame)) + +#endif /* I2C_FRAME_H_ */ diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 9c9c1b0f81..bb0cdbbb6e 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -93,38 +93,11 @@ static const int ERROR = -1; # error This requires CONFIG_SCHED_WORKQUEUE. #endif -struct i2c_frame { - uint16_t frame_count; - int16_t pixel_flow_x_sum; - int16_t pixel_flow_y_sum; - int16_t flow_comp_m_x; - int16_t flow_comp_m_y; - int16_t qual; - int16_t gyro_x_rate; - int16_t gyro_y_rate; - int16_t gyro_z_rate; - uint8_t gyro_range; - uint8_t sonar_timestamp; - int16_t ground_distance; -}; +#include "i2c_frame.h" + struct i2c_frame f; - -struct i2c_integral_frame { - uint16_t frame_count_since_last_readout; - int16_t pixel_flow_x_integral; - int16_t pixel_flow_y_integral; - int16_t gyro_x_rate_integral; - int16_t gyro_y_rate_integral; - int16_t gyro_z_rate_integral; - uint32_t integration_timespan; - uint32_t time_since_last_sonar_update; - uint16_t ground_distance; - int16_t gyro_temperature; - uint8_t qual; -} __attribute__((packed)); struct i2c_integral_frame f_integral; - class PX4FLOW: public device::I2C { public: @@ -150,8 +123,7 @@ private: RingBuffer *_reports; bool _sensor_ok; int _measure_ticks; - bool _collect_phase; - + bool _collect_phase; orb_advert_t _px4flow_topic; perf_counter_t _sample_perf; @@ -261,10 +233,10 @@ out: int PX4FLOW::probe() { - uint8_t val[22]; + uint8_t val[I2C_FRAME_SIZE]; // to be sure this is not a ll40ls Lidar (which can also be on - // 0x42) we check if a 22 byte transfer works from address + // 0x42) we check if a I2C_FRAME_SIZE byte transfer works from address // 0. The ll40ls gives an error for that, whereas the flow // happily returns some data if (transfer(nullptr, 0, &val[0], 22) != OK) { @@ -469,16 +441,16 @@ PX4FLOW::collect() int ret = -EIO; /* read from the sensor */ - uint8_t val[47] = { 0 }; + uint8_t val[I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE] = { 0 }; perf_begin(_sample_perf); if (PX4FLOW_REG == 0x00) { - ret = transfer(nullptr, 0, &val[0], 47); // read 47 bytes (22+25 : frame1 + frame2) + ret = transfer(nullptr, 0, &val[0], I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE); } if (PX4FLOW_REG == 0x16) { - ret = transfer(nullptr, 0, &val[0], 25); // read 25 bytes (only frame2) + ret = transfer(nullptr, 0, &val[0], I2C_INTEGRAL_FRAME_SIZE); } if (ret < 0) { @@ -489,46 +461,12 @@ PX4FLOW::collect() } if (PX4FLOW_REG == 0) { - f.frame_count = val[1] << 8 | val[0]; - f.pixel_flow_x_sum = val[3] << 8 | val[2]; - f.pixel_flow_y_sum = val[5] << 8 | val[4]; - f.flow_comp_m_x = val[7] << 8 | val[6]; - f.flow_comp_m_y = val[9] << 8 | val[8]; - f.qual = val[11] << 8 | val[10]; - f.gyro_x_rate = val[13] << 8 | val[12]; - f.gyro_y_rate = val[15] << 8 | val[14]; - f.gyro_z_rate = val[17] << 8 | val[16]; - f.gyro_range = val[18]; - f.sonar_timestamp = val[19]; - f.ground_distance = val[21] << 8 | val[20]; - - f_integral.frame_count_since_last_readout = val[23] << 8 | val[22]; - f_integral.pixel_flow_x_integral = val[25] << 8 | val[24]; - f_integral.pixel_flow_y_integral = val[27] << 8 | val[26]; - f_integral.gyro_x_rate_integral = val[29] << 8 | val[28]; - f_integral.gyro_y_rate_integral = val[31] << 8 | val[30]; - f_integral.gyro_z_rate_integral = val[33] << 8 | val[32]; - f_integral.integration_timespan = val[37] << 24 | val[36] << 16 - | val[35] << 8 | val[34]; - f_integral.time_since_last_sonar_update = val[41] << 24 | val[40] << 16 - | val[39] << 8 | val[38]; - f_integral.ground_distance = val[43] << 8 | val[42]; - f_integral.gyro_temperature = val[45] << 8 | val[44]; - f_integral.qual = val[46]; + memcpy(&f, val, I2C_FRAME_SIZE); + memcpy(&f_integral, &(val[I2C_FRAME_SIZE]), I2C_INTEGRAL_FRAME_SIZE); } if (PX4FLOW_REG == 0x16) { - f_integral.frame_count_since_last_readout = val[1] << 8 | val[0]; - f_integral.pixel_flow_x_integral = val[3] << 8 | val[2]; - f_integral.pixel_flow_y_integral = val[5] << 8 | val[4]; - f_integral.gyro_x_rate_integral = val[7] << 8 | val[6]; - f_integral.gyro_y_rate_integral = val[9] << 8 | val[8]; - f_integral.gyro_z_rate_integral = val[11] << 8 | val[10]; - f_integral.integration_timespan = val[15] << 24 | val[14] << 16 | val[13] << 8 | val[12]; - f_integral.time_since_last_sonar_update = val[19] << 24 | val[18] << 16 | val[17] << 8 | val[16]; - f_integral.ground_distance = val[21] << 8 | val[20]; - f_integral.gyro_temperature = val[23] << 8 | val[22]; - f_integral.qual = val[24]; + memcpy(&f_integral, val, I2C_INTEGRAL_FRAME_SIZE); } @@ -544,7 +482,7 @@ PX4FLOW::collect() report.gyro_y_rate_integral = static_cast(f_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians report.gyro_z_rate_integral = static_cast(f_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians report.integration_timespan = f_integral.integration_timespan; //microseconds - report.time_since_last_sonar_update = f_integral.time_since_last_sonar_update;//microseconds + report.time_since_last_sonar_update = f_integral.sonar_timestamp;//microseconds report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius report.sensor_id = 0; @@ -828,7 +766,7 @@ test() warnx("ground_distance: %0.2f m", (double) f_integral.ground_distance / 1000); warnx("time since last sonar update [us]: %i", - f_integral.time_since_last_sonar_update); + f_integral.sonar_timestamp); warnx("quality integration average : %i", f_integral.qual); warnx("quality : %i", f.qual); From 36a70debf4c51c7ad02651f2eb6a24da71bad2fc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 25 Jan 2015 13:35:57 +0100 Subject: [PATCH 175/269] add act control 0 to 3 as msg --- msg/actuator_controls_1.msg | 5 +++++ msg/actuator_controls_2.msg | 5 +++++ msg/actuator_controls_3.msg | 5 +++++ 3 files changed, 15 insertions(+) create mode 100644 msg/actuator_controls_1.msg create mode 100644 msg/actuator_controls_2.msg create mode 100644 msg/actuator_controls_3.msg diff --git a/msg/actuator_controls_1.msg b/msg/actuator_controls_1.msg new file mode 100644 index 0000000000..414eb06ddb --- /dev/null +++ b/msg/actuator_controls_1.msg @@ -0,0 +1,5 @@ +uint8 NUM_ACTUATOR_CONTROLS = 8 +uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 +uint64 timestamp +uint64 timestamp_sample # the timestamp the data this control response is based on was sampled +float32[8] control diff --git a/msg/actuator_controls_2.msg b/msg/actuator_controls_2.msg new file mode 100644 index 0000000000..414eb06ddb --- /dev/null +++ b/msg/actuator_controls_2.msg @@ -0,0 +1,5 @@ +uint8 NUM_ACTUATOR_CONTROLS = 8 +uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 +uint64 timestamp +uint64 timestamp_sample # the timestamp the data this control response is based on was sampled +float32[8] control diff --git a/msg/actuator_controls_3.msg b/msg/actuator_controls_3.msg new file mode 100644 index 0000000000..414eb06ddb --- /dev/null +++ b/msg/actuator_controls_3.msg @@ -0,0 +1,5 @@ +uint8 NUM_ACTUATOR_CONTROLS = 8 +uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 +uint64 timestamp +uint64 timestamp_sample # the timestamp the data this control response is based on was sampled +float32[8] control From f9b4a96bf254f12d7ef97a09fafbbff64a9d6e54 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 25 Jan 2015 13:36:10 +0100 Subject: [PATCH 176/269] add msg/actuator_controls_virtual_fw.msg --- msg/actuator_controls_virtual_fw.msg | 5 +++++ 1 file changed, 5 insertions(+) create mode 100644 msg/actuator_controls_virtual_fw.msg diff --git a/msg/actuator_controls_virtual_fw.msg b/msg/actuator_controls_virtual_fw.msg new file mode 100644 index 0000000000..414eb06ddb --- /dev/null +++ b/msg/actuator_controls_virtual_fw.msg @@ -0,0 +1,5 @@ +uint8 NUM_ACTUATOR_CONTROLS = 8 +uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 +uint64 timestamp +uint64 timestamp_sample # the timestamp the data this control response is based on was sampled +float32[8] control From 265139f836b1fefdacb1e9498f0d25ecf2ecb47a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 25 Jan 2015 13:36:36 +0100 Subject: [PATCH 177/269] add msg/fw_virtual_rates_setpoint.msg --- msg/fw_virtual_rates_setpoint.msg | 6 ++++++ 1 file changed, 6 insertions(+) create mode 100644 msg/fw_virtual_rates_setpoint.msg diff --git a/msg/fw_virtual_rates_setpoint.msg b/msg/fw_virtual_rates_setpoint.msg new file mode 100644 index 0000000000..834113c798 --- /dev/null +++ b/msg/fw_virtual_rates_setpoint.msg @@ -0,0 +1,6 @@ +uint64 timestamp # in microseconds since system start + +float32 roll # body angular rates in NED frame +float32 pitch # body angular rates in NED frame +float32 yaw # body angular rates in NED frame +float32 thrust # thrust normalized to 0..1 From ee6395c5029737baeb328bd10ba82c8f05bf0648 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 25 Jan 2015 13:38:47 +0100 Subject: [PATCH 178/269] enable mc att multiplatform in makefile --- makefiles/config_px4fmu-v2_multiplatform.mk | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/makefiles/config_px4fmu-v2_multiplatform.mk b/makefiles/config_px4fmu-v2_multiplatform.mk index bfa26f83cf..76edade4b8 100644 --- a/makefiles/config_px4fmu-v2_multiplatform.mk +++ b/makefiles/config_px4fmu-v2_multiplatform.mk @@ -86,7 +86,8 @@ MODULES += modules/position_estimator_inav #MODULES += modules/fw_pos_control_l1 #MODULES += modules/fw_att_control # MODULES += modules/mc_att_control -# MODULES += modules/mc_att_control_multiplatform +MODULES += modules/mc_att_control_multiplatform +MODULES += examples/subscriber MODULES += examples/publisher MODULES += modules/mc_pos_control MODULES += modules/vtol_att_control From 14aa5f9441759672a3c0d6ee9b6ff5256ae0a77e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 25 Jan 2015 13:39:34 +0100 Subject: [PATCH 179/269] add topic header includes --- src/drivers/hil/hil.cpp | 22 ++++++++++--------- src/drivers/mkblctrl/mkblctrl.cpp | 1 + src/drivers/px4fmu/fmu.cpp | 6 ++++- src/drivers/px4io/px4io.cpp | 8 +++++-- src/modules/bottle_drop/bottle_drop.cpp | 4 ++++ src/modules/commander/commander.cpp | 4 ++++ src/modules/controllib/uorb/blocks.hpp | 4 ++++ .../position_estimator_inav_main.c | 4 ++++ src/modules/sdlog2/sdlog2.c | 6 ++++- .../vtol_att_control_main.cpp | 4 ++++ src/systemcmds/esc_calib/esc_calib.c | 4 ++++ 11 files changed, 53 insertions(+), 14 deletions(-) diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index 9b5c8133b4..6ffa8252e0 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -75,6 +75,8 @@ #include #include +#include +#include #include #include @@ -250,7 +252,7 @@ HIL::task_main_trampoline(int argc, char *argv[]) int HIL::set_mode(Mode mode) { - /* + /* * Configure for PWM output. * * Note that regardless of the configured mode, the task is always @@ -269,19 +271,19 @@ HIL::set_mode(Mode mode) /* multi-port as 4 PWM outs */ _update_rate = 50; /* default output rate */ break; - + case MODE_8PWM: debug("MODE_8PWM"); /* multi-port as 8 PWM outs */ _update_rate = 50; /* default output rate */ break; - + case MODE_12PWM: debug("MODE_12PWM"); /* multi-port as 12 PWM outs */ _update_rate = 50; /* default output rate */ break; - + case MODE_16PWM: debug("MODE_16PWM"); /* multi-port as 16 PWM outs */ @@ -513,12 +515,12 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; case PWM_SERVO_SET_UPDATE_RATE: - // HIL always outputs at the alternate (usually faster) rate + // HIL always outputs at the alternate (usually faster) rate g_hil->set_pwm_rate(arg); break; case PWM_SERVO_SET_SELECT_UPDATE_RATE: - // HIL always outputs at the alternate (usually faster) rate + // HIL always outputs at the alternate (usually faster) rate break; case PWM_SERVO_SET(2): @@ -659,7 +661,7 @@ int hil_new_mode(PortMode new_mode) { // uint32_t gpio_bits; - + // /* reset to all-inputs */ // g_hil->ioctl(0, GPIO_RESET, 0); @@ -691,17 +693,17 @@ hil_new_mode(PortMode new_mode) /* select 2-pin PWM mode */ servo_mode = HIL::MODE_2PWM; break; - + case PORT2_8PWM: /* select 8-pin PWM mode */ servo_mode = HIL::MODE_8PWM; break; - + case PORT2_12PWM: /* select 12-pin PWM mode */ servo_mode = HIL::MODE_12PWM; break; - + case PORT2_16PWM: /* select 16-pin PWM mode */ servo_mode = HIL::MODE_16PWM; diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 1055487cb9..a4505fe6fb 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -74,6 +74,7 @@ #include #include +#include #include #include #include diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 112c015136..2552e7e6a6 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -70,6 +70,10 @@ #include #include +#include +#include +#include +#include #include #include @@ -1144,7 +1148,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) * and PWM under control of the flight config * parameters. Note that this does not allow for * changing a set of pins to be used for serial on - * FMUv1 + * FMUv1 */ switch (arg) { case 0: diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 1c9a5adc9a..0451bbd1bd 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -75,6 +75,10 @@ #include #include +#include +#include +#include +#include #include #include #include @@ -1199,7 +1203,7 @@ PX4IO::io_set_arming_state() if (armed.ready_to_arm) { set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; - + } else { clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; } @@ -2590,7 +2594,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) case PWM_SERVO_SET_RC_CONFIG: { /* enable setting of RC configuration without relying - on param_get() + on param_get() */ struct pwm_output_rc_config* config = (struct pwm_output_rc_config*)arg; if (config->channel >= RC_INPUT_MAX_CHANNELS) { diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index d8116ea11d..b267209fe3 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -57,6 +57,10 @@ #include #include #include +#include +#include +#include +#include #include #include #include diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 247a2c5b83..a1c4e205d4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -74,6 +74,10 @@ #include #include #include +#include +#include +#include +#include #include #include #include diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp index a8a70507e9..491d4681de 100644 --- a/src/modules/controllib/uorb/blocks.hpp +++ b/src/modules/controllib/uorb/blocks.hpp @@ -47,6 +47,10 @@ #include #include #include +#include +#include +#include +#include #include #include diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 68ebd0f4f5..6de283396c 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -53,6 +53,10 @@ #include #include #include +#include +#include +#include +#include #include #include #include diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index a3407e42c4..ac08b0d237 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -70,6 +70,10 @@ #include #include #include +#include +#include +#include +#include #include #include #include @@ -1117,7 +1121,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.system_power_sub = orb_subscribe(ORB_ID(system_power)); subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status)); subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate)); - + /* we need to rate-limit wind, as we do not need the full update rate */ orb_set_interval(subs.wind_sub, 90); subs.encoders_sub = orb_subscribe(ORB_ID(encoders)); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 8e68730b8b..0a333eade6 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -58,7 +58,11 @@ #include #include #include +#include +#include #include +#include +#include #include #include #include diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c index 20967b541d..32682f8908 100644 --- a/src/systemcmds/esc_calib/esc_calib.c +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -64,6 +64,10 @@ #include "drivers/drv_pwm_output.h" #include +#include +#include +#include +#include static void usage(const char *reason); __EXPORT int esc_calib_main(int argc, char *argv[]); From 35efcaa92c575c60ad31f5a7ad9f8e5756cdf2bc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 25 Jan 2015 15:01:03 +0100 Subject: [PATCH 180/269] update topic header includes for multiplatform headers --- src/platforms/px4_includes.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index 90ec789960..ab27e6b4d1 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -76,6 +76,10 @@ #include #include #include +#include +#include +#include +#include #include #include #include From 1f0daea66586c32720083e5b2d703ce5fb9477f4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 25 Jan 2015 15:01:37 +0100 Subject: [PATCH 181/269] remove uorb hacks --- src/modules/uORB/uORB.h | 19 ------------------- 1 file changed, 19 deletions(-) diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index f19fbba7c5..672f8d8d1b 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -290,25 +290,6 @@ __END_DECLS /* Diverse uORB header defines */ //XXX: move to better location #define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) -ORB_DECLARE(actuator_controls_0); -typedef struct actuator_controls_s actuator_controls_0_s; -ORB_DECLARE(actuator_controls_1); -typedef struct actuator_controls_s actuator_controls_1_s; -ORB_DECLARE(actuator_controls_2); -typedef struct actuator_controls_s actuator_controls_2_s; -ORB_DECLARE(actuator_controls_3); -typedef struct actuator_controls_s actuator_controls_3_s; -ORB_DECLARE(actuator_controls_virtual_mc); -typedef struct actuator_controls_s actuator_controls_virtual_mc_s; -ORB_DECLARE(actuator_controls_virtual_fw); -typedef struct actuator_controls_s actuator_controls_virtual_fw_s; -ORB_DECLARE(mc_virtual_rates_setpoint); -typedef struct vehicle_rates_setpoint_s mc_virtual_rates_setpoint_s; -ORB_DECLARE(fw_virtual_rates_setpoint); -typedef struct vehicle_rates_setpoint_s fw_virtual_rates_setpoint_s; -ORB_DECLARE(mc_virtual_attitude_setpoint); -typedef struct vehicle_attitude_setpoint_s mc_virtual_attitude_setpoint_s; -ORB_DECLARE(fw_virtual_attitude_setpoint); typedef struct vehicle_attitude_setpoint_s fw_virtual_attitude_setpoint_s; typedef uint8_t arming_state_t; typedef uint8_t main_state_t; From 8da83cfc80d2209e41cb47dd1dd2831d18a34012 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 25 Jan 2015 15:02:06 +0100 Subject: [PATCH 182/269] objects common: use separate files --- src/modules/uORB/objects_common.cpp | 24 ++++++++++++++++-------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 78fdf4de75..204a114e12 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -121,8 +121,10 @@ ORB_DEFINE(vehicle_vicon_position, struct vehicle_vicon_position_s); #include "topics/vehicle_rates_setpoint.h" ORB_DEFINE(vehicle_rates_setpoint, struct vehicle_rates_setpoint_s); -ORB_DEFINE(mc_virtual_rates_setpoint, struct vehicle_rates_setpoint_s); -ORB_DEFINE(fw_virtual_rates_setpoint, struct vehicle_rates_setpoint_s); +#include "topics/mc_virtual_rates_setpoint.h" +ORB_DEFINE(mc_virtual_rates_setpoint, struct mc_virtual_rates_setpoint_s); +#include "topics/fw_virtual_rates_setpoint.h" +ORB_DEFINE(fw_virtual_rates_setpoint, struct fw_virtual_rates_setpoint_s); #include "topics/rc_channels.h" ORB_DEFINE(rc_channels, struct rc_channels_s); @@ -192,13 +194,19 @@ ORB_DEFINE(subsystem_info, struct subsystem_info_s); /* actuator controls, as requested by controller */ #include "topics/actuator_controls.h" -ORB_DEFINE(actuator_controls_0, struct actuator_controls_s); -ORB_DEFINE(actuator_controls_1, struct actuator_controls_s); -ORB_DEFINE(actuator_controls_2, struct actuator_controls_s); -ORB_DEFINE(actuator_controls_3, struct actuator_controls_s); +#include "topics/actuator_controls_0.h" +ORB_DEFINE(actuator_controls_0, struct actuator_controls_0_s); +#include "topics/actuator_controls_1.h" +ORB_DEFINE(actuator_controls_1, struct actuator_controls_1_s); +#include "topics/actuator_controls_2.h" +ORB_DEFINE(actuator_controls_2, struct actuator_controls_2_s); +#include "topics/actuator_controls_3.h" +ORB_DEFINE(actuator_controls_3, struct actuator_controls_3_s); //Virtual control groups, used for VTOL operation -ORB_DEFINE(actuator_controls_virtual_mc, struct actuator_controls_s); -ORB_DEFINE(actuator_controls_virtual_fw, struct actuator_controls_s); +#include "topics/actuator_controls_virtual_mc.h" +ORB_DEFINE(actuator_controls_virtual_mc, struct actuator_controls_virtual_mc_s); +#include "topics/actuator_controls_virtual_fw.h" +ORB_DEFINE(actuator_controls_virtual_fw, struct actuator_controls_virtual_fw_s); #include "topics/actuator_armed.h" ORB_DEFINE(actuator_armed, struct actuator_armed_s); From 7634147c0f9bb1ee27464d678394f22339b5ce00 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 25 Jan 2015 15:02:43 +0100 Subject: [PATCH 183/269] starting to port mc att ctrl multiplatform to the latest api --- .../mc_att_control.cpp | 99 ++++++++++--------- .../mc_att_control.h | 11 +-- .../mc_att_control_base.cpp | 78 +++++++-------- .../mc_att_control_base.h | 24 ++--- .../mc_att_control_main.cpp | 4 +- .../mc_att_control_start_nuttx.cpp | 99 +++++++++++++++++++ 6 files changed, 208 insertions(+), 107 deletions(-) create mode 100644 src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp index 822a504b52..1e40c2b056 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp @@ -104,15 +104,15 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : /* * do subscriptions */ - _v_att = PX4_SUBSCRIBE(_n, vehicle_attitude, MulticopterAttitudeControl::handle_vehicle_attitude, this, 0); - _v_att_sp = PX4_SUBSCRIBE(_n, vehicle_attitude_setpoint, 0); - _v_rates_sp = PX4_SUBSCRIBE(_n, vehicle_rates_setpoint, 0); - _v_control_mode = PX4_SUBSCRIBE(_n, vehicle_control_mode, 0); - _parameter_update = PX4_SUBSCRIBE(_n, parameter_update, - MulticopterAttitudeControl::handle_parameter_update, this, 1000); - _manual_control_sp = PX4_SUBSCRIBE(_n, manual_control_setpoint, 0); - _armed = PX4_SUBSCRIBE(_n, actuator_armed, 0); - _v_status = PX4_SUBSCRIBE(_n, vehicle_status, 0); + _v_att = _n.subscribe(&MulticopterAttitudeControl::handle_vehicle_attitude, this, 0); + _v_att_sp = _n.subscribe(0); + _v_rates_sp = _n.subscribe(0); + _v_control_mode = _n.subscribe(0); + _parameter_update = _n.subscribe( + &MulticopterAttitudeControl::handle_parameter_update, this, 1000); + _manual_control_sp = _n.subscribe(0); + _armed = _n.subscribe(0); + _v_status = _n.subscribe(0); } @@ -180,12 +180,12 @@ MulticopterAttitudeControl::parameters_update() return OK; } -void MulticopterAttitudeControl::handle_parameter_update(const PX4_TOPIC_T(parameter_update) &msg) +void MulticopterAttitudeControl::handle_parameter_update(const px4_parameter_update &msg) { parameters_update(); } -void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg) { +void MulticopterAttitudeControl::handle_vehicle_attitude(const px4_vehicle_attitude &msg) { perf_begin(_loop_perf); @@ -202,95 +202,98 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi dt = 0.02f; } - if (_v_control_mode->get().flag_control_attitude_enabled) { + if (_v_control_mode->data().flag_control_attitude_enabled) { control_attitude(dt); /* publish the attitude setpoint if needed */ - if (_publish_att_sp && _v_status->get().is_rotary_wing) { - _v_att_sp_mod.timestamp = px4::get_time_micros(); + if (_publish_att_sp && _v_status->data().is_rotary_wing) { + _v_att_sp_mod.data().timestamp = px4::get_time_micros(); if (_att_sp_pub != nullptr) { _att_sp_pub->publish(_v_att_sp_mod); } else { - _att_sp_pub = PX4_ADVERTISE(_n, vehicle_attitude_setpoint); + _att_sp_pub = _n.advertise(); } } /* publish attitude rates setpoint */ - _v_rates_sp_mod.roll = _rates_sp(0); - _v_rates_sp_mod.pitch = _rates_sp(1); - _v_rates_sp_mod.yaw = _rates_sp(2); - _v_rates_sp_mod.thrust = _thrust_sp; - _v_rates_sp_mod.timestamp = px4::get_time_micros(); + _v_rates_sp_mod.data().roll = _rates_sp(0); + _v_rates_sp_mod.data().pitch = _rates_sp(1); + _v_rates_sp_mod.data().yaw = _rates_sp(2); + _v_rates_sp_mod.data().thrust = _thrust_sp; + _v_rates_sp_mod.data().timestamp = px4::get_time_micros(); if (_v_rates_sp_pub != nullptr) { _v_rates_sp_pub->publish(_v_rates_sp_mod); } else { - if (_v_status->get().is_vtol) { - _v_rates_sp_pub = PX4_ADVERTISE(_n, mc_virtual_rates_setpoint); + if (_v_status->data().is_vtol) { + //XXX add a second publisher? + // _v_rates_sp_pub = _n.advertise(); } else { - _v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint); + _v_rates_sp_pub = _n.advertise(); } } } else { /* attitude controller disabled, poll rates setpoint topic */ - if (_v_control_mode->get().flag_control_manual_enabled) { + if (_v_control_mode->data().flag_control_manual_enabled) { /* manual rates control - ACRO mode */ - _rates_sp = math::Vector<3>(_manual_control_sp->get().y, -_manual_control_sp->get().x, - _manual_control_sp->get().r).emult(_params.acro_rate_max); - _thrust_sp = _manual_control_sp->get().z; + _rates_sp = math::Vector<3>(_manual_control_sp->data().y, -_manual_control_sp->data().x, + _manual_control_sp->data().r).emult(_params.acro_rate_max); + _thrust_sp = _manual_control_sp->data().z; /* reset yaw setpoint after ACRO */ _reset_yaw_sp = true; /* publish attitude rates setpoint */ - _v_rates_sp_mod.roll = _rates_sp(0); - _v_rates_sp_mod.pitch = _rates_sp(1); - _v_rates_sp_mod.yaw = _rates_sp(2); - _v_rates_sp_mod.thrust = _thrust_sp; - _v_rates_sp_mod.timestamp = px4::get_time_micros(); + _v_rates_sp_mod.data().roll = _rates_sp(0); + _v_rates_sp_mod.data().pitch = _rates_sp(1); + _v_rates_sp_mod.data().yaw = _rates_sp(2); + _v_rates_sp_mod.data().thrust = _thrust_sp; + _v_rates_sp_mod.data().timestamp = px4::get_time_micros(); if (_v_rates_sp_pub != nullptr) { _v_rates_sp_pub->publish(_v_rates_sp_mod); } else { - if (_v_status->get().is_vtol) { - _v_rates_sp_pub = PX4_ADVERTISE(_n, mc_virtual_rates_setpoint); + if (_v_status->data().is_vtol) { + //XXX add a second publisher? + // _v_rates_sp_pub = _n.advertise(); } else { - _v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint); + _v_rates_sp_pub = _n.advertise(); } } } else { /* attitude controller disabled, poll rates setpoint topic */ - _rates_sp(0) = _v_rates_sp->get().roll; - _rates_sp(1) = _v_rates_sp->get().pitch; - _rates_sp(2) = _v_rates_sp->get().yaw; - _thrust_sp = _v_rates_sp->get().thrust; + _rates_sp(0) = _v_rates_sp->data().roll; + _rates_sp(1) = _v_rates_sp->data().pitch; + _rates_sp(2) = _v_rates_sp->data().yaw; + _thrust_sp = _v_rates_sp->data().thrust; } } - if (_v_control_mode->get().flag_control_rates_enabled) { + if (_v_control_mode->data().flag_control_rates_enabled) { control_attitude_rates(dt); /* publish actuator controls */ - _actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; - _actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; - _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; - _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; - _actuators.timestamp = px4::get_time_micros(); + _actuators.data().control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; + _actuators.data().control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; + _actuators.data().control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; + _actuators.data().control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; + _actuators.data().timestamp = px4::get_time_micros(); if (!_actuators_0_circuit_breaker_enabled) { if (_actuators_0_pub != nullptr) { _actuators_0_pub->publish(_actuators); } else { - if (_v_status->get().is_vtol) { - _actuators_0_pub = PX4_ADVERTISE(_n, actuator_controls_virtual_mc); + if (_v_status->data().is_vtol) { + //XXX add a second publisher? + // _actuators_0_pub = _n.advertise(); } else { - _actuators_0_pub = PX4_ADVERTISE(_n, actuator_controls_0); + _actuators_0_pub = _n.advertise(); } } } diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.h b/src/modules/mc_att_control_multiplatform/mc_att_control.h index bff5289fdb..95d608684a 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control.h +++ b/src/modules/mc_att_control_multiplatform/mc_att_control.h @@ -59,7 +59,6 @@ #include #include #include -// #include #include #include @@ -80,8 +79,8 @@ public: ~MulticopterAttitudeControl(); /* Callbacks for topics */ - void handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg); - void handle_parameter_update(const PX4_TOPIC_T(parameter_update) &msg); + void handle_vehicle_attitude(const px4_vehicle_attitude &msg); + void handle_parameter_update(const px4_parameter_update &msg); void spin() { _n.spin(); } @@ -89,9 +88,9 @@ private: bool _task_should_exit; /**< if true, sensor task should exit */ bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ - px4::Publisher * _att_sp_pub; /**< attitude setpoint publication */ - px4::Publisher * _v_rates_sp_pub; /**< rate setpoint publication */ - px4::Publisher * _actuators_0_pub; /**< attitude actuator controls publication */ + px4::Publisher * _att_sp_pub; /**< attitude setpoint publication */ + px4::Publisher * _v_rates_sp_pub; /**< rate setpoint publication */ + px4::Publisher * _actuators_0_pub; /**< attitude actuator controls publication */ px4::NodeHandle _n; diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp index aff59778ee..0de832df9e 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp @@ -92,28 +92,28 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) _publish_att_sp = false; - if (_v_control_mode->get().flag_control_manual_enabled) { + if (_v_control_mode->data().flag_control_manual_enabled) { /* manual input, set or modify attitude setpoint */ - if (_v_control_mode->get().flag_control_velocity_enabled - || _v_control_mode->get().flag_control_climb_rate_enabled) { + if (_v_control_mode->data().flag_control_velocity_enabled + || _v_control_mode->data().flag_control_climb_rate_enabled) { /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ - memcpy(&_v_att_sp_mod, _v_att_sp->get_void_ptr(), sizeof(_v_att_sp_mod)); + memcpy(&_v_att_sp_mod, &_v_att_sp->data(), sizeof(_v_att_sp_mod)); } - if (!_v_control_mode->get().flag_control_climb_rate_enabled) { + if (!_v_control_mode->data().flag_control_climb_rate_enabled) { /* pass throttle directly if not in altitude stabilized mode */ - _v_att_sp_mod.thrust = _manual_control_sp->get().z; + _v_att_sp_mod.data().thrust = _manual_control_sp->data().z; _publish_att_sp = true; } - if (!_armed->get().armed) { + if (!_armed->data().armed) { /* reset yaw setpoint when disarmed */ _reset_yaw_sp = true; } /* move yaw setpoint in all modes */ - if (_v_att_sp_mod.thrust < 0.1f) { + if (_v_att_sp_mod.data().thrust < 0.1f) { // TODO //if (_status.condition_landed) { /* reset yaw setpoint if on ground */ @@ -121,71 +121,71 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) //} } else { /* move yaw setpoint */ - yaw_sp_move_rate = _manual_control_sp->get().r * _params.man_yaw_max; - _v_att_sp_mod.yaw_body = _wrap_pi( - _v_att_sp_mod.yaw_body + yaw_sp_move_rate * dt); + yaw_sp_move_rate = _manual_control_sp->data().r * _params.man_yaw_max; + _v_att_sp_mod.data().yaw_body = _wrap_pi( + _v_att_sp_mod.data().yaw_body + yaw_sp_move_rate * dt); float yaw_offs_max = _params.man_yaw_max / _params.att_p(2); - float yaw_offs = _wrap_pi(_v_att_sp_mod.yaw_body - _v_att->get().yaw); + float yaw_offs = _wrap_pi(_v_att_sp_mod.data().yaw_body - _v_att->data().yaw); if (yaw_offs < -yaw_offs_max) { - _v_att_sp_mod.yaw_body = _wrap_pi(_v_att->get().yaw - yaw_offs_max); + _v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw - yaw_offs_max); } else if (yaw_offs > yaw_offs_max) { - _v_att_sp_mod.yaw_body = _wrap_pi(_v_att->get().yaw + yaw_offs_max); + _v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw + yaw_offs_max); } - _v_att_sp_mod.R_valid = false; + _v_att_sp_mod.data().R_valid = false; // _publish_att_sp = true; } /* reset yaw setpint to current position if needed */ if (_reset_yaw_sp) { _reset_yaw_sp = false; - _v_att_sp_mod.yaw_body = _v_att->get().yaw; - _v_att_sp_mod.R_valid = false; + _v_att_sp_mod.data().yaw_body = _v_att->data().yaw; + _v_att_sp_mod.data().R_valid = false; // _publish_att_sp = true; } - if (!_v_control_mode->get().flag_control_velocity_enabled) { + if (!_v_control_mode->data().flag_control_velocity_enabled) { /* update attitude setpoint if not in position control mode */ - _v_att_sp_mod.roll_body = _manual_control_sp->get().y * _params.man_roll_max; - _v_att_sp_mod.pitch_body = -_manual_control_sp->get().x + _v_att_sp_mod.data().roll_body = _manual_control_sp->data().y * _params.man_roll_max; + _v_att_sp_mod.data().pitch_body = -_manual_control_sp->data().x * _params.man_pitch_max; - _v_att_sp_mod.R_valid = false; + _v_att_sp_mod.data().R_valid = false; // _publish_att_sp = true; } } else { /* in non-manual mode use 'vehicle_attitude_setpoint' topic */ - memcpy(&_v_att_sp_mod, _v_att_sp->get_void_ptr(), sizeof(_v_att_sp_mod)); + memcpy(&_v_att_sp_mod, &_v_att_sp->data(), sizeof(_v_att_sp_mod)); /* reset yaw setpoint after non-manual control mode */ _reset_yaw_sp = true; } - _thrust_sp = _v_att_sp_mod.thrust; + _thrust_sp = _v_att_sp_mod.data().thrust; /* construct attitude setpoint rotation matrix */ math::Matrix<3, 3> R_sp; - if (_v_att_sp_mod.R_valid) { + if (_v_att_sp_mod.data().R_valid) { /* rotation matrix in _att_sp is valid, use it */ - R_sp.set(&_v_att_sp_mod.R_body[0]); + R_sp.set(&_v_att_sp_mod.data().R_body[0]); } else { /* rotation matrix in _att_sp is not valid, use euler angles instead */ - R_sp.from_euler(_v_att_sp_mod.roll_body, _v_att_sp_mod.pitch_body, - _v_att_sp_mod.yaw_body); + R_sp.from_euler(_v_att_sp_mod.data().roll_body, _v_att_sp_mod.data().pitch_body, + _v_att_sp_mod.data().yaw_body); /* copy rotation matrix back to setpoint struct */ - memcpy(&_v_att_sp_mod.R_body[0], &R_sp.data[0][0], - sizeof(_v_att_sp_mod.R_body)); - _v_att_sp_mod.R_valid = true; + memcpy(&_v_att_sp_mod.data().R_body[0], &R_sp.data[0][0], + sizeof(_v_att_sp_mod.data().R_body)); + _v_att_sp_mod.data().R_valid = true; } /* rotation matrix for current state */ math::Matrix<3, 3> R; - R.set(_v_att->get().R); + R.set(_v_att->data().R); /* all input data is ready, run controller itself */ @@ -266,15 +266,15 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) void MulticopterAttitudeControlBase::control_attitude_rates(float dt) { /* reset integral if disarmed */ - if (!_armed->get().armed || !_v_status->get().is_rotary_wing) { + if (!_armed->data().armed || !_v_status->data().is_rotary_wing) { _rates_int.zero(); } /* current body angular rates */ math::Vector < 3 > rates; - rates(0) = _v_att->get().rollspeed; - rates(1) = _v_att->get().pitchspeed; - rates(2) = _v_att->get().yawspeed; + rates(0) = _v_att->data().rollspeed; + rates(1) = _v_att->data().pitchspeed; + rates(2) = _v_att->data().yawspeed; /* angular rates error */ math::Vector < 3 > rates_err = _rates_sp - rates; @@ -302,8 +302,8 @@ void MulticopterAttitudeControlBase::control_attitude_rates(float dt) void MulticopterAttitudeControlBase::set_actuator_controls() { - _actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; - _actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; - _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; - _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; + _actuators.data().control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; + _actuators.data().control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; + _actuators.data().control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; + _actuators.data().control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; } diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_base.h b/src/modules/mc_att_control_multiplatform/mc_att_control_base.h index cf4c726a78..124147079a 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_base.h +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_base.h @@ -62,6 +62,8 @@ #define MIN_TAKEOFF_THRUST 0.2f #define RATES_I_LIMIT 0.3f +using namespace px4; + class MulticopterAttitudeControlBase { public: @@ -86,20 +88,20 @@ public: void set_actuator_controls(); protected: - px4::PX4_SUBSCRIBER(vehicle_attitude) *_v_att; /**< vehicle attitude */ - px4::PX4_SUBSCRIBER(vehicle_attitude_setpoint) *_v_att_sp; /**< vehicle attitude setpoint */ - px4::PX4_SUBSCRIBER(vehicle_rates_setpoint) *_v_rates_sp; /**< vehicle rates setpoint */ - px4::PX4_SUBSCRIBER(vehicle_control_mode) *_v_control_mode; /**< vehicle control mode */ - px4::PX4_SUBSCRIBER(parameter_update) *_parameter_update; /**< parameter update */ - px4::PX4_SUBSCRIBER(manual_control_setpoint) *_manual_control_sp; /**< manual control setpoint */ - px4::PX4_SUBSCRIBER(actuator_armed) *_armed; /**< actuator arming status */ - px4::PX4_SUBSCRIBER(vehicle_status) *_v_status; /**< vehicle status */ + px4::Subscriber *_v_att; /**< vehicle attitude */ + px4::Subscriber *_v_att_sp; /**< vehicle attitude setpoint */ + px4::Subscriber *_v_rates_sp; /**< vehicle rates setpoint */ + px4::Subscriber *_v_control_mode; /**< vehicle control mode */ + px4::Subscriber *_parameter_update; /**< parameter update */ + px4::Subscriber *_manual_control_sp; /**< manual control setpoint */ + px4::Subscriber *_armed; /**< actuator arming status */ + px4::Subscriber *_v_status; /**< vehicle status */ - PX4_TOPIC_T(vehicle_attitude_setpoint) _v_att_sp_mod; /**< modified vehicle attitude setpoint + px4_vehicle_attitude_setpoint _v_att_sp_mod; /**< modified vehicle attitude setpoint that gets published eventually */ - PX4_TOPIC_T(vehicle_rates_setpoint) _v_rates_sp_mod; /**< vehicle rates setpoint + px4_vehicle_rates_setpoint _v_rates_sp_mod; /**< vehicle rates setpoint that gets published eventually*/ - PX4_TOPIC_T(actuator_controls) _actuators; /**< actuator controls */ + px4_actuator_controls_0 _actuators; /**< actuator controls */ math::Vector<3> _rates_prev; /**< angular rates on previous step */ math::Vector<3> _rates_sp; /**< angular rates setpoint */ diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp index 67ebe64cdc..b356b5dc0b 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp @@ -52,11 +52,9 @@ * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers. */ -#include -#include #include "mc_att_control.h" -static bool thread_running = false; /**< Deamon status flag */ +bool thread_running = false; /**< Deamon status flag */ static int daemon_task; /**< Handle of deamon task / thread */ namespace px4 { diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp new file mode 100644 index 0000000000..e982ae3545 --- /dev/null +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_att_control_m_start_nuttx.cpp + * + * @author Thomas Gubler + */ +#include +#include +#include +#include + +extern bool thread_running; +int daemon_task; /**< Handle of deamon task / thread */ +namespace px4 +{ +bool task_should_exit = false; +} +using namespace px4; + +extern int main(int argc, char **argv); + +extern "C" __EXPORT int mc_att_control_m_main(int argc, char *argv[]); +int mc_att_control_m_main(int argc, char *argv[]) +{ + if (argc < 1) { + errx(1, "usage: mc_att_control_m {start|stop|status}"); + } + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("already running"); + /* this is not an error */ + exit(0); + } + + task_should_exit = false; + + daemon_task = task_spawn_cmd("mc_att_control_m", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + main, + (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + task_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("is running"); + + } else { + warnx("not started"); + } + + exit(0); + } + + warnx("unrecognized command"); + return 1; +} From cd35ab26610650d93120d3a9eabd196a733c2e44 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 25 Jan 2015 15:32:40 +0100 Subject: [PATCH 184/269] add another set of uorb headers --- src/drivers/roboclaw/RoboClaw.cpp | 15 ++++++++------- src/examples/fixedwing_control/main.c | 20 ++++++++++++-------- src/examples/hwtest/hwtest.c | 4 ++++ 3 files changed, 24 insertions(+), 15 deletions(-) diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index eb9453b503..5b945e452d 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -54,6 +54,7 @@ #include #include +#include #include #include @@ -114,7 +115,7 @@ int RoboClaw::readEncoder(e_motor motor) uint8_t rbuf[50]; usleep(5000); int nread = read(_uart, rbuf, 50); - if (nread < 6) { + if (nread < 6) { printf("failed to read\n"); return -1; } @@ -131,7 +132,7 @@ int RoboClaw::readEncoder(e_motor motor) countBytes[0] = rbuf[3]; uint8_t status = rbuf[4]; uint8_t checksum = rbuf[5]; - uint8_t checksum_computed = (sum + _sumBytes(rbuf, 5)) & + uint8_t checksum_computed = (sum + _sumBytes(rbuf, 5)) & checksum_mask; // check if checksum is valid if (checksum != checksum_computed) { @@ -156,11 +157,11 @@ int RoboClaw::readEncoder(e_motor motor) static int64_t overflowAmount = 0x100000000LL; if (motor == MOTOR_1) { _motor1Overflow += overFlow; - _motor1Position = float(int64_t(count) + + _motor1Position = float(int64_t(count) + _motor1Overflow*overflowAmount)/_pulsesPerRev; } else if (motor == MOTOR_2) { _motor2Overflow += overFlow; - _motor2Position = float(int64_t(count) + + _motor2Position = float(int64_t(count) + _motor2Overflow*overflowAmount)/_pulsesPerRev; } return 0; @@ -242,7 +243,7 @@ int RoboClaw::setMotorDutyCycle(e_motor motor, float value) return -1; } -int RoboClaw::resetEncoders() +int RoboClaw::resetEncoders() { uint16_t sum = 0; return _sendCommand(CMD_RESET_ENCODERS, @@ -278,7 +279,7 @@ uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n) return sum; } -int RoboClaw::_sendCommand(e_command cmd, uint8_t * data, +int RoboClaw::_sendCommand(e_command cmd, uint8_t * data, size_t n_data, uint16_t & prev_sum) { tcflush(_uart, TCIOFLUSH); // flush buffers @@ -299,7 +300,7 @@ int RoboClaw::_sendCommand(e_command cmd, uint8_t * data, return write(_uart, buf, n_data + 3); } -int roboclawTest(const char *deviceName, uint8_t address, +int roboclawTest(const char *deviceName, uint8_t address, uint16_t pulsesPerRev) { printf("roboclaw test: starting\n"); diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index fcbb54c8ea..d8b777b91b 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -61,6 +61,10 @@ #include #include #include +#include +#include +#include +#include #include #include #include @@ -108,7 +112,7 @@ static void usage(const char *reason); * @param att The current attitude. The controller should make the attitude match the setpoint * @param rates_sp The angular rate setpoint. This is the output of the controller. */ -void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, +void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators); /** @@ -133,18 +137,18 @@ static int deamon_task; /**< Handle of deamon task / thread */ static struct params p; static struct param_handles ph; -void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, +void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators) { - /* + /* * The PX4 architecture provides a mixer outside of the controller. * The mixer is fed with a default vector of actuator controls, representing * moments applied to the vehicle frame. This vector * is structured as: * * Control Group 0 (attitude): - * + * * 0 - roll (-1..+1) * 1 - pitch (-1..+1) * 2 - yaw (-1..+1) @@ -226,7 +230,7 @@ int fixedwing_control_thread_main(int argc, char *argv[]) * * Wikipedia description: * http://en.wikipedia.org/wiki/Publish–subscribe_pattern - * + * */ @@ -234,7 +238,7 @@ int fixedwing_control_thread_main(int argc, char *argv[]) /* * Declare and safely initialize all structs to zero. - * + * * These structs contain the system state and things * like attitude, position, the current waypoint, etc. */ @@ -300,7 +304,7 @@ int fixedwing_control_thread_main(int argc, char *argv[]) if (ret < 0) { /* * Poll error, this will not really happen in practice, - * but its good design practice to make output an error message. + * but its good design practice to make output an error message. */ warnx("poll error"); @@ -340,7 +344,7 @@ int fixedwing_control_thread_main(int argc, char *argv[]) } if (manual_sp_updated) - /* get the RC (or otherwise user based) input */ + /* get the RC (or otherwise user based) input */ orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp); /* check if the throttle was ever more than 50% - go later only to failsafe if yes */ diff --git a/src/examples/hwtest/hwtest.c b/src/examples/hwtest/hwtest.c index 95ff346bbb..8fd8870da7 100644 --- a/src/examples/hwtest/hwtest.c +++ b/src/examples/hwtest/hwtest.c @@ -47,6 +47,10 @@ #include #include #include +#include +#include +#include +#include #include __EXPORT int ex_hwtest_main(int argc, char *argv[]); From dafe6654ebd2730c0ac5c3c60bb5fe8946ab67a0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 25 Jan 2015 15:35:25 +0100 Subject: [PATCH 185/269] add missing functional header --- src/platforms/px4_nodehandle.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 80be9ec527..83a3e422dd 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -53,6 +53,7 @@ /* includes when building for NuttX */ #include #endif +#include namespace px4 { From 23229317ad5e08f758a4d48148d36fabe2a2cfbd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 25 Jan 2015 15:35:42 +0100 Subject: [PATCH 186/269] remove unnecessary include --- src/platforms/px4_includes.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index ab27e6b4d1..1bd4509caf 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -70,7 +70,6 @@ */ #include #include -#include #ifdef __cplusplus #include #include From 9b4a21049550f12f11ec0e6dd2d2a55e1b2ef1b3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 25 Jan 2015 15:55:32 +0100 Subject: [PATCH 187/269] missing headers for fmu2 target --- src/modules/fw_att_control/fw_att_control_main.cpp | 4 ++++ src/modules/mc_att_control/mc_att_control_main.cpp | 4 ++++ 2 files changed, 8 insertions(+) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index e0b61e2e27..88918333d2 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -59,7 +59,11 @@ #include #include #include +#include +#include #include +#include +#include #include #include #include diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 228f142d97..562033db1d 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -63,7 +63,11 @@ #include #include #include +#include +#include #include +#include +#include #include #include #include From f7dc81ded1bdd7eb7d88532e5513b168c6d4f118 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 25 Jan 2015 16:13:39 +0100 Subject: [PATCH 188/269] missing headers for fmu1 target --- src/drivers/ardrone_interface/ardrone_interface.c | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c index 7f1b21a95c..7d4b7d880b 100644 --- a/src/drivers/ardrone_interface/ardrone_interface.c +++ b/src/drivers/ardrone_interface/ardrone_interface.c @@ -55,6 +55,10 @@ #include #include #include +#include +#include +#include +#include #include #include @@ -98,7 +102,7 @@ usage(const char *reason) * The deamon app only briefly exists to start * the background job. The stack size assigned in the * Makefile does only apply to this management task. - * + * * The actual stack size should be set in the call * to task_create(). */ @@ -319,7 +323,7 @@ int ardrone_interface_thread_main(int argc, char *argv[]) /* get a local copy of the actuator controls */ orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls); orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); - + /* for now only spin if armed and immediately shut down * if in failsafe */ From 2c0029235a08864039cf1239f86a0af80039548c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 16:17:03 +0100 Subject: [PATCH 189/269] Documentation: WIP on better block diagrams --- Documentation/px4_block_diagram.odg | Bin 42200 -> 91712 bytes 1 file changed, 0 insertions(+), 0 deletions(-) diff --git a/Documentation/px4_block_diagram.odg b/Documentation/px4_block_diagram.odg index 95b6f600b87aedcb06535d5b330afbdb76d91c41..e66e1b1cdd704ebe713a2127004488aed7fb2218 100644 GIT binary patch literal 91712 zcmZ^~RZtvE6E3_sAtAWCySuvtx5b^{?(XjJ26uNBcP9|s-5r9vFYL+pSN#{~;#^GE z)YR0})O7cBKTpr7$iu*50sx2r0E-2mbUqC85CZ@J_+R@s1hBWZH+S=PGBcb zakF-GVDWJH$?Rz2YVFGG=w$Bj)6vY`-rT{B+0wpL&-)Ra#f`$Q7Z>ScAV()fc-@Gc3K?mvJS;9EbmBs_Oy=GK^ZnQLcdl_7BJDsNjd zs7Zbr_4!kD+GD5Jz~Axtx%#cQuD5{ok7yLCSa{<$4l4PO4>_f1lqkjjQ_#X1$B7}n z8qt%~JRiXlfg?px-}*-1J_*Ihf*?%yeY`23@j-v;>aTvOsvz#pZ~`Nl)93>GSAwfK zf=SH4BCjkkm;l)9$a56k8GRz7&r%I=<9=X)aFh&`^gdMTZ*pysih@5cF0_z?Wj)*g zf6^2x#Z}^gJ)|4k5S9e}x4s8X${ra$=jbs#w)pd5(M?T;YT z2v_$Wj}Q(JuFnYG3l_`Z?pH%o!U}tFnPXfdH4hm^w@M5RM;GY+k|sB)zN_-h>st!b z4Y--N!fSVk{cn75D*fN6pj4zNyaEj~*g>IOKjY=xAS4Xvk4;QoRzxIV8Y?NJOFNwB zF(_o1Hkb>VpMq2!D&o5-t^y8R-E=Kr`o|yL^b8)R7pA)oc%GGIcd2?rz|h0*7thZ_ zTHZ#uZ!N+9!O#;E$b}rCZ!Qe~hsN6r?XzE?ju6XL*nm1{=}=Z}td7KrW1L!vdUbBz zfxo#tz+Zeb{6mF2Zsv*}w#j>z0O1Gn=HkS>Y80Z^A)!zs3^GH_en<%vQ92opAB53s zgjrVbnEfy6pAJjYTVCGSeyX#BPI4z$RDPPEXFF5Si1b&0X;MPJK33e%)oq82>V_xU z?)rWWq9>_=uNXCw1HDSKzk-_ZRLCyM9C&k&2I;BoKtxiUs6gX6GyTvrCGiF54*CfV zF-uH^ny94@f1Y3un4BCK5KI|igHJnVuRnql;jRkml9(hGbr5PKX>{LE#I9&PV!!)0 ztl)+F$$meiy6SglWK25;j8GCBymZ+aGa!!WkTjS5^!)G4lkYGOa>PZ^2dRO4vsftdA2&6pYoYbrGuPU8F&tH zp-Q37Jy9DdZarB00CH{En?2OOY?$$gD6oOLX8oMQekEci>*R{Km7@4vs`l=A^JG!b ztZ5hs^}p}F;p0yLD?`G&^9?c4jXW|!UUnbbZYUVA93GY*4FZY|qKh}NPs;h2Ay#GN z4wN^Kun`29=OM;uK~TcQ#kx~MLTO~Z{bPOUot!N9PKI|mKia~b%zfM{SQ;vM9J(;Y zq7F-DN;CTsbceL9z4Sh~1_DEzJ~CoZF}|-EZ4pI!5y8d*6l4$m#y+piPV^^2)im(( z&!Vqy($(Zj>%R&~3mYRtk5cIW@z1APc@yy2hk#MOkGM^?;J0@+xsK5cG7#7FF0N+V z`s44f#2KUYKff_lWrT1Q?f?>JHOTKiH#TOJQl_Ui^beFrTB5#}r`xOB9CaovpmnRc zbogKZ_YRRn%9$&NLsA)Z?S`b<;f@WAyNm9o-67h1tjm@swU)x1pn5IdEhD zKSw@T^u5)+c}K~|s7my~{>QXstE}bSH#0LZn%{3>H^v_35GerjcaTh%da@^EdHQs% zsEE`^dKOD<5Jj69OHU)tex1(n#PLwA6K;jV>r3IghldBHn-U;w_Hi`JiI8j$zULFs z1kHkxrHoCMHG}~K14R)8o)~@w>Ak0HAY>ICNB;a#ZuQY{J!R(;lr=r|W(drMjcac{ zMwJ>CcOrP#zNvySJkL6m)qqtO9l@H2E;3$jq|$uO7~d5X!Jp}a^Jz&gbleavBbBt~ zh4cyk+pq9M6_PMpBltxfN0nmq7vyB7D^1=Viw^NEIDd+C)`!*Ae@E0u5^Yy*F>UIS zB=8wTA}Hi>3~mJNmyvS+a;*9u|93Uf58LWyZyZ&Y`R&?&WcrEwpc>+p#h_Ii!mz~P z-0wm6N2O7eQKss~+`zo!MsR+M2(sGs6h-jGyN4{(!nI%RS&7=*S0_7yNelGOt^^gKLgL}U9WYwtEhlm%-k(-5?}3vH_b8SnJwuw zRXwx$^D8wEi8qh3z2eMWmrA>MP&{Sqc^+nZze7s{zA%TY42UmSDS)_Q?RIirVxJ$t z7paqy!GxQ{1i6*pJ@EN|T3XhAY?vwcw%-kUO)WZCw^UbYCqxv)l=20i9Z0CO{*Ao4 zWD~qjvvo#Z?a;pc^3+sMp}?uJhajnbjD)y_5|Y%)M!#Dw`)LpduN3^-iBTXj2y__u4; zBv&LfN+o_J3@)Itf1tU!9mChk!30{JO)biTp!pI;@^3?3I2FQ;mLRYd5ixS!~#Zg-j%pV8+d&4Olr@ipQSrmVilp)rK0?O{cy z`ORh`DE^z710{!3$Mrl3(pLd-?Tx^Py$>)_w#3qmiI!Fp=olWW_>}%IAFESr2l1bi zk`717;|3PAD&;#$D(Oz6IZBe3tB#UDt*#X5(wKP-`)1R>m$JM{-1dPO@|+uvCg8fV zlARuF!{kxkR(APNW4uJ@!M(`Wm;3;3Y>BYaKW%9D{?IZ2He0#iU-Sa{uBf3952K~z z(8aAv+fK?e8S$vU{K$3QdODj~oOt^3UODY@=$HudJ0Bp!5VnIIoV z+HS+gf?JPFQ@@_RnvDB~DohKO$?gYMP08k5V>+QtlW000$Q3rEZ;{>Y99l@aK^ts8e|uxGExwljh0We{=~$P@M@MaWuc)DSj*V)^ zHrg?t+7|kfmLzFGk$7C@iP)HNKp0wb-B@^9ew;?FgYgYV!=RvE3d+q`Fjl`J38=&x z-_@q5Ih~|L^&~?Z0ZQ|QHzdAQh*ePV-4~^1miv=i#BI55P*U$cei>zeJSap|M&s}R z1i=ZUYZoug?2oTdNt9`h`h6oK^03<^;-7QcAmGr`qM^_4>>+51udV6}56{|US%xFH zqUioysvf--eVcWeRmRQ3Gox%QTZ^UDSrjz$(V8Wh%zdm&yT103_S}{EBZ5VUKBfP}ZK!>9N#rU0zdJz?V4V2_q{}?UjUUNbK}Yui&Y8w|!#@wDX<9tLOz2qO?nn3_#XcVxkH~`J7?FcfY^b$(8SpL zj?h~&^#nJD*UER_Kp43q4hsv|jL(*>vM7n>cpGSZnv8a!9}DB>qM?wKJDdegSjABC zmJa!ZY`2_Pz^#tZ4-c6iel@Jal3bzZSkm0!Fd=EB#E}_>jfr3FQ;MzAyLr;(8RlusrW)4sPf2 zU3bqc8_9Ly_MQ+LUQycJ{&-CfkR;S>vE?Oh!M1#GW9d`2N&Nj|Wo*iPMi^Nswf0D= z*wOcM6@%8a>Q>#4c3-WeQhY+v^cp;-+6-GTUJrKpn2)*`xNH_^{p2DG6$g;amxQvx zY;zz6_@KE_n)Hqrbw(KwDExk{kxih{n0(OB;-MK(!_$UcBrK=`aIk8@w>d&P!|7|I zaS6SmXdNMS4Ss{u1yy{c&6(ysK&DR#pZRvY2oyQ1T^Fi4e2 zD-#7qqd0?G(!+?xmOS8uAoET7-kMygxn4j#%Oz;BYxzv>Avxtg3*GX}LQbNqW9H0~EPkSqD!naDm`<2Q;?tuvN)CoF{@@?Jk08+GL$H=VpGSUt~_sugL^}_Yxsup6ikw9 zHbJZo;@T1$rCD!PY$JZX>C+|@sQe4BJ!J(I@5iS3Ji)oW(1}G|T+{I?M;^Tx{0up_ z!OH2m-?>~okfWLavoqc_+9Sf_+Z3!WO0;wNiTfyb=qPB^)CAyK8Agf#;Dqf7;8P)i zJDSh-rjj+`Th}v%+~GdyRWXG)Rzm%OdT95Wie1CE8PEI@{%yelZj1p6nnuMP}D=Gt@ z_+y}{hQ`dW_)=#l?GgnYZBx5i;;inuNLAk&JD~Dn zcrGw-4!YG3Hbw)Cm$0S}wg;!h*!2RiV3j6gQwim8q#86633gpuDAe}OWnR>kJHrrb zQ3mrW zPX*>&B|zk+eFdO}y36ys-<*DtLfwCcAtABWHAJIEd)x&Np#^Aw#TLEfnu;p&K7$KN z6YK8R$6b6Zn;-Yj|4E^oV*N&}gd=}!)prFV#{%F#&*~ka*~; zWX*%ep`%lP21U+IS#MBP@oF(l4T4i~<35AV^7E{Ae^32L4aCwef1uZZXi!_vUeo=V zw2p0JyhTe|f1JB3?&00wC-R{F53Xd=;U>me68rkZd6A2XYI0#ZSSM5eX`hxbqkf7) zoY9XzPPugr2-Xwf)7D%h*K2DId@0OT?h*9&_}O8&ua6CLRP0QlGEl$=ZyVN2A6diQV^s9LsCXl5s1b58FXf}zrQ6V}na3NF^YeZw&d)dA z*kI^jJ| z!Ne7!Qad=`0}+ZZKhQ*bzBXwNq3T>EjO1hq2xs=38#`K*=IAgpkgutYy3P+t#aK{>vbgt@cAub<75hd~Oz`=74A{rgwJMa|N@dnbX%6RQFfdzIPc(T8m9^n*dX&>! z#>UuiDRT_{r{%6wx{t?S6s8Jie>H!CGZAYZ;zl^9+Oc2ypUrkzp1e{{Rv#q6L}WOi zC$NtoF$hgmeDVsnvPzux2h~)hgZ0LG<;O*fogVvtas6#u>)Q)u`WbDAYCij34aV7> zS?mCwESAX)8kV$ambD1_6kHLS4CUYbaBW*T0V};3p#I22=B>tBe`OxQUMnVxsLGgc z*&1pXAhm4gmczr8mO%LuwPKPkBjvN3>i}L)VTp$Ea#uF|{CBKm6UOE2M#XD^q^pDj z4UO|31zI!=p#}|;KlfZk^!-LQeBlpPXMbIO8Kyj}>*b&k|6w9Dj29JWd7HD4U~**S z@0P*BznQSJZYJ!{xUoax|LN5_bYrkV9-YC?2YegQIiC@k58M}DtIG<0J#^f)nR*4A zO;nY=w7pvV55bK}{fvm9wx~WPI(;2JlS}>U43BpqXQK*pwT*d1&LyO|-9yz{shDp7 zQmExcxlqoETIE+of&;Nog>Eqa)_ z!miHnWG`S=%5EQf@Jdz9Q<`W|btpHX&0DjY$@jjy_kt7H>bH_WJNF`aWse_&jDmGf z!u!GeBO%z`=ulxPXZNm=#Q*6}tsvTq(} z*Ip>XrIr7evnR?BwINXoI~s0lm`91?Tc;@vQRGr#G!ywISL<-NC}|o^J5*?z%@-2<0XeQDub; zG>zIT1#BcgRJ0_n;08(zZHXo&)u z9K6>bJN=37`ei-hs0$klC;~+Pj(FeTw!f~?H0b01Vry4_7w8(+QVLY&e&mYp92;TM zMUJA4*24MGN%OtbRpl7g)8)vxpwHe1R6#F`pKe;Nqa3Ae%BV+4hcDQ)Ya$if{6&tE zoOD>xaPAjYRaKPlv1RK>ArZw!Ysssn-P(V|B#~Lm6@BDBUsettnZye#V_drNFD`?1 z(oGk3t1_sKuA)|mxWMe4zUbNSuip~?l)LoaUU<@7DIhIy6GdXpRpK_6erzDXw>5R} zEtcIatpD`;9=OFR8t3|(48ie~M`P47-u-PZpal^hN>b^jEbnhQWK4`qJ6V( z(r7@s6Fr8nB*t6c-*XKpw|tS&rJ--`q4y>YHOz$WkozLnC^d(DcCfvgW+DQ~82Ou^ z^@EW829@EVxp+oQT{(~v#JF8pq%VLry)50xE%{xjj;V^T%w zdRLhvQq;V(+7p>BrLD--rK% zT1{yv#>X1w^N8%T|3lIm2~W1~G74ZWmzH7|G)f3-6E1QgC@`ISca*=rQek1#m5MFd z2V-fvYqN)xYuQOiWJSmU8y#)^OnP-aw3qG-iRx}*_d@j03E+zUV{UUQF8f1rPgSb_ zc-7l-e<@Hj!jfV?N}Zl7^Z}oKdc-~loUDbM)=XVn@tNi3)SiaGxX|bwEx^JvNf~Yb z{FfL~f}KbVpdx}jzf10T$Vb8%%>2$p`Z$-t=`d&AB=J8PSlm|&w^OQN?0}JJ4p8Ug z(7a^b2Zqk%{%y#wAtYgG`wPR0v{y}0y0x*s?cRpB_lEIkk-m9N?B4b3ZJC+Nv8gw# z(U-QfTm~Y(WF32;KhWdtNsbkHa3cSO2&<~3@Jga@2$rg#&R`-X*OORIs+c$53@ul$ zOxL^N3FOPqO>?tgiFB!|uDX}7u`aa#`R=IHMQmv%w1Umeh$3S5176k%)Sxl`Qrsr> z>cZA)MOB<+YDK;10B4lbghafG4ivecVoM}guF2z^Ipe!`R60d~q6~5hjZt7_?fU6| zX(`Vj)y7bXHO@R#v@McfLNr*#b$w#_RX@L8zg6vqe|@Pvn4YVOwcyN(?L?dT2&0~+ zNkTG}jEG1mcPw>_=@(GgV{nKn_NKB^VO1+X2Lg|opdq}$0S7sMeq_Slvb4N^wu|-q z7>UiZYrvdy?%hXS^ZHX)H(EM1yIag+@!dWeyoC|#SDHD~*-4MS#(*`yR5+r++hJ@S zF^@NrHc}zO!n{$GR(An2F`PDfgFwmvh4D()nLuqh9{BipCt3~?Yt?tNC4T>Cd+aPe zBFSe`A1s2OPTz{gT1naPE7-P%9~D;l*877Gl@N>1_Jv%0zMp9`?G`_>xq(b+%z1gv zXc6U&dZ6nzk2HyQnLZDF`>0E>t1Kh)p%xIU>#TE-?8`;g`)ZD0eDs7>XOhK>xofaS zIGm6if%ljn>gbuOkACTCl!XgdfOyA#P=v@xRdlS5c1_H^d384ArG>2lMqGy?21^W! zTPfWTY9sc61{g>CWvig?br-4ez@?|-BT?Ad$)ux(uKkqnR)W{MSgbxNBtpIZbji)gIvZY-C$NHKD`15*Ow}!X9VQY@j4v9btfS*v{BLlm<|M4Ia zHNJ7E*@p9v$s`PFR9sCM7`jrlx4l1qwdWe*Wj>G?R(!nr5OOSOSplloXj4QFJL9fI z`D{=~Y|U95gcyCUgFns5JJkpN_D^$mdgNzluAD42Vrn`rP+;~Z3xJU&cM$T= zUCtBKb3h+Af<3H2cqv+-zY6Pxk}=jmn*LyQ#qs$^arYRM6pDp$6}y33Pkc-PYCj~) zT^}z&Bo+{?f|eWhB64%*7^H$i(b6HUF1_QIw(}OENBi9weLVmWQ8MEd&-cy)D@GR7 zm#W^{C^Hk#I%QYQC#9%Jx{FnlDs58k6Mq!8rrMg8DI0N8lnAU@@n=8x|LTZO8b%BUqE)xsy3rPJ(Q@lprjGFJw#A?A-?6(|X5(brs;&1|>B^!x zr50O5mNcgGYFxu%$Ov`$HPQzIN=%aWcSd0KIV2Z877)yi@97-X-m&nNKbiS2FL+8 zNXUg+fBFiIw?--EE|U{^98umf(RttLi-G>*I>PB|3T7SM_7u#Xfh5aY~+Le8f)cBoNQ40b@b1QAGPdkA4b)=d5(nD^z>S_BmqZ! z4A-wzq4>9mLpx3?nDJ_fDpmAVCmrne=}ySFgYsTf@;@680-sg*x1Ls4@>!Ucor<5G ziZRoF)3`6pY8M-#^)*+SyZ)BOpfaooaXt?Moou}qW_w(1|J1vZo)6;2{zvLVffmh} zgNtaF2r4|i9}R+i;5)xb9lQ3W-d`|^$#F!)Nfy~RZqSW0{d8AX#%(wX_oH8ujwawx z4_BBjl?19R#E9Z9$z45!mpwhQvhCk=7nOQT5<#B0iZH}>z1e*l-#vo_W!Run7hW&u4^rar(-$O@4 zS~C%$mPb4l!Ti$iGsBB>j(DvrkRdR9=0pF@A6hib7Vi2+Z~HZ2PAy zGw@vof?R2E-&Bm!dy-%Mv?=0u;98m$f1YX^&K(LwOZ;Zur%^HspPO{T+mr3Fl7c5O9mB9!@=vEmwCz5% z=dvR)QmfaW(<0$59r-<8Ze9~p_-;69$Cb9JHcP<+xkP2${Cwga<`WOlMLe0j5)d5BC9{cPt)Ri z=T%k5cX(I~c@kOlNtQ5jutbC##fDoss9JF;lbbIG%EC1!mp_$f#$4-8JFPc>iT%Ob z19l#trCNDVaoKm|cm;g;Jg!mlER{9r|DP402bhumIT~JV`k0-+Xn-8d^cpYZ$UKIr zkGIzd2mw@~bdO52Y^ft?5(3YcI4022HvG@GB2ftG&C~OQJp_s8{CVP_Qy-2cJsRV< z4m~r{)MO>`zk~v9#J(8-G(wf1mi@d5ZC6eE|IXqc1(q~Q>G-CzwfD$}pOwLEEzoG` z#v3E!VR<&}4;+orF%`Dg)Zm&1I`j&bNlIDT<83zZ<0B|l{39jLcy17#kzrE-kAf_5 znZ6IN*Vd!C28J%1NHDq<9$^`$2HCBta`KoeC1<5--VPQ+Pd9rXZAQHJ^Yfc8llvi> z$~i}A05;Cc-2C>1)?8q^QgIi*?J7EebLupW5rj=7waBQ31y>nY-_`?BIU?eko!_YF zpi!xFfHtn)+Q)(FvEu}to>pD)Z?lX>e?b$49@YI)0eaJQfHMFABG%+3yl{# zicn{q<0>$ec&+6$$4 zW{2&ilN2G}VKcO8Hf%OCSxlosRSsRI^o|n8vn_L=Ci`n+8NEVtV{^wQ#{Te>l>?Q8 z`8rIWcAIn>4bsE-lM!|p0B$K|aJO)t0rU)0gIi<)d6#s_W77jF4y+nZ=LV{bc(nAVAYgvRK+9$^!b;6KP zBzfZPCMN82?M(02*d55WP+RZE;qZO_rzPZUt-SAc^j9LO)Pdx0`Xn;ZB8zB3c}aOGu{rJ>9FvGk3)_a`FaGXfynXV5&tE0Fi@=7uu(rCEA!{tgXmrKe-te7BC3i76G= zS_z668+!25qd(Q=imCv~l%bD_pW1G+j$+Xx{TV7#vU<|8Ub~n46Xcjt%aePF9iSjSa2J!{bKR=j~6;jcJ1%Ub+xAG%GqxhVddN`%*Jo zaslQiD9Lte*I>JfIczOKHFgSKTg8~IniNJ>DFZB%SVo{h1gEMt5BgHjCW5Rd!S61g zZv0C^2L?vXpDV1BxFYW2KPv4}y-Nra&!{}pY>h5M`ETWXBqO5c@5HHQ;w?a6Nr9s$ zXr9&L8c$z=zSShyKEOX%nPf00R`G|+yv{Hys4-vgiAmT}fEdHRj?5olh`F0tmMkw8 zi`CgVi~$#XZaS%sX$;igQFxLha4WD~7ZCOn;Bhr+vK_{1FR@cuC`$_jBU*N#6yR4K zuWuWmY&Ju#?32WHd|h?M3jchE>9X^Kh`DkGklJMAN%)g2B7b7T;n~1)*If?k znqzW%{;H$MYvMDe{pbS(Qvfi-`-h*fe}67;CAyAnNZ|P%B(WV^g$~&u!~#;#CdSad z@r<@X7{s(kVocTv^w~)|VKe{}ZZ5 zZolu_n`oN4y|O(Vpr-a>7+F#UQ3WNrJQ;M>13tM6T`F1bJ-feERHA<`K~Jvl((R~R ziw{AHOp4;PAaHRB4-j*eI=5A333#-P+ph0Sczhy=>uAY>{Fr={lmfsCZKx`!4?TNJ z#!J5N(DsUsX!63SN&sstEyfe%iOE2g0i#e`0JXITmk(4-=$HL7-j>^BtxkcBudg|H z@xT}<?T)3a5a;>iu-4@Icd@~2J`ldEVHk4@>SJx6Lns=EY zKq}a8-yGwu04DmAmhDMjYCR9of_^{)rlF{zDFGdYuSw)eU-&z6GS3*}a+a9Xi1OLH z_^agQM`!O(W+*+bVjn$#`W7cmyqUzEr`Q}=T*oJ0acFvCmi!L~yo|=_bcW4Dp5Nmg zVgE6bE@tT#>}#OBZ8hfX2F97V1X=%stR-E}dUd|5PLR)-7Y|}V@T^b~R zwso2IX2=fduP|fFkG4T^3raG=b%Q!ZYYHLJBZI5%mib=Yg4yTo)-v8M-GbcWDSwwy zx-BoxjbguDi<+Id;#jK8`yPpW0_`>o1&kzdv1!VpE>#kbWu!6m+S*@pei?|xG>Vn| zD`)|tcBd+%V*`BA@yua3))W?W257wctH7x)-rZN5=`^pE5wWrR28iTJ0Fk!p7Sm1l zKVpmdQBeLXQ|njC-9+qvcSx}zHA2`*c6j6Rx9W34fu(-88W9@`UquX(N+VM?n?%M0 zKVUT$2P<7&gPcnd52c|bTTW@Rg-JwW!UzBz=|&bv(pldd2AA8-1493$ZZ}55tO~UB z#k5RRx#z;u%bnWsQH{sCOebnPaDRDB$`{=Y9FQm6WV*TY%@*=UB|*jIqD6F%;vKfL zhTwLp5_Pz*EDpdG4G!`e=)Ueba|W@6HaJa*d}blBZI;^yxt2fIJlH^~OXj}S$_(tN zYgz{#Gr{$jaqt14xr*P9vqf__th1V=dm0fN>Srx%cRg$1!(R@452=Lfmca1pxB4TB z+l{r>9?$FT4Tr8;6PF27YL5aA@z>4(ZWa z`>|(j;sOG32-LrTRaI47Ts$N6oKt2^obPfpbNv-R9i7o#o$ogXP}N|ivh5a5WkG|v ztk>^v)-mt&{QUetD1w%zCNGri@4eNibIO*iQX72C!WH8E15?T!=10-jwBL+h?GNJDICws^Ry(*q+hk9U z{J!PWxst6Dk^zYhK!(=yw^E|>LAom~MauOG%ac1PgOyivU*bLdyps<6K#;B9e@0ar za-rE7C(|=CGE(jo#hP?{#9YY5y0tM#`jUdJj1#QirhS0}OY}JI%KBv?(KNky0V|y* zF585WHv_Gl=meJMw20OJ6d9DWhe;NQzw&)vNaDI3GCG=hfYJ07d=f47pwEqhI{y-j zILTyA48DBMq=bxDCg*KGG-xT6%wK{IDv@+`4U^Fsk=Sk!{KyXn=fQ~ke76aO6rO}R zxE=K=23Kc9^gk`I8;_=++-h~V8yuBA9FF&2I6K{F2Ap4wH6}lX))(st5Z8&%tyf~q z%XfpeLeY-@tBfl)si3ddc*368*1NW0Ij+$vzsRG-H)uvib|#l}(*W^taQ%)c;q1?p zgm1ls(2xW?&shd&R(YXGG+p51xWYH!vEkrgI|NR^wCih69xZ%1dexf#{{0)K{BYj( z_E_x6bfQvB5mwP>nBBQ}j$+r$icD#{};6kg(t`y$&Mw(bMX5Pm*DJI z<4XpkxBZ6)#uG#lKnbeO$&d@W9Io2U$aJ}=gOl~^gT=zcB^T*Yzpz3i~m@qC`j z7MM|*Y8@7UJoSBAf*Noeo;EyDBtjhd-Ac+WE7(0-EhwvbF2Qa2uQgTFSzK)|nYXfb z4-cm>o*&ChJSYi?huZ-&mw3lig0{k1p9>-%=}kB@u8Oh?6xZI@*F*nBOHMLk%jrc% z%2>*{@@>}g(a12{HKYC~iY`jumMr_Q$S||xxg#{e@hkCzRe)RAVEhE$^O6$^SBQtb zgI$1~ZH=wK7kIb$bp{uWql#|Ms|(a#M=VlJ5OFwm;cE156v)9jo3Au{ za)MnPjp<&+99CASs4(^5)|}l?anvx8;&P1RrM|c7cQl(zG)e`z^lcJAAYZsoMIU~? zmKVGoY$R5H`IbAcIz`PDyN2~X7R{c&a^M`+^nGlt+`_gz zi8&iX{d2QB(+(0KdGghuB_%&6D|X0;Ow+`}d2LbgEZK|wNQVlg3LF3g;^>ITO3I`C z(W0@pxmfM^H9Q?`rYur+gtVm17^0yd#FIT$zHq4?yB6hsc5ak=O|mH{!lmXKwl&pK zZ0AsFp&sttS;T943q=qK^AFA$VTWhegPl9nKHsz;V$?-9p-4OjhJHs>s^;oP*LpV~Sq*#q$p5TtvO^>0G>g>aQ#)EBZ2+z5$r#CG7D5ckE6!l5T;| zO%56_uaPH1;jhrcKz}LNS27Ty_abQRi$x2~20L)0=;|wtaum8j9DR%&jsL;|D zva>iej?@NKbp*C^IC?lNp+SF-rfs1lLXpK5CZDqk6m;Nqz_L$qC)I}CrOvWnYuomz zw^-v?Sm-6*@oQP4wExRpuNv#>p72~6sptOwe0_q@0S``2^gCRvR8RK~*8NYHS!_YB zck?SJXO7MFy2FWMS5tOpUOvHc4uiiDs)Msdqjb}CS$a4ZXlCH8t8FfLCLdC=ou(YS zE;^5_+je5Y5x9CCyO4md1((Hhu`+h=dmxDyi-p9)FmCLdt_2xlxRz%&4A_!(_I&&{ zpOy{6k+^KOJ6;9Yr&KZhduj|goIl?7UonU&(x<-F66xWSXuBCGM@aWt!6LpC@_pL$ zyPv8J)jz)l&efAwe7y~DdJ7h+Q!NRR-#&^>kDJtsC&5lonLDg(*w91iDT&J-+Zsza zH6S9&tLEI;Rq2x`QwXs|>;l{Avee1yy{o;YQ|e)O*?HryC)g55Ryuk%y9vP{BfSym z)nA$0WJx3MG%#i(n-CXI$Nz-?Uy+&M;YiEK`|Py*;~0_H*yEqJn}T_~tj*y80kLpH#m>KVOS0j5#>~(xm&IFa{U= zrzklQJV4Wi^=Fp@A{!LL-1Hr6{Il0-m1}NB;z3qC=p45y7;`5arPA`<-LwBZ^Zz*eK4fLOv#EN!4g=~mO;eF9 zLo9IhuTP8!AXSGwsB085v`S%KOI7C5?G?9R%WiA;KG8K~PgNRHnDB@)n_^;YwQzKD zaB4Xzj3nM~6yV`$tAdPnBu^;HXeB1#K}aX|*6Zs%!=ZwbRs}VI>KSu2z|2uljng@H zrS5RikN|6VYtxN?KU>He!+O|FK#9J@h>*W~zd$C{$h4B;w5djjaJWeu&c z{#syBP+hTX0w+-*oV*XlbmO?6o24tL_|m~7dNpBN2(ddWcZwWnzZD$O7{^^qwWXkw z!9SssTa@i-MN4ia=e&HItKvN_myn{g8cP9W%j{~ijm`H zmIoV-G^ub%oJlS0llX1)4{% z&e{du+l$j4p35^UTcc?vJ&lHAmb{#?a?{KFw}rq$x~q9L!#Q<-qU6#^IrgQ;)IP{& z!@asSNb)1(&qSXGY%-vBPgsDBiG=0(^I_i~MObt?QRx0+=9yS{ z$kMrxy{ExD7Sf9p@hV=Hqq5G=k8vto*I42Colwwe!gLt_j6hgQK2iAdKsx8R)Jpez zx&yXqV)Lc{*D(b#A>C}gOcCL7YM)8dWy3s}2E*Miv0Uy*6+XrXxT@-TfLN_C`YAC{=Y$Equ(NWqF&6~Yxl&!d5DIMhjKr64Nu&xS#u-*E>j-C8>Fv%q_5??Zlj5evai zyKG`}PNm8Un=7BCc|?qX@fz;0xg*N}QilGOYS0qvVp93mdGM z|Hvy^vMi9R`l{z{P{ioCG?ra}gLLaU7F&1heUm{#b@Cxe}ps4SRcS3D8JT5aP^Z8y0iQ|H64fu3?J$ft5*rZN@7E%@=$iUxIbm(hh{ z>CG3(T-GvGZ@WP?(|LDBHwGdFTox_EcJ2S7K9T@dZFM!(S<`K+wD#ZB?+gJZx|~@( z#@D2FGLA--YNtut9Ia=*RR}%7X}8Q@;_NHAjeWPLf2w(2=(eNOGw<4b%Wp*?#S4yyJ37L*1K@p1uFD#kFK+v#C<; z+!2G7tW0ga)}S)?}3xBqSuXgkOhZQ@5jo z*^Ch0-rknE@vUSjSBmME=mP^&mnOVHOxdc3Fzt*>4ve{(1w&oaofHb#S;t6uwEMrt zNL!z5{tFTas5YI?e=rV5j;bpgxp1|z&Vg0(1HcP6~2W8+HuvN=@0(lVxn50Ko;r#v2zqMBF2 z5iKm>K_FXu62cJzPfitg_0K7ET=y9_9sUa`F+yL*pXy0VZU*ifg~4Ow#$gKjbv*5F4IlUayg1cOWeFLV3WTf944- zaH2?9%q!tvrV*MG^oaryVrV>TxW4y0;m6p>*+yCt)rytR0a2u#fMm@Ijn($X@h!OU zpK9fx$eTJJLd^;0ywJGHI|GDll)X!?t;pDAvmdfQ?)0GJsJj%`IJBvKgvc#2Qc^2X zfl1(yc7!mConn`Q$i3KZk2zeRbg`|-g~W{-+)ol*I!2-xMTTocp}bE%f-k@%??z&p z;#3hN^d)VOaWOB-NEbPTRq-BOp^INO8)LpwQ#-QX*J6R!3C7b;%elA{TvlRfO^C8#k0Fx*qZXAcdIe#+N}{ETiw^ zKodU-i`$RriAqm$c%vf4xA4vis0ZoLCx-_YhDQ)hvj^)npK_RI1TPN!O>XjYGs{DK zJM#_QeXM*rWJ9^B=r#ag}Px+BEuvS_YsiXcr{ea$n;p zTQIJck-^s*g}_dy+?NNUhL;94vIjl3h2@Tx^&&sJQk~{8(C^CoTT)Z?Z$BOpWF;UV z0csliOYh5h2!Vru{GaOs?0dzqH*|0?wKBH^mJ|A?Dt)W z5v4pz^Z#-*QmGH_!CcBu8Yz=Qg=i|SQJ=G9Mepicq_L*J14>i00w^*jyN@vE>UMOW-y%_;7rB8%fuZD_wY zVKC-V{x=&ZId)oDZwLpD(Yg+`Ql*K@twet0pX3M|XueW3NygTS3?d?_D%;hccUN_K zC5!zd!75F2a?JKZCVf9?CO_(fd4&)8*i;q^!U}D0f<6?4t#^_6i%DEp;MgF;O;_Qp z-=eu?N^WKZQ|!#(hCHn1;yB6UbW246dApTyvuQbu4|2T%SEvdm^;|_H#Oh?=0WJOuT+?N;97A0(sj!9tr3_Y=HXx1nrUya67-LGj3 zksYNu24p^Ytdu_H@1k5Z@$j5kxF>_yM60li_KhXUGYlxqC)5pNR_pvl+vediq zxI<+`golX~o7Z;0T`P-cM17pD_OSZYV_*e zoLGu^4M;oIxMAq_bevK#0mdV2h8!REw6}x^qaBlk0`WfZGb7 zvU@dm7Kp#EyrZXk{cCyp?*5`s1^e7yTsFb;kTm z6v@2$n-LGr8Eiu1`LrJ-WRjv|@dda=7W8s+BmjBP0DVXfm$Iv)(_2l5{-jSI&KnoW z*T#-ai*bz8NM<4+sWOvU1_Hey4#&&$ZwY*3%WpM9Q-Eo1a0(+*=A&4f2@vjk_@X(| zchF5??O1THh3<}38@O98jmt0>X$=7+hCoBwSF%a8GZZhulPfK0S0Ie#s zb9K`dNST;T2iuh>8Ypi7pV zjYCDj9=bf(l}A(2^hIliBI&&OgT4fGv2$?ZZxbvqv-C=i2!aEk!XBkC$%m$+yJEQ@tfROJpH0M`;FDs-*4pQZ2?5t5V8Ug^WM+!@hu;^}k7rw{7#~4EzLEU@ zT~J}(FR1#~Rt~=xXL`S$9=8MRh{O^|?s>Fzdopl__r)0xD+=h*GPX9W4aC_Q+tTm= zfSXuQhzMfJZ$4l?4tLKXu4%8~39hGwhj*{R53$Y4aAD;>AB?`BcUJq*)qgZoR-9~3 zEqT4d=jDa<+cRLm4HsG0JP+0cA1>nUzzjd_XuZGbmC^OM7*u{+=8dwphIrXtPPKhI zAix~&d39m(>-@07o#K_6@p8Il%JAs4k_un29_fZKe7&stwtS(_ptf>jd3$L6Vx3Pi z3rN%jF2D2xT8)SIRA*B!mR8!^qsF}f+G*`iMl(;pu8rEz{ZrasoK~)PCuenCI6oEh zzJDslkDr9Ivyz?fLpZ;aW@>IJ`>YJfZnmI|_Z5xWLUL75G|``fA7^ zc|KT>$hDOiW{w}LT50-nzGU%JSOx((&P@NLEg)@!~}#D)wW~bf1=f{j899V7q%LV@~_VggF)~wtM-yH2T!8>rHWSA@Fl* zgTo6-*?*YP>0##!KcA$z6=LGS!A}2d>XOLY$>E9P8;|RLISrmg2OCa!LG79eN%BFj z&SDeV#!`y(GBzBR^T)OZCI`|*Of!-STC!oF3xhh#l%+kgk2aD7=ISI~;0gbyP~+_`JC$D<4eyOY{Fy=Jz)ThY2hSSR`DM|10qW8_NJ9w>)7G+U|D zgcUbVFRazPlW1tVhvy{8Pc9{_*NvX3?)(k)fk0e5_C4SjiXAbtXdukCT8I3O{s8b? z7aAAy)A9Yq^4O~DL@Jku)h!!N9x!!z3kAk!E>n7^tc2IY#XDq=KhNV!{R6GxcU;>G zv+RXDHkNh`^c7z(l(pMOF_*aOpjqX&w9DBVHqof+wRO$53|Kb-S0oT;stO;Z2Scwd z!l9MrR!kY;4D-%bm&_GzIQO`fhwF)R8q6QWAV&q6pJosG4gJk+0@ z3%j*rWxAgpa!X2;nodbdcHcYC-`5BTGI?6oJG!ue19zECT{&*FQ(owv1Oi^Nv`NzH z3=)bxD|LyBT;;s z)X=oXd$=6jtxv<=+0;LyxhmOFKO@b5|7sts!?@oXTmye0eElM6okDYNak271y-8%k zIBdWIBHabubtW~?*$tO_0r^Wz+kx{l{EPJp>_&BRkBuD5TTJF&kT=HhnUfc;8JBFj zmJ4qN>?^&nBjqt1gmc!-^V6&YOVf*d+c{?GthVMY9SDiL97a6d^YOicOD62;DIcKa z1g_BL*6I1ASEr6@Cnn?xVRTEz51UEdHIF!6Pr@j#Rr=Q!VopY6kY8$9+7oco<#M&( z`;hVZZYp-JMZ88GS}Efj{w@SYy=(!MIXb&>LtFz#iPhLMZ%_ht;?J50AhltsOg7n- zfJAF)hDBr<*;jXWcN2iqT-=G=Fw28UQyV)5V1V|^avAw-&3OT9z~d`Nnv`3kpPaAC zIT$JlYjFx!r`dzrn1cK6SPs3F^^@n`?xfegl$Qjl?82HsLyNMCxBYFXfrEmlS6s#{ ztMEuK3wKy}k%`-BjthVU&z<#i8muu;r0vHM|HI&gMPSBTs6y|yQ$mdR?wG;F(?K!y zV-n{H5BjK=HB^E7nSMCt>=}Mbl&SKm9s|cbd?54%#U0(=n`<#1=?ibrsqRm5Qli|b z`lxn)XTQhAu4@A z*bxN98k&)uj@|%Nj>m=dJ!w)IBe5AUwA{*A3Nwn$0E@h)Rrwj(z31X+2ly;$(6Kg; zuutm=guF_bQ|dnHYv4cjBp7fad0#YFUSEhH4IOl{8x!yOp7|A#D+A6PU)c%ZSC6Ps ze!j9(H&_INvBgA=RIg-{YxfvCba*tw*iM_yM?IaU(8J`u)85wo7{m$dR%0JB2-7 z47Tly8>2I2C*v!lQOXe z@$9c`9x1#?Sh>77J|`R>kY4bxfCzl2z|;p^zo0cuQxb%&YL&gjQNG6kjj2x&?X<^<5XmjF%XblPdp=uiL)X4WwmGI??cUaQ>_f*s z^ACsaD1#qn?qmoG2tm#o!;6PIFT1VY~^lK1%HQn3A{i1}O6yq8c&+-6U z$(`YVW%w@7&}l1V36(XqGl+c+Q|gce)*uD;969boU$r!w9O@e{0#0oVj~b3U6axI_ zqQpiL*YDpK(AniWwhlTE`&J#aAr$b`3)w|)M@`F>=6OOCaSbYlRo|ByvsI0N>f zQ<&2ld1^3zizM6tXca*Wy)Qcm*%&ikU7(3G8*7Q{(5*kZ#rCXbi3@p?Rr?^r9HuFp zW}VzuVcOC~Gy9Ryv=QxkbR3=SE1!Aw34cDVZb*7RPM)Rzqg~(v=8YDbShFFtKaUT+ zlS-6>TJ6(X#CjsMm6))4dAvJF?O_(^vM`GQ?%e1u$o#a~w6@?V(}Q z_+|O2!m2uF>2Y=+9xKy}6I?f^4#q_wl?9+yppJtt*tdb$HmHM zn*P~GJKBrPN-eQQ+C+C(Ux_zsBGJ&xlkKi8r)6r$;j)!+g(BbyIButg!+AJ3HDN(R z-^P7b!g?`uiqMGF0Y!*u_HjD+$#nyFO4(pKxaC{|Cu@KFQFCh6c!Pmxb4Dwa+cIk3 zJxSRjcixwIYkAL=2g(Z=nU36{wF6E#y&A{8_ChOwKsIG!@0!wtrR3@ZNpClkcelE9 z@9hyzLq!Ci%xXxBLN;f;t-7pp&gu7k>FqRCW8JxZRKhI_3ECvi3bDpvk=~uHF*|Lp z_7%O#Wopny6&Vf1vbJ8rofSJBjM%l>0Nqom2Gev5=^$SG{gsNdG{ThA>IGbb=_h8> zg$Zhc1iJ3_b1PG_2q^kqZzzW^#e`mTm#EdR|urkv_ zQGwlAW7D7nK>=mJ?MSI!t$PE1rN~aCZwDb(I%I+QWBa)a3b{V+i1Noc?S~b#7EkAD z^h*P%%NZDX)KS`xKhu{xKaq|`_00xA7-{Jq41mYIIlB_TLF8_)4}$Hh#$C+r7szRk zyVewld54$ZlZ;tw?LWR4dTY`2aix<)*$mG@K55-f-?gj0@mxnFC5kG}?HOK4$9j%! zWIHDstcK;=vpX9s)sqhE&-mqBTR$X-*Yn6lxx7m;@bb?dQsW*TnxanBs3(IFpgkYkjwMT-ez z7S!TsB))vC>e?D3`-c6QxwhNIzxPWYkaQDtJ+LEHu#_N$_aXn%fMf{O5Agu8OX;jUNV!ew3a1yx+N4 zUDK>iUiO~5*cJNd{$)H$XQ>C#M@QCA$3C0(Iq(moH`rNHBXL-JdDu7@R-|6UKaMkX zm$pL@d(%*Rw;QMk0{;Qv?X455Bno7w2pm4S5QG)@qt-jnG%iN+_YnkQy!1Z5OTsWp zVo{(D)|C#`wOg)5?!}4LRV$~pheoZ5aupIJ5D_O}3NjB$79yMxw)%R&J-*4YvN8aN zqL|vS*s-9&am^%l5!+hZvBKuo6!A+nFPbxh}&Xr+~X(L{C zu0;42RkdT#_yJlRvqvvrym& z68uY>IRM8c)io!&T^8zZ z`p45A(h~9eW_ez(81%4zjJ;w8vWR{8o81I~0EFHt!TZWUU6UVj|7Psx6r#5|I#|Hp zVx=9UV+wdJ)lGV)`*%M8XYUm3)B9dSe-GrU8<-7s|8F(uqkGc*oRay5Mh{Ev>Wu?y zzb4T(6?>y8l>hG*o9|md)1w)h5<>rzlQ0ah;|Xt} zp92%TxZnfX&x#}Ku7L|8L`?dxrP)YJU++~+{9(};zIR}~=go~aT(7Jf{og$qVWsN- zN$&4z|GNynyZ_1kKUMpM4pyq?zr(~H{~ezEk1$N{=adYYzb_R5*joI+?2xDbE{dlA z4zs`fXE=$){xNk%lyVIEdQx}I&86+;%>u8@wD(u|FkrjbNwi=uG?!F9AyFhUG%vXE zr$cIUxjM2F_eX1?V#jM0v*JY1Sq#w9@|5~<{~CRZ`Lc;b>>nZB-o^b=sZe<4Y95p2 z%K$Z(!9`+AhMZJ*{yL8mi)`(nTCp>9Ca*eg`CgMVftxbyTB*xT<fpmd0wY2j3GCnUC zeB9)XGcE1;9V>>x@TRLQ9}AjlfMiOi-7qL^=Vi)aDbK5V36Hw+%Sc@5VQqPdHiJ3B zFTINt)(Wmhk4=}-J8FP?M9L8e8g(*-`b}v+axI$UI!HN0LiJiNwCP&tMf2=7bUZ z-$$KQxjwHF=Xwk=wBx`g9OzdfoUu2F4IF&E4ve~M&o91ZI-JayDS`>C0!fA-A5`qZ zJ-kF3D_BRYCG&fgzsw}{!+aFR8~FT;Dcu7YRK!2?yspkZD8VcdYzlCTZ?<`6>QyVx zSu9oPXsCS}zCSqSVdhN?G#D4ErHxWt;EF8tV`3Tb!pM_~KCm{Obmhj>0cjX8!X&N5 zT0RtLwBQ1tTUp}ROs zaKFg$s&&!YNjFv33wNS(!{U{?vh%jGQ~=E^W! zfiag#3b4W0d%bwL+WMt27RIOwRYBark@{01ph4O8N97H25YP4LAVRz2H$}yh9YAO( z?d05dZu^dkOXkG;0tq~j&ofpUhnllq)=5-TG%XslH36039?T6Fk_uePtP zXLWblcxQ2P)(tp&>R;SA^>sIOC}AzCBV#CKDzMZn21Vh%KQZ5Evw0#YzoD~$(wk+- z)kgR_x=DKU%O`2ZN^tOV=&jdXn}&90AQ>f=2^--R%lJg->j4vuf%i;+rjn(;fpUOK zoy{O3HclEGC2DT2@S8s0BZ}}fx9=^73JqB8T)x>!Xg37vb?oqPOJk?dN=av27CY@Y)NY2!ESwb^4W zvEpO3WRekN*!dE0liTfH>HRG_o`=DaD8EX{`8PDJe_>9r$+XE_`CbTqjw-~4;c{vs zZ|(Qs9F&ap1xpf61ggK5Le_xkZ96ZnST;uP!)HVoUa&Ef%0_iXS8_9 zdxPKfRe}V!jB%3{!y%ulajI85E;Oa1H+uo{K~{^r02i^%V9r_T4qd+TlwA+V`O_`V zJ+M*v&Gif+xMWv{NT+>9kA+$1U3jVJF`_av?ao zXgY))jl%4D$0A2q{oBZV%EON=Yh%ZUe2-w2fH5z{=EEn=QY8%7xIF{5GaLNOK)UEz zI5`h?F7FOraW~52^}7x3*Uq=SH?0oKU4l~Qwf$#J$WyoZR1I%epS4z(!FqG=?VW<_ zv**%E_hf1<#pCOe18$yM3@=Hm!8mYb-P^FHC&nh|tExMsQ&k#XUhTn`Yhq%~IK-$v zRdG>A^6Yyml0441=-^+f;zJIk1g;5})vLpMlC~VDlKh8~woC7uT=khwaG=ESP-jh~ zWt7e)0+*NhcrV;-HfVmlT=!ki@lMKk;H-P7;%zwL>VS7X8h2U`5~=(+fX9Qu5P0*V zlX$Ex6suVQKNz*Ej7=5kSZOi|C1qaY<#ldf8Z>xS__7WWKfO($(70^-W|{)HgOTCk zf$#KZ9d@u`O<6u4y^eN(`PHNC0?Gx?jS?l+9P5md0azq%To`HXPu4wyIlYDU9*{BV z;#&9Qw7%GZZwMP}HT*+^KD}zE&T3NIeexw)lD@82wNKwF zF9!ZKUAr0;YJR&l?5j)FNBU>D*EgUgPTP&Q=DD_~D`i^ac{(leP2+=h+y2R=`?2)~ z`II-zt6eqX+uKi42BW8&6FYUBeH)oxyvPQ_yOOpa7}odes^^)7e$!`fE->0&Z){E5 z^iR8~HwZ}mLEkL>-l+IE648n4UX4`SFs~%u4x$CUtgn(wH+Qk6 zE(Vxl>buh$F5&p5j;zB2BG$5JVhSJl$ zu)0(vt?XLK$0wz9EJ!x@5Eo6(&#$YNPMTdh?%26ABMr7A&7M`$hPI3Z$kC{?P{Rd_ zLv2#NrU{r)!DJo6Kq@I$nI#A7l_$}i>-rO^ki6s8RQXx1Gxu2`a86Pey0n^NzhoW* zv8+Nz5%flINfb%F*q_w$r_N8E8-HEJxt1S|Y-y%%;pfOwK7O7MhFnC<-p2naC}%JmY_P(2Rho9~ba$ z@)GeJ$+{bU%RuZ73HzM7`s^W`<$UL z$sw5`@4TUP$URM%G-af(LOWaqg);sNNjaiOibn0K$j^9nPNQXDt!pZUE37{{jrJ#h ze430#0^Q4el#(EA`eY+L(5;A|rc2jCBuM(#8fc7bNv^a15HAV022$jg@j`Dt6pQy@{I{E-PtxloA0%D*Gcr%L6pa~u2&Z$s|F zdK@7&VoxajvJdS_A7Jg95S1@kOnGY~b@)sq; zXh`nKq1DpVH1B1nPFHS6^rlkmiwNX>jr#>9PbQkqrt6sFW2L+~+z7X-ATy!-&f!YJdSn3bWihp(-!!~+3{=Ru}u>d#+QaF+3ZKJd)b_%3OVSxX6V z<8pBk*5@0}K3$L@zV!}n8_E6h6A|$2X#E_u7U|9kz;kadk*+P_(|pD!UH3S9vorU9 zcrC;)Gn@t%z4w7Crc_=wWj7xs!Zt{ya8`UiT{><{Y7LWIT4X>^9-yJgQ`LP-fxD$m zdmD}F1iw%`3SeN}GJYw^O~n}8qokC2rLfw-5+M!Cf(BokVAY}3dZ~P?9+xibY(+VdS5}!vk|K>upcW_w1M?acgugcT67+ z7HAIM-rE|FD=#UpkfA?g%QE+ylH(1M<7(jXf8O1l7ty(2+o{%m^n)jT(RK7&*z0Gy zl>F)JH-^S(&qA^hAEKu&Yppk%-0n$QW%fRfuqkdcobCu-`kM%BsWY~t6@W(Q+hM5Z z&2GSf{-|6pVg+Kgjj$L2CQyC{G>AoSJ_z15bZFh8itLa|7eM+vUZM$2$n5Oo#W)tz zg<)TC+0IaKMcFh|X_83wh#hs@7`WY6wWCZQ$>8NpDZQjg6RmFFnN+?>xb71+t zE-dO^T}6-C!p|Arl+Uq4gRFW}D^Z-pxrDZAXf(r%yYt>09ncuWZP_g%SOTCxGJJs= zA4-EKS=bN@{2EW`U$^XS5>R!?9MtHfzYd=-7?W`E299Vp79>XL>E=!ro# z6s{;N-D(%}X`R$)^5N)-69%HUL^D)ihw1qIW&ZN6`WzdofhEAf*Rg)DXZxtPRR`&9 z2`l!4C2M9uo(xl9>*M-C^lL37Cp_--KK+>j`XiJ^l(S3 z%?r@aamKM5@$aq$HylP!*0gPw)7i{A=Qu4xh3TZ;d0x`3N*ipC9^;zZ$y?Hdn`RbuW(q7If|{V*e9y?lnLvo}hc6ZQtw0b)|v~f%>CS zP%qiaegO&@7-;fEV&?C&GN2*s=0SaE6c!eH+#&(OpW!w71l^flcYqG_y%e0h#Ef0F zA5P@NswCfeAXj;CLDz(CO`vYywWoHo-ABWs(Bk*#{{ckl0s`gZB4pg{Pd=!=;AniZ z=6x1TlKjJiD#?RAD6sV-4{+f)RXfVsI1>cbcOmZe1?1$Za5$$#HM-SR12!RO%^^XO z-(%`|g&Cw@%GwUh&)&nGbDvBrbQXlS^Msqv@!Q7Bxf86T`b4?tf3#1aKj_BxAB+|p zdcHiPH^xZ2^n+qEG>UR20-$I{WQa5bP%l2^1ltBgOwaCZXvYqdV>(G8+27NEI71~| z?0)Rn#X~83EzofX44ju}H>X-_jgx3G3pkEK;`Xc8!-bzmL|nc0(k^HVE&r;780sHr zK=P<>=+3caNU$?qtXi4>)C!*(6}M&(Ejihu`733okHZ?H*0e>-&&U<-o_oL0;SuF@ z{?(N0AtI%yHXHa*tcmhMhZz;SaQ2GxO z+dsMz{%T0V=$7hR>OH}}XE_*G{5P@hKOJzBCc1BFhb{13U%_ktL6)?adNUtNZWU3* zO9oUuJGFYh<&7r!Yn%gaOG+WMakU^k)&t7)HyJQf?Jl9O(54+HWR=tjLxgE0e@WRU zVE81|uLz}QCH!&Qm!;$kSMQpp$nKfoypgNv3h&zpmO>#cEAfZA|I%%w-vfHHf6c$v zWOQJl%grKcm3`>IYyMFPT0@)0vn+&j?8cIDeTvMl`Ue2z8^Dv)2@L~vx#=q(nE%Vo zjrMxmD~!}9qCW}Z#!d57P5iM~*DQ*X*nQO5fgA2cRQX|(>L28Fgj<@u#~Rrr@&6Kv zH%y1<@^Y0Fwtf{yl=#CP6!FQI(P}yO=w^1C{egdU0=exsubcA}09+G_i|6|Xj3S^k z{?Tb?gT}>R+cO=kExZN_a z*m<1+;oRiRrc3o#*%1dgOQx86y5y&Jg1={ftZRE~2M^~AGuB@TV0}}tw!4D*M&QDI zu#R9vx1UKGR$}zwbk~+%1zF4@5+7w1eNt1=X2UWJuUKSpY9(l&!9@cK+l32 z^^35FT3FM1X!FSM@q@H`QVlqoumgPPhPSQ_1wv&DK8ip#h;NjyuMwmIj|k_y&Q=?( zD|~sQ=;XHkY081%ev!%LTfdVag#ROwNOtPXcM_r38bUUzdpw67T_j8*1+>2Wqge-L zQ5ZROO^SzZePCPT_N8Q2oWukn9vIu_17be1cYE{#d%|4d>Kg=6dc`9t<^?v9VbxS$ z^rNBl+>9&r=7PP%9kPNA<~K-Vv&6>`nlp`(ovYQ_iJNkKngwR zwb_~Nzb%z7>F-|13EGi-t{&f~&~<6TcWQ#&(~t>n$L-bt5c%r@PmWkCNyrvS|KM?7 zv?_DH!m+v{)jB)5vBF2NXja5FTjPC}Okw3?`{N$EM-{I}RRpo(sui); zJq+p~wRVZ*YBkRD+#mw}bK9Myinxjks>V>U@L!NS64FhAME*Z^SFCUO^nZ->sm+ttzTV4}4S zVUfIVJK8jr+HapbI+W&uJyl)qcf?__g>sgI!L=r=P@#-fU4tKHFgZu`VULlXF}@w1 zlE`(^d4q`s2ZzMY)yE4yYV6-A;QSEft6igl@;L&v30k6qOP*Ksg-!ZK zpF8v1&0N8fR0*5iij~0}9^N@$@#Cl6*X};ojNU)hBk25WC=a6XA9ao}!+NJ4NIPc0@n}oA`$pa_0h^ zDDhdB(Iqfz&)n+-=&~L7aC+jlWV@n1 zQGNG4kR6_aH0Iamx6DG64ybiQ+U z{)g@lC#7!o*C;Au4I1@Qg6`zU2ZB^=S&@XF7UXCbXyu-_s)U4@%zAz74C(Qd{Dl^< zn$)`qD04{faB1f(1qU9ProiPY`uK-L-?_96sqFF7-_Wlc(Zq+s>YLkR(q)C0;1Dj& zNePibKC+f+x$-gDe>e&nacvBMkB4V>{^7?P`!NOctb8MURCa)6Bc)Ip4h^!25?(JP z)7KuU0gB!U~mwA^ymfEt2Var=bs})-dN5PA(#BCjRZPX)VTn&^Jx4V zE8fSmxYv6tR3uWAKQg&UD{p-@uCJe`Tk?@O0?j+&RDDLN>pmW7d%Iw?{l2A%C;fwm zD1M$jex5dJhQ+RcC`qf8A1a&akL#GR8lHxL9F@G?QNe0I8Wcr*POUiSDTA2mk~XkT zmib6fpq56t;ElCk>sg$E^@kmnj@k*+j;{*Gd(I1eXa5lCIx9r$w80&qedTx`)a7xb zZ_XaW>oNFa`-GnT6gEb?^X;7W4b>y*`mvJ)B@dbKe>!TBV}yQ9YrHr&z0S1iS~KOC;APXlyLia04jsfNq zE8s8IKfKXwqy^he_s6+dPt{2G`7VjI^Hh){#-mMW@4|4AQN8-VY4o!Y;=~E3Ar8T`&eC#wX?VBpbC#ca(__Z$e z*_tT88Y4K~#wmRUe#~wTJKVPscmag3UAhVza_(>D8f=DRF{P!A*3Qz1V|I7J@Q^qP zLs~|DhljL$IyY%O3R!xE9dBrfwO6=Zr^(t!^vfZE1(5}&5bk^#zs8k@dQja!HbT}mF*}fo>{=xGR(JIAR|LTiH>nGi{ zet$nat66ChFOP*h@erlz+HC(TTr4bC1L9rv2lLzU$13KWPOm z&h}CP)`Ivu4d&o@t^IEOPyn4{1sitdSiu?(=e_CiXBetLJt=31ZX_^v@PEs$-U$7ZB#Cql4|x zg=8zc0sqrh1aByTsq%ce2GELw26O(~@Z<(9E7#r?nD?bx53lhlDp)q;0SO?Xxg;R8 zS~)HCpp$i9e{l`E;f5zGXaD+R;tZPQTMGaArJl-Jzrsz<`*%Om-2g`mhxsrv2LtfQ z2cb=P{J~O(ZGgswu!vR~MVak3RR46`p3QDJ^Rp@@!6r=UInb6`5M)nLy>OqWKRMHp zDHCMr2l;A3j={4k!@VA*W{w6uUf09?>rVUaSQ@#i6WXg&jmC_Og09(Z+0Qq10QGeH z%1~>YQoNh7T*?qY<6fVlET0KT&N?!!uFcz+YDpoz_ATvG$!EwwD>utU`=M%kv))qh z5#6*ac#b^@an!rKUloioEJ<|QsKr35^l#}pCqVp>U!hEy$BC0nrr#Fr^b5qikdmy> zms;Fj78<=@M?0f$MCXZAqYSw5e%v%c{jgL3kGz7!agORrr&6|ic^o{ysV4*{{e$NN zxE0qCUFYz?Wt9N9QaTmO(<8_I4zLr+oCVWB!6({um^F9tS(f(rNcDQ zPELG{jxx4l&n@NiSE=23T>p$+0j>Xt_FaZdpKp@2x(Ior0TTvJDVN%a>T?5+VK-AM zQez<`XP5?GQcp@6n-GOzHcMjQg3MwWYN6#9sogfFY^A$TiMbeuTFQGfkcb6{kNt+p z32?$bYMmsR9j%Z;#Mkmhqo(r zGf%3)@NzfskIbv>S8v&HJH0q8K5MA7!*c+sgS`BjmnfVb0aadh)fguAIQ3Ot_xNp% zHLn7QzDiL`?pcJ?k_~$miMQy2_2qS`$aC_GpoO+ad(ryqF7TV5d1Xu2hO@fb=x{bI;v+sV=KW&; zxoaap4yzwvTG!etI!KLV&&AW)Nn|?Ju+Qmiyk>N5#Rsf3&tk#ZXt{`XLAs0n=5%#D z6&Xx^DF51QZ@tN+*dR{=u&i0QUKFNWDdH^CyGHCH9SD5Yxg~LN=3R8;n9yX2(x0A9 z2nq{Jc^p1M5oT%aQLRhKtc}ebF)_~Q3Lk++h8(|}kOWRXEttAj*Mmfm&m(7)OIaxU zg|sbh_mk4zUbt4Xej!>P#;z3<-Zj9)Vhpf1aiEEc(Acl2vW-;#*hjgEwAKcau%hof zw~lH^BO{$6f1wb!YZ%!P6*BOX_&-PHLPG}b$fa^F;7v6kb8P>*0KI^eVCoPtMUHS5 z1y+;(=MMVsLwWJcuQkVZgGD!F;9rJ#SovT6332Ax z8U_FF08C#m`}93tz8KRQK}2cN{~rBo`^Ke)9C0R>A_&BtaiQ1aW0L+0PgUr5sxlZN z_8!{2V2+xUs-%>r=$)3)9Aq_TIIVRx>jNi(o($%n1kSBmuH-oeYdfM5V@wEA-=`yi z<{aES4bvs(1wS|PJ}>Y-tKX?-wmQX5Uo^zVlq+dHR0^oaY>Noz7P1YW{f$lv3P;inQMKe zNnb9tn688ly2;s&(HQ4e3z;cTtH^|r&?}Uf?N|M^&S&?yHFtMbp;FemR<@~2mmE&E z`QpNGKj^}A1jjdg2v;cfdTr0}gGP(XRj>d)DnGPo`h3tkwpgG5KAB_C{18Wpdjth{ z!yfzN!P8n_CruH^8R@XYBa>K*inrXhGecl+?p5xE=r#-SRty0zRt(I$(S=hHFq1KA zX7G;*&nCFyv*}g%C;KL8Q2$xzbpIB5(j2n={{;B)ge`*qR+Dnu{QoGy+g!W9)gjSd zGPpnFFPPoj>Gy%!_+5%xmy8L{HR$Eh@InchqvwPL8nt}LP*Bgp)-d%0F>kGy=Ya(d z>+zyI98Ual~yE~}Jtf`Jidm0kYlewwMSKynaS96=oO+lQ;UB8bi z9$vbX_Lwt9j8Q_k8^hsn5n1l*5zCN|Gvv;!3^M2oZ?|-*{ zXHF(_x~r^cDSDu13zk=+4QLrQ51xiX47S zJ(nZ<5A4yL?BE`!+ zQ&T`l&HG8dl3av06V3ZcdD z*9WThQh5qyvE(h`R%8N+TA<`DWqy^7T0j@}E+sX;*!adrj`+V+7aRK@)&2iS{5k1QVEz$H6}cBNwN5y(oAIq#6EUMC(a(~4xLB4ioU zp22DnP>UzW&JlT~Du*rmS&hp9||SX9-~hed%U z|CdFvN&d&80>AvPMakik{cBN^{r{ynN(9t@Ey~p3zZ91Y8TpSzJ?XJBVLhjAh{+wZ z(&;u&K%qlCU{=%0`^OM%&Gwn)~e zK090`>r<)~)tso8$%lJl7o!t%HCoOlAGq=?p%3<>32l_?um|;@X5zy#)&}1YhtGeL zjV7B~xf1x97208Gk?jTH)HJI?@xzFOwYR#P?D5v=J#(9mS6GwcDowMUoh4ZBm-2iz zpKQ`j6_gD-_NHp|6&;)UunatA!U1k?zy7?=7iaS6dlz=~X=@Z5o++Agzd4HMVpn)d zb!s^2=C}|RO+Fb3@dKkPmU$d{8z{DAtonDLXUV9P8^SlF;Bk}BFju3=0|Fz(n)cAT z<(xSb&mWuC94A=vgurcKjj9Md1phZm5H28|6hWU3@o_IP4dV2h?B7=g&`odbt}}Mf zJ4*Rlz71xN4l{Y4$eNFn4M|LW*98*=o(*#{(T%pX1^ewCaw1&c{n$cN0TZmuF(Aflgaj)2yT+ zVIc>EUy6laPCs;;!#AW% zkv3VH%Sj_02zEf(=Ub4jdi?*0Tq7b=|LcwwkogzS=mDO#-^9$uYeGHX!tX0|rM`kn zs(H!>({&njDP;K*Y{Kv4QkIaQA%B1f2HBoBRk$TnN>M4ji9s*&PNr!R^)zB~MyLBY z34(+>+5;Y7M79bIUjG3J_U@vQq@TfyJ#8{bi;nqm4Y0A_v2d$nTVVM_?T9qPav;6CSK?z@-we;=m zCw>_%`h`qWy{?HI@9O*NGn@i5oS?pI#inyM_UeE00TrkYJoMyG*RLo zwW|exZ_$J={2HLLpwr2KX|sfOf1;YLa-@fdg2cB9@L2HJW3X7SX)7UuXGMf(#qoHl z2C0S@_JOLJP@v;qg&CvYV*dGCg)QOB;61O?Op>+oPIxjegHl>(^y!-E1D5mkIaY}~ zCp}AnJ-lj!84srpJbgn`0_fT(RpB*M;nk*io>qgYo^7$Il)ikRjae2?UvG!{4TBVO zhKicZSf^?Ol+tlXp$h>yAa$zFUddK*QFBOI*=sabZ4Y(?7!mp2Oef>lbhAUMeOhBd z{AmxD7WRMKTj&FSOz&9ZR4;d{8GcF+X6=nL!a+Oy53Z7L9qr)}(2D|}bZlJX1ZBYY zk416(_oFMhcSm1Rp!EZoZES3e1brxFPg{sY0#FqpBh-zpK>jh5^aQ&v^X!vOSfn-#t#dQI2mw(t1nFwVsm6)(!?_>#^8T~7AIGaQLS#(df+o*R}-JU|RJ93JQC0TA%hNt_n z65Ag1o%mQ8M0NaG^h3Qo6<#?r@l1J!86QTA*ZL51pkku;5dE2ia@yd z-B1v(IY)37{`obuC#!JiQSPyD0e+@hX*-N(O;ezRAzen3t{bV9vhppPcTFAjh3mb$ z;xWurc4@ep^V)SD?x_<)nNvV4Tu<8S5qTEreZNm#VDIa-TRN?!y4VZmV$0{xBa-=C zjgmy_5=0!d!v%4D6Xev<3(>!9Yi{Ebd)Z%$vmpEXL@MMJD(X!Nl`7)tRK6<6j|y*s zSKGzd<#c3L`P?mO78)=&=9 zFJ>uVhdjqQ+hO|1+VDmtjIyJ z%YfqWRQU`4Bt^X!+OL#o1f-3GGf<^4A@HaWuKfmr1)XNrM$Z6@TDTDGP?)KrVd&Z0 zh|ckyOXTp;y#cb{7{ypuXHApqxDClJU|ZFv2%6 z!zlIU^IJv89d0v*KKQY1frr;t8Pt>b69M7X3l-NBJ$0kxpf_s@{T2c_#Ue2h=kN*| z&*Js%vcLL!*FgHvmDi1K2(BKXH8g5C(brzd;*?|ZM#e~K^4{lyp=Z!Ah->~?{G37s zaHJLyM`%J1YDX+wsxURBStdt|YtvT`k)P5vjB^V=K_*bOruxrCFm}Hz=c^V5x~F>7 z-=~*A*yttbku<$nzh*O6U^%kc$IGM+j>s6gdodveR94= zKhbt*3Bu}!dutT1|IM7O&$z=0uh-n(<4|!kD|b^pRNonoggR*)8jg?T+e5x};*ImT zBnc6AHQ5+Oq62dQI$D@P6omZ-{LY6>hF9fD!bZ{h8DAPp*xiu?k+~g`A;hYYX{3p% zh%PU7K_oI95@{Dh$jrfJhrjlBqkqP*F*fD(9rFgvr~@pcf^s*u9~@8em5aynQxjbQi(m?C$2q5uC$nf3l&*ZdZNpAT84$0?5k#-7v

w4joZ?AtCduB04lJm?*7wrAP!dB2&9}=2*34nflVrVXD&%PM zr#GgD9%Nkj?*{SN`qW3GtK+)~pAwAnM?_983EfcSujzb5rb3&PzoV@O)A|KZMDfFd zLuK7%kd;IBky1SJ8I8WrvGh=S)9naFRKjPDJfhG^hBqHGl$LEQAiKFQ#p6UkD6Sb2 zg}jVoYsW*33@95KNI|i;1z1dRla2eQT#%|zJydE3WHx1+6&lZihp09#msMywn4(+2 zT_>v3sg&%f)_zrjqo#X|3PlN8Th#<>?O|Su3RQ@~LXU19i$bq6M)JW7RG^B_oaGN_ z{kBiO*DJiCo@bAMoC7u7-$t_63-jiXHRu<0i-`dUR*6wx?O0CO`sNdug;gSB=rfQ* zZRFp*2YMdgvBCndL;fiXXb0D4_ZwajB$JP?h4KS;9h_}XF)6F54SR44?RS?uc{_kNB4*)5Nm?iSRXwkU${ zp%^werhugJf)rcS{jLf5V4T> zp}*cYP?CfIME^SoQST6;QLTh8J7a`zpy==dHZgnbAev!9aoFijYI|VfgwS>{A`%U( z&vAt!>54GphV4nK3~@-I+AisW)8Nh$E}I_-`+|Wc7G%Avu;n<0Po$>~cF*-PMcM`x z_I>+rbpuk0Tuj^{&?9j8s);$cK1iu-tyo>UfH*KMD|qNg9=@X1KInm*LbZz)F%_-G zV(k(#3lw{VA&=O5rs!!zW1N=oes@^x65st;(0WKxV`dfq{eJhte$pPfRHE<#4*-$T zFDPbwzt09xmSW@5BLvMxrTMJU{hu*)hQ%HBZN^(!)0m--GvoG)kkH||=xfrVL?i;l zn-jwje*%J;2~DhRcwy8~oBvdy&|7VHBL#zmgI2T?dy{D(Ib$;pHim`aj^X2AsY-zG zfMUxVj0~^hR-{|_bFW8ioh0{C`=S}sp7uN0>kdPYEaS<|f;8tzUD`4RoB!rYg?X%qTbWdC|-XH?;KUBCuJ&9j!uf$uZSME$iZT&&-j%5Xj=g zkX61*tzCgt*U?dCIg3&qsR*~4kL-5H9UVtNmJ~pSix#d9qFNLly4c9t=%;l72s9W( zW2zJmrH$+(SSYBLP|Zs|dLHrshlDS;DltY98ykTZ=4?6LFWz7+!m~|1KImE4QSne= zCUb~UV#Z*W6EV~)PJA5to7tVintf>b!b#?XgAaYo>1_3O5hFh z0eHdqV1^Ntbo_0_7V8Bc(mmBK1nQp3=Nb-{hgkDkY6U=Z63FVVBnY!vXayByJ2Nv= z0N8bf@($Nt$b9Alm`f0cVG(0@*XTL8fh!JrBPS%d@s^*VB*fU(YK0zeu=qe?l@N|P z;s9jBSYf0I8}81sHrO?|OAGD&8@45pwm2q+1a zPYN5KE*q8 z8$L!>`1ncFrRQ0MHGFZZ(HsXN>Z@)J7>1&Eyy1)VR%$w?hdnW-0iN?!xAdmUDMp1O z16-NKuW=REw!Fck?N?{}Pn>XA2MSDe3!oISU}E=vix!e{cA>H zcN(X;bpdrZ*OFRK>xxbWxH_1Fdo28QDbmwE2}aakxLA?@y1?R!sdWX-wl!IfStoR~ z89vp8q&b2%*8Q3=UH?TlP=RzW$9}epH=VOEc$U^Btk5)E_Cm00(CsU;77=PVkH{!$ zH;nN~8`G-$oN7aH(v}tUWQ1z_Wvu*)mUQ7a2kfr(!ZKa#fdIYhWx@3M+Ya|FSmY3n zk-(lLyG3=O!l|y_%y9=Q<4F`ctWVhY+$YW+REcoEH93fuXe7e}i0HZahn!4`nZbDw z^XB|!y(w!2II1195aX7RAC0*PO8sgI_MleEwlVUweJ{edMvnXzuUG%*neDtdp2r#t zr8Kv}=%XD2gAr6gQ46T_q6y2YW>hQFd z5>;aA4S2I)r z^tgHu|0Y(mgeDl?hi zqNTml>-$lTayO>B(tEmwU$uh7un`g5t}Mio!vEMt;lul*Re=*2My*JHwZX>taT;iYt$t>(DPfyO z(Uo8{O%$*j_y&m^d@^I+#`I#vMm~MGRt>{1ik{>Oo=xoc0igYk=tt%^`-C}Qa0HAx zL&Cw)>W+)bj7h?TsJ#1HK>kbb_&4;1XRcU)oIy`gJdboh6e0%-q&kY3kquio=Acd} z1nW0;q};9J>=Ab#Yo1@Uq`>6>ep385s#}4&{yO(6#-1AsMO95)0t*UKd%@U( zc{O8&#lDCY`VO&)ABvon`C{89rqtS70_kGnwkMc$;_ywl>)5#U89WC44FF}_DF}f+ zEaW)L$TcuEsfOacGdn}W9o*DYOx)Ri0KKqdz(w#H#4Hc$=vT3DLU{~dmxWLCg`+Uj zzJm0NSj={zG%PM)p;{%vD=m+&GqPV!U~r)t<7+(C;e$dSX}qq8&pHu_2S|_$FsBD% zngb2HtP*Xtq9nXpC>_d5n|~}S6?_`3)e2yzl8zJHWx527{T6dy)}4q5$xhJ{ysUXV zy+mwftYHRD%aq;Y%|GkhMyRz8-J|1yfEt!3^4XOOTs3OeLecOzTurPzC)5#|09FOM z{?{t^pawjx(ID(qT{_8&Dtc>CJa`GB0d^qQo9D?a?5{bQXxDZ2g5|nOBlT1ZIu4#` z1C2l_dxH#gHb0c%#vqj8jF=8gQrB*41dh+m22(q>)&y4;iDoixaSRAlLQ4bLVW8~F zDm(4+E(tz%C{?pi$DTZ9f{)Jdfp9hOR73qp0sIA7NZAo=H0Q-yT%^M~?1EsR2my<^ zVIk2W@Q{)Lj`G;W3PMxQ21R1mO}g5duq(Xqce%f_4I-Hl+T%KN?kelBciAAJOiGb7RlzN8Jz7Ru)v7DmrG2)a)5PM>94i5$5saH~|sepSBJ zFke|nU;$ji>RIzx})aS8XbXN5TfgFGmuWSTJ`qvurOUuwSc7R=blJ zl(6B}x8)pkv{RmzsSm3iZW;`LNlyBFi)%Y{5-eAA5ylGNMr zTFD9}c^>iy-bIo1YNvz06L*h)dOM=0<{<6n;|}RN6Us&ucR!A~iuxfr+Snj9>@@x- zJ*VU85EBwoO{Mn%<9vsJLDXHmQR1H--4UgSh{7!P4})@iPi7+jTgdex8|hclt%hfMy#q zUpi9BSm7XGJCaOR)l#zY3GT;(J=r}-?!Os^7fPY~LU~=kmLUCdqV%btksSBjKpNnDv9xVX|{G|DP-BG?u*0eU>3m;{N z=5Sglj@a}vsX zXswMLb$1&Er!wMdBCo>t-|Y_+DDOh1&{*VWetfn}MQsA=!tX(J8-|_fZd9bX!^MPW%aEhWcoj7S*>ARrQ_{Cs%FOZ z@F$#38n?Zwp$tN0Km#HlkzvK2M!#=y=}1!$)1b-d-m^)I2^nNUbS<<_XDDagwaL@< z=FGJ(KVSw9;D@I3=jFY82Y7QPk7E26Qak2N{yT_&5a*R2!pr^_1c3e4Ymb`N=lBxs zXlZr&WqnljoJS;(&}po%o3chLf)e9g9vR+F?*Z2E-rS8l=E#fAXK6mT2xbeq5Uq^c z-BjejA*+s zL-bS{8TSce4eEaKD2on+&$MBs+BV#V#Z8l>`L zr7k-`(KB@wvHfDt!;7446m^%DB1+_-jM;uw)U+&ZX!fC|b}pI9En*qXv=|_8B-^j5 zm6SS*{OWnhRJ({q&@18qr-yVkl05M9odm$VU7Wl1XS$lUleVRxN4XU8hf zutViwv$Nz$|D+p|^C4EO>j*z$mgJXa$z~6p=2e{TV|5Y5>`dqLocOpMUwLbscdlFn z!Wlm5+}RKO4w?NuT=u6@vO#v?Puj{9ZL%|S6@B@i%A0F8||7Iv$s1r|>+SyhMm8e<`_t9k+GK9*5*?{3qugL8~?J7s5Peub1y& zig4Cf?0NK3Pn_z8<21VQDh(%Y%et|oHHGqY5{ULb0;SB718|;^# zQ*z%KEz3C=EmvGY*L1RB1n*DD+8!^mt9KCaWxWRS^Z4iESW~>jMWuk~+<%cUHC_+} z?CR3{mz*6XHBB7GJJ=NiXgZKuX1^eS*QJKa!9f5QZf_fRBJSG@UJW6pFm5~F&4-sVfO{%5p zw^WO%yE%R)BE?c)?!YcjJakiX;F$U6g_yt3F6zu+Ges%N zgcJEsy?jr0q(uFt(0loSjrfz?U-+3l<>{Ncd#;+snQ%V6V_AshM5S%G#9!G{?4Ppv zu;$4m8H!TL|Ajw1)gPmdBnBnCHYp7~)_NI!CWXELdsl;gGpUthDIV8mxHt6!sWfs; zME%-ylix2rOG!1_eQi0ko-~T-h+o#*z3V%;@9B~Up36eirq}YZxcv`#ysPE?S8BvZbL%xe{U+P^meKZKj>%81 znycpnu0Nh!b)$3@S1sSx;dou}N-l7}bbW%8XWVLcSltb4B&hXyo0PCfdFOv~7a(g2 zX^kfT5og2u^!hEuG#)I&@OHO#%2QnbIh*?HOT z;e+{8Z`Y`P$%Xm>_>>uyqx~y`!{+q**2+8Qv2^jGt*$?&s03O~#7V?SuUApB%^lD& zSZ!GCiQ$K<8k(+IXS_+ZEq5*E(HgWa?UuCLX1lQw75QbP}JuAJ3HIs?!IzaGui z@*BSjsj*YzmDu>)Pl$)A>S}$i`BtSzM69m;)il_2^+@d=g>g`=&^?7*bD4MQGrD%I zyG;kYcJ0=Hlvj8<*%?7lmsU$KXIATPBe(}T)eY4!#hPe>g<8e^vlXbCEs2hSvirMx zFZjiu{3*g*3I}Wq9K=FF-wP1b$CleOjLn36L~~p4UDuaKG{h6v-PH7c*4$N~N$h{p zG56gekDZQBzOAYpj*{ri#)$VjlQmzl*VoT&43oZOn|Js!$VY(C#*w_dSpPb2V$9Iv zS^XJa%cKX5&ZxFkt>erylA&$v+()1r3k#+Ss+J`hYdol&3XXv^@q)18bIU2=T`A;; zhga@_Be(f{SE$XkH6~TvXj_wff8oISmp^_19VxSL!TNd0F!PUMno*qba7mtkVdBh? zr=jN#YvR@Htpi9s_rG^{34O~z{QQ1+2N}W(E&-3unaa|mvC97Y?7V;K@7Xy)46Evo zR)jPknrZ`%m8wh**ThS!+;UdU%Q=U*CU$IPVeaJdYKr^FMM|aoI62?-n9@#!v|G}& z5ye1KSG{1;&*Xo>r~lcu1>f=uC%bD^CD$N?B)3Fx5)8F5CEWse*at@8!niKlaKSGa zh$Bd-k`=LktL*9&sF=@0J+CQALRq(r^MW(#>JLs=??*;dNA+aZ?%=d@BrblAUZq2I zGlxQ1!K&925K_m6A7$r-Zo5tczNUZJ=*dn^;J7J>zkKLJ8@0wU@4@Ql;gdz4yi*@c zjo5Y0xS*-j1a@mSx3TmlHdC2`ZGMmHj7LuKX+UZOEm}!k~0j%jFv?%$h1dk>Up z%*S+>7N()GuSkJ$HO>rXS(c_D!vI4~@`RwsfFCkGsw~xD(=WnMpJz!G0@VWHMDZo@ zsPHs!G}=Rl0})NAGzx@GcY;~Dz8CZhQzMa^)1;7wylPZlpxI^A6TTbmk2Hd8fkKsBz}M7lDSxG_Ke*lRFlsES0iU z#srg9xMh`(0nBF7cs(Z%3PLun6ty~c0rw(>v$`apB*GV+p(BoF0ptNSQf8s&|n9JXLKt(@tv$i;QokI zin&KdiP+vOFoUu0G9RC)G=4_Z(54*AGVuQ-%t9_kggsvI`O`4X3ZQaNK1K8|TzpcR zDGav9o)tkNCnoSeWpc@XVXiMBL-~}bk(wz{mQzj!17F|*2mw(aN7e|HK8}Qm7zB}a z#E>E`07wdF2*SL%oLGjVdQd4uG-$o{O)#1gkUCQn7byTBs8Mhcwnb0&6b2akJ!5^y zTuv%!V;U()IA^?{*j38%V&<=+p<#n8S`Jo3VX`H+(iXmRMfC2tv2M$3sn&@Z0gaiu%E3$TyKD&6@=yhyN@j=B3$cc|BW4uSDd~*5W@mdI3NQw( z2AY7ClD&2+3<*uS*ggfAKb#*D=Rm@88VuAMMQUsxOqwRD154{SY|{|887E^{PgGcf z>H?BduXRrEA;XZ=;YaxQBV7HZL%Z@khSQ#5z&^_q9t2uub0pZfC5mXEehiS_ct^ERqXf~F1UN{^9 z*GV0beo?Z*)S6k*`mFQ@{?-=quludDc`KcGv<^nHDWxzkCp2I)r|c(aUQZ}V*1>pT z76FMEZw-=gmAW0f`d2$l`=@FKJ24lcBD|ujFeaq2E>s@fm!S+jdoTb#tXc%PcVP6(SAxKLi!XH zq4_<|AzjMQ{1CKIRmJz!4jwct<}!oVYC86P<*6H8&w8ei z$5ZZjhwe7Ky63u7AR?oxap;gOpg9Z5 z1ICO)AgK&Gb7!^9P%?Npn&4Al31UmHs>x!?;t_!;GB06UjXzMxQ_K)dgb78lK4310 zRR-2MtN3$jPBH9f0EE02mQy`jY^04c&i4*!kdf&Rb(k2h|4vt9V4jhy zJ5WG({wR(LrwZS*cba$E<_$X$st}~0H{((+WHp$w%FsxraiSPEiaHDS3&4k^fEP1n zvrw{jf*c|e3FlB#paOe_0R=Ocf-_2N+rcCREoy=R3Ijp_ssXnAD}vpe4)In}-qYJP z0cIeSRE@<}Q#WV_6xXVxFv&`cYe`7ZxWh9<#)ooEjo_!ghk8~p)gyv(lA}m1$s$(% z*ANO33^kQ|CBFnVfLoxhf%`0p-hl#&VR2>Wy4kz~MeU-BB<)%DxKo%C&N?%B3%OKC zNup|@PL+=;N&;jFh@~8*ta0H!-Hqo@H*qS8@2h+_~= zuOS=njb=?vZr;{y{%(r+Cg;>6YP`PSrWY$YAN01gH=h9|N4F}BuR4pe*IN}6Y1li|izey`qM)X163AH{*9o5u03LQEbU|J8S3)(k{R zPI^37q-HN|U*=g-GFUSp17bD%MyS@|VA^Qk&S3-Tv~D2rMKPLXBD}cJC%|Db1VbG5 zTgq#pn;nz`I6lZ|bcpdCW-A)Ls;E=pTiws~HD(Ol(23BILVV0(1jdrRDGP=S(J7c3 z&}yvMr6YdoWpZ|^TJD3Y<^eVnl!J+w88_iwg?<_`#)_kE^eY$x3=If1@5rUFAW>e6 z?DOs}y(M%D&z~peN4enEu&4QMW3UuoP5Gff*PZO_g;HJmI0OQ<3NR}!$gOyT*cDGN zK!J`|n#v64C^ZZv3j7*ky-BG3unnL!0PCy%tl1J5P}=wy3zG>CgO^s&{DG*LmwGN_uiZ)i5nnjz$E0Vk-p6 zh+$Yj0JK2Th*&j|kRf(0)e({b%;zh0RCfbbg}tB}AZh{kr~|n6>Nd982a)eob5iTU zGtC95btCFhd*)s4*6Qeya-k2VNkL~C5?OWbC2K)nwDg(k8Spp7DN$kfWTM?TmRYWf z&|bQoHM3Om_nyQkn;DM3y0nQ2bQmg8wbpF)cI5^9w!xEyRkD0=F{zdI2 z)JIx`CCZqN+~ZP=ig{GLb7Vy#i$g0UH1$he z8?UU;-J_CKKs}xt3}hvYxPGasT6G{jv>#P%CumzUnkh8E_@W?)p>QFff|Oh!|2bDY zujI@WB06`>I8tXxA|-UU&|}#N*!Vh>^8us zkC=$ui@vUJs2T#o% zcED0$X|ch|G`p=w*UwkSchW@rdGF;I!9b!u2TFtfxOjUx+kf<_;YaljU){s9M=PGh zBa(4j+z8TW)n>QbXx@psBk|MA=-X@NE9Ug28$s~E;1QnAD_xf}t5sVX?A6xS7l}w2 zhBDjW==iLAKd?i=9k- zNLI^uMZP~W3?k1A#lE5oXu4tkTnCAA`_q$u`ts=cWAV+)^Rov*xZwXHChzc&r%B_5 zF0DgzN{;eI)>U3yy*T~+-sbnA-3#_u@VEg>20Vp6crse<8Gmxi_k)L=H6avoArmP% z%G=j`J8^+JGxtYj05$+J0AzP&R#*5RKTOXun*DofbZh&vY`3Q~-w>9xc$y`J$MUzCm5itMCxx;yzL3FkQRlO)Jg+4_Hs^)lKV-ZY)VN4<6INIJ@H5EJ$iS{z(mu$<0mN)77Pu51yLme zt@8P7#viX~MtBFWy&isQ^=o_BtMi2^Pe!7CGBt46fLQ|`Qy)B<*Bq;MbZ>LexG9># z-R|b_hr1+0P~Q&Y(+{F|ip7~}yD>5%-BB(ej`6@hWjHOTZT1iQoMT z@11zoqq2zbt<6FF*TT;`u6U#gSO*ZELRUbc)z$_#O%&FYf2_ImbV zvZi*lcc~rUOTR+HS41_-iE(5*m?r@Ta9!C76H6h(i_xo!G`tRo*fk|-t*qB=w(<#+ zsZBG6a%Rr2(LK!0Yb@Gno}AcZ9vRhp60&u(upMTi8W9c1hPpB#sq$geNer@Ndtl2a z^Q*}^xG-WFMKKLC)B{uIJzm`y>5WMt5YG#At5n8(4LcMwFUcI#f%%lk7B}}r!zM!bcBZqRo^_HYCMR5)S(jerXPhCgOB z;}&O7!CP)h002Hd^uU0P3YNw$u1==TjLb~H#KQFV4->11@W;PQz*hrVIE2MTIXRga z9PG>^733rkU~yrA|;BHIcs#>e{ zxD`}Ebs31J6A?rVI)R8hMm#Psy7g}VLmOkv!ztMXVRBHZGp?BPAeM|O`r4w1Jr9?GP=o2eyRhu?h8zD2ZlrmPs zjtUW;pcEHY(Hvj⪙-Xr(sctL-lNXF^mYAeRPpyTF!C@k1@Fxv0w#yy2dgNlbESzW zS{Y*f2`i*zYL#AYQ;T_ix75_-+|`jaRsGZ(s@=<+u0Bm3x~q4~R__x>Z>u_X{T&MS!7iZDPy;e}K{9$X zwQV1Y`f9k10T?Bv{?|Aw(qS^XwyPY4e*IvBB6TFsF@}C2pCo?cieJC9H@H=N@47J| z3Fx=`8=BU-L+!C@?N6y#Dz6NEiGQf=dd^0#X%VAb|(bZFODwOh~GlFv>-B$bx8Xqzwq4PCX>eOrq6kc0g_g~^` zvZ2JZIFW$ojI9i1x*s8!0f0ZDhB!1b#d8j+ZA1v_d9gTjeLkF$p2vmRG@n!j9f;m| zsUmL2(Gnjj#1ZkU%YF+YT>yF}*89X!`o0KexmsMi3LPiJ>JZh_HcS6)dpevjQ!3V2 zDbw0+LQXwGPj(qcehNXy=ia6;bSDe<#(cK`zqc_p??s9q56jDb2Ft7Tts`@GLtgSKq_4tRLU=I#DcVKN6&v1cnS6&5>K`fdUxhm@Gx zt0S8_uwf71T*pmG}pRUc>Y=i4+k!r?S_T}x9jwaWz z#rvp&QCE|isJyeEF*{v*exb~$wT z{pzQa%_D92nUBH%j9|mHAPW=N@Fc!OfixT5b#+4D9#lw^yJG5@HX441nyiN>VO}nO zvxPR1qUn%#Ylw@ix`NG*fdSA@0puL;HgzvAy?mLe=5(Ngp<`sry#+thg}A&(I9aR1 zmmU%3*2kE|{|m0B2H~U3wI6ocbjq2Sc!bOSmMJ-J9Jp~JgMk|GE^JGs;@sJE`7;72 zdC^TUsm;hk-T=y)$%5VAe!wIKEEgeOygPv;!I)6D!7t}`Q-#-rjNZR(!6xd+xh%xY z=T>3z5=6o==>54Rm(`4cvEqS$-SUGepbUKQD~`h9!3v#T>u=6ednn-y`Y?+}054Pe z!5?$OhMu>A{@5ZU3H%0}O>5ln(d4;*xE3G`vP7p+l(^8m?5-@4^U5#IBTsQEI7Y2< zc@Kxkk7D?|ELhz9+QqUpkHsBtY?CeC1cuWE>I5##6jMp3o3+PbtF!)45o^=2lKmu^ zhcj9GY4FxfOu$AGL-yz6?8EsNv<_9i`h`o-k77(5@RM{mM}7yJiIdT3ZyZ`ESmyFw zP*d4qlHBvLyl2v!mmL&8=Wa?b+=+aK?RD(&E*L(_0gMM4Z1#X=_-${=+YOz5b5S|< z9kp63I58Df)5FB{qA_*5`3AP@nH39;C-hCS>tmz`^v^QUWm6r=IE~l8wqd?ExD}jX zwEBU*pfk7SIPt{me6wFZaDF8+_aC$;=+1IO`zRAoXrRF%G^`zGNA>2mB&li@mEZ=~ zFF$m}PG#JbJ-0+Z3k z^anVBE!r$hju_@ zSJ!PndW#<(*M)(-0~=7ItNmaNtHQqV62W*)={RBqj!3rMEDy;U%6o7>`4}iMzVb|I@@Cz+nfn zKG!J z4@X_%Dt#;51T*MVDY3Xe+DQ^I*W-Qfy~YzNPav|y`a80{VT76TQO8PSCkX7_O-H>x zURbsP8bWFKR1+3V7f}^8`o`E#0oJK}TCtkwxROUX_R9CQ-jel5|Jf1-uDJ5%BP z(gj>j0RVJpVTVrhDPy_hXuUmp-RB0fKr|95o(1p>9S$LCyY?`7Ye>nLsL8uXXyIgIgyvfw*WB6aY*5d=UOVVcqxz zkrxxv@##%?UF<_@gI*@OzR4is zrB=(v{%P~@R~R6!#}@Y=Nel#{iPB6*XTDFKv+mO(P!9;}0qDxw9T%usEd+zkpXqZu z`1RC*6TwI7()`sxr`xz6aIpUj2CXh;-GT@MmvW|II0f$w_COBBiL;7msSPTiyc{ zH^9(B5@wu$>$h@veqR;!yNQYaznS?l+3Bo-lktWe8rorceJ4=c01;)B@fxC+O7hU- zpS}4}wWS{d6zI^IX)FU*h0g17(drwW7m`5b#gt!%A6~GD4X<4I=Me}4e*vb|LDDeO z*pdn>x05lb2`HqW7C-@RFzB_nWj@hIx<0wuAz%C*KnLe4thA=tBH(}4vGg1H#xFU{@xlMu3 zNj>p!$YlJdHcWhG>~yTvddIE!j#!UWV1klGbt7 zitVhI>91M=D7ekrdRsgH6m|cp?vEq$Z;it>;U{rJzcG|=wu2A(bQ0Dh`7J&x9?%{=gmDnrH>{6o&A(My9NqvIfr?(z0!UHzl6P+Jf%K`!`aOHZB*AJBBY~k0F+MB)k5u z*G>!$8|VZaZ?wl_uAmk!0f4&_60w}05Bu&Nd|VqL=exKP|FmyRyzAiCy~|{hjvg}~ z>4+#dSU?7|>kO@*-491~ovX{o-BoBO%fG!Qz_HOvL#@;t^4JZ(O+=&wA2?DPyr;+$ z{y2Kbd06GPD#i*Q9N=~N5EC**)z=6jTrdrRzmOx=UIFk&uw?t|g_G4n;sgK)ZdlE`6SJ&diy)=e}ni!Owrc%tEVgOZf03H?4#e923GCaa2on?2f^A`qdJ* zRp(z^LHvoR)oDS%rmw3{3Xa|lfC z=1I`Y^5B^?*3MF%Sb?4YqoVq(935Zx7~b@-<$c-+wHGWpub};(V^Ky?#nNXrH+R}j zzXt1{Ps_Hc<$YXrUx%J@c1aSP5%?LAz7S-U*F??gHlBhH(BMJ*Yr81++j(o#rz|~= zZ74JTbAk&?Ct!@erHn|j!*Wp^NIdT&riJCB#G1;Xr6q%ZGxWcg;s}u)WWFl4(uhHG z-BBcj12NiE>ypj&z8d_4tRb{?0^8!+hm@E#l(J zIdgD85hF#5fJM{okE`=DN?i&R?TgQ?w29C1PZ(cpUfs!*^UJw*%&C&ZIFi$kwuupU zhgBg0*__nfSard!%uEy*qsupA{B>2y4N5AX|^D#79V6+LPV=e5R?_LR# z$AtV`RQ<9zCYkxAL3%mPohj2?P9e~~`*JmgZ}WKp@=@>OX(e2Hag z#MhN4eoW|MHyJ^qe8~BqVFwZSl94xT^J4* z3Ez>7k4L;@y-mJ)qNcSU zq*?wms|3Y*Byl*&p}ypbY!;xPb;kN0O}9oSR&`F*1<+EzC4R2q+8GM!mr zykA=*ZNf;;d`MD^UrjbyiQPZPkqpB@8$t~?##lQhl}vB1tHPHzm14Gks)gNAs5xk_ zKr=VxV71vI$rmLvJ3lD}+hj#i`g5y%zamK6b!lqq{{j0--__NXpAdi4C^`Hh%nt-b zWrB#MrOS!?c680nVr&22?i^!5zavcoa+T~k3PTBwHq+oIm&b+#;njSo=-bZnTKc{p zErc5px?h$nbEYB~p$r&}af^NW*c%~K*ljdnq%!dKnU&?`v6rqFJ)h!}YA;3#xBSRj zxyuy1yU?Ql@e2*z)zZ-6}E9G#S;D|#@JL|b2~Q%nq? z7Gvq>&v~MMl%)H-Zba(;z42#;(p0g0k(mYOsx4_2J!_vU=AW!li4!jDQCCUM$`3S+ zCV3?|3xzWzE_zQ1LU!BqB%YHUx_&;~ctl;L+aqgl-#nK;Y`NMw(>Q6Eqhc(BH74tR zQROAomhU0kpN2gQaDaa;&t@5G!j)u|PNb;#`I1!f%V73nCc^EV$?)s6O7`9i_V)aBUf7j_P8T>U9SO{SvlU;Hi_Gd`f>Q=Nfi0N%dg;>0?R+R zewH~%y0wvx+eY*4#bSi?K;KVJkjmPONA;eae_%&F^>BS3O%L|4v;(;^XNCkL8+S6U>JbRIypv*~=#c3`%`U z?iWRZ=%6x?@;i9t4~J%o+M4Nm4poyDGpvOR_rbmaif}C{O5f}7<&y*%@Zc0P0?Wqb zHHY4(Rd$V$uu!H!ye1;&Z5dM_hzXU|?WsUVn0m+ws(K zhFlJzBzKvjlbqbv-WOGzG!c)XNOY7~Pm>5<`2@u|;f$mHv{7bw&A0I3Wfc$ZA77+^ zl*Ur#i7xXL6T~6sTSP6JL8V)qmCgj`6kC;5uvrAp`!(M>r{k#)==Cx&ap(lTm16(q zcR&}7elThH?;!K@Ns3A{hW_~e>ShannwjtSh2nR0o;PmBd@XYzc_vIK@$ZtG^{6d) za>qL<%h&l(#HNvQIXij%Qr92OyQ>LEBR_^2G|Jd@;dX)Oql@-FNGs1onci7f#2o2n zd4|ch@7Z_Qr{9zPm&(GdZ>?K$a)D=PY<{9F{IlyTYTI%YMO>x(?IT%azf{WQ&OuD! z^QRB*!K5K$;LO?nJXd1#zFOzvp-Wm@(FQk19G9CF5iZ4{+}1uxu?QYtH>>_N{VmHB z8CGYZVZdGzFI`AdEi@0(7y#Ya?1t1~v}6;EY;46)cd6w?Y|=;!2y5bOu=(VyDguDBrS>1P<Ywj!66*bKKHJq%hl0a4*rIwO0ohm={z{6-{@jqrN>^I zg~U`gfI>GEo+(PRH{8w}dwrq>advUTM~nT+y4}o2{Ka}lLqnQ2s_;szrx|fYEFtB4 zjD^0~L*EG%2geKXUlv^oeSw9t%gMOI&SijesxemW8$qj*A1b?Q#GPXm_A!$~a~C#or7t?gW}ZV8N!9*+Fb59O^*=tNqBZ@IU=3f=1{#cL)TdBEOd@DFI>xTLTHjc%YFC7R3K_ zV7g4N&PX!8nHuLPShDT&mikv%(4b>c@q=+>%1?v5VMPo^OhKj#-)rLFu(`23{R4++ zxg5sL?fPAC!fla}gy6;rX=`h{Mg+fQ4+{%J6%1N&(qTegC9InRtS45h9FzRHvirps zO*l8|uC@DUp^+BW7~>7fNdGd;(!+V%ZrG~!N>xY4b2#cpo+SX>KYz-{mHN8u&Nl6U zngimmf5gp7!ZyYEh9&~@!cx5}26l%}&rM+J0mS^o!hAce)~@!_*V^lUI5gN;+wt(X zReSGrxTh5c=KO^A&I7;jlz7;kSM@TQyS8@1Zjs+xmyDx6pkYb33w3O0v!S>l4XqKW zMop|Rr_KSzJv77p&b}-6`PQ2#Y!l0W8Ix`o@>q2a?$>4u?_<4GSt<7Fs&Lj9He4wj zAz9yV8qNO+GDkn~!X@!V2(j(VE1CReonMfV!1k&qKL?*`?UYhY$e;5{&_&c8Ri}Hg z=vAZnwO8*0gUuB(Mk?J~X`^OV@7~Biub%n%UR6y>7$f~Pc+S=RR^V0;h&~Z@Yz_%xpC<m2G6Yl0SG)fJ^OWQ#ns3thZ>aQ?nOApwd$JX69vZy3tw!l z@<{ceCgiBQ{tsDZ$qhPvk>h=o6cXt23jE~NGK{qwyr+XN@Gvk3t-c)Ci!9XI%D~%x z%BAHIJYvd7!B93PSNY2{>*8MQe(|05KX}jjzCPG|bSv4jLX|}15dmfpt+?yxVvzZp zWg=ivAr^yLcrZ*WHwAs@MnAemSxHKf@Q2fH%0m~oU0?ggOAELDd}>)Ts0bNII2e3B zJyiD@qw$vx_J0+gR^kCZVmh3kN9sOl_@XuIAmh`|t!u-IT;!AOx>ZjgN3R<^Pu=h+ zn%m3JVv>>B3VAG%=B|PY;A3!!FoS;2z;!DwpAv-ayb*3^d`e>f1eB*!WDw8y6~dV% z*A1n`f@l%~4Qc)9nZ`l%@I0lI4D>gSu@y7pYIWMb@Ik*t8dM_m;*YDPNXJ&5{O6dl z***W3@LCn(1r4;OzdkIJls-}Se6irEo|$5zoJc4*cq5pTf|0J-o`nDCf6LP|r;c3- z?(ZFsCjNj+h8d(L7t*Z%mXhCBUbrniTik(uN<6QM2XZ6_y9f!+Ro?6>e`1N|c7P7` z6#UarKXLAn@3cnl?JZPvHK7)dOA4Q$o(2ps#$;QaYk+NG{lh>%t#n0~NY0-OLs!54 zIgG<)MNI6DF>mz_xLt5bi7N**2lH6Iwyy31caYj2vXe?lk`>1M^^Y8ka<_mg7HSFp z8AR(FHwaJg=W$kFdIs2x?hdheed#^o{DXgzmy)SjpgEa6UpNX@iYx7limha3Xn8T- zzR&bOM^er;)~-1W89tn$|1?4K-2!RS80u>5~_ok-V=;~~Ji z!H@NiSFri{Z=S;hFa6(*PuX?k&rpek5_ylOn1q;oPCm&<_W~0bJv}I_)#JVQW|q=x zn)g5Rt?o1d>+bw*mR^Tsss3pENJB6V$G+cR%8iPDIEeR%u5`;RlU4=b6X{GkdM_o8 z#|qoxLI0g;z1Lb<(V$h`!Oudid(-doahzln3xBj7_hI~Tb<0RmWhW2(wCoc9D*9`` zRUU*iX8VI01FGzuFgil~+HP{z(ntaS$*h*+wZJl$HfuOhftJUxzQ+_OMEJPyr=@A(p`6H%DmoHD* zCw?W1@~k^!lqIk(I@$cc>1?mJ%8Kf)t?G0VgW0m?AFUd!e%keY5g5u(IQV_1&7#}r zj{|Y~_!^^vw`|BbiXNzz1fyFnO=A51f+~8=8*kufU;mYSeOcyxa3EtE?7v}3JRWI} zJY1c>P<#I8gzZbZm;AH!p-&&S+g7|-13xilIuM+9A*6gy$lr^3fANaM=$2sXDrfeI zoC(vMKBfEL*&LOEkbl*SCA|ZQeV-%3FvQkVu6ilTzToNk*r2-Lv*vK{2NAsoPz=GYyMW7EYXdgWIz_Tc+ zT++W+4HPaXtKSWqq=m75nf&cX+A`melH5UWiS*AcE+Pj%^|<}I8<3kyjwpNjsYmE1 ztZw=RnR|>J6Y#zUsmXgpu=ETt!bT;UIl@)CtT8J2Pghn~FIHAp)Z>VW;6!mx;ml7T zIzsb9N(X-y7uD&}M<8N!b#)PeuZ^3vSi1>B+qF+%cpNFolqxSN!$VF=F(5Z2haSmM zQK~G$!nLv4uT->k^tM%4V;VSbHU1Jki;-&|_f6F;9LX7@#MA6uaU`1Rccs_#0#w7n z83a53bJ2wze03zR8_SM4I_3=dyfr*5g~F#m+PRrBh_6 zJ-vedH7uYPtsM`ywXrKLronttuSAyHL)C7FhE#rVr6!#x_anOUdnoa##jH=)I#DPT zPoer@(~@TK)XK&26+XPaUX)uJAJRBC2M}}Bgnmr2Z8QfdWHo56=)rO}z9j zJJnO6a$A@g8Gk;dpcs!4#G?@un(OW5Yj06ZahCXqacML1le%l+Ls{*rpG=O*X;SU( zw=84^z9JqT$EQzs+=g}-@mG6uSu{(1GRLfXOneH7X?rye&#(TtUB|aPLWG36F+9%X zwX zCfUA9vi!Pwi;Jg3uSrOZhX)Y{>o?@GoBG{^<7|b!W_Sm4D-AkYvGR~6tso9sixFP{ zEOB# zG7~ACq{Zs%b9)motCS{&lYz`+S=~kTi>Dm_9UeYpN5xizi1``slrJqfL*3RT1F#-F zo2s#|UiT5-aVUKeBd|EJFGmA`%np1f%=FwV65vQ*H3no0U5p&EPueivYo&YiJv}|W zw0NkVLC-wp6K5NUvs%91igD5y+c>ZF(?)_gDCQNdxcJ=ZpLUkY(z!2W&tiz{2X+Qy zq@FoOi1|4ZI;$P$laC8ktu)LBJ>oxf7rBSx*9m&`fTG^2YY6@@@J}o#0^|LydBXRl1uej`l_|b(4wJhrhqu?42Px*3IQ&u(*1`8GA484a!*7}`KIKd}r~D!l(OvSPNpG1-&kQ>+_!Guy16cZo}JlBx2?nFWvnV)Tv}n_yn|deT{pGTozubjkC-n& zWj>RIylccLC@9DmK`Fd1Z)>YZVnQoZ*?W?7?M(ijfg$|j-3 zU?qWMt2C>2!*)=)29Lng)kpo6gKI5YL-|WUY?uwI*V8k_3iW&t_*!;i{$m#6(D}ws zOJmlH`}^Y}d`!qM{+#{(-o*+=Nn`dd5h_=hFF1>ciK@R!Z|GmV;Jew+GanarwQb3j zUsFue?GCjp!|h>v%J97Z%6)$*XE1i}T#vZN6X$J2%Gpl$sLhj$qn&Fq3igdrY&W-4 zZ2bn83wn{O<+UnZ7uVIEN&F-Mn}&|rS+Ot}LLAxP<0eumjETbYS)Z#t)x*XP9yz#5 zzNLwk-xaezEujIe0=crFfXD$OME{W`?IX*8n4V}h9iPo!>kj|~PZaFSW2sc3Ls{FI zYXej1Fv@S0Jw|vt;@-UObIk!P^pW29Q*62Gy`%I=wcEHL?#;|gV0J)*Kao@=;TD@- z!kl-JVOvSDsT#rPtJ2;bPAOh)J+fzf4XwRGr&%gd*r(&uARg=S0!KEw?8$uP^k1iL-55t*l8W%ZNE4#)kXXYzb?Z#DYWKa7+@X6s)SIzt@AdB$C07XP+1Xhx)O79iW1{0# z8?hv9s+TTiy+sk0@okyrJ^Z#HVkqT;bkO7pGo0Vvj#vAUb~$H!&a~r!2{B?>xz6K* z3dG+De9eAgcetCkJLfm>0~VTJ@45w>>lxSPih2=_vIseC$GD1-%c&59iI}J#ogY4_ zo-@Dx^SVmJ^QaE>r(GOLBj~hVCsb{nl8!WGM8aXIAW&aZml|4LfQ$MDBsHU+UR@eq zyP-49DYoDUO2gj9NjbQr=IHg6eXTuFLw2V4Z7--sd?i%O8!e7=KqVXdF6Qh{3@m`j zJ#r5>Z+vdy&1R^7ny-O}^VEoJN<45*O$YxHA?CkVKytT9f z*;{UglYk&}R>1~lCBFiZdx=HCmnjzs+5XPab*k6w)Z5M!#c;%*5D#F{7IWf)L&$>8<408EMxxCl^Neqb3IJ>w=c0d-a?4n7(x9~S4_wEs%4qIjt zpKK5Mya-#fv^N*}u>O~$4cFdubG8@}QBqQ}-yqVF1avp!XBvO7AVyrPoBvpZ(%}UM zh(Z>cLPB#C3&)Ct1&;%|e1Oc+5A*?}nI;lp&&zOqo5^N}5+*p{(b#xcwoXRyU$}C1 zPIGx4Z*x80S;*@15Uh`Z)~+w=9R)tquO(VH_{}4L{%&3N0yVkdL0jkPydT_j7F9XMB zoo<(p6Bl=1|9hv#@HGN!V}q>PPiJ4gr>ZMT7!HSvW+StdddLnJj042Anoh=-KDS#+ zCkTAQqY`YeUpy;M`F7sOozSx&C3U7}Dwaq4pA}geTB2XOFwujb2eZ(K&Ou_vZk&DHQwKVMLLSms_Q3hDF}76C723W0 zz{lIWz#tPWq1t*B7auz2GIvbDWeh^$Z&tPQGe>VjzgwZV-;R?(G-YLCj%*;^I>*fc zakj*S!!*gTynE-G4n9uyq5>QwKkTYnK@EO^aF z47%iv1dl_%$yHJKH^pDe*6vKWe(27AUea2?GbE|c<)SPAU%EN zRN72Wv^3V%Ou0ZmPb`aqm$?!gV*<6G)oIr!m69@-ZyH>Rer1S=tHlHMCVnMm;REKWy%e<`xw-9n3t#5E?Xy5Nmk&V?I697y0zAm zzaJaMaTqoWZH*R=^BrB+b+p+g37SlN=L)K;P0I63Ao{tB4Ys>9Y_^626@Xmkk6Pc< zP{C_`kIyOn#L^7G?do|ua~&*sLiMsN8iw7N0x+jxrTz7!l33zkTSv!>I9P-jul2}> zLloRCSloTDOcX^-^#PAs%U`T($3IfABjP%> zr4!_C1e!wFU4OKN)F+2{1joQ|%5)9vOHMOdJAJ7L`9Hiw5p&{$3;lb#W_RMRcNk~| z+3hGYEQ2C;8FJsHj_9P}&<{T4xB3-t*-yC3Q+PXUpZ_xs%CThtLQB@0Pz#ph<9G(3 zB>j*JO+G$$=dEjKXkH(!ynH7qDy9_kBTO_F1Jd&;0pgJs6qeHt8MH3a-2~SWWXxd;1FX4b5}f1t>mv z<;wbe^lYg^DKyBJI`u`Vf3=?$1Tw$ND@drvcGr{0mEY4E)L6c*FqmRC^3FCP03BOL z36;Z8@WOmG=7rq3TGX|7AH&DhviZ2s(8J19Q8k&OgMl#l5Bj0fe$ZPZm%rMh4MxD@i04OC8}m zitu|AS?TG&Y^KAhbgSm7fM+!rvuDgTJD=XjKIw2x2X?j731n~ZTANK@>*{)BRjZ~3 z;AP)eSy`efxy{W{3L0~>Mw8<^{k@6&f?9cs#Y-LT?5R2`Uy7w6ke<&kH2-X>v;F2p zhK6k=f+|5JM=oZoO4pz2WLvnb>8H#l=vX=mn1sm+oAr*Y$@Nrm>T*1y5OQCR%t6?5 zT0)_xYLC|jGE7+#inF^9qs+Jye9pT5mf{kbi@ z!c$)ED+FL+z}rmrZKl{74gm2qSuP1bMr-^M@SMy{fD3T!2EA9{H(qoMc~wnbyVQjw zP|BBObCjr3F2qDXV)!YOV23ZrcAU6;vygkOi^*cnJ4xlH(kIzecfdD0tD^np=Z(iW zZtm`Cs;DfwpZzVb2HiJQuYh)p&L#}}I}-ilrG}S-_WAeg_3Ztgi8w(g<}tEnPfYQs z#k#%iD`2ZQ_kYri{xl9?<4sL^E62r(35mxE)BBx;gsh?rY;DJ9W(<7$`od+=Hk3+h z*|XEosKH0!afYKCfC6i52gk<8*F;dn`upx8?Ry!;|g->?wC(;UI!UBNg9s%EyVe3z1*Cp|B5pn_C<_S(r-&6Lzi8zkvA1M4dbxo$|_`ua3i)YdBH9bbw3+n(edU7&Sd^0EvOMj;S< zv<_q_i;tj(@|EFoK2N9_HyQD91!sR6gTS{iFHTm*vK#g(wu=F{W(1idK8wJwzTrkB;!uv2rH z?PaUmj0HZ4ot^VKnCTII6uBp=Cu8rUOO9X$NI7obfHM| zGV28ubMW)?M|Q_F(P*BOQ0nGK>NX}7B2K$KSO0SrA4!;y_3ni;GdnxX z=Xlfgm5!&hQAgpo22fRuspqR%G;yC-+X%cc(h5?|>%aN6in3yU&G=OA?j7T=%8ZPw zhv)6q)rl-|i1w-0-0W;ATiF&GK;$+HI3QCes$`c80lt&$P2^ML$A2*Cg}OKfLMQVx zdO>oi{nkI z0+pNtb-ds#kMRi5_$uDg+xYk5aJq*+|CC0AoRWe>VDEQ8u-!yM@EROc&Qog4F%3zr zCtS*0_wL;@5HK<6F?q9GTV7e&U$A9(`FgZK-K0@ObQiFCv*qRGT|Uzh=(LfXJd?OV zR{Cjpn1@`%>!ixnWau!6IY!Q^$KF)vi7QMosv~TyWp|5S_h@T+E8uMeO>LA69cZ;@ zUIh5uTT^rVF6tMJG`Qr}fDe5_KDEd&MTLth>zSDissu8?s(Egb7y8^j@ z81Ym^dkC|7qr>_VR|1cc0y(yWS@_J1cgs)bwdN+r=N9QI`MgOyir|#}4o^}X?R0B( zq~Q0~UwZBQ5mVbN$fg^9xb4yVo4co%Dj z4Ej%qg) z)}_Xh*b%(dUUMFwEsJS$=(R+NAipU0+>7R#Ta~RxiyZBaeiC5fdUUyJgJI&8&7uf# z+hz-7(9iAd?Sd*V7suP*Sq5MzQ6Ak2+SAixT_x<&?yW#7ZvU>|jcvl*icL3&wN!6% zYbigqRTjmP*2h(Nz@h-h;CjI6@736)>UhZ7wbzxJ*UMQu*neAo%#_ z3}fTtC%$X{`clsk1DF+*rb@86pR7rZ=`s- z_D_$UmPKFm>DI8#PKoHHZwAsNyiTvgyKS#*F!#vf#Bjgp?&}y?a7$m+J8rjePd&>0 zbRO!~wr8vwj{>D&V`ynaf4cJs z=-X_n?x8$|^K>NW!z+3E^r>1VQk9^m=1mt9Q*lD6O_lD^S8bcclN4deG*JHl-f6~V zd%!*=E^eG{iprt&`7;dS01PlYL8)6Vq3L8!z(;{@YUOC#=|Ix#oei35q?UV~Y-`H{ z7)}Y21-S&c#Z|3VtDpBx8-QjJ<)b>--hMs_vN^UD_A29Qg~gO8k>mA6uk0#6D^Cy} zkTX8)4|%PYMSaZQJ34Zos+M0pvR2Odnoj$NyiE7Jh0Rv_d!q#wpWj^aS$8t0=4hD)pUdF`SdF z5@s%NEsPbHPHW**fA*se$U&u`!pQ)mzq)#Qdg4=$h<@w&BGK@tO1RrbW@3I2hxpn( zBJA|bTxb}DNq8%_a}A(3%ihb!-aKvXxMk1%)7#6IV;NyjGrYryL*3a$9k-1EiGL6d zM*w^C3JMG6CT)f!n*W}WpR9IXqxU-9H3TWMQNN)!R_qut+Vo&7JON2wFWCc6Lt6bjSNx!qdK-fz~4mF1Q*lD4QCY zN~!iVMOa8Wz5lmsw>Yz@N9jr5zrQF{Ocgp7oYrVnke4saP{}V+xW3`}8UOjSrD)Zy zk9O_68lT~umqH@%>??RyX1raziG2qyJ9A`(HOEO~@=Uk3_T-JdeMtmR8eVtep<3#_ z1WI-hRdKF>xK}{Lvejsw9tz9Dvx?j`s7tiI0X^m22NC@xD*=D|s#XiAOl2-yDeY^b zEIdafUY^jf`JGJ%t0KthN4t(!QTA07%$b(`_J$x@1u8SLu1{pNa@gu2<{iDgX#{GO zD=A}dClWbrk0Qj`T6)IR5zr4#j=qz%Zv9YToq*0`C;40|&_-}!^rlFU@`;tE|ITY& zenZC>o7*m+_c8~mLY<$_Aql2~^YZ#r1BdJxOAEx*m#oKr637B3e5w>1p_C$a=m?0V z=cw3&FMg2&`L2Zn?N!3Kfs+(9eS--F=-C0P z&JM-i)l1if7D_6rsI9Z{`koq}xZxq5J5T}t8+0LyfsblTT@ksVZrn+S#l??!u&GDarn8AUXs?s5)^6@21 z+M$T+GQ%SyDajnTf_44Ti5U33$CDccrm>5^L}bf+_e7c zkm&0Xbf?}cYz>1FC*V)w7#PQ=Iw)kzyJC$)Xl*oTB29P+HwlVNW2i;H96-ETdALLx zNR1kFUpZdI$YlAV$J<_~z=`+Oj*Ubh``cv^LcMnznOrNRtMh z#Bkn@Gj>IGh3|@HRdT2@92AoHkv$E@g7o!otgLXr%^5GWj7R_t2%Cv1DXO6U&xbyx z^e!?$Hef^D)SgD12cO*2D2Hj)*Uk%yrFcPgkWfsFp=zUk@%AuojC) z{Q^qEXg2aaHDHulP(a&}f=kYAjvvl(5PsF?UH1oQ0~ktl zn%G}o>f@oPRgGMMc{>U_pMM_SbSX$IL<>7uc6vQ^O6z1TQO3r~x2>9+v1- zcN0yqbqoN)3!hpv+=1c8ve$rp1{)*iyjno|FDkIr<4*GlJ|C5;+k?CrK)wmhj}`lV z1P4q~NfdwbX0DJ^gSym32ud-K0H8uFzhm8tFR?U~rw`}(ciNaa$5sv~r?wYx^G z`D`@Web2d@%5!iL0MQ!BO!xydZ_7O`VB7tgj&SV}HLnpafU;*xJrym6MJI;(Oke`+ zRiO5hlC)EgDWY^g-%;<KKlExSnnMi z1GjY=5uolBO-w{2#B<8T9rqo~A|S19g`Ex#4wwgb7$dBk#Vq`ULzG4f#;a~Y9L_3o zI&@;&v3>$JBY}zCGj(uTGY6vk6KArEfij6K(8&SxxP(cYgslZy%_ce1-fJ*}zGtY{ z!dzrvM>yLDxXuTaY+qIK&DulSdeT{cJXM3axX6H6!iFwHl$vV3+UqGQ^LAU+rS3(?s=Md3-F-5O5{r4t|Cd-2@_ z2&`(4OMl!-fL*;*R1D9rqUe@nW^>FzLK;}UKdrDJHLa_!*XbeRw`XUUjd%c+1jYhb zjdiDaMI42RsBvb#LkqON>+Me|JQExwaGRK2>WRo_2W5d|=6ax8qpGDY`Z@hib z#DGrc+1r{&i1s&H1k<=rpAx$d0MC=NFHr)QIJ7WnOLgke3N4e_I0eTAKdw3>5-JcKtY+=u6`Gj! z=g+e-TtrVO%vm?^RDVWq(v`R=J$)brhG~bc6#;`i`zt$pt!VQ3rB8z^s&S^b7exT< zuy}neJcd#2RNY$f#_$CO==vC={a7>=Ow!#yIJh8)Uh9u8N^nJYdn;X2#L^_ zURYRIqe(WT^no9#7_aR}CcQY)OyfQ%lnR;E3q$~3xvm9Z0kzxe^r7jlb{iSG`;qkz zzMXqK43jigi?uHkgt3S9pN={1N(j&YT`4->Wh4$gfC`XCDKka`xs^=!&Yq_j72LB!UeB+6sU4Aq;oO;JEj~2!XeZRr5 zNn=mmln+f!HE89lRhYOB!lnl7AxI{(ZI59|R^l}fWB7l_=xJ$b{R%hME_n?8e2+XV z=yKiB8&EYDk10+;4_mDph?bI;OgTu5Acm(8 zb8N|CR~`aJnCG83gckTFZdI!jI||G6NvIv&lyL$POFzQgNf%UM^V{|7y9UkP+HtU8 zvE;8IiL+%AzZ8{}`r#liZ8!jH&+>=qRu{ZGewM&zND1bK5?lisH~?mHoS3+u69_!X z-MC1Nc~9um3NQJiV1-llhxhJ;P7}TQ=cydqO!2u|%GSjtL zRHbDvF83Ou+~fAjKmGmncsoa=k5wBuuyd_iJEuTkkzi$`)PuPv;NGX=$5be$`!!=q{+kEm2WJyYU~I9 ziKP_#2&O;%@7+z2^aU((hjH&(7kYKlfg~Nac^ZZH!1ZpON<<{~(4(ka%9|S_N2fnoe+AKOu+gHjhXa%Zu!p0x7 zcGe)uJVZ*`A0b4jy=MPW&X;35v5pe;I<<>ItXMR;Y78B&{RZP-=SBYlW!)o`A;;JG z;Jd4TIrJ>rpFINtvfMyHxpwKR2Y#JPI_2OS5>_895>Q1&MOv;we+h4Upd;J$$lsLx zFJs%o;*(U-#90o~7#Fm=`Kg00vTMMweFYWwhGDR|#B}ayb*qz?X(uq^Iqo2$!(wWsOEVNcD*1Ji4Cf()-V!^=ETk!Dc?|Vh(qgw(NRX~r#0Se*y zI0*T4MmWKiEfT6azqnwa7r2o|ylS5X#C<*TM_xOaV|XVP@U`3=Fk4zwPZOJAGN`TL z!=zrmHf{?9)AyFKyU0)~A`-V29uyvVIHll;r0A_sFcl@G3J46XEqI8Yj-gMmFm7`& zf*VG47XsO3SRW$1WYHz%3nkY3lVE;w*`Hu;@yob9iB~RW1%)d9&SGZ;e3K}mvw(%j z<-MNXcE#?m1@kIls~?~;@H(ZM5P~kcSJug$7ki~XX!vX}jEC?En*eymoNjw~%;ff2 zjam*sUMEb|6%~#0?7aja$yw}30w_$}-X7~(&ypp>>qNSw-x}bxssO$tU$+4-h`@b($L6%f1 z*Ked)Qi<4G`7vK-dIgoDpUgovvkqAIg=^)|&XlJD z)jIsjQnrdf2mjBnB^)I%P$3me7HE?wGd_g@ZRHujGxU$|_xNhXcA1@colt5?!dum3 za^CcLozhH+*!TPeBi1haeJ$>R`mdkl=)%VNG;krL*quee<}9nevQq?D^pq3EXt`M| zL604T91U+9-v=nHffRp^Xn+R`GoR&SfX;I?$0&uE`w3KQ=Z-yf3X_VL@D|Xz|sqU9gG*cSGC6BoA~g z<7FX@LqLOEU8Y9w<_{pO)T257lv3tuj=#Q(kEdyPv;Q_K4MWxg5=Cp*wY3lLds!ob zC3`LjZ6)$@6DX)=hoG1J}P~r^L^xx+SJ^!i3JjPnbL>$_45h*Wk z?BEawSU!=8iV4>mbrNmu1U|H30|NBP!F%2JpH-V5o#Q-#x|RLj5xpGuJ#SBT298dT zz`9vzYOPSvS}Q&1^ed){UFlfIFk-8Y6a{H>(0g2>+=+Y6kfKHV1NHdi0vXUJn`dUw zwAIQ-<4KX1f}@XheE`N4zM5YyJbBDp$0fpyj_uob=wx-{(+1!s}Hd=@|1*`2!k2 zLl?wld36PRm8FJg+F}skjrxX$HjtGfz z1xj-cNbM%&c6e$10)@p{+}zycsQ`>9!UF<}V5rH@DC%48dsl`=$oNrzm1cIXO>iIj~^8BsEh>q(e$VX$k4>6e;N(YCt-N z?tJ(7yZ>uFU*7p3Yq=(_*w?wwIN}+@)%gxb7^rO$EH&17r(1EV?bGQ*+<;$dAkws~ z@<2LAf3wzllLcj2EerfnVELVfJXjzc z0s@MOST|C6isYYk!xZbGPN$ab>K7RP3PCGZ19`d+PMSSWQ$ajVOicW>uy98W#~Q)L zRyd~xP^)4@8*q6g!)14(q7#bfp*7s_{424Wck1<1@cqspX%2>9&B2WYp>HIC#<}rI93vYg( z5@H(tg{5K(x+HP9^kUsh_Q_kcLY*^=eq!b%2N*1DyG9cxDPF82hz`X#|AHR__wolZ z=orzi`L-A3?CRRBY09y@w&w8-+zAU2^`OJU$1TXnJNtPsBlL<2Y|l}|aoF#F1JkV~ zFn*|dj!?7MW2IFz&!I((&6Tgsse&Q>a|s?AX47(*+Bqa&{Ww)HkiJ!H4X_8Qw|xad zRX38w4NBz$ylQ8J9e;cVhU1mB5Q!p>y1EXEp(OpcgNW(61~(DAFMJ?<_^QaF zq+f33#b_4GYi=$z<@cd?IO&2|2(nBxOR_FmqV^QBJKyAbR-EzxAV?A*!)<`!%DDNm zGGRy6BOUM#trwP1S@ENSbekwN(4dq@RU4M(n~AQ{IPga;Rj+0*>3U{o8Cht9DrMwk zt;E3c2T>{mgo;}*lkRtLaPX#%U9B=B`dmMKorW7Yu&=ah4YijC50#!Oz1O#{yFZA% zfF`MbaDR;J#TNihr`^^$X=YC2_Z4kgTTHM9*8*eTIp1O{bv-x~gbY=AVR(?!Qplq* z`4Iup9-P_7p0rmMzu(PbzExj7-Y9O|EsOg$M+s_q{Z?UBBjXhi%o%W>%r%`GN%2_E znlORHP4TBnvv%G7Nj^+bPq?+UxQTr!!)~FOYs=AFu&DIF@EZU+44^+jpNB6g(nSOO zexjEKz5@334~wlv>!1S6QRH^5?P|f<$7@C~8jWiO1|#N-el;UnBa+!KkYGJc=uhPV zps2|J$I1+8IALL-tz!r6Jiyd4niMKM06pj>YWwas#IQ`S#SGROnx0NNEKZ?_1&e|& z?fzF@y1Kg0fyls5z;w^lZz?O+5fV`|H!okoCG?Lih#Q~bVm!B$3RknuPRrtm?u|u9 zb<0c3Qs)kAq5>mEFcR`lUtTtXf(`>FXwQb!CDHDwbBpOPt82oB_p#>o<|4twYdv=Hn4yZu4h-Rl$`a&6u8;)CV(bPxw3f=(zkvct-Qyy4z1Ef<; zkbM~2O6SiHfO`Rskv*hSF^m=~l1`_ZUS&}f$wn>p%ezNuGgWtKnPMD9YuhptGrQ@1 z;*_5M#Emdop8E(Jf|@*&VSNfN15gyI_OjP1r)&}L^_Khq`fZ1CF4z>FY3aIqahUjm zHgX3*9rz4ztz?#Kwf$f%v6iiL(*s96-DdbNddKydHL405|wVLn~;i$77XI3v_-GSjk< z87S`Dso>}>%REoM9)L1b0_>q#|#Z*nL|dwR|ro;f=sfs1Q}N7&fVn6Ws+Ut_Hw z?RONENXs2VD#BLhd)tVH7|(#Aqr1j>Bd>7Hs|AiM_l*Z?t#c>_=(%A(!Lfee^eKR7 z+(nhUs@fRFLN1uLW2q z@R;iXewy8REAjyScCilIM0dC3SnY>RiuE}_3cFPjV5~Y)ayL`h(e*mB|8~_OzO7yb zPQh$?C{3`+eIqO0#wQjt)4p&u330L2TiBmLY!ysmZJM=0-tYRQiwV2~%Uf_YAWL_q ze*344^m~OO=acEMNuTSJ)OX-K6cx?e5K|k~A{F1ru9g99gWJY@05B7bhDdpsnwedu zhw?UL14<5nEF?^hn-4eYHTeofLZl>6P*90MU+Wnt+3({rzJZQwPPSIv6Br}z6Y7CF zA28%8)Go6-$q9*>9o;Joy|}ngV`JmV=d_(QAsNI<3pUlWU6d9LnJQZ|AFi@n;Q*j1s8Mz=^U@lh9 zKU4Jbx^~gRUzk*eURQ|*PjFg7nV|ZmryII^Au~&7 zYPPm4N<8H&As|-9`&*tVoy&?7j>XYFQ)9m9B;eKLa?kG?Jr+$@W-Qg2G-6i}I$L8^ zweb{PMcP%Q?Txl})AxTBG%&y^aZuDAIP%+2kXAy}IGuqJ4l(pPza z10p^r;jK3P1tO^h*QOxj(^+mkOWn3R!L>j%$Q9T;1lCG=iiy6y0zw(&NA&8UK*bgE z=h~6?x8=3buZkDr%%cp4)t}s|+wyEc=I|C|4!(0I*QEdkf?cQzn2~BGQNn(QMF?Cv zV8yZk@ZRQ%^5?|!ZqXSw^%hPG9QWm;HHKnuWfuLo{RYF8_3`mMc`U%kqG@;sA2p_( z681f*$An|)a$?#2ZbYH)q?Et?ty8Li7*^wz&(+y-+%Mj>EeJIY0`TNc`%R0*F0)T) zMAPEsUd!|-V%U&-c*Bp~k8zm40$Gi@3Yt6*d9WFAG(<_U+uu4T26Xs;Th2X<6WA4- zrQ0_>pOmB6oQi`4ryqu0?8p>*Kd>rHz9od(PgncRCoARvHUUWRhOe7F6JNRCi1Y63 zshVA=o;P)_IlZ@Z^Izfuiq`72SpAV;Wl3t+!Fk#3b~o+vApQLfn@<A7m>0`G##ltCH+sp}zd(2qo6LA{6pR zJM7ohvnG#X&79l;aYLavaKbSbZplPu1`B{kHS`Jj>N2R-=8yxPUT3IgaO*Qyd;72D z!Gx#SR_ezKdKS^3)va@W;Gh5iX*Fa377`xSv}n2k>ei58Oj09y`yUalg0RJ0pgY@N*olxAk+c zj{B$rKbGb4aTl~pr|}yPrNH+#8wwfXHC>TxECC35-*^PU&kv~Bgsj~Y%xxxm#kTV& z*L^1^7yyz|@ED_@Ed=uDr2>!AX4NU8)|68@@rFYeX2EE4v`KxKWwA~-8*G!3A!oLJLa#hLYr$E;0bHmh zkcf+z_+#0gu(5EN9;mP^xVY!F3`(;rt14(iGOxn{wM34J_ZP(6vQHADo(BzgK9;06f5YHYuMYSG z0(7W&_k8nY2Eh3PG==U;PsUf>o%x@g+e4s!=v#A2>~_~P>&Qk%!)BSp8nEkj7dA%6 zv~BnVn*VI(CvJNSbsut-9)pAd7vx{qn~Ub2nt<%2;0z#<8~d{@Zi(bg7-6qYu*vw& zcFMC1#Q=lN6NE@^G7MtfdIujrRZsdoUDOB~5$7m$O<_iki>U4A(0p3y(eoXd(Hs3J zudgYcLTru;G1VGt4SmCA zCWcVSBN1@3H$C1iy3ev4b}$nf-*V7mC~nvjlPs-4l9x!bJmlEmI*cNC;W`g^*Xt*^KrMQKI`+J^4adcd}8hvJzzq*TEN~*_1OqkZ0n@^D9Xah)qMH-#?aIHGO5}Tw~ zC94NT3COs*d*=Uq1X(wCRU;=T-VE`X<{Eu^@($u@+wAA^PPZjHF#VuCz8wQ_d~8xd zV(-JF#I1F1s{5ZF(&Ikf!i!&6uuf=bY4g{^4nkQFh(r(PY_~Gfwbr2Tnh$*6-V1Z6 zmufk8h#~MoTxRhJ^_m#jPnsBhx1*()EA~vS2 zc}MCh3v~%!yVGr7MIbpDTT$VtdkQmU^k`^U0Eu83A#dg)ub4;@kN{k-#B1^(v+HW@ zH<(co_vK7Qmpb`DlI=!Z5gJN^;qGi9yDILug%z6-K@J2ALiwqq5@=dz2v3{z-aCXP zT2uczKBZ_*6%JY~ef7-6TC_ehL%)vVRF#Jij(u=) zaSi2D0+<&m7X8ZexFNSu3(k22nlL5s=I~T&1(3Lw)cV#wmz3B>e|*ijdQ|TerPZnW zmEI=&N7oR_CCc6m-%O$@`ztT?OU8UafFP{~vR>El`3Rr@`Ut8 zqy6iaStCBN^#I_{y&w5AP+vkjI+(Sop07pN*iZ?k*Zc}M&;3ZJ>J5)D7HD|K3_vjF z14*kM>@lJhi+>+(Lu1VFJ+omCTwJ&2Ej#iCwIN{q@I3m=c`ky_A|&|f(KSRMqH*^Rfcq_!NYl-C3qOYVCG$p}fTJ1I5x+r|_Qrkve z7sS~M9v*Feb5pzfzT#GJssxo)>Xx)0JD<(ql%Bm#2a=|0>1PIA5l=4!)`iX2Ghv2L zv;dpweba-@r%#I8iewY%^?&5$O#ykkZfK>%v2L*5j;Q|Gp1^^?-0%goOr58(w5?6g z&S!J}=ZZS$q&(?QTCF;pB&?D)11z$P>yvH1!H>g+<%XIFJ?a4bLE64DJ!iHFXQX8I z8mf>t%nRfWgvrYE7xnb*W*U7WT1n%BH#eWYAVfuh7i-mo ziw-Hhve7W(ExtE+6S&fREoDqW#oObc6 zYVbA;3|TJnTYWqSz2m+(=wAO)1zb#w;l8fR+|mfSSN(U$Av_hgXRp-NlakW-A*{XG znSrQS->WZra@lO|<_CM-RgWjOsv;WQb_BH-V{E|>8#WoqpDznN3nTRSq*?Ot$w&O{ zW=0AT*F^Nj>ddrXOY6(UAq_BXkCqyHD zG(@df^KJL6yF;)hArQf>4DNkVuW!!|fGQwXMw5AVb{=S;mX8Cino?mh=-*Cu&@P_2 z3)!Nj&R{aRB~$Sa|57>&ETmpK!41n%-XKMG09J3C?ws0F#C_f#ZJFDhb2GZeMKs{* z{Ffe~qJA&chTh}AqtEF8nrOc1{;%vTNh5przFS?X?t1yHvpUEd5S>#W&^Z+yQ0u<( z)CmN&$0!Kq;ZyKFb!u8e2D)Y^8hz+fkapj8O_j{? z5M4E3(4CrEwN!0nx9v=4wJb1e^85jrm{VeOQJY@exbyruNO+LOvtLseo#TLva~$h| zXL4ehW`)t4_PJ2;<>p#j2enrVC5C~SC2j!^Ljcv;73geV@nq^}lz|zv-pHPN20}5a z6zK7Q-9{bO2)XRjXj_aTbi01R?P>*0b`;RKpD2 zv-(POx@6pBrjKK~mxNJ%v@lKlX7bT`6y+@4TeM>=b791_aDmTS(m9`ff}kb0k^@SD zjGUaeh6+{JS(aJQ%nA?HkCeW$pt6r|ehZxMYM0&8Ahb)Gn+0EhcxyYs^QIFO^Lsgf z%+LK}Gi^-f0|lf}#H?ZXJ6j1`NayG#ppYY~+HZcFH0Q033OURMr(}BmM@r`AU*c?O zLoIB`X6X#Pq18S708-sppz?F&nnpewSYpRCxEa>YVoj_eSZKS(Oj0#943MjX*jSkS%=tWSae&DGy$nE!qtZ{ z2diHP8_FA^FAR%E-FPv{|U1p_c)~1(aq<+DX9R;|K>h;YQQ0j*niT*erwO6xlnd&yFJ>RG5rP=`hw}h`e|57CZ$h5F z3+@D>P5P5X+UKCts!ZexdU$!CF&P#hKZ<~-M0`aCkw48^Jgs;wZ5(>YUvB27mEvBT zClZAIwf_EBil^I?LSPOyZT{E4btPUojtDW>1J)h%rN%v2*3;!zu9lW`e$djEZvehi zR99p=6|yDBiM!h}IL6+bp#o@H=gpjZ;DP$0x|FUp8-K5B!GP)E4EI;Hu?*B`{vr55y= zy4gM@;?N4>yw1&z5jE9VpFH9-=5dpCS!%|ETL_9EF@gF-sre4+*#}Y}lZ_GKLHnfv zW1!%i@WG~b3@Hp>fS>;dIHWTtH#aX{ql*!`Z*?}X!dvE|;R{VdDf9djM`OuT_A{R< zyx6Z_r#QE5ICttu4i68*tY;`pP~ceI?RP%3LLPBA`6~39EvNgjls&!$8~PjNfq`OH z628KTAZA{}(R59`D31~!KQ6w*M*66Sl5s0;Bk4j*%yzf=ts-i6g$D85On^uuIX6{; zKL(D+8V9#N$2y4_lR_aa72Kac!3~A0fsa?bW@ctu5REbV#PB)*oGP)Z(E?mrf`M<( z!IEW75h_kgOM4;Uws-L!(4&VDsueWxe7g&720Lgkdek==`Dy?_)N97DC+_FW2eeyx z^eK!7{aIgEzXb60hjq@DS>*d0Qd!6FRZ}9lkb|Zmr4$AcpnMNVo!h0En>YU{2)RxA zkO8q!jsex{_ap=!fXUPm2o#@NZdtm5P+q8N(GN$c>{JWf{f{x->|rf!X7&|$ujmST z{y%UMZ4WH;j#8@wE!>!)N{d$TtWnDh?~ z1armNO}HI>ydxdlCqyts{d}iL)h)pToh@;^El1{g<1iXA~WJoB7+D><@b4KNeo&MEzx{m)}OF&eYM5jp&>v8M!PGr{ifFD3LlH* zI?vZ@^j@p_rw{<{8R)~ZAt;?cV28}eSEngAfQKms0#-VR%|IRqD#Ts7RG!&>)%54e zxd7~nr7=*v5T-@j0WtyW=Rl9rm|p9S=$Q)!dB7g4-N*w5)c498_xP7zUuCnk&@x1) z?`U~b5$khy1Qf{VP1`J=t@0xDc}bA*dUi!qCI zRU?M*iqDWo{#X-~A=qilJ1fqy5OfWEvqfci`@GTVtUe1E)inMR3g z*KtW{;wf(#0hEP(4!C0)f1qYB<4^_acmv%ST( zNYMB~XhSmc@>-Y=5^_;2o*BzIft{5^XNtATF2iIp!USO|VjNcrd&f%**S9EQV^tU5 z>ZJNrq8-)nd*F*grww2E>=y+jgEs?fNr9**_mNCqKLbp+Mq74hRSQ+ZW?GFmJZtTh zjjg}{uSLLjcQbt;cOTGD-UB8&39rL6V`%`QK+!_zk0RQn*%DQFP$RoYkJuxMFOZ1n zV#xkX-62q5n(&>YKBN&~bpD+UWq!7UW z#2=>HKgIiM)!u|l<`|wnRW)ln2l!gGlYQuhejO|K4A^ptHr(bdH{vE6I?cU-Ia>R4 zI5sh5VeQQC%in~*91U*w&{1hc{32E%X>y!SfeY^jcKe{(UIt`ZFV{nR)lk%FH{#R^_HcEgO z5(4<(6P*CYve4?94=M(qm!SP3=RDKjDvP&I#H3w2?QwaOs0d(*xwnKI!o2kz(SCDy zzkY<620Lify;^24)|P?VS%O-weR?&iEN-B&CA7cqG>9OS2fCPdac)fIw@dK59l(bF z4p89G1$#dqiE1gaJxd;?^a6?|=J&8BjW?SLU@3U8TK<|Q1^iARqk@!x$Dx9;x#;#z z_nw~?fUM4ofM`-a>^BWSgbDC)+Kf2-kAq4dt~p>m+=^`GIH&yaqrnfqG=3>C$yWx+ zo7n;oN69HD_%dmJ%k}>tgzo)XQw)y)`@;8>AS$tL&z#Q4uEKMY%Rl^MZX1=RD00{> zi$2a_h?wO@GH#Tt#FIQGs%*3r zkLKu%bgOpT@6J`Xem(>yHr;vT)Z*+bL=*zV62xxz()D}AzQ_tJvM==GZXR%iQE(5v zRB-#~^ICW9HvL?*kby%LlR&K3}lcQca;zP1v7Nlu!S*(q7HxQ4`Gk? z-vmg+N$FFikfw#+#&CpMoLPvKUD#Cq=&y?pQ25&F@uCCW&I0@Um^q!<$D5<$nL8kl zbIAnu7o=IC?)G8`EZgHa4Qx*r9|a7gfx_VC3ulF;d}rZOf)~!te-00N?HLcF%KbWb z=jO`X_Fs{bCPp4%Rc6s8%8o`0U324b)T_e~+8IDvbgGW!KX8J z&J8B%Eh2#3XDKYK$^tBCS%2gKSrkwZbs(e&ua1Dq1D%9~QbhCWA0VC#2Z@Nca*^2} zI#5n#tY4NZswRdw8@bK7&W48>!S}d`K4lPa<=-py~hWvYZ z8B3rNllvGv99fR>l2V20=MjLOfB|cAP4U?nY7quPL<%4W3y&1MRs8t&R%qZG5@OPv z9ov#u_(Fe*`NCHdVdYT`9`x8#Udg~{Xl$?W4GYU~5Egv)j+l7fp{gOH z0m+?c{kTzgZt56F#x-0lwA3pmmFlFVCY&@sgERZA9A8RA1q8;_S(04EpE=(H3a1#b zyjepGfg<=Am(7fgDPN7$4dWt62Dis_BqMFZ-#=*A?g0fYhIQADA=q`T&(UZIrUtN` zv}yY`mUUt&`|hwbkPBpV$q&u4isqL%LMKD^#5pT@o_!#D=gw6)Hbl<@*Z(5!%bO~# z4u5@gtRDJonCd>vbTt3X;50P;N;VZSCQEp4 zUqKtj=&RRQ2IxDgy%3uC!2oNE$-^P#=_z>t8pHXp1|nz#QwB&q4-ZentjFHC^HhC_ zYbX)0d|F(iosYz^vb=l)lv*ADJY&M=h@#{1qZ@Ys!8ifW z&CvE(!}x~Ves}Z~){t^sx`_Q-Ifo`XaC>Q^0*FY_fz)Ias0u*x%l+7H+CZI}r|uB| z@hc8O&mY23o!h^Am@eS#ES9ls^o{2W?aj9e9F4AA)Pj=)ZhRnq!ve-g=1Cdvp# ze>mnu&6afxTkyP!sHy@o$~Qj8%xZ$A*&bDSMHX%&2rVK-FkvE;20G1;-em<%0@zl+ zDu2epM~3{%aw-X%?+DY{`0P;}fT^&L{&mTK4lSa8R&6JDp{v!Zi5q!#rd+rym>t>t z&ZLJ0M{`0o4Q;h8?sG=r$Z*xI``8C~rl7Jtxmm!gf&Xdac790~3MJyfx{KmFIzINX zUu+PNzfSTw2X)w~Wyzi;=&|hX?7UDfvxCJWfG$@0;>)%EPsZ+q)6KC!S?4|5p{$&* zR`Oz?!&`)lt7IlH1RGV@Cv|#V!m6+hFluvQy$&u80|jb>=h1C`@Lo^XTWpKAJwPrJ z7XyU;BEZzLL)6cMTU^LRA*QH+y|?4!Xx*}#*SVaiK>F=BfgqCd4_QCa@N?FlES|+W zr&96az-^qJzl}laZ8S?LQ&dx@MK7>2?EN=}sB+KKan=Sy?A5B(7CfX?Ec0q$neNM> zA!q&ToQN?Eo8PFL#o`{(`WL`dd93iFiJC^(I_djOwxy?}q$`m7GWX9{ZE9@%;pO2e zT&mM>1WV;A%oxRTBSsf1%jfEq_EV+h z;1~~?sqOuOIV;NB-`Odoczt{)*%p8D2(f2YN>*P6i z>0)-i(M^rkia2nw0eXLsmk;(UVX~C25tis<10nZbeIMSEt~LGqFRWP2BX1K`Vt~^S=s%?d(2!2#uK7xxTwSBU>SjWEn=yj{dJ7@e-*;-# zY^ws$^gnJikKNb&Kr&E!1fMhEC(&K%Y32o*hd^`-DX*(I*c7#{aKjZ^B;aYrgBhXn z-bv1mOBsn3GhlttDNUjDbmedQN5p$C2+VjGIoxZ!hwAkf*j@{`{1(L+Ksr#kp@};B zp2I!0d~m7sKZ_WHR4}_UJJ^QuE)!POSwkY8(K^U*Bi+c1@N@QGPFxWZlaocz{j>DY zP62vZ21Be(uo);#)8)aj#>rEpAQ*qH0LgVOKf=cIpNS9BxY<-E`^uouK;1bPXfZ&6 z@#?hNllFIEsAuw!>2qaF#&y6O1avLKGsOGoXX?(ldof8o5Dwb!f#+`IdqGd>X5gA~ z^FLP;BrlEM|K)Nm6;1N;JEJkU6^>_MyJ+XNF_)`1Zab<%82y5mzbx8kdMl-aZQt;IVqLV^)3*m~t%w)GYkL4E zYD*|(XXo@y-Oj2h7&qa8NL=vJ*T5i4dBUC?h{H(z_tm$I;Q2-e;I}P&uLBu*po*xn z#exE^My7X=5WhJuiPqlm(lHQLKzwz(pn=96HUh*5ofiA#0n3~J_|G334e3ueWc~Nt z8H2EG_AL!Q5W)kmmX7ip0v+}IM@c&Z40v+-F7K~)rrv@}asVMV5+nA#_HD87ZUgBZ z;?4e-;N92*vNKpatyqKY! z!ldly4XW$Vs>`iG9sMoJo-r!xvtP_4dVKJEzGP$Cq5rIk*PnJC2^>7MA}u|+usEH+ zBU}8MA+#^wgw|f^7%a^J~E38{Xlq->GL{ z#^LqfC3G^-U`7)SF%xN`O`p%;xrtz?Jm>|JVx$40r0Jo{zs<0?6gVF-R_*71YbEDb zxsE(B+8n_Ut(q#`g8Qah-;aGsOXtLg2A~O|ebdWHeE$J~{EMEB^m{bC59l`ImuS$#rfSuPFP?c;cUp zFpAj^NH&zavurTK>tE5}ruY2sbdg@Z{CXO+D0sjF4SFFE-m2-9=P-Kp5v<;LB)Rm& zX&Ku#vpuWPO+COlL0o*K8@ymsq2&X2c3@+rH50izL+N*sFMx+I-{pCbOs{yaCj%jB zwH$VJl>5)b`2YykVft_E6YP0z_~*FHoQa`S`R)~wD-|bW9*`j(kd-_SXB3z=(|=Cl z2i)KT(l>p*cfUH)QdeVAymWlengzfU(s=P3vcCBZ5+SnP{_N-|E@3Osjy;h3(kn$Lso78DIyLTJeEANx=shGwup~>#DDoUO8noDU9ZIN z3-f~a4~M<7G_D^wNoIjFRfVh6ZaR0DEWZq4!v7u9M^f;KP)($PvmF^-Tj7@~MGot6 zFS?qdCw?ys7)0~_mNdSe{83fAD7QVhbo?G+-3VR@1i_|H>PzJxM+*=@pAV7>{P-KV zKSgh3$2V&Ca0Huyf^h!&C+yF#r?ZgG%7160f*Wiaet~Jd@7Pk|6=S2Y>eRQ zBfte#NyxMLL^{QAM)K^hu^VyJ7DWW!%waWKc#sj~G`;}M2N zAQ^75ecf_I8Hz1Y*?+erlLdU@5dys)`8Mh4}Z@(*s1EQOq(i|pHqkP=e_v)m>-BY zh*!0t1~)QdqkG?-vjO9P>@NlZ)aU2K@A`pzS6P z*s7%d?xTwk{O6!p>Edi*_fEt@m)pO=a zGlKQ!)`oT5ecYX%yQ^;2d>NfUtjWiZkmZj&CVnM7V{A0oOWfAGzc0FnM6B@+R#3(o*)(=T6mn+ewX zb#6?&XD_ytLVQ~5yoTdaZBcN7g9VQk1#oLmEO%W=y0I%6dVEy_Jab;XZ9@1I0#`Ic zjbOLs%j?e^As79J;yFs149u{!LS7%&%;k5HZOhxp=9sZY?%YTW8?yAiRhiHLEONF0 zVO!|2l}gkP7@*ICeN+Z`8>FyZ^CAH7eZ&Ljz|l?JW2NII@bG$YROE)7=tkC9IHv!^ z!mToftD;o1WV%u2En%DOxOD*YAdgd#r(T}!zc69Y2fpbuoA$Fqv4`AjAr~TK%4@4o z6}@^6Jx9t?*DaWo?;o_s(S_<;58);sY9?LKLtUc^yQWU9O*KOv36*2n;#k6}@9?8* zY7!u?5XXje1LE5VLutf{vfcez#?$M%`}-KnC%Nf=1P;M*k=8J^+*iw6ddGzv$y5gN z^F+K4O*JYZe~b==HYKaKSY_3;Exy+iyZ5@GIi+ZDy^S`kvHJJD;~|ExFrqqFBa?p1bZu+`gVy!Rx`JLvyWs1(X3E|xOVSD_h}vN@ zELTlcD573?VFvgI_RVVvS4@f4$UFKpmyR(1yQ==-o#hx44#g!A9uK&%IeOY|@9l%P zl2W23wo*vHuun)ONw#AAt*qJA@*Ze#VBbl1$9rKp4pm( zzRlY1$6iNB?}nOFU-?n#$$VkBmmsv~7z;YN#TIYTy!bDJLSoIkN0vBxU_`9A1MF;& zkYps}#Y^8A1blpt{0J8b9UUEs#f(q#-v#^cAH?TAS{l2!I+;4Nu(7hTvH@~2_`}M@ z$;*TI%g)Nn%Ok{ zJ8-M1yUU(3U5js>jupDh+)cJ3)^$y1usQXlHWyuv#q$W`Hk-Yvo^R`Q@Se`e>oJ^AAPAJcI4@#DN5hob}2(?Uza0vIVXk{P`3lR#Q6J>42E@Gd;ocY`^9 zqTQS7Ul}Yu9eQjp-OcGS4)Aq7nnl@oRCFIxby>`rR^J$_UgN(=>c6{cf9etTY)Ve6 zb9>Uv=lXitJ>G3q$W^|p^69mm!Wkzfw~s}_<97|LyjEL*`j(Lv~k zcFo;=IXt;Z*)(1eUv+lu>wM;kaeo;oU7SEhHJ@}CG)MnP9#eqa^mFa1JT-obRz~m5 zUS7ytaMK|1Jw)-*t8ZB6R)4uXmP+jY?E7zg$Zsx}j}oZHQ=47#{=`@vopV5rfi}*^ zCC%)h$3?){PnQf`sPmL2xi(mGxTzIA)Gy=t76FBB4HKcl_8^Wm!oE$J80Q&0w-Wcc zi+f-Dc|ulv&nw!d19*bv24?C#7ovWL%5eq}#ZoAt$G}4UrlKr95!vyFetT(%_pT)9 zI134@%%Sub^DR(rPeGonZF)Hk6#Qtf->sHZN#!lKYdrPBGJbX z4$C&%EDUj`Pt3-!Hc1IqtI*7jJDv?Tm#!iUxGMD)^o)T+TY1TT- z;m;FG$xj6eR4%1@NHGaRYO56kUhc`R&=)!f^fZ?9*qN}4cPt-KPW9%jP&gi@BX#`_ zh~se78Av}A9qWTrCH9-?i9M8CO%cLzSdrp>c>G6B;-#xjq?|-IHT%QQnZhz&G&!#u z-loik{(KiND~@I)0ZlR}v@((G%j&IGQcH6%35k~@u)^(6F!r8&jGI`vj!SjSO=^_B zPm{rh^Rnn|uQFY-CQ*W>KBnl7dZAWIM|0yBLkKh3A76D3J}OPsXw}**@q?e)$1H`L zk)rZhRX6?#%g54m^@2uM+~{G0Bo9i+RY}iTkRrad6LIK!wa=koQkk-`cOJzdpCfB@ zC9Vh3lf3BYahXw;g;b$6jwoTqI|AcUxp!S9UQH;74$papb~Bb>%Z!53*BNKA42nlZ z?>L4XA&s{dH^zAuwqhfV`-$o`jfT8++G`O_4Rs7k&R^FicHM!W>)nT9LX!0r*?IEV zKVd(3swT#-MI-Vf$;c=$?M97()s*Rwqq?+HW{>rIfiF6edgN2u2fsWMXw*wghnQc` zRRtP(um&!+f3Z&VP}FC)P$l`qI6|uEPG|NiH~2}AeQ2+EK**bF9Gz&CiU!$>iN$03 z1%Aa%8&At0JTn@6Lt&O;Q z`!#rxAJWJ7Xy}^tlD>B!KcE1$_5c6I@&s%w&MqD{rq0anwl*+gUFW&_!4FJ*ixlfrq5|O zdSA0#ed5o{QigHhsnT~N`z0<>ov+WrBX^lWaip2N(W#>BM9EBCfjdEmvS}h*xG?K? z=v+2+)e?iXNfLjo317lXN6~y{5BlKeB46hfK9fZhzYe!`IPBHDTl1{`SXlogV9m*+ z^80smKI~sxE`$~Zro6T864VXdvdjKX;Sq_V?$1#%?IiJD{UKX!WL#6_&T%TY3Z>9i zW1?bufbv_SjhH+3{rFK?$fd!)XYzRgpB@h1^f0H>oGMM9gL(=Tv#sPrc28?k2NTk% zNFCXfFLhOxHM!k;)e4g?12)Xs_bfr>#T1_&Lk8_QNi$H_XFBxu#PwzK3S353(l*(a zDkkfD8xv674Ut|7`@Bu6b25*_{z~ZJ^*w0SrhNOETfZtbeVh8Ps<&hBq)qYSuoN20 zWlfa3-Gzy#(b`Q;UyZz?H&syd6@A8F_*D}|nmu5qOr8roq!s(^9sm}JkH@#ec#rs8iu`_Xx5BLm8 z|6XC~gz*EGhQ`y;eBS-c$-q{U22vi^lR+=R!gkFFDvZjo{0xkUHmLFW=!I~QasVoI zF(a?$r7iC8ubluwS&?u77a>&Y$HdwV{=atf!|o6>_{E}E5JKr2rgoFG=G1>#_m-N>4Qd17)#UEdN*G+fp_2dhaN%cjmlt1g`x9ksJ zseiFmadGJBVs;s(zNf5>on(BoH6m6lGCbD#;zu-K9cy23GUTa#L>eYLC{ED6znqgdE;8{>2VgXUy#aq*1+|N1j zrRiedPgpeYLgjKeJbNFbCcOX0bbRaba`lbJwM+k9zPVQy%T_urUX7B#~9^XvA!?nRVx;j?Z%&NCWgj`Vgq`XIKw5Z?OGZhd%AZrt+I z-NPR`^;LIIhB-y=>tmSr$wpN3&eK{ASzFqWhV*$jWsN7lpsNPI{&v0ToH|^g@KT;- zp(H;z`;ox0q08Z(jNCTdk|kg7f>W=DcIC|w@yChoNKe)hoznx-K4E8SnJ1#3$6mGy zNL0G(J>7E18v50t)KE~W;Wk_*3;bEH^-HDojU2Wxfv|8{3VkS*XT_8fVbBNq3>Db~6wyx_?>>uCH)~7> zx+<EE4F6^*rxnve*^Z+~Zxv z({MLcPka;cMa$eEu}UkGk0z+HcoJder@j@8{C{ewZB>>09y5gJmJyV3{WOu#mOHK1 zeLG61Q(1--E&nvPhj(=)e8BKcmgj4H)lw>r8>pYnhn@Ji*ybtWnAw_nYR)Z6 z$k$gF)Bf9BMAFPhwy`cO-^Y7~DrL$npYf~Lj6}OK$(1#8D>1w#+^HWmbkVirHVyv& z8oLs3D7QC08d6CjTNrIZ$-dKOSES(%BP9)uY5Zq1W+-k6DG}xpB_=W5LDv7Rkx`^n zl;xVtKSPU)EMx1RC9>pxQ_VA$(Y^0H-}8OX_k6$KIq!SUdEay1Z=UnjTuFwWPx7V_+prq z%)^QCEvkfQJ&S5%l`3b==$4({dM>|2Q?$>Y9E}F+hk8?NeP&BUt?wE4*&vU(b?+;U z#(?3}s(9<(o{GxhZi>e&q^Ki1aB4r(>N85N7j|Z4OqWI6fCG5sQ|iaiu}5=XYo>lP ze9@K?SvklOtsFXY9{jfOkF*l+=hU>{siyrlUL{V~!M5=?)$U}m4zy>oV2%}OR9cpi znjiDe7i&)yY|4Bv{WkG-)(e)A>`hy|n@x@byQw>!-P8))pk`Dakfd%pWRq-~ml@Z^ z?Tzo@*3r;?W5Ag!Z9HZ&YV^K9<$xV(pGS6Z6}eDuEXCP!dPRu|dqTnaUhV_6!MB~&yJK8Kn#DuI$QET6S}tU}mE9^VQK9(TYTwT7k$5z` zkvTqMlH?_KFNa==SH|3!Q7kUNm|34b5~5P?SpQpD(vV^h8h^EgwPHsE|aJEoQQk1?T_X=+zsX8cGJ=gv10n`m~zrdMEmA-y&I&w zD_f$W;$s$NRR32u)swEzTKO?fu1Tt7Liz@`8}B)^bx305t4DqZbBLb&|G}n&!S^4itJyD$Bb=VU&!d*quC?bRaXybk_O;`fw~9^@Rz&{q29 z+OFOJBiikP5BFo>A<4V%c_x^|AvG^ra=q+`?6&n@^4ba%Mt(+K`770cm|U%nnhSOJwAY%kaXO}ID42VQP~r+Dgk}+>TIsQ zS)S~#;`R{k^}WU(B|9#;9@O5|UpC9gik~PyCgnX{JUT`H=-KkctMQ^i)bLzSXU`mO z7Tb+g4o(0jsp>ZTSu6w^&0o7J&lNLDpyz z7C{L5QK+!&k%ka3!p{$hK?2so6LtN_L?RML;;jI{-_{Pm^Pq3(df>dg^%RBB`eX12 z67pvj5QgH}{O5KPhT=7@03}RuIiQAz>|j>f2VCrRu?QU69~tyb%|cimE=q6x!(J^) zv7AYh8dO$h2RdkY%Pgp6*VCj;wTYTvrdPPDx~JskPWl#y>@jR(h5py#ptMSrXOVk^ zuSyn3ws||89I-d}jh;<@_sK1JopPSnGNr7hU0R(yH{)SNdEF*(SY!jFjtsuTu6Kg! z%pJ*`aK6@OzdtfH`LCDoKoP|B`;4Ohr6_F03kaBWHb zEox_=u3~%tQ$|+9O~bg_^^gY@*D_;?h|89n0%`r07G<}MDLVUN>>_Dp*bfkxxPwFa z*k-6iBLnw_6_MsqG|`@9ea3WA}pgZ`Ag{Z4LWOb<;<;wWHqiL zs(II}M-r0zhE&<;ugg(Y{pJ3boDL6W^7&8kZKvEnwWXia!B}hhZN4SR*ix@N%6U2$ zI2#Bpgi}Qa&HeAMf_#abnWCop@WyL^oU?{IkZ0!r*;uHP$O>5yutWHd5t|9td8!4p z2=4%HEfOpmizJ;PAQz(&S!Y5xRsbV#@OptjT!Bm2DKgdY69Rd!;XW{pX zR5h4g~6Fx&{R7!ah%`fNzES;c=uN1C9^m8%YPI{Q52h1mYdvv&DRD-~+&< z^K-yJ(wCPC*3&bzvHo_}H#IRZv9`B0HPsVlgZUYOh17Uwk%iR%lwRi7ns46uZkm@; z5J;_?$h-stAIH~L@Ei!lbBzK?@D^F(@aDVpgxZ2+w>MzdR4m$qVGG5FWw; zm%lvILe0#MFV$>*2w0xy`@cK*bs`AiSrQVylkuC02j%;9UQiH7p%AiVLd4J7aNz(I z^;xQ^AV4gy=Ry_>AuNJ`@o!lOLRbU=WTBbnJ274e5eOY%5gJgSPGT#0kI4YPLBPlC JMF7qQ^f%&EpRWJ_ literal 42200 zcmb5V1yo#3v#<-n-QC^Y-Q5Z9?(R--cXtm2hd^+54-6U{f(LiMgg5!l`=4{~zs}`Z zGi!Etbyam$?VjCxHi|MJpr}AVkU&6~Uzo)6K1oeUi;ERCHN{||+SmGd12pg=&s zU+)S{T@2|xY;AZ2-rfk^Or4zHRPqrs(K8Ygn%WuLn^@YJ^AW46h|_Tr3-Ckp+MAhK z8k=(ewL-~HjXGqg2z=5{gW{+t5Mk&WrUjT+in+Wv3V{FZ&6>Aw}2JDHfgIr2Y~^5!!O zgE@nVp^G7%o298c$zRX^naBTqwp{;hwkA%7?*G>jOpFY_Wrzv?(a!y^LS-iA|67&6 z)5p*Io(VVMs{|SKc@f7%0E{B zbACJjE`DbhOJlIE*8Ns&u68bb#GD)~#0>nrf0D`lPx${H{YS-n^FON)BqZef zvi7!;{pSz%?US>q%kNk^|6Yl9wAP)s*b)6s^%Kso3VaDepR9U8?I43;oP)muE$FQW zcYMOuypVK)eDx(WR-Rnk7%l^3jP;S3%d9tPRk;gP`92=d5mMB2gPApK&dOIzLMg`G zr%&Qcnyggm@Z@C*tky5VdMn;$V1NmlihWA93g=V;d?~B3=g1BRaEpA1;uZrAZ@oIC z1#QhxTgTUp8<|&MgZ#_@XHI0Q;l8@#a(7~8F)Tz$NopJvg=9Ci z^!XYyXVX(D`Fj8OKDy4m=sNhi*Qjr*d=AF_`E|kq$v`<-Ca%hZ7BXex<3&yl;((A6 zCZC)6oOQ*`vR_jD>Yhc5yLi>%A=9K(eTz1c>nl)}(#Qk(`Df%Ne zHe4|c|AAVcICcu_wOi*-qXNA7txtorF6I0N^~gh^d$Lz_|p9#t2k z4KO>X0@wxP1ePe`P9i!?iY=&3(FIqE6;`2{s3W5#8 z$W^qVHSoIE5@AL6l6+SEqcuo4xV51gOMMsB#sX2LzX&NK=h zIu%6|)Q#TY8TqMiwsy$a_kPf0utH}Kv!kMrkaS$6UqP53{LRLG$u%>Mca73{L5M4( zq4fg_Q1*2Co>v7C(pRYPhPvlWF8r@ZQ) z`7DnWxTknbxO;2<^r?^$vL3+3dEO(0{zNj;EEWr|h+a`+lZxzkIeC68pfSY3bF?3! zac-6AT@I#wl29#+q_mG7e61f1robwKk6OpIz2#<2c?x&><;yj|NHGR&AQ7G zjTnxp{*I#9U8!opkCfhHgulGCl(F*`$Oj5+{ZJ)oPwW zJ;+7NW_y$sqSFC!0mCfg(~6)OQt75eOPry7U0OIYL!rR1<}FKj8hdgh_!$1$>p?;@ zOydu3&#Z%LzZS4m$dtETjHJinUtx#MlZ#;hOlsGVlA^feE~T33l6rk<)sBQ>J*dqy zR=}gY<2c{ULZvSzxS%nLC?Rdg7C00A;t+e7k8hZ(+A+~R6jbG(rvu9)d>eioAJ`T~ z)S8OnNn=J_G0P~K@y-o^i3T!#z$#IFme4ne{!Zhg>rZknx*QCJGj%vD0{V-U-e|wW z7ei!2YQN&4nA(6bl#tZhO4ToQ$`-zvXT6}=Q=gsf62ndG^2rMTz+2*zAdZ2IxO9Aq*Cecy3{wrYAO^4D_qhhL|j5HDABlbpjFF7B7N=; z74ShJ6UzvJ`GJay7e!xx+0Gu2%_@75`POiO697Z32yy0D1O#UxFm{>U__{NH^}k=; z3wzx6WX*~zO**ai=&dfCHsS2RSFr19dpOu8Fm}IgTX{0Bi#KEb7C< z03#p#Bphx{UTnQ?+A2zyel8yv@aOvTVDU!puG5FE%M(Z>&FQY2)x5C#sB1Cn`?T2m zcsFx;-x^(cPG6a|v%8;e?Cq}O(oe4W^5OVcd9moh2v4l}vDkWD?%n(Puz-CL;Dtxu z7N+B0kE7`Fad&Of;PrbvoGVqxOH^Ran6%S(JbkWkaMRt~d}tjiUGi-{^t!Wh5$I*e z>e_beFl!|fX_(pL@PwH*5S#t(dw-Qw^ z9cOCWe^y93YQR$lj^!7KH8bXAYes1D&GHtGRT?aML>sT{>N;dn1l$?IcfgA;T%9kE zuX+Y%^7+`^aN;Tfj__*>whphQp`?V&U#xT) zJ+x%&_Taa>i7yvk6YHh!uuAgkX4Z9e1uiN~dnSOyCom|bJ-PgFS7NL6=GMJq9(23iuTXNG(q3imoFedU#?q)~(z>rrnt-hqT&Snm9gHq2M5`W~)-`BNA|Irm3 zFD#xC_w(+Ksh2X;2@$q8d{AbdH-rlx&g|&gN_LyJvsz>Fp#lu!CYBQ?;0rxUa81$V-cY|l z-Dai0zQx(d8$4%6eV$aX9EgT}Ij(s-9;`Gq?#*KXDL~O6wGMcuJ-7Gg0Wi@;&6HUQ zKle?O2J_;K7!s&o$&}qmMC)^M;a2XrU)_Nj55k5q@J@PTc5z@RxMUD+i ziL7nf*vvFSxWGFWPigi^GK?8m`a9daSU@ywvAmd__0^-7hR!dX#ErxrcLc2;JuD;b zC2s95M31Sq(rVe3A2^(`C#hKb(-^szp$~RHYZ{GA2VWu2n`;xX;gw0C%*beDOQ_&D z9yvB3BA~2Feu;8ZXi{IV?6kZWIUhlBzGj_E?!DiC3ers!-r z8{6hlco(Ky85`0uv=B^PVmx(6pmxbCtJsc99Lmq;K(Bs@Ns(QRX#H5gb3F#d_^{;A z!p-7~3p)f;mT2}m>zeN&KwxB)QUiTRbGqpbp=Bmd+T;RedOZXv>}N)Bx~0R6K`i1(VTX*I~QF)$2 zc|5n$H0K{FJRl70X4Ng8Mop}4H#Sp%0GFQyey_|N4jJYA3 z)ZKn|Xuip)N&?%9nkLb?VC|t#@x59#U}}#N$=V@PR=qY|s#h=}VDZCTiLKY$n`y~T z<35)9I=yWHBzACmLHBi9?g^4ut3PkE)BlHCPZ}NuYS{H1zQWeRARsI29+KupVcIIb zt5!(;IBPt&9g6`-Z|nYUL1_nSg~0 zXV+bnPcYG$E(EC~tb^O+O2ywp3$jSFh%yE4OD;^vhEa__9w0(4-&fvNX~fL7XU3d=q>5PR z->s42@2$2R|40h^yvtU~{BAAq;5~rCT>x~{%B_bs zl=sYpixcHHXW&3Rdt!TiJA!%g8Sa7kgGb4z#Q8&q_Qq*dIXH%N&>T(5;And0)>spl z0fY8*?wrC4lX8`As)F*y$X`^9tV6O!g8F7W%_< zI3s0KG-S{pijWKyFEEk7{~-P*pTsL3Jk8QesFlw}-H+XmEnyzR{-v}Zo6k4~R~;7X zg^Zu*9Zw>D#|DW}uyd$7@OK8MHG|xQTL|aN@^r-hE2ITEihnZ{uF$%HlBweN#YdP% z1UV1!jdt3+?>?6@J1F&kvZ$A;V8Q)Rav{|TKzWxy%)`x6u=w`Y1hw=hrwn5htQk3X z!G9=*;V)=8NgJbR81@#p`&o}6Q6RR~#XXb%c zAcH>Fp%^0mft46GvsC`yfwCZe07C+w{!k)=WQ}Nzo5N<&dJk479!q19;J$_OMpD`i zN~WFPTm4iDz2oRIpD(q8(T~jv760zs&tMq0I>Z->?Ag;>r8^-1v) zB~$g^Ds1LAqIXmE5p7WeiAHC}5r|RWNX8&V&Ghzb2^N+iz6nyK_ogPIi@3Y41D6x* zQkl<>v3=~tHVKW#x|ly?KL*=UM?FePhU4rIt5N>GFgxK2eW|B~~j-HSq)=nn^hikxy`|2hs55hWUlV;`5N0gS*h#De3$$sI&KX!a*fDvR&*=i3CuYYwNLR=O90Z`cNHu?v1+rt&LAckOdY@Nu zcB8a`f!CV@PFm{aE96$kl?yk=f27K-A1I9VZ74b56^-%35&sYkW+)R{>w5Nmx-_^O zvz^a#Oa!+?QEJHtABjQq9|gDEPT&P;8&WoR)`IhwN7?!fVRrTS{PoXOG>+5gIU!b2A)SeTeoUeME zF3@Q*Xeacs8j7`RG1BHAxMPu^%?I|yDdjqV@9P^$3^b#An%G22*PgA;tD}aJj;2Wo zI;9yPdh-O}L(ej|kEU6DLwcUKR}HFGc;!N)N@qdhHetWZ4pI)vRiYhUuvnmX<*^Dtcw(Vh{*G?{u}c#v`D#)=hy$VmIZnEtcc1D7E* zpIiW8V=XU@Kc5=AgMbdb%+vVJ%qi+Ri*F-s^Gnaq8%u{M`MRqYM3fw%kj__;UC)YJ zt#u4NYqS9B4u(aZBF%VNd15hfeZHED$%X``c#d3NxxC$ba8ElY)oRl z?c9wvU2_>W0GmuFXZB?vG_Q{XU}Fo~BI`pvcrN3%$Qt@uG(p#B*UDwHbMsCbc~$ZK z4>~cl%zH=0O3qUICPfh4-(0#=277SF3#XwKS>keHPKJuIB5~%QTj8bVF=(f$dFg%N z`;Jb=wpT5fHKO5bze?L>iCc-H+uwY|9*fAOm`R*in-8@#wwYb=DXUVFx93j4Y+CxF zi;c5$=8d55(Wcv(r>O@ods?zlvqc!vL;cONEro&uk@lv-pXbsc<)>@w14k`_l&<~f zYI7X0qbp8mG#S;##=uM`b%}Cctp`?#v@1|RI(v4;!p)@SoO|)1eth|<W1n9B-(oMF&{}^y&#OR3(FnSg}gzHp#_g)XXT8=XSkhff#P6WPs!*q*}~Di&B#7 z6_U3No8>qZZDu0D&FqbA3ZmXdU6cL3F=Lj@_BBUUHfHu;#H*&E=Dm2ehk+5Y6v7EScAFgjNSfvb&s$?yrOZT}3 z#TCglp(}|qlGw`R*v>E=oTZmLvo~$dU*B#t-J+>jav~F#LE{yj1@y)yA5R9j$|hAy z#IV9@MKq#Ho$6?=laF+Xu>@r9YpNAFh0XK@%f>|yy~dT(}hG|t;0j(k%>8Z ze1-<2xuL`eK*Ntxy~;!52=1);r`Za8Us^{aKxUp-M0p$;J8whNY?kDgi!{IWu-+4v z6!zICfCvAS?)31py5E+v(Qt~^X`FRo7Yj`II)x&%3qqfd+sThtMLAjh;Hz~GL zO|s+s$K1Ju^>l9e8g6;2BVYBQ5jKm$CL{Cf16kERFz+(&64+5UtzONV#ml<(GbJ9& z_V2ws1Y7?28sZ$JgC1gTt`Gixtq`#Cw-~THiUB)r zK?h$vJUD<3p8Q{@UOQ3^cZqPMPGC(dGeLeOErl^4oS|1Vt+e)PcCsbEl%5?f>zT!= zqd|07c_}~+4hh8Beg2AM$BgEd2W{@4UHsTHvNHI^;%?2Qy%~kP$o^vej^^D4Z4n$aWG9F}7h|1-sI&8w;B*51K&+BNVACVTEm=Sy0x>mo_|zyZ0@mxt%Qo?j0D1NO+Z z@lf}-+da)5nKARr&3>8^Q`H3bUJ@-_tzu_F1H(Tac!a(qP*yI9d`{$eyMxC2)tM>R z$EW=jSwC)fdwFCA;p?Oqv_80d_wl}*7j6oTE6;X@FTiPZcUy5KbL&lP%=*U8^>Cg)#FeJqj*wVj|pdMti1#vY})9CZ1%{L z_u=Z1%1)Y~PST+-RUqy3l~W`C(ve*Y_7uN#TWYZ@m!)YZkl`L?X0~X_0x0IT4F~J_ zM7+lz?scI}*WVMj0Oi8c32%GwDrn@`tA^WR?tH?7H7E;>g2&?Tp$#?6s@SaT;N>DelFbQbGR=;%;2s{_%FlzI{LJ+8o1fgEHAa?&a8utoJpA zoX9+^d&0|GWIs{i76do_2wuYzgkkNh>+62k)KJp;(+N2L)2`!FB+tzR$1(wNBtpOs z@mF0Z5`9gQiJL6bbF*={*S+=CaEnLZa*uVRhs6zc-F7;YoS`;*)&U#<=@q#9k5LX# zA^WZg)sBtHHXk`Q_qAaeUy2mj8}a?sg7YCx*GirCY7pRWBqNA0Qp)qF` zOzomOXV`u=KaXCUm(0tcse^kLQj!T3tj~^HO=4E<(OH6yKl5=TlAg`g`@Q!$}T(aF2 zA;D*C^4`WWLFgLqChwlgyp&oG2CrjvNg%@A^X4(m2vpZ!(x5q} zfI02A9@q3=;e~|O^gv!;6p8~-0{@`~DSHPg+ckK@6EFKw9`R}7mH;sqvIpv&Dcl}= zm@b9XY@Kv|V#=Z5`b;QwDD^2+b%oryXphV>D|DzM`Y-pwiYds2P^wU>3=pbuxrlIO zR!&gUAEI?f!|)aPSqrEO4nqnLr!NbDRc+}Bv`7nsT=ttzshl&I)1dO8@}|)9teMju zw;S1p&X#T)y*x%&7mUI=xbG?F6tRJiqjLI}Im_qzdK>7~`E(|?{Nh~J zoe9p$S%GE{6b@dqI4(%apU0nZEN+mXn-8IYnaVkklA;DJ1J66dEqSzGI$xI~>+w8E zZ^rTEN56AhlB%ElnQ&Zq)hSevM!iU_WkqiP8(LO(X0@xPVi3qZ*Dk z)+JW(eUUo+y~|QAw&o)g4o4OfOods6-f-m3ER&HLaMX&3v#qa{E%c$HNFigk;m|yK zi8Sp+ThPO#YrJGX1#*VE9h91k-V`?Sp>vAZmc_PZ9Tww?ldofr#NII_O_5U&65+)p zvhB~nw8DqF%O`;l@a?xPwl$IaDc6sdOoal~v!P>RbU=d9m!w0xAwz;r)8kTUViU|! z$5h9z2w7@`;pc=xh0lXm0&(V2Wdyu#VMJhw;#?UR*Am?vR)HH=+{DgIJF`a3Ipja# zG+5zk%|_PDj&kf`q&X6*47L9K$6h=R@kI-I?GK2=~5KthT(0#2X9x<;xM# z*GW6@qT(5prA@>|&OgnFRmk4x&`_z;mvrJNuNQ5L^k|7+V&(3J*<&fQLat&^S{FS6 zrzNs%`HmhQ5})u!UDvS2k+3C;E{HBATExKoF0+9kmMWKMLsnL#JmQtTM#p)e#V!eZ zbP9qU3?3Dp^9cz;7iknLOOJLBR$844`_|h|9u5tvc~ne(OQEMFT(vm(d%_Kpg$pw? z{|t8L>?3CG#&?3=%_Zjf-a%ogCvGGekoAhHm`z}<0r4Sx{>!VhB=~Z&7}YbGK6Q!8 z5}soIu{I<=RWXs`pIlz`I(r8r?(D9i`w{Pak@2$H-du=aL&Nv8_n3t1wShGf zBScNxu*R&x0nH31EmxO@UrTlO4~D$=A&QyrJOnCg?J2g7jI!YsHORFeKz?qjRura` zduKIHbDmF|ERxKsMI4o92V1Fe3;8-=n>;E?bq6=tv<@B>1EJT7q|BdS{l$-!TZ^_O zX1UTi^mG*Z5%7p0di)Q3`8Su)x_9s*82&i=ai57CnA$!U5cQ9qB_}89Ko9J9v$D*6Ll$3&gRW;PxxT1 zU(@N2#SFQHwcl8l5n7%Aol>P3CUAx7U)f>r<8!bqKj;$)lo}{Hcb;g+3^aUa^J}nS zN|2!|C?|s1h1~e@egWSMnlFAG4>}pW0vD_6r#MI^n;E@OCX`c&s()E2}QIwg&|S6E#j43>CP!Z@_x4)>Uj6*Z%Z@Qy1e;MUE*4@H;%y;~nm8cjb^Gp&a9o@0XtlX0< z#<**Q1J>ZuE}k2=nzTGWenx@D5QMeU?i;(_bxNQb*Q4cVbK!!!>j`oCC2uj9$MvMP zmPS|J=)uhnr)JFKKbv7m*tSuXxAt_GkRJKY;gKPA#$A;x*?8h;dAhXPd4kob#7xaS zq@SC(R3#P}RNi*+^`;&V7rG=>7lsi;K|wqTS_grwcrumPtmqrFFjj}qftvV%nhS_4 zV!5+n0|w`;6tLv)baKM(da|XvSAoOK2N>^)T=jW$N-}X0Ba*+v4}oZx$z^;=*v`XZ zserhvD~pM_p*KIh4kQN^_zw(_fnxlkRBl6*x4{NW*q{6{6~z16dTfNwWZqeZxZ{&g zPHdC?AQ%3zTqk~pTzKcg^2y0I!<5cRNjisSGL-b|7nY+jxzDxnK3^mCol0?|nvJ6Q ztjQO3$3u+ZjMARt=jYQ;a%ak;ZssLv6lcE3E*f41?#l-vCd=7$Ss5lTwMUwjw81%MeS)mNnPz@Vy?DSO^VIVl!4kokl5 z2kkkq#ZJ!$-1%pq{v4EftgY2&&KfZ*+>K~Az8ts_!F3x%j9G*SF z-THGGfW>@_u#%F@KY0v%$OFIA*8yMlb@{A@$m8Sky}i9wGJ5i1KUh9`gR7K$1u4)b z#g03RX6@j!b4D*VM~))yvkSg?wMJiF3KBAZgPS$JstH#>gy@%Q(A`_qbhD{mnh1hP z2?N?T4PPG5Dk>9%0Ly{a_%uEZJ@WH;bFy!#!&)ZvBXH6#ISY|CbU~k)x8Y38@*@)r z_E{D}`GyFEpZceytePQ;s`KzWYZkF(5g8&UoYxgCcxb~?oK44Pwn2|6iYP$_cZ0G-K&K`s+`JZ@84{-3A?t_KyTI;X zHm-ySW%PjRW6kSJG#W889LeZ=LZ-609K$C~NY_#vxvnEM1Is0k50sZ<`v7}%? z7;rJF0z!(>Y)sSYXmX;;|DW>+7%cfvc7U3iG$$6rRSX z9%h%%#*euaab&6Gf$7E{XM1@s3EA#kF8qc<5tLvbSPv%>q#GEQ_?vZ$y%qaAiEX8k ztRuE~ZST-7zr!d9VwC{XeKf2t@y0x_HdzBZAN{Nh*I(f%-URP7H+3E{=q6RpJN?Tb z-Q+uIPLD}YM}*!k=~#Vp%^_l~A&5meJ# z9d1_T-PJEgj3rJ4$Hw5QieExy8>y(pcQX~hm|^=bMNBTgmV;Rk%gcCtW1qAWhrQPy zd6j-yc)8biCoHN|_+T9vJwC?FY6hM5ecu-!r{%k@;Wly;bhv6SNxLT)ZOf2o);iT_ zJ2#J|^uUx{=6%fCqv*CYatz8JFv2Cc3eQQTlPuarurUHHvj5O7$lHq?L!gu?x{Wu} z3=rM^3)t3qw8c~S3@P?${2)gL?%#INZ5j(!(QS!j@@WrsR`?Zi+G)-|G>?lEq5kO# zW1WQkPlrc<2PXx#Kp|_yAWyO5PWWKNA7J{6$foa*N~BoVh+;5E(ErOwgz9fayIcT| z+^M+6z}ULS>inR?M1GI4?Q%93{ zW=Z(d?fT2q`iu7|Y0+wDVnUUg-b0_kx8^8(@TR0`Bi?AigVrwoL<^JW`7dp^yC@NA zC1mGOg*~ceE_PdCGPB==T)VaIDZfs=U7|5J$E_)tM+G%utq1(NwstIE!PlnQsdQ+z z<#Ds5`DT2JlXkS%i#>IK>`WhZ4*@Cvwv&E-bRR#c=D_lJ-{kvnuEdN@dELPcpEJ6xKK32ivgllZ8f!;`3K1>UO-y z&?EUQ58NDH`#*QZ)VQhFth*%|cOLp3^{p*U58yf{SeT8(lZ#}+S~3@R*@?6!qroli zPflDM)iG9a?2%7&di6z7?e=f|WvAG}=v+QTiq@|I*7?5z;3Usl=TX{AiY}lP$W_Pr z2M;c?OpW%BhR9I_G+o(}e`(J~k|SOF!Z?-Dk8`5`og5I)fFZZe08sok&iZsP5l6r? z0;yAQ`cUhhslY^{n0P2sj_`%Q_#yuAqqI>{@q-K1xU-tC*U;8|wBWy7K*ss&f;i#7 zT!@JI;{qK?yhipP+Ozw`Y2RFk7>G+jk|dS?gPyz*3G%5*`kw3-Ln0Do@+uFBV&gcZ z&!sPi3~2S5z3(E=KxsN+ndBwWL>McK=J+WIL-U~j#Kcz;{^YoIQ{?f zH3jisaeDLhPn|ZpMO1RmB}gKhQSp#ph_bSbn7FhuScM~*!TS8=-m@K7CE|o!QTAz$=A(`<@Ieo8E_9k}Vk}LIZLHBR%Gzk7JXKxR+Kl)H z+ggY^=(W=o(aUywz!kO?+boC4f1lL{pk05uJO9o*mP2jyyn)7J`kDD2l_ud_WPr>Q zF%4nr$!ry7lsF)^OV`BVC9bGv%ku4a0Ju+aZ~JPibKPLO@^nXoZOQ6QkSFrhWS5UK za#QLYY>l5_S8bA5yQ~tQrrG_Hva*BOG$@2KDho?cNSu0HGxno1U4byiDi-|xZq<0m z+6Fhm&TZIp2HXKlcMAt5OwD1rEX{dw7o~X;Z;w1`@Gg-&?aG%2m>8~~0`L!jRG(~wa=+Kh?c$nyk5u>ab?4X)40 z%b5i~Ht=Z|>E+4o3=|Dm31K^nYSs$Q{9t5g=#k<4+Je6`r~eaO(cFPLoRmbQ*8Gxf zUy!cl*!Y%Ms0OD(^LR_7SZ|R2%Z$!IZ*4zSb!CIHhJuCs8*4JGZ;Nd&UA9BQJ9R_N z!`6mMCLO3=a{!pI>vSp&Wv9N&;4idhnf*JAD9|SI8NB0`Pkqbo)B5E3$rAtVrwa?8 z3nCQm-{b*vM^&A*g9OkNdC7pv6SSHeD3Bp_kZV`vyIMcwmI!zVsVEj9pf-*sM`lWwKRF zP1!%D7nZAYhm{LM3eVw|YmpLG5f-+ zk{#J(&V}TcH^`I!kld_P5l*U>E|V_v8Z0ZfR#IgbRHf4i*;Z}-Q0|o`p}Op(7SinX zeDkcpT2zQ#kdg}3r0iAQPV7Ve#t9k!f^7I{+^HG?zqSigT#cCfSQPeXnTinTWE~xct?corwADV_3xY6>cQpJ=&EfO1(`*6imxhrBDR;` zPX*d3hG@CWpst{Pnv zGuP@wV}~;pdON3=%npS?i&XzpXA(`=SGQ2p4I`ORs!vUov&s~qyb>iP&wKTi8l&VF ziuNjv*T?O`c=!3c+Gk)B-(TLhtV+2Wx|vl1^p;T!f&M5mF^XLHiB|SizbS;zBbcIznW+`OR-O_%YVGm!=;1W4VgRp$fD^%7qC3MVU1Ps ztzFG9C^d}ie5v*~yd``TU>Ssdt6T+__j=;7;^?Iap#cx8_*)kh@Pau<0vo>2`t zhKCoX{HLuq?FmP%{PV>Zb>wqn8Ym|girKi|j|`IHBWOI8)f@N0Fw$1cAGDOeGnUjWPxd~VfuGUJ0+o2TEDj|hco%oT;k;`+b%5MPov$Si$ zK;u(y8>_bu_PY!WYpAFv&aoUwJ1Bq1BVTigjb5i*105f}8W_mhs0u#RZ;wzvu1t$t zO>Tm8q=4mcR8rcMH7s)y&ylLAJf4RXO7Z|ehs=lZMRPdjm0d=12rXapdVGuwzUmq+ zp)%p0Hb=Mrp}WHp!EqjWANoH$HEM^t3(8giQz6?!`CY^J`Bnls3UJ$UK;3)0zE+s_ zfe^ZHG6297_nK(3!XNxIJpfqDeqTLMp!YAfAU_@<&hdJpdE1hpPAR ze&%vuD&q8?|gYbyCi(-zxyIp?E^i>(C{8g1+N;6wO-QGL}0p^lQ z?UB5#;3}UnFP~9=gu8^1K(o{&CpXTu6-Ct(MupD;OGJua;Nt#05lnB1*jsSG67lqt z?+P8m9F2m#?6G#m+WmO_xbN0~S7Hhqaq$<0uhE;oXK4z%Ll%0!QAJNOPBFerRG$B8hK&bD~1Dwhq#NP8+>2LMX6P!rojBG>xIP&?Mtv20zw%0bL={#FuGam zJw3KiBl2q4fCYP|cz{(`C;PB$oXs3~eH6Q3)vqf##h)Mzq||Ud?3=bJ7E@y}JAiPl zgxB4-ivf618 zmLz4eSTm8|PpB?^3T_ka->jH%&X?d1CcaBG{6%bMY)AaXh=^pun85MvYlBv$c&WZy zT)AIZVsywr)Gz|!mXr2;lgRGlr?f>4MxRKNJJiW@a$2Oip9xNv8sQFv8wSnX2x7dc z(2+*;!V$1!X9H(&%v;#jA!OtqKQx-w1>3XVBNIrg^cUzE&^%}au&Pi_YkX`L?^!tF z!r!ojDh;dP@gdcrM0T!Z&JANoanK72MULEPk3m(KTD}5WhZN}Bentd?SSKSxxAd?N ztsiJB=L1=&FZEL`~!GJfDzL6 z9Xb#w0ML@WN9RWyRNtF6tiTvPVFYhGtc4nXYuD>`$85K7_1;w6F38!Ih#FHeAmT~9NHGj2xy0Tydq#|z37EYghZ#VE<<1Mjy_YGw7iRYD zVX)J{NmOpUT((WxWD6hME14K~D-ReJ1c>j;8&c@iLkzuk5NuZ-98LEhaM{mHFxBo( zvZl_UX?;S4s2!R&qr9G~5KGSdED(dM2(d!I_)!-X4Qi1U5v?e7DIuHERRadNAJ1sx z7_!m31@7!nzreB$>l6xu2?N8ZpE-y%L(23P1dDGks-77|Y+&bF^?jUpaPdMU4e$G) z1r%&uH>oH?E7U%QR%Q=!P}P>Mt>!2!N?oLXXja>fp1I^CK4+c>5Gj}&<3$6#@qAt42lV3 zauP&gAE^2cikc>r5)s#e-yks?=P5j$7dHI3x5(5>;YOo72=OBpv)n$3Mm+pMx32Vr zXFk!d6RUm9a4@AMV}jrZsj#Xc;_b~ZjEp5=nc9M^yL^d1YfQBv(ISlx(G8YyKWkb* zWPYk0wX}`Q1XC9e_y#0!5g8(l+Fq`zWrWdWD8&olkZ$ErLjlM)ACsHPfG~@V+I#66 zL$A=$p=av}O`%bsQ)M@M&qHBsICq7{fSQGBIgmDxeP-{F^g$AM`Mv?)A-l|{ zFi9WqN$T-Iw`%82J|GeS>W)IXDWD*+Bx;qx6(3W876ALT5I_csPa-o24rLN?(?|Bu z=X|yZF<3hclWiN`3-(I_4IE1dLPImKS;}zL)_~6d{H+kWMFLhg+9A6Uz5{zS_j_VE zteHqZA>WyPiUkT&-1u;Iq)4bNDbc+k(;bFDFIWjzR+9=wk1DCc8w0Ei;N(RD=dw!2 z*Necx{DOPMw||yj*liw*P3A;*)!P?pHx?~~X@zFTMCT5xS0%KKq09nD?rM0Z#gLq; z(*rT2Yk1YGjYH9*#CYiVwPu)9){Blc9Bdp4QzfMcqZ2Nfy%DQT0|*bIK?MRbf}5|< ztr@Z>yFd-EK})FQzEj`VcaxR4@9LLFv#7d^Bt zWxZsLGuj{#C4P4p=DE<@|H#BGN@*!wBsxM=X&fw%6EnI8(wKvCd@2(IWlv>7*voLM z-oFI}k%y@$+4_!@k7Qd|0OH+k5DmP9y(}M?0(#tKDH`>IgP_--y8wuqei$E$y&HFM z*rBqZ0GwH3P;5@=Ep&q@>5lN)AlJxv^b$Bsn7+)crWQsvWO$nRl{8@nHbI-9c!EbQ zKYMPNJ=mj-BewO5N$_m*3`B|twQ$9qt-QgC6tEjQl<`O?JKQueGRoJ^;Ccu@Xd_+A zg*~Ifee?iGiog*UBNBRhAaA%9Z#q)9Lkum5!|8$Yptufp3uz!D;(Y>7*AKuZ1Ccl| z$SHWiclqW31D*ekLhZ4@+BN8*y2U&9i)?a@q?B8v{Xv#PE*D7I7trqDruq%7T4_!} z%$-Wd#RWkSL~!Wr6lp958@XGMkPliyHV=8I0f;^9624rjglLJZtaw`J-%Dw)c!IR> z4_Afw!oNcgiH8U?nu85>(+4r{iy@zK7(^3qgHYy!BdiV@c6G189FHiXNceuR1&!bX z<$-d_5e^(mq9sf=0M8#1!PpL$$t5WJ(P+t^a8}+ClDszOqlR-Jysc>sjS5s{0_4W2 z{HaeAF@^IJ&h{r@-cJ%aABK=9k|!Go@=+R~P(+Yc_=G5cD#r4|gO~_0VQyeVs0D8Z z#o>Nnu|mhHz>PLvK(c=l!!Tv;%pG4Yw9*vEY$UX}aoA2)+NO*N6y8kGY z13d!j@(nT*8WBsYt)#{T62y05{2yK%Ykb_om{BHc|ZoR@rnm6-3t3h(8a)?z98Bl z9dWh-&N$I~RMFavdK^;)M9Cw92u2Ez=x{U1ip^p{#wZn&j}LLn!nr+Q0rt?}KZXBo8`>Sa>+o z2u5pwKezFaBZWZsN#J&st=|ZNl~-cjeY+U+aZ`Hx`eJSW7?uy683W@d5R2O4$PJ#C zwk1oJGRSE+9^4v9K*ABjyAsw>R`?rl;O17OJ0!V6B9CxvB&4e-inkB!EzA zdPv*QgJcnyrck95MB9v@=|E5)O%V@vOQfxr)cT=7v^m?Jv6*LqqacWp(j~OOG)(ST zsIAYf9=-}8LL{fa7-}t;=}H^(SKB@{r=pZSTd-*_wazVhv4w4kq9JFr?y(YpKH5N# z!NWRV#`1oK#~ut)7-zdjPpWCBB3}_*XMNm+Gr8^>GPMFJ`T_Tfi#K^VWRq4q)(Wje zgfJQ%m%xaxej4)w9>ml><9<*QJ8sqlryRtPS$G&zcI*tJn&NqoHDC&Q%9bB~+rape z(W?3Xq3RugEQhwR?Y3?AwC$d@ZEIT7wr$(CF>TwnZQHiKp1Jq_|M%5X=Ts`$*-6em zl}grH@4KP5k};cg*TIM|7)UNG3FrHR*Jfj}_$Xl(bc~A~Vi<7*B#L%?bq4nUgdQPw zd>up8A`LLxYfL{11EUKf4i+jd^BT`}fCr5fy40XqAU(A~I z42UvBhe)EhDv~Qblq{>!P43lcGCxxN8m=}2cFGRRfXmVA31DOcdl2Q~^yE-?sN|}U zd<$o^MYFjZlSPXSj`O*unHCr6k!wfWE60&mVYGbkjYu%Gb$+L}-y9%=CY&*ou0lkz z(};D#ZHv1>kEY2v4x&F|U9bS7mn_4#z@{-oNSa6)j5){^I^E%Dph@G_hvpg+)^sO= zDN*+uqjSiH7>sOp^Gk>A5PY)<-jiGn)IUG8x1{c&>#kEq@Zlg9gBHtObhjLH>>sDb6FmBFz|;6a=u!wo ziALJu=~+W4oTwr7jKC1hm^n}kGY6qa{i=sSAfP8jT9*(3S5>-ZFVsf}UN&v5Wkl6V zwX|o{Q3YFAW-=|H-$eN(MrD-9Z{8g=9x+Tht`|jfZ$$rNc4E5>hFu6f+ZHUB$Ttvx z^cB&UKww;?+^>h4ue?{l!ouQ;nOKinASYaLCpd@XUK{@%VjVnF*hfakKQ5X>G9V0& z1rbaY(FlhnO*CA$q}LAqhWJ;hVNxpbm0PbCY6dgWAssmRaWfM` zlTN)`AHX@F2vd5D-VE(-wm&3?@M;u2adr#1eZA8^Dd!lNw|xQi0(K!ZAnu@ZJc*-> z!fCm)N?(tQMfC)-k+I%=G>RGY_Ms9Djv=5P!~!SJwqLZc;-}G>;r0+TpeHc`!najn zKKbXJxPAOY$@#d`{0U5n`rsCb*6LAY?~UNMr6f(e$_n`;gESfd+~txqeS7uyL9qfc zu_IiGzKc4f48c3rZel; z+dG)?K~lo^u%Q&E=Rpk#miq5YrppZwa81oI0I%YArwb1Yr1VY0ck|?^Lu7<7(_-(c zvk?v`u=61UN&C#_1|@_iz>tas#L8mi%khuD>f{MO*J){_LLG6!-1&27=!7%CHb<>y zz(ZDI>@kC_F0fTG?>Xj=<~02>OyF{`UfC{@A@bx>LoHr>?pEy~1%~D9&eN(xkLcNM z4N@Q$lp0ZhI|sxDBITd?&7S6;g#LcOS4_H{*QZM&x5tRV1p~=U{Rk%wuC(m8R^D0S zv@gk;$)?O}UFy4Jv8GP1Knie}S|FmesmI0jWu8ShKr%#*#G3Q!Yt=MLm-C{){5$MW z(s1(8uycNL!AN7`!p0DpK=nJur|=!)18P7KSb!;22Q$M_o9OL^C*t}SU^;!tGsBL? z0kf6FYthsVD^-8SDfP>gDNYkqHpLAY5%Yy4{$Gr5vRIwhTAjs5S*sz;`i~7{+2(=n zf>E**#S}V-&=ya&oMy-8^$z^ccuC0o|1Z`TK>E!kiNKPJwX>V~8G2*DW#=zk%}NgM z9UIBl`6$N#b~>7o%XaJF1DnsjN?|lOkMlhpwlJRc=-{95ycra30gv?-3_ILAAHTLe zn~rJkjypB9wn8rlGO=6vpLnAWzx7PB+?`)Lt~S|^cz(|Rc#`yUP7HQlz}x(g-~Ono zyD_O<2I{lK`vfcAbGoH3U9$=Ok@b&HE&}Z%0_~Q9g+7=Hl(at!Xs!V$-W`&1(9UkH|llI1Y+Kf2~XRtUzTAH>40SEnt#TDZA# zEu(u}vK61UllLu0oY#Ihl3fyn8|@+vDlM8udDu0#{{5}Na%~ESsyfPS=k&$}M24T=67?OreRI+?pK{<@INCK1 zlSJxT;7(`cbs3Weeb686hKqZY@rz}>DglIsvk*^T^WT|cc;Nk~vF{v+3|aWf z{il}_vM#X&>3^vlFv{#PS^;F~O3%Z5N*>W;Zo|~PM-lHe{N~`U=U_E+(e*>1u+9FA zy4z@=_S~p04i6|nyjGI$CBW73814M^^dWu6284qS;MwthrwIQGViFU-yk?`={&h@kL z%rNY)wb%sfnOKzj)G*5nC|t-VsqNG#kT|{CiDCC>ZgTYjJrS&qY;Q^RD%AiM$lJ=oUhV=L|hyb_DvfK2#*HP7MBaf2dQYpO*xEkdoO zT5J*=A+@fj^c4;WxLu+a;hXE<6>xh48-gGToq`%3&yv&P4yvUZs!Wi|UNi`f0S$bM zVG^80{!xAw(e#QQ3u@@+Lpi1a{bBuKM}1+LJ?>sKF%P=IuDG~8Z?P)3Z+#vNUd{LI z)&kDoJTVD8p{Ju2A@w*(him+y#E`H2FD@v0MSG;cMNJmpeQ*%{cck;bpr;+)Y zjn)+1-TEs?XX7=n4{izYQp^)VX6PWtDbK;#B@YY+x);lIFA)oK}Oc&7tCp!I!e*vdm+Cwf1SIOUstE@XR<1V#w`&p)0 z5zN$+l~`TG)F4T?HvYQ;eI;(yECG5kc36ntz>j}GA$Eu56jyDHE_;0sX@a69_&NET zC#2q#mjgNFw*$E`FR|nlxIaZ*EqGkCgHxg*243yyk%!K6Z)ecXq6*aU-?z5UGymsz zDB@CoR56LrkPW`~uLtJlEYSN?y!|gWZEA=u$bZnmY~76wl)@#2(-;yu`Com3RfODi zh%9aFy{)Y{@I-%)Y2A#c0J-cxw!jm-Ya)cQi!-4gs;YjekX}7i{SIwhy|QL1Tw_A& z;CN`gL&Xd=^i5XRASsq({H5DM@TO`=Tz_Nq#(HEBDj<(TTMskVXkYB50xkecb*C^1C2LVVI|@r z{ztnM54HQqs7J_Bvzug%*Ilj|Px;zvVSjK*vJ}%N^>XIz=W=fY% zf$I~8H->b|L1)*&CP;f%#1+fB_$8Rm;1EhoULp?Nq=+jyhh{;%IXY9YBUStP3srmS z^q!6Kr5m`QRTmd7s%O6jFh)Od^B)yEWlLC;5!l6z@T*{`zC~2b^RXA7r-{y3!##xn~2C8)Z z(R`q7g?&W&O$)7`W9D3g(#yL?+Nv;28s6KkItcH-=D&YqHd4?; zwX!`#El1sy6}1_SFPhTgrku3)uotF+$A^kBq31DpyAGZ+f7Wx3rET*s?F_UzPW1kA zu?-1E%Qr{gnc7RPP3M~Yf9d1IVKlkaGqrQ)E?Nh}+e!x$N*fuRiw1o-=p?-OlOQb1 z?rFylKiu!Jli2Dn%MDgd|3p%is}!@<$(Yl$Ps54u86aPE*T!wykq<;}cD7KKi6|69 zSS_2B;U&Hkh-wM;Qmo61_M<`V1j9H#vpehX2>0M&Kw*}M?Iel}6@5oy3E^aq5IuOUSck7U zb0tX8?A!4=)dzZEcHr>%3BtE?t?>4Ul&Dd?d?>5cT;HvVgTiV`oiHYcdWTq4kgkF_ zx-X!v!l8ZdeI*xvnygQ`$! zRMcF3t9(I8+XK_>+cWo)dlzi9`=JW4H4sqX^N?$Nul(KUA@gFdiCxe%v9Ud{W2n4> zvUhMBUK=c=b-pZ3#Ol?B%IeV*WNL?r*AK6uvmDTyozP(F`!OdY0jN?97jemivrt?W z_T7tyw~T<@sSqpRp~C>8-X!J|Xyg>Fo%6!(3DYe;?>6=FkC2ME%^*?AT*z8e`1G9v+tG4Y>%R|T3 zPQBP9fd&zhiT1c{0l8-3pL&J0h;L^4d5Z{*71|LkP@M>a4Q}5yx9EV{aw?swgI1Sw z8aMr}Q|;AQUS_1wbR_guK-ys%9S0S+PV*-t+H}(9dhY+_O}~a)HeiXXI6Vr*ot0@7UqlGovw} zBloPkkH#A%`~=dBo9iS(nwH-AfwmE4S?&d0FWQ@`~(-NN@y%GG;@@_t9%!Czaz zlIx@ynX1csFr}JtwK5KlO~=xSk_wx@4wxY6dcq6t;zzjhAdpi<4e&DaT^-zt6bX9! zl`e|rN`8ev0;Z@A>A9UbnW5?Tvfh1W;K)YiNnNX#4$8XV>5}Go z{FnE+{?y6X8*I=t8@r0lB%hhKB{L*bzouEUbS(QcHDo$)W|z3{#IFF>V8g9e6ZHR+ z`#ra)D?!xQm8SAj!+&JhBt~-Az9CI4&H}fVh>iJF{Z3PB9=(q5N^_QZ(B6>7x`eT! zjuVHm#-<7x&Az0F7d`Qy?n&oh#NL(ol+6ZHEMSUT^=K1HLv&#;}eH z<-T%3mgo5|pQgLg#>3AOyV8sd=zq7ig}B6vy|)ac8}!15u`bZsPo-aY;+*z&}TzG`YGYe5FjMjWP4fa6oS-2a zBY*9M8h&V_SEHn!dVi0AQCZLW5>|3Rd)Z*H@E3ELC*qb5G+fAM&1_!8?~VzH9^Tqt zBSzf#>5gQ@i`MZC*h=tFpEdmq>N_TUS$zNMMuZo<+<>sbzHEjvm^Ai^b+4mI&@o>7 zJjvQLX5^NxjOGxxk_Ehlp8rP?FA?Ms0qc{%zY zSG6luRGQANn$8=$Zs;l#!P#E#`cEmJ!BgQY0HJYwng8ej&(S_>IDB~dX+)+|CS;xA zoL-$LztW7#8aj2h2QKPyzi{^bfW?vbLZ{RU`ylb|NFQh8E(kbji z&x`~qb9)e^;UdkrfK7t9(^H<6F3~Mp6aM?z_NT5*K+G}z&{H^~D}2b? zUtYUCTN1@@cUog8kfY9!k{!!X#T%o(jwB!cMXo374c z$j$9OdTi#-u#QMz9yWfhc_#lTalZWN3Q8vl&3*tir)sm3prj2MgW<|;~-X}0VSa@wEVHd9_&ydhook4dy7%@vH z_M00I-I~l%bEyu(?}lQcssrrPe1Up4DTTPG4cqc448hfklSb;Yq~4!~Bo?fjtLFNAO|Rb8;7S@^Q%pwy}D(K0rd;^_01afH@B0tKV!EBOEy)%l0@kn)hjwyO1VVUJBAOE z>zxZHv}y%rPX^pai|qt)^34nqv}QJU!XDw89778Y&S@Vfl5RNT%Fi3&2BgVwOWj_) z*Z9<&Hgd6Ozr2Yl4xFLYY2KUB;@9*IhiTS`m8{SVgd#H-sRVbm`KfoMC?CrMTl%z+ zZQ4lcH1rzGt4CEdf_-ZVB|d?&T(TzgkwfbVj}{k#X?strw7(4f{=9DUxPhN8&RN{k zr`4~$ofVBVZvgZ8zj~?HB|V)u!7>$gRNa=fS=y;NRoRVl62@BmW^t=))Hog*eNNWg zwJfYgh0{n;EY;UPb$Qc{r4epKSy3>M^h9l(kIJlKtnQF}NOYgo*%(ayrD0Zgr%W{A ze4s3JU7DcyQ6B&no~yhEu^TaH@_DRnEQU<})ce@}w@BSsMjMu0n~sQP2L_K=qnbmx zQ9ZdPqRQ>F`iOwA9sKBq0W%DLvK=V+Ci*?S6J_dLyqd@NB|8nhblkcZp4jjL!ie5?aP++ zG^j95kI~14dilAxAzs0sV*M|HRrzHR26U zvuY+cHq^&P(bR)TrsJvuf>Akup92CxVPm|EKcVHwIJ7{9Dk0h)+ao+ zsY~!%>uW=sLd#Krs+o-Tkuy(IN(CqBfxQ+AkSJ;d?35<$VhLGF8TG+d=KO9WfWQ|T z#T`Wo76`!o{bL;xfX9Rb_yT-Z2@wXT8DKCNAXl&q3Iher2ZaO02NObT5KvYZgJ0tm z16!q^(t~eXaXAdkyYIIP5{$(&l1o@HT<*4l2hzNFMnQ?@f+i=0Q!N$@73c(LUw`R) z4$xhcAo^p3_z>c6B540nELfjooV^ry`4y!};JDCo+=vu^WQpoJ!J?p=e4%q9Vw?oF z3{Y2pX4GB+0c{K>xU|3?6MoVF0a)QhNgXouy=Q-0FaYR1CfrycY&bk%eepDmRh|J} z5V0sCh*!W}a(_YaWX>OAx-xSre+VVzsNAc82zO#IDYRwXF&C=Z>S$wH2yFGo; z%2uLV<{w_{-Cj6dY&o^dhv-wekF-5)X6e6ycsEWG$twTK*CYEDcQMJIQefLH;kj3 z%1PG`dlC`w{x5pW;WwdU_V_*_mEd*e#cTJ6MqD_7@*L6EM@{}@$Cvau!8a2qub*zO z!fv-WH=mLo+-cF)WDNAnCXTPlf|;V$sW5eG=<-m31}qB`S)WL&nMrYzrkFu@{rUb< zkOvd16hi7?rkDkW_wr-QpoNXF2}v2Pv`t*#=r1&Rm^~E#I63G&;Rh41FfuzuQJg`I zdW3*x7X?Q{R^Xtco(l;4%f~?P+f4x$aFZ1G%4Gt@xaG1`GBh4$X7(TC$)tBewGu6R z{lKYJ=@fDip?`R#9lCQgHWCrhgJkqz6!? zgw_w&qtFx$f(1kgnpAWL3x^yCO&LaJM~A<~7U9f)MytgNF{LtkT){xmkWKVPQhWE& zn&Iax^#gfvNc+hVcSHk212uIzVM(ca*dsBSHha-s$3Z|YgQ^}RL`SV zUODRB7bF5Cf*Wj5J$NLEW+9DTN+NX_=3Xpuj>!=Y9#;eVSU5R2EMp;cr7TU<0s}g* z5(*JAV;_5mSO*m{1DD#Mfb0E{x|YonylBlf@Xp&tqBNXM5$8ylF*u%x2vRH(`3gh; zX+7FL-7umqN}Ae6g8HfEkS%p=5baa2L|8S{PWQGZoSO+z2IpIVCaHEwxHt<~YX}}C z;$T2&RB4xg>#?sS^I5Ve8zA(Lc}{`VIG)wv8+qT5(NP?H%w#g+Au335wgSYmTKjg- zAH=CL_n@s1cNZiwi;uoZJ}g`|D5}lnpp)EDLTd29UT*g_f{vIS?po6 z*(=s-$(j|3-H?CKZFh}RDD-^?rs>LLGZ+xSeq=46d0E&x_+4Tv5hE&*AxfyY%^b#S zdb&Pq@l5rxH-3)Tf5FR@$w}^xyM38SOSm7a6>D6S$XC=SXt5^Mmoy~VK{9t@t>wSH z-R!lyso{f+n_~|p6Je^QNEGc^r7Nwe9PorpKou&F`OsrbtupSgTP5jju+DJM0m*PsMaYYoT+y)p ziNVaBPKQd9FU|I}7eWw``sF$4V+|LjI&6?{S)H~SoC2n@jC4{Cfv;4FN3j}pH23mX zy*U)V(oEaHc;|@fG6uA1bk_Xp2_bT6?cRw5o|5*p4iF&O9fClsJ;EI+W#Q{RNV+^X z3d3mOvqeIMyn);npukA4x@DxU^SRla^*H2d%Uxx~s=Rrg6dztdHqsOo$OMQyi7!+3;&HQUGL`Z!-mp*pSBgzb4@u?)UCQ@9)1q9!mK+js#s2irYH2I*Tp2DoP{$VO* z8pW)NVA5pFZyx(z)*!c?WefPBtYmzykZvm+L9yKNb9BRnrJXnGM%%Z+b&ne#uab3R zjstJau8PsuKB-N*par5&iyUxo5UVc$Rg;-@ogXOb+)0mFuilG6Jvyf&gk$1RqD4rT zoC>Kju1`A%fKsa|Z$a_|Aen`s79yE$3Prg}bpl2Dq#F=7BO)WLC+?IGdb*f+6*R5!QLiAmMkl(#x1?n?gFC- zrQ|1&fYvt~k}B6ysLM)jCyYr2@dECaEwkE&G~$}%kf@xx^$+94gww}{E}H3=y#yf4 zf4T7e!_sSuav@{2m6`@=Ssx1}k`cGQ+|H6uL(YuF%n6Dm3289T2r7#1dHk|1njQDz z>~fg`6pyvF3go9wTDxMsXnKOnibQRuOkYgXeZvS)!CvAN&>Yb0DX_lDFI`>QQwg?) z>A*Hp7{|VqIUnR+$hD^}rggueYiH(zwWW0(mlO2OpP^ALL&mR0>JTlgaC9n+o(hPx zW9x-5O-uE$Bu83jYud~eOpCgNaE28_TIgSIT(K$Z&D_5V8(q(ngpZw;l1SE7L2MWU zbwdHD3>pXZTCi>(>vL%wH)|a<>Nr$sHV*H~5=~Ug>xvWX<~Xc>X_RoXk9iEqRhYFK zZa2o%u96xS+YL*tX&-e+m(ql=aHMSPIjz2 z2nEbPX4JhCt;Nhg9!!+2+DO`(5m~HMwkLInrp~AaQ@L0;Xz38JX^=zb!XSZpBd!A? z|8f)_X*C8CNV;r7c}p$j_33D&kwU^qIfu%qJ<3DBI3k2i@m=+4GRyYY?`sNExk)O* z4pvUtT{TGg0y-SG%rAO;WwH^k9Ys@FopfCy6w*EH+n*~@t|7Tt9XWV8v`!jomhB8|4?M=#u$g z9}A@Fsa%z%mwDm=W&gpmZ-FM2Jkmy4g?fNIVr-}Mh&H-xJgzIz<}VL=whcTQGS#Hb z>Zm)ibM=g`r4)y@j*SiIJ(T}2+tP8A6tE_8R92FTNTTg(!eR=%2bdU!@uzB_|#Z9bc2aWvTg>d?uhYv z^w-0mi^xN=*4GzEShEo%t1_e9m2+z5Y>0`vhr}^ILF37n*Y=&tQy8Si);W$P;{#mj zNsAXG@TC@>z5^3GE0rTpWAfM66gz*`9Z0I71pB}qQ8Ejb#~#;R2UQN)?0dAd95FZO z6Plx(CT9P^*J>Lk69W=nY_wAxW{F&=>0{11aQA>nX8zdf*y!3@cd~y;W zJpL9Gzoqq`pziC|%yZ`K;7C-!H0??ZLmI;q4x>YVV_=YH;g!#h=wMm7d{bU*isCtF zBwu1${F@c$heqY?WRgXd;wBbey7g-ydTnCHINro>E36y8m>R>{P(##~;G_e4cxfv$ zR;*7yyH@ahITB40%?>%u1J{t{t$ef6MM6eb{dJT`+pW}5SBh#0JUJj`poLF=vlZah zEi~qghS5XR(Bf66p|bbYfytv=aZD47M0aO{yLM+J;j)Cyv@prehFuT#i`&_DWIIJF zAlGkRzPdW>FD%!V7q5R4yqkh!7Z@+DLo@TG0dWtQR@=ulF0*anh}t@qs`>8U7koUp z2eqaMMv18x;7WMvs-Y_&1&7bpEtaoILEnn!IRn=gD)2c0&DO;rnPbN8$y)&_Hl%?| z!!LsbKn1DtJvm(vJCeRQXhD-`=Df`F?5s`#Ml ze7yNI6iVgdt0}_9n^yu*^(fn*YJ#eM@n~97;9!jC{0xk!>jwDS5>!nVrTpYRz-qQp ziI7W$o{HI<;RlTv^sHZfk?3lcjwOYGOSul(e~%V8=?$AtgSN!wDa}VS7(ND>c z%+iU)%(5@0G5@XlHdqR_%Ftm0iytjXLE5Ug6w&OA*OD{Ha!48PapQK(>KvEba$w*N zU!v8Bj{8$}O!)D!@}2h2+Pd~)l&vdWcu}j_`={w?>&?BHS|Zg_#k@us0C#1lho%S9 zQ&-N5dxxOF#mUZ@27B^{9cCicn4yDdi5q2T(m;Eeeod>(=f>&XYtw*5$EMbHplXW# z6Ma@3NlqbEbKELBZQ@}yhz?{oQc3~IHS?>VMO*Lkr8icju37=Zqpav0#*JQ#=xKy1 zo!5t{*PHtx1LvE=GKNp*e@ID@tZaI?Bt*N)jvCa%lG$BaGmq0UqPx62bk5$Ia&qEI zfg$xD(YY0;Zz+5ewZOj%8Tr#jp+)31D_}4gg#40x_1k!>;_%Aa2!I{{1^~EQ5#ajv zaq%lHxs2Hpz6s*QrqD()WjPh8MPYZNCs|?Eg)^vZVZ$gU*4m@)!{~;6c#-K`o0e&+ z;BBdGbn&#z)Sj8{HhLZ7U?vYk-+O=GI$b8CLnlj9Zl1Q-5`~C@@`+v^b5TP{DNG{r zM@ioVVk>cBz7AY?e?b`|W3X_26ZC6Xh+D|8IZ={r;|t3}{EUlN&jxGNqeFR1G0q&# zvs**EhPJgp;8Ir~j2|Wl6NJ$;%6QI=OSwMYS*$CyNJ>bhaoPFMTHDHL-@*g+8$|>R zHQ2Y)Vf8by+WHz9xgMHxmC5`ia=pGg$nW+SOj`bH^mkD2qQ0%65m~fAqtL*$y>|nd z1@h;Z`TehzebOe8_BO>hd{Gpo>-kz&nmii~vhmp7=cS9bgN4(N8U+knJE0nZKfX}x zSy(3wn#x@sUs$ae|8N0nbrHHB#pvK>cDibrX4`&xzNN~7p%D`48 z6}zq?XXH-mK1KvlAr)VTJ}0=vnj&ZiHorJg>tVMVstwpi3*rayhX9)y!4TRb;Tu(H zAkmml;wf%C<~Ivpn8P=u59zFAS4vh`;E^B;t3?w}1(!IL%QF#FDAc+Kze-l%l&f+s zwpz(;%_m(UQd;lmNU!*wc5NZ?t7X#KI;S=FA7nYL@Iegh*1r_WIPGJhDv>v|%zdS_ zlspF-a{#4_?s<|u?qe?XhyBpk2lzM4P(127mv3UV=pwSUJ0+lZLkrkRP-zEK+{?<9 zGFN2B&dLVftCE6@ruY8?ayU3IuC-|BYp2WIoi}i!4~-7=GEif18LSB3#aUU~_b9Wt zGqq`Nu1^ALbfZ;+S^%zrS%}^cyR3gEb!cmDY!YGsOas*e(C}%1Uy#!Vcz3+<$`+j; zEi61-@zKaZ&jU5^S#D@>0-1p7>F3axK(G2WY6V?DoCNSvCtX#d{r7;#eisL5u&kWd zLBR`5@KerPVU&xT-m4#MJlYjkMrB;>Hf>iG;C*jp3+`X~nqJTJU0I0=8pb=1>19)& z*V^4nFTsWQV@+)h0j!+7p6c(ZUD*{!tW7OsqZUo>WP3xuRcGsIseWnLzJx9UUo04i zpbOlTZZi7UGJW5d&_5MC)98|%=geC2p*B}M$RUViCUBa$PR9<0WV1h>xZWH);cQ&q zioO=TJ=&Ed-N&C?seWt@YgKqevZj{B+B`pqjfM1q`N9HWgRl%EB6p?(KkYcD5%2G4 zvvaFJQ-+Y3;j*-P-<5o>FY-?U{&ZG#t~l)CM)O892s<5djKpv!cRXT}UHpq+ z*1L79$e@zR_}y1?F!OQzF4$4QaQ0)N-mN$Bq-+vScCn2P4K-=(bs*1gqblaJ&;xHl zPtNn_Qk|Tcz_0-HtcrR?{`++-^iQ#GYY6kCYzblf;-kT20 zK9_DEFU@MQ$5oxb7&Km1ygbQ@p-TNqb58Z;9*VuJG)imzkg`~`ZK0Fqz%41|5(7~l zVJ4)J3_dbMF=R%?$^b-s5Op$WG2mY%;8+EZkEl!?;Ee)*)}G4KM$921Ca&^Uy>S{C z2{!%7?D9vMT6UCSe5N2L8ciH47-<+6Yt4usF1UM&G88%8?%l>rPOVSf-$`Qneplx) zqF|#AFm@z;cL$Dte-k%U*dO(7oD{FiuurD4vbU15dhgmY^UD@m)l9WHOL-*PNxIvX zW>`0}$a&Haq1!qm+Ni$m5SV3D5o1{t9p%I4|A(|ZKJ3kgqAoI5f{4)RaH<;aiF+B( zVJ55uF*E9BQ)hgol4$ST@6vH9=r77x_sHHrs>C=|tVwfnTKk*c{@u~p3+fz}6%0zFWmb#XE(d#$mY2kuf1`z1*r*d7o~~5h%6Ff zT{4`M0bRorB3VScuF_gIff%8^(?!$QM$>DZ%uQ3+2kItxU%U}h6_?5m+%m-?bjH1x zxSt{zKmQD@om_0q%H_@aO{2NlgkD*|Q|_Jp z@zqu5{mja6L5;pn|JBDNGF@kpjfTiG8?&eoZ#QXEk>3xY+Puv*@!pai)AV$Xh=>zX z>#oUf4YG%uEVT%>F!2U7^DbpE#x8gOj2WrlxBBAN(wRkLboOrblQ$t&gtZCsvAdJv1XM|IsA+Gm!PoVW zzYZK1hjq?C7nkYhK+L$ew7iG z+G?R+v>Qy--<0#}KHmuUp?42f@mEPtQAr0|j~{gC@}u~^o2{?A^sNy8Tv>YD#q-o7 z5Wtm*2*gm@?axxsDu@eKU--6o{QVpcUsMS#xi{g96$p|j2Eh#ZkC8zU!?;QL_+bIo zh^tVOxW}_aS&eTs#?2wx^goQzeoD38I=0RE6TdJqt1~h7Q;AU1HODB7=^S#N5%Lr7SHgUomJn&_T_Nv}ke21sq>oS-vi3Y6X>qqiucHio^aY1aaQq%T={>?Ebkqge zaEvz#FfABEM$Ocx&nc{-MU)?BEGE)dTrI&k`zaC+jL9z&bK@|=HvT9r%-Rsvto94v zXiS(>z3SNh7s8;LJUPuU7J+!rDu`mwJhGA57^Q}1PHBdvf-*b0zH;fZx#FPtlbWT| z#sk;Y@#q+BR+r1&XhHf7(5ynJA2xx2Up7ddugMR$xDXzM(c^POFO1cB;8g+Aq?wi{cU#g$frm6=07~(D<^?lh)tKFdozjb- zDrKD&ZUegC`7-9iuoQETIDa_zgA(3gUns`ZTzk@PK1}paqKJ_PB87C#i)`B)m`K7w zmCLKubZRpE24*~>Su&eLIGug)EMxe@pDoufx2H*J&-{+K~hK3c74#t(nE!3E;ck>0cxP0C|&_U?s zgrNq2<;o^bN* zHw2J^tAdA7_WZJW6k_KDtPI+e2l|d4{0kynj4cR#t7*%K*_xp${mQl}n^%*|N@y2; z^7iB$L2YKVTQ9|QFc?BB=y3rIgFN=?iD zn(qI^CVCcZ_td_-``7Dfu`RU@sv4+@udhzla(TDBKcC!fSR{UM{0_$7U*lbJu8S7cY#FNnYKM$Y#^)KYGTsW1zFSLwgS|?+ z`YJ!y*7p8nyOX$GrPA~MdguK+Lvhq^m>g3V0Dy5wsQ=DT3;+S}Z{~NGTuUo01OR}) zAB^uAij_&VY!`0fO&*jsDlZlh*ZDR6? zYWrgH^8C5uD*feieB8^{&TbY#sKR5$85kl@RtzwtlmPTcX%LD}F_r>Ap4s0OHI&__ zr<5K?mzR}xu$;(6@*uI$WkAhh_PI#ba~d!0??p9tufL%j5GqiST^d=|QiU$$I~H+(ZgaUmO7Xh7xRB#lR#k!jmQC>E2aJi3W~4}~ zZ*FEwm3lsc$LAq0T&mPkr1iQ#l(c2b{ABz3BIJF)v*ta&xDd7`hy|=ej*te=V-0oO ztoq&6>HXDt;a8?wMQPmm^~u^8UNqweZ47&d<7Eg)1OEojWj z^0LeOvu#4@=grXXuM@+Bw2*bBQh@S4Baz9wqp8F@AIa-?I=cIK-U+&|&^*2Yg%Y6i z^K%z1?}Jg)XTR-gq!~I#3IL>`@NPj&C`Javy zKGpduDk>~2Ec=8n2;>956H)^wMn0@XZd0wh%;)>6ObBqQN}Lp72H3@oSrrr`Qse?1QOL9a3-EHIt;mB9 zV#}X=ElCtaKr?fD8xi1cxq^fIi{uAmRs*3OTWl@)uc!ngMkQE#<3 ztD&I*)yCQ=Pm;IX0zjZDZ%eez06nMHhozInJP)}BzL$Cj1Gm>yjG_hF# zv!nOeMci94519)l;i_Ygr=SrLpxLXttr)wotD)F=71d{rH-Kok?No>!(U`kKe%hB1 zX8tD-dAF9GjWw=LO~6n6?mcl9dP+VP-sbQSVBu)-0&N zIIfTg$mh29yhG0n&5hwl;K^lTmguscJ9Kyud4R(OOa%m5-9DEjIW(wg() zV|%o)ERY$WT)g^4-k}kYQ_teU4*pnw%~8P7Yd{5ya6L}_^hl9j4X`uG-9tD$0W zE+DW}zK&r2{GuP%9#@I}Me&w5fNtYWMK1Nvn-9t3q3!7mA4{+Qr?TsSYGPa0p?5-) zBE2IZO+=d1fI#T|C@qEnq4%oNB3%$d2~E0yQk9M%0R;sFq*nnc5|mz~`@*@;$0?rs z?)|f7WhRsF+k5^!v)BLqd+j~GvIio#SJ!kmIfB(rS?_Rd*fD=U2$(wFnQJDl8L-GT zT6}c}H8KVrd`x`ajb=2_0iqYw=JLo1axXzZTpR=|vi9S24r2uZfQPyf1^3HHaScP_ zy&@iYSQK5B4`sk(a?G|UB#?-(=gpW3-F$F!$!N-vO`L7#x z`@kQ)hx^a3Mb_nFqD09FMK}}$wPWNsFUr0d-r8cY#G7FV7#Dn`y6-u4vB@83OoD{n&j1 zj%Q1BSxa^*Md*OE8V{CkFtdc1b*vg+{}LyIF2pOED+E~VHV*XIzX_OnUps)f%}H$Xi${`|8{$Ul8iNFy!|S%xEKe44V$~YqBN0{lha)v)Jdc0I~)OG z4IETr$4TRyhQmY8wvmLaxiAOu&^dOR_emRhZIk#&Oz)Hj)MyB6Dze zYwe2Y_))TFpEucX#Kz?T8jC>9Yf+}%Vxcw~lTkLf*oK#vx%JBV=o4#5gmJYJt{Gyf z8;5 z&u^y&v69v%KvhMc+9#ueeteHMixU(|%!N|o_g{DY9E014gk%yzA65HiGj>K$Yh*RK z^EXUC2CuqY4)HfmZeb&=f`G>D1h16z;O9Hggjb>x{AMpIUQ^ z^9cH-zZC;SuFWU+nhnX2rBW_*4tKu~sb3)}TBC8U)l^(KIPmxI5O1&}pQ~1r(~e|w z!7=M**0k*jt8JAz*pd30_Eurdt~^x_SJtK{(w)^|gr%#r_QjR*Y4uhh_}lsDX*Dp*Ui(MoZj|)cm%1DmA8!*b(9Ya&beoat6v!CD z?4BI%^tniKD^j}SNtY*C=C62lN8xfZi4+XU;*NuMW}5w2*&o@;IBnnyDlLP|y^-Qt zfZ|#j%XUCD@>X~xkbN5%$P)4VO|f2%Zo-WsWoIY7R`o8%5e)BeIQBX73+vHAcnlJV zceFAcHm7vtYG+(xg7FP}p$;KCwKKaTa*aR^t(K6f`PzHg%)Kb4K_c|~Qec9WDnbF+ zp^0d;5SC?Ij21&Buq(2$vNDm5OScgsKBOQYC5lDjYsaqK%#wxiwC7h6_aKRfJlGZ8 zKE0agg;|O=-9O$hzf({C-92x1GV>85G@MyUv}h!nonWxwY=#!1siS^#XM_g{yv4kY z4kwC?jI1-PU>3Zcrq@8_rG{#aZMw%MqaxB6*ByDt{PN|?1?<3E@UnjZu_VJgpp6>J zgTd!yh07{DK;K)8n+sb73xS)$nM@OmJK3b1+pTpwT8W-FpVfFU3AGzaqw^lE8k0PG zU`FnZHsS;rNYUrf+}HirsTO>3BVNPhk9w7Ipli2_g2&0b@SkN2N}&CTjY9;Y3rz=a z=C6QI?ats;>xZ*eswG4m`Z#xalPuVhOD3&oAQE0)4w65_$TB`Cs)B``d!=};y5K8G zP?wPShT&MG{x*-UI}@_uMCI<)$_{$*4<5e%1nlL>xxc)@MwI`?{xZ>8RiHaLAjr3A^3ze z>Y@2ODbg5i^_$k5ebA_>K>cBC8I(%2PXQuHXacZpYoK1}#GDK5Rq~g5bjMvENg@iA zK4}QGZ;EaUj*u)@&BEo08q^Pc5~CEd=HvI0*QfH!%FDNayPxNW$@!_5Yp=#=+bzjA zvzJIPtr z#;B6ucS7y-!Z~OlKCiu(@rFO*^Y&$zU8yqy8 ze!5Z^gdcGUl31VwqRP&i3l9%BU#pt0z_SWvz&<1~xG-*aHAW;jb5~SUG`8P4_c8F0 zsh_?{>Wc7zfc@7DVQS7p4{Ru}5q7FZ^@1zzw7I2a%}@^cKxFHrV>_$UnqqKChptkx zW50Sk=H*1NLB5h&QK$iR0~fj2gShq&dJ#T9N-;8;47{W~MWLBAqxcID>1eSqV(cwB zP7G+kIY7VWD&sDDX=bY88~vyqE;^?*1=HZey^huuySCIw$>T6+pESyTBZRvv8MhuW zFjviC7J2Y32{6^1((bMYm9dHrl*ocENG#&yBfo%&J{wH>YAy> zj1hi_(r~4)My17XP1nvl>5sIK`Bgd(5vqB;TwJ;}9HHdev;{Gx&j3lwIyE@27UekE zLTsjO>)AR6b3N_1$oqEqVrTGj$AKeIJFz1_qf%5Dln=$7gx1lzzL`P9Ii+ZcU7S_u zb;wK}Ttr_A_ZEQR_*SA1IoyVvHNpHLxrM4{&Las4^v5EpofxT5S?ZE2P_^+862Gv8 zI=ly3yHDi0W9pja9)X$9ytTD@_Bb_6!0&vI-B zxpN|cE}+svGMwNHZYII^hrGoHMG_~itp_?0Mlo|8g(>*Z=Zad_B#e`Gz`9#t2s7fV zL@2RJ=pXg=-vs!demDb`P>TkCds@bOo;}c1(*;$i*o5hW@$l&YKT{ItXMP$xc1P9C z)edg&;|cMGySaKvh{9bU_AoC|6}Yzx#NA8upTU=Z0d{wD_i=ZDx%&Kd3i&;eZ1~MJpT=j3e{Q5-RmRTQ z4dM;^>9Su{<~$GO_mxav^+aisQ#*}f09PL7^^ zK3d}v4qSIcenYrh-4*?Qy02Mx=fxv(DHI~hR^^!?%%^8{!|3_c3@SlUo^LuJSHhmF zW%ck>xjzpsz?CXe5tyTR+LOKepy6z}APP!9_#02ViHgzEvSSB)y=_r4hiTr#jdv)W zL#D_6il&2t(Y&^;F;&THvb-Ek|KKG_v1YZEaF&nl1x^-^iQ&6Q=?<3ygX*kcCi z+TyPKuZvw-&DHLziMF6>Rv|VS@J3}IxadXtm-4Z%XMy7kp7V!8nBk$p?zyVL9@8YJ za)Die;s*2e!~3&BKD5s?%#QWPq^?-}`86x$u*?q@}xhigLKNjW4QK(T(5^E4pcaw;1)wqkn0-7nXn6Cw^HZMw?AX4SH z-&$+0l6kkHC(F?gvv;e8eua-ZyomMw zLqf~F2ST0eVd!k2las-l?KtG4J7yk&0f|POM@C%F#iO+58ZDHZ&wNW19}=@XrRj}4 zFw5*PR2#xhxfxeryjR6{9jwvRHgbw-sIeEZMO?u#epO+9%58qyXU-`d$In1v&foF) z%@%W;tbCY{c~{H=f&n_eVB%&mLF@YMxhxQgb9Q}`YPJ8Rzdz~fd+K2%tuLb@#k;3R zUh%Ayg#8n980wOtKK*crSX?_{2^CMVz31|mgY6|qVOsy}ka@7xm*KODHMK0}?nd)j zf~^^g7@x_gNe4Ogk(Yb%9k=2bbd;TBTN)Uj@G2$j4d^*!wjb4L`!@E;V`%4H zTs0bhsjMdF_w81tcPyII5k$VFj32Vidq>4qV|s^=O5L{tgMP$mU{}+A8AqH`Y5BsC ztDktXlS?p8aDF0bm$cS7CBm_YeS5E^GEfFAA$-JoO{LiroI(;_&MBG z`{5kWT?$p&)?MVePrx3lo<4|OV0$b^*hWA?ixXtRi%zwE;BuQL#xyB4;*9vlm%cB_ z58GuV~kdAku{Ij<1* z@?BvGmrBKNg$uw=Hm8QI(t$#ivBgQxyTRrU%QNH=Mlq$sEFUs(`4;yvvRyZ{Edp26_zm>4eQH^u@uA4 zKm9m6&go7l%XGGU??i@b{?P|tb+~4`VH#4VxAf$oy>Bp>KO({&`fh`0BPs7A6H~@3a73p)RA43H6R#?RJ=Ebq z}Z2ZQ-?9ZrvNT@}=4oZK6AzRZ_!O$nc#> zALeTb5cY88DL9fQ9+{hj({(iERTtW|Vhq|#?BX%0kp?bHHF$2LplZVg8xzQHTrN?! zwW@Cm&*U$_NH=F(hFlOk`!`NBKhBClmsTav549A!pY^zsIiN|SlAc~0w*B5@Q~%0( ze#=<79HW9sU$sj5VB$oyv^;vKBe0oSIVLMUJg%}fXA=9S(-wU*cXQG*nI2?XSx8|N z8{>f^JttWA-l_r!G%6H^C zt$~(P(Bwne=$8aM{57d>Glw7Gkm!AI>LlA64@TEnu^{H4 zfu+ZpCcfbI)>%V3)3Auso8=;9ZbzGiwl4~K+9N-WB?#Xt5glhCaPcO5;1{ne$H;rD zlIEFJLSbN@mFcJ#x?rd@D6eYUu9W%ut~4r4l}#b-BO>V@@9WNhTI6D>+TE%h0@^~p*>PqRyjMRNj z$W*RW{t^;gyBL-YLBdWtg^l7@_eehr=8S~H!o^V;hAmZk5*%t8b*I#fM8WS=lO_mK z2nuEh-1418=|1z#3(@(9$Iu12_kXApjiC>Med_(DzaCJ2Y(h+vB(#RiToY9W&{0AP z{DWLxOFQVBRWhucI7KqI45% zO?%c~%WsWMTDtXYQ>I((=$TbwXN_S4z7h^U?QjwR;7Imo!1t@r?GiS0(@`^4zN)FK zF6shth16nbG_|6lqMl%P2&^(bHk5@k5CnjYDWb3r2@fvT+ zt;9WplMhH-3cG&W08Y#Q?LZhM>a)%H%w1AeyjndH&+t{5e`MO6_FchBTpcCjdm4H~ zqipcWy@`fMsIPTNoKBbsCs2BtQ(TvVNCJn7Y%l-^%LH?Q*m(V02fLr2T$aU z5Z0aN+wb8v_WQzI;4U!l0C(75QLAO6EjIW7!1?xPlmQlu#X!zu>Yu2eO*m_VN61D1 z05D^f8~H^c*u60 Date: Sun, 25 Jan 2015 18:22:13 +0100 Subject: [PATCH 190/269] reenable mcatt ctl and other nodes for ros --- CMakeLists.txt | 84 +++++++++++++++++++++++++------------------------- 1 file changed, 42 insertions(+), 42 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 25822d7190..f76dbbf418 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -169,52 +169,52 @@ target_link_libraries(subscriber px4 ) -# ## MC Attitude Control -# add_executable(mc_att_control - # src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp - # src/modules/mc_att_control_multiplatform/mc_att_control.cpp - # src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp) -# add_dependencies(mc_att_control ${PROJECT_NAME}_generate_messages_cpp_cpp) -# target_link_libraries(mc_att_control - # ${catkin_LIBRARIES} - # px4 -# ) +## MC Attitude Control +add_executable(mc_att_control + src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp + src/modules/mc_att_control_multiplatform/mc_att_control.cpp + src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp) +add_dependencies(mc_att_control ${PROJECT_NAME}_generate_messages_cpp_cpp) +target_link_libraries(mc_att_control + ${catkin_LIBRARIES} + px4 +) -# ## Attitude Estimator dummy -# add_executable(attitude_estimator - # src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp) -# add_dependencies(attitude_estimator ${PROJECT_NAME}_generate_messages_cpp_cpp) -# target_link_libraries(attitude_estimator - # ${catkin_LIBRARIES} - # px4 -# ) +## Attitude Estimator dummy +add_executable(attitude_estimator + src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp) +add_dependencies(attitude_estimator ${PROJECT_NAME}_generate_messages_cpp_cpp) +target_link_libraries(attitude_estimator + ${catkin_LIBRARIES} + px4 +) -# ## Manual input -# add_executable(manual_input - # src/platforms/ros/nodes/manual_input/manual_input.cpp) -# add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp) -# target_link_libraries(manual_input - # ${catkin_LIBRARIES} - # px4 -# ) +## Manual input +add_executable(manual_input + src/platforms/ros/nodes/manual_input/manual_input.cpp) +add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp) +target_link_libraries(manual_input + ${catkin_LIBRARIES} + px4 +) -# ## Multicopter Mixer dummy -# add_executable(mc_mixer - # src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp) -# add_dependencies(mc_mixer ${PROJECT_NAME}_generate_messages_cpp_cpp) -# target_link_libraries(mc_mixer - # ${catkin_LIBRARIES} - # px4 -# ) +## Multicopter Mixer dummy +add_executable(mc_mixer + src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp) +add_dependencies(mc_mixer ${PROJECT_NAME}_generate_messages_cpp_cpp) +target_link_libraries(mc_mixer + ${catkin_LIBRARIES} + px4 +) -# ## Commander -# add_executable(commander - # src/platforms/ros/nodes/commander/commander.cpp) -# add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp) -# target_link_libraries(commander - # ${catkin_LIBRARIES} - # px4 -# ) +## Commander +add_executable(commander + src/platforms/ros/nodes/commander/commander.cpp) +add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp) +target_link_libraries(commander + ${catkin_LIBRARIES} + px4 +) ############# ## Install ## From 092a3c5129e4d26463b298d110dbfa8683fb9c26 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 25 Jan 2015 18:23:46 +0100 Subject: [PATCH 191/269] add start wrapper for mc att ctl multip --- .../mc_att_control_main.cpp | 66 +------------------ .../mc_att_control_multiplatform/module.mk | 1 + 2 files changed, 2 insertions(+), 65 deletions(-) diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp index b356b5dc0b..5a79a8c6b2 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp @@ -55,72 +55,8 @@ #include "mc_att_control.h" bool thread_running = false; /**< Deamon status flag */ -static int daemon_task; /**< Handle of deamon task / thread */ -namespace px4 -{ -bool task_should_exit = false; -} -using namespace px4; - -PX4_MAIN_FUNCTION(mc_att_control_m); - -#if !defined(__PX4_ROS) -/** - * Multicopter attitude control app start / stop handling function - * - * @ingroup apps - */ - -extern "C" __EXPORT int mc_att_control_m_main(int argc, char *argv[]); -int mc_att_control_m_main(int argc, char *argv[]) -{ - if (argc < 1) { - errx(1, "usage: mc_att_control_m {start|stop|status}"); - } - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - warnx("already running"); - /* this is not an error */ - exit(0); - } - - task_should_exit = false; - - daemon_task = task_spawn_cmd("mc_att_control_m", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 3000, - mc_att_control_m_task_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); - - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - task_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("is running"); - - } else { - warnx("not started"); - } - - exit(0); - } - - warnx("unrecognized command"); - return 1; -} -#endif - -PX4_MAIN_FUNCTION(mc_att_control_m) +int main(int argc, char **argv) { px4::init(argc, argv, "mc_att_control_m"); diff --git a/src/modules/mc_att_control_multiplatform/module.mk b/src/modules/mc_att_control_multiplatform/module.mk index c61ba18b42..959f6492b0 100644 --- a/src/modules/mc_att_control_multiplatform/module.mk +++ b/src/modules/mc_att_control_multiplatform/module.mk @@ -38,6 +38,7 @@ MODULE_COMMAND = mc_att_control_m SRCS = mc_att_control_main.cpp \ + mc_att_control_start_nuttx.cpp \ mc_att_control.cpp \ mc_att_control_base.cpp \ mc_att_control_params.c From 8dbb1fc8d443b1f8e2e454d4b82051fa63a48398 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 25 Jan 2015 18:24:10 +0100 Subject: [PATCH 192/269] remove unneeded include --- src/platforms/px4_defines.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 325832f0f1..c8e2cf290b 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -141,9 +141,6 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) * Building for NuttX */ #include -#ifdef __cplusplus -#include -#endif /* Main entry point */ #define PX4_MAIN_FUNCTION(_prefix) int _prefix##_task_main(int argc, char *argv[]) From 5c776e2392f8a6575934e5db287d09aff6b6cf1b Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sun, 25 Jan 2015 22:31:11 +0100 Subject: [PATCH 193/269] Revert "FMUv1: Disable stack checking" This reverts commit e62c8d73678f87b9f6cab1ad3a33c8911277a8a8. --- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index 539634e3da..a467fa605e 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -92,7 +92,7 @@ CONFIG_ARCH_HAVE_MPU=y # # CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y -CONFIG_ARMV7M_STACKCHECK=n +CONFIG_ARMV7M_STACKCHECK=y CONFIG_SERIAL_TERMIOS=y # From 18d756dd599f93abcd5dc89f323a5df77384ceac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Jan 2015 08:59:19 +0100 Subject: [PATCH 194/269] USB startup: Ensure that we are not talking to the peripheral too soon. Startup does not take longer due to smart rearrangement of launch calls --- ROMFS/px4fmu_common/init.d/rcS | 26 +++++--------------------- src/systemcmds/nshterm/module.mk | 2 +- src/systemcmds/nshterm/nshterm.c | 6 ++++++ 3 files changed, 12 insertions(+), 22 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 2c9387ff06..4d337171a9 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -5,6 +5,11 @@ # NOTE: COMMENT LINES ARE REMOVED BEFORE STORED IN ROMFS. # +# +# Start CDC/ACM serial driver +# +sercon + # # Default to auto-start mode. # @@ -43,29 +48,8 @@ else fi unset FRC -# if this is an APM build then there will be a rc.APM script -# from an EXTERNAL_SCRIPTS build option -if [ -f /etc/init.d/rc.APM ] -then - if sercon - then - echo "[i] USB interface connected" - fi - - echo "[i] Running rc.APM" - # if APM startup is successful then nsh will exit - sh /etc/init.d/rc.APM -fi - if [ $MODE == autostart ] then - echo "[i] AUTOSTART mode" - - # - # Start CDC/ACM serial driver - # - sercon - # Try to get an USB console nshterm /dev/ttyACM0 & diff --git a/src/systemcmds/nshterm/module.mk b/src/systemcmds/nshterm/module.mk index a12bc369e8..4e27105723 100644 --- a/src/systemcmds/nshterm/module.mk +++ b/src/systemcmds/nshterm/module.mk @@ -38,7 +38,7 @@ MODULE_COMMAND = nshterm SRCS = nshterm.c -MODULE_STACKSIZE = 1600 +MODULE_STACKSIZE = 1500 MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index ceaea35b63..50547a5626 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -50,6 +50,7 @@ #include #include #include +#include #include @@ -67,6 +68,11 @@ nshterm_main(int argc, char *argv[]) int armed_fd = orb_subscribe(ORB_ID(actuator_armed)); struct actuator_armed_s armed; + /* back off 800 ms to avoid running into the USB setup timing */ + while (hrt_absolute_time() < 800U * 1000U) { + usleep(50000); + } + /* try to bring up the console - stop doing so if the system gets armed */ while (true) { From 9cc94fcb2dde1a591c20e008ca59d1f876c2070a Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Mon, 26 Jan 2015 13:51:34 +0100 Subject: [PATCH 195/269] mc_pos_control: Protect against NaN and Inf setpoints --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 3b631e2cea..962dc6d4a0 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -726,11 +726,18 @@ MulticopterPositionControl::control_auto(float dt) reset_alt_sp(); } + //Poll position setpoint bool updated; orb_check(_pos_sp_triplet_sub, &updated); - if (updated) { orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); + + //Make sure that the position setpoint is valid + if (!isfinite(_pos_sp_triplet.current.lat) || + !isfinite(_pos_sp_triplet.current.lon) || + !isfinite(_pos_sp_triplet.current.alt)) { + _pos_sp_triplet.current.valid = false; + } } if (_pos_sp_triplet.current.valid) { From abeae7b6f6d70042c6b0bed493ade52727648c2d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 26 Jan 2015 14:50:23 +0100 Subject: [PATCH 196/269] extend subscriber/publisher example --- src/examples/publisher/publisher_example.cpp | 25 ++++++++++++++---- src/examples/publisher/publisher_example.h | 2 ++ .../subscriber/subscriber_example.cpp | 26 ++++++++++++++++--- src/examples/subscriber/subscriber_example.h | 3 ++- 4 files changed, 47 insertions(+), 9 deletions(-) diff --git a/src/examples/publisher/publisher_example.cpp b/src/examples/publisher/publisher_example.cpp index 9952a16e4b..e7ab9eb5af 100644 --- a/src/examples/publisher/publisher_example.cpp +++ b/src/examples/publisher/publisher_example.cpp @@ -45,7 +45,9 @@ using namespace px4; PublisherExample::PublisherExample() : _n(), - _rc_channels_pub(_n.advertise()) + _rc_channels_pub(_n.advertise()), + _v_att_pub(_n.advertise()), + _parameter_update_pub(_n.advertise()) { } @@ -57,10 +59,23 @@ int PublisherExample::main() loop_rate.sleep(); _n.spinOnce(); - px4_rc_channels msg; - msg.data().timestamp_last_valid = px4::get_time_micros(); - PX4_INFO("%llu", msg.data().timestamp_last_valid); - _rc_channels_pub->publish(msg); + /* Publish example message */ + px4_rc_channels rc_channels_msg; + rc_channels_msg.data().timestamp_last_valid = px4::get_time_micros(); + PX4_INFO("rc: %llu", rc_channels_msg.data().timestamp_last_valid); + _rc_channels_pub->publish(rc_channels_msg); + + /* Publish example message */ + px4_vehicle_attitude v_att_msg; + v_att_msg.data().timestamp = px4::get_time_micros(); + PX4_INFO("att: %llu", v_att_msg.data().timestamp); + _v_att_pub->publish(v_att_msg); + + /* Publish example message */ + px4_parameter_update parameter_update_msg; + parameter_update_msg.data().timestamp = px4::get_time_micros(); + PX4_INFO("param update: %llu", parameter_update_msg.data().timestamp); + _parameter_update_pub->publish(parameter_update_msg); } diff --git a/src/examples/publisher/publisher_example.h b/src/examples/publisher/publisher_example.h index 4ff59a226d..8165b0ffce 100644 --- a/src/examples/publisher/publisher_example.h +++ b/src/examples/publisher/publisher_example.h @@ -50,4 +50,6 @@ public: protected: px4::NodeHandle _n; px4::Publisher * _rc_channels_pub; + px4::Publisher * _v_att_pub; + px4::Publisher * _parameter_update_pub; }; diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index 781dde486c..e1292f7888 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -67,9 +67,15 @@ SubscriberExample::SubscriberExample() : /* No callback */ _sub_rc_chan = _n.subscribe(500); - /* Class Method */ + /* Class method */ _n.subscribe(&SubscriberExample::rc_channels_callback, this, 1000); + /* Another class method */ + _n.subscribe(&SubscriberExample::vehicle_attitude_callback, this, 1000); + + /* Yet antoher class method */ + _n.subscribe(&SubscriberExample::parameter_update_callback, this, 1000); + PX4_INFO("subscribed"); } @@ -78,8 +84,22 @@ SubscriberExample::SubscriberExample() : * Also the current value of the _sub_rc_chan subscription is printed */ void SubscriberExample::rc_channels_callback(const px4_rc_channels &msg) { - PX4_INFO("Callback (method): [%llu]", + PX4_INFO("rc_channels_callback (method): [%llu]", msg.data().timestamp_last_valid); - PX4_INFO("Callback (method): value of _sub_rc_chan: [%llu]", + PX4_INFO("rc_channels_callback (method): value of _sub_rc_chan: [%llu]", _sub_rc_chan->data().timestamp_last_valid); } + +void SubscriberExample::vehicle_attitude_callback(const px4_vehicle_attitude &msg) { + PX4_INFO("vehicle_attitude_callback (method): [%llu]", + msg.data().timestamp); +} + +void SubscriberExample::parameter_update_callback(const px4_parameter_update &msg) { + PX4_INFO("parameter_update_callback (method): [%llu]", + msg.data().timestamp); + PX4_PARAM_GET(_p_sub_interv, &_interval); + PX4_INFO("Param SUB_INTERV = %d", _interval); + PX4_PARAM_GET(_p_test_float, &_test_float); + PX4_INFO("Param SUB_TESTF = %.3f", (double)_test_float); +} diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h index 8da3df4387..9b6d890e3b 100644 --- a/src/examples/subscriber/subscriber_example.h +++ b/src/examples/subscriber/subscriber_example.h @@ -59,6 +59,7 @@ protected: px4::Subscriber * _sub_rc_chan; void rc_channels_callback(const px4_rc_channels &msg); - + void vehicle_attitude_callback(const px4_vehicle_attitude &msg); + void parameter_update_callback(const px4_parameter_update &msg); }; From de44c549eb8d195a0356afdd875b23a277fd8c7a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 26 Jan 2015 15:53:49 +0100 Subject: [PATCH 197/269] mc attctl multiplatform: remove memsets --- .../mc_att_control_multiplatform/mc_att_control_base.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp index 0de832df9e..fcfee28dc7 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp @@ -55,13 +55,12 @@ using namespace std; #endif MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() : + _v_att_sp_mod(), + _v_rates_sp_mod(), + _actuators(), _publish_att_sp(false) { - memset(&_v_att_sp_mod, 0, sizeof(_v_att_sp_mod)); - memset(&_v_rates_sp_mod, 0, sizeof(_v_rates_sp_mod)); - memset(&_actuators, 0, sizeof(_actuators)); - _params.att_p.zero(); _params.rate_p.zero(); _params.rate_i.zero(); From f23e603d02ba416ae250770cdaad6a859d6bae69 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 26 Jan 2015 15:54:19 +0100 Subject: [PATCH 198/269] mc attctl multiplatform: increase stack size --- .../mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp index e982ae3545..c2b8470757 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp @@ -71,7 +71,7 @@ int mc_att_control_m_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("mc_att_control_m", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 2000, + 3000, main, (argv) ? (char* const*)&argv[2] : (char* const*)NULL); From 9c627255ccc980270fe56b6c4ddeb494e1ce0f50 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Mon, 26 Jan 2015 14:22:36 +0100 Subject: [PATCH 199/269] MPU6000: Increase gyro offset tolerance to 7 dps --- src/drivers/mpu6000/mpu6000.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 168b34ea91..81d156af1d 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -921,18 +921,18 @@ MPU6000::gyro_self_test() if (self_test()) return 1; - /* evaluate gyro offsets, complain if offset -> zero or larger than 6 dps */ - if (fabsf(_gyro_scale.x_offset) > 0.1f || fabsf(_gyro_scale.x_offset) < 0.000001f) + /* evaluate gyro offsets, complain if offset -> zero or larger than 7 dps */ + if (fabsf(_gyro_scale.x_offset) > 0.12f || fabsf(_gyro_scale.x_offset) < 0.000001f) return 1; if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f) return 1; - if (fabsf(_gyro_scale.y_offset) > 0.1f || fabsf(_gyro_scale.y_offset) < 0.000001f) + if (fabsf(_gyro_scale.y_offset) > 0.12f || fabsf(_gyro_scale.y_offset) < 0.000001f) return 1; if (fabsf(_gyro_scale.y_scale - 1.0f) > 0.3f) return 1; - if (fabsf(_gyro_scale.z_offset) > 0.1f || fabsf(_gyro_scale.z_offset) < 0.000001f) + if (fabsf(_gyro_scale.z_offset) > 0.12f || fabsf(_gyro_scale.z_offset) < 0.000001f) return 1; if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f) return 1; @@ -941,6 +941,7 @@ MPU6000::gyro_self_test() } + /* perform a self-test comparison to factory trim values. This takes about 200ms and will return OK if the current values are within 14% From b8fade7dcfa98d0963c7f23529168baa64f7fc1a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Jan 2015 20:04:56 +0100 Subject: [PATCH 200/269] MPU6K: Improve gyro self test to allow more realistic deviations from nominal state --- src/drivers/mpu6000/mpu6000.cpp | 31 ++++++++++++++++++++++--------- 1 file changed, 22 insertions(+), 9 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 81d156af1d..1c35bae2d2 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -921,22 +921,35 @@ MPU6000::gyro_self_test() if (self_test()) return 1; - /* evaluate gyro offsets, complain if offset -> zero or larger than 7 dps */ - if (fabsf(_gyro_scale.x_offset) > 0.12f || fabsf(_gyro_scale.x_offset) < 0.000001f) - return 1; - if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f) + const float max_offset = 0.34f; + const float max_scale = 0.3f; + + /* evaluate gyro offsets, complain if offset -> zero or larger than 20 dps. */ + if (fabsf(_gyro_scale.x_offset) > max_offset) return 1; - if (fabsf(_gyro_scale.y_offset) > 0.12f || fabsf(_gyro_scale.y_offset) < 0.000001f) - return 1; - if (fabsf(_gyro_scale.y_scale - 1.0f) > 0.3f) + /* evaluate gyro scale, complain if off by more than 30% */ + if (fabsf(_gyro_scale.x_scale - 1.0f) > max_scale) return 1; - if (fabsf(_gyro_scale.z_offset) > 0.12f || fabsf(_gyro_scale.z_offset) < 0.000001f) + if (fabsf(_gyro_scale.y_offset) > max_offset) return 1; - if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f) + if (fabsf(_gyro_scale.y_scale - 1.0f) > max_scale) return 1; + if (fabsf(_gyro_scale.z_offset) > max_offset) + return 1; + if (fabsf(_gyro_scale.z_scale - 1.0f) > max_scale) + return 1; + + /* check if all scales are zero */ + if ((fabsf(_gyro_scale.x_offset) < 0.000001f) && + (fabsf(_gyro_scale.y_offset) < 0.000001f) && + (fabsf(_gyro_scale.z_offset) < 0.000001f)) { + /* if all are zero, this device is not calibrated */ + return 1; + } + return 0; } From 52f3fe1d9af53e5f1dd40714aa177fcc4c5e0047 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Jan 2015 20:23:42 +0100 Subject: [PATCH 201/269] Added more docs to offset as suggested by @velocoderaptor, thanks! --- src/drivers/mpu6000/mpu6000.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 1c35bae2d2..2be7582442 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -921,7 +921,17 @@ MPU6000::gyro_self_test() if (self_test()) return 1; + /* + * Maximum deviation of 20 degrees, according to + * http://www.invensense.com/mems/gyro/documents/PS-MPU-6000A-00v3.4.pdf + * Section 6.1, initial ZRO tolerance + */ const float max_offset = 0.34f; + /* 30% scale error is chosen to catch completely faulty units but + * to let some slight scale error pass. Requires a rate table or correlation + * with mag rotations + data fit to + * calibrate properly and is not done by default. + */ const float max_scale = 0.3f; /* evaluate gyro offsets, complain if offset -> zero or larger than 20 dps. */ From e95e09628484875cf65686561eef8434195df96b Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 28 Jan 2015 09:31:42 +0100 Subject: [PATCH 202/269] added references --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 962dc6d4a0..cad6cf3aea 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -35,6 +35,12 @@ * @file mc_pos_control_main.cpp * Multicopter position controller. * + * Original publication for the desired attitude generation: + * Daniel Mellinger and Vijay Kumar. Minimum Snap Trajectory Generation and Control for Quadrotors. + * Int. Conf. on Robotics and Automation, Shanghai, China, May 2011 + * + * Also inspired by https://pixhawk.org/firmware/apps/fw_pos_control_l1 + * * The controller has two loops: P loop for position error and PID loop for velocity error. * Output of velocity controller is thrust vector that splitted to thrust direction * (i.e. rotation matrix for multicopter orientation) and thrust module (i.e. multicopter thrust itself). From 97471bf0aae5c069fe37f063f2a8b5eae08755c9 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 28 Jan 2015 09:31:42 +0100 Subject: [PATCH 203/269] added references --- src/modules/mc_att_control/mc_att_control_main.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index a094ed2c60..c828f1f942 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -35,6 +35,10 @@ * @file mc_att_control_main.cpp * Multicopter attitude controller. * + * Publication for the desired attitude tracking: + * Daniel Mellinger and Vijay Kumar. Minimum Snap Trajectory Generation and Control for Quadrotors. + * Int. Conf. on Robotics and Automation, Shanghai, China, May 2011. + * * @author Tobias Naegeli * @author Lorenz Meier * @author Anton Babushkin From 5444972347606333dd61206011263e4a2e5d83e8 Mon Sep 17 00:00:00 2001 From: hauptmech Date: Mon, 19 Jan 2015 16:50:52 +1300 Subject: [PATCH 204/269] Add call to access the mcu unique id. Expose via the 'ver' command. This is prep for verifying calibration parameters against the hardware they were gathered on. --- src/modules/systemlib/mcu_version.c | 11 +++++++++-- src/modules/systemlib/mcu_version.h | 11 +++++++++++ src/systemcmds/ver/ver.c | 14 +++++++++++++- 3 files changed, 33 insertions(+), 3 deletions(-) diff --git a/src/modules/systemlib/mcu_version.c b/src/modules/systemlib/mcu_version.c index 4bcf957848..24f4e42076 100644 --- a/src/modules/systemlib/mcu_version.c +++ b/src/modules/systemlib/mcu_version.c @@ -47,7 +47,8 @@ #ifdef CONFIG_ARCH_CHIP_STM32 #include -#define DBGMCU_IDCODE 0xE0042000 +#define DBGMCU_IDCODE 0xE0042000 //STM DocID018909 Rev 8 Sect 38.18 (MCU device ID code) +#define UNIQUE_ID 0x1FFF7A10 //STM DocID018909 Rev 8 Sect 39.1 (Unique device ID Register) #define STM32F40x_41x 0x413 #define STM32F42x_43x 0x419 @@ -57,7 +58,13 @@ #endif - +/** Copy the 96bit MCU Unique ID into the provided pointer */ +void mcu_unique_id(uint32_t *uid_96_bit) +{ + uid_96_bit[0] = getreg32(UNIQUE_ID); + uid_96_bit[1] = getreg32(UNIQUE_ID+4); + uid_96_bit[2] = getreg32(UNIQUE_ID+8); +} int mcu_version(char* rev, char** revstr) { diff --git a/src/modules/systemlib/mcu_version.h b/src/modules/systemlib/mcu_version.h index 1b3d0aba9b..c8a0bf5cda 100644 --- a/src/modules/systemlib/mcu_version.h +++ b/src/modules/systemlib/mcu_version.h @@ -33,6 +33,8 @@ #pragma once +#include + /* magic numbers from reference manual */ enum MCU_REV { MCU_REV_STM32F4_REV_A = 0x1000, @@ -42,6 +44,15 @@ enum MCU_REV { MCU_REV_STM32F4_REV_3 = 0x2001 }; + +/** + * Reports the microcontroller unique id. + * + * This ID is guaranteed to be unique for every mcu. + * @param uid_96_bit A uint32_t[3] array to copy the data to. + */ +__EXPORT void mcu_unique_id(uint32_t *uid_96_bit); + /** * Reports the microcontroller version of the main CPU. * diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index 2ead3e6323..087eb52e3d 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -54,6 +54,7 @@ static const char sz_ver_bdate_str[] = "bdate"; static const char sz_ver_gcc_str[] = "gcc"; static const char sz_ver_all_str[] = "all"; static const char mcu_ver_str[] = "mcu"; +static const char mcu_uid_str[] = "uid"; static void usage(const char *reason) { @@ -61,7 +62,7 @@ static void usage(const char *reason) printf("%s\n", reason); } - printf("usage: ver {hw|hwcmp|git|bdate|gcc|all|mcu}\n\n"); + printf("usage: ver {hw|hwcmp|git|bdate|gcc|all|mcu|uid}\n\n"); } __EXPORT int ver_main(int argc, char *argv[]); @@ -141,6 +142,17 @@ int ver_main(int argc, char *argv[]) ret = 0; } + if (show_all || !strncmp(argv[1], mcu_uid_str, sizeof(mcu_uid_str))) { + uint32_t uid[3]; + + mcu_unique_id(uid); + + printf("UID: %X:%X:%X \n",uid[0],uid[1],uid[2]); + + ret = 0; + } + + if (ret == 1) { errx(1, "unknown command.\n"); } From 21d3dcb1905432e349f70012ac0a4f79e5e7935a Mon Sep 17 00:00:00 2001 From: hauptmech Date: Wed, 28 Jan 2015 15:40:44 +1300 Subject: [PATCH 205/269] Fix config utility to work with all devices of each type, not just the primary one. --- src/systemcmds/config/config.c | 70 +++++++++++++++++----------------- 1 file changed, 35 insertions(+), 35 deletions(-) diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 077bc47c90..9f13edb184 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -36,7 +36,7 @@ * @author Lorenz Meier * @author Julian Oes * - * config tool. + * config tool. Takes the device name as the first parameter. */ #include @@ -71,18 +71,18 @@ int config_main(int argc, char *argv[]) { if (argc >= 2) { - if (!strcmp(argv[1], "gyro")) { - do_gyro(argc - 2, argv + 2); - } else if (!strcmp(argv[1], "accel")) { - do_accel(argc - 2, argv + 2); - } else if (!strcmp(argv[1], "mag")) { - do_mag(argc - 2, argv + 2); + if (!strncmp(argv[1], "/dev/gyro",9)) { + do_gyro(argc - 1, argv + 1); + } else if (!strncmp(argv[1], "/dev/accel",10)) { + do_accel(argc - 1, argv + 1); + } else if (!strncmp(argv[1], "/dev/mag",8)) { + do_mag(argc - 1, argv + 1); } else { do_device(argc - 1, argv + 1); } } - errx(1, "expected a command, try 'gyro', 'accel', 'mag'"); + errx(1, "expected a device, try '/dev/gyro', '/dev/accel', '/dev/mag'"); } static void @@ -133,41 +133,41 @@ do_gyro(int argc, char *argv[]) { int fd; - fd = open(GYRO_DEVICE_PATH, 0); + fd = open(argv[0], 0); if (fd < 0) { - warn("%s", GYRO_DEVICE_PATH); + warn("%s", argv[0]); errx(1, "FATAL: no gyro found"); } else { int ret; - if (argc == 2 && !strcmp(argv[0], "sampling")) { + if (argc == 3 && !strcmp(argv[1], "sampling")) { /* set the gyro internal sampling rate up to at least i Hz */ - ret = ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[2], NULL, 0)); if (ret) errx(ret,"sampling rate could not be set"); - } else if (argc == 2 && !strcmp(argv[0], "rate")) { + } else if (argc == 3 && !strcmp(argv[1], "rate")) { /* set the driver to poll at i Hz */ - ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0)); if (ret) errx(ret,"pollrate could not be set"); - } else if (argc == 2 && !strcmp(argv[0], "range")) { + } else if (argc == 3 && !strcmp(argv[1], "range")) { /* set the range to i dps */ - ret = ioctl(fd, GYROIOCSRANGE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, GYROIOCSRANGE, strtoul(argv[2], NULL, 0)); if (ret) errx(ret,"range could not be set"); - } else if (argc == 1 && !strcmp(argv[0], "check")) { + } else if (argc == 2 && !strcmp(argv[1], "check")) { ret = ioctl(fd, GYROIOCSELFTEST, 0); if (ret) { @@ -206,41 +206,41 @@ do_mag(int argc, char *argv[]) { int fd; - fd = open(MAG_DEVICE_PATH, 0); + fd = open(argv[0], 0); if (fd < 0) { - warn("%s", MAG_DEVICE_PATH); + warn("%s", argv[0]); errx(1, "FATAL: no magnetometer found"); } else { int ret; - if (argc == 2 && !strcmp(argv[0], "sampling")) { + if (argc == 3 && !strcmp(argv[1], "sampling")) { /* set the mag internal sampling rate up to at least i Hz */ - ret = ioctl(fd, MAGIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, MAGIOCSSAMPLERATE, strtoul(argv[2], NULL, 0)); if (ret) errx(ret,"sampling rate could not be set"); - } else if (argc == 2 && !strcmp(argv[0], "rate")) { + } else if (argc == 3 && !strcmp(argv[1], "rate")) { /* set the driver to poll at i Hz */ - ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0)); if (ret) errx(ret,"pollrate could not be set"); - } else if (argc == 2 && !strcmp(argv[0], "range")) { + } else if (argc == 3 && !strcmp(argv[1], "range")) { /* set the range to i G */ - ret = ioctl(fd, MAGIOCSRANGE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, MAGIOCSRANGE, strtoul(argv[2], NULL, 0)); if (ret) errx(ret,"range could not be set"); - } else if(argc == 1 && !strcmp(argv[0], "check")) { + } else if(argc == 2 && !strcmp(argv[1], "check")) { ret = ioctl(fd, MAGIOCSELFTEST, 0); if (ret) { @@ -282,41 +282,41 @@ do_accel(int argc, char *argv[]) { int fd; - fd = open(ACCEL_DEVICE_PATH, 0); + fd = open(argv[0], 0); if (fd < 0) { - warn("%s", ACCEL_DEVICE_PATH); + warn("%s", argv[0]); errx(1, "FATAL: no accelerometer found"); } else { int ret; - if (argc == 2 && !strcmp(argv[0], "sampling")) { + if (argc == 3 && !strcmp(argv[1], "sampling")) { /* set the accel internal sampling rate up to at least i Hz */ - ret = ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[2], NULL, 0)); if (ret) errx(ret,"sampling rate could not be set"); - } else if (argc == 2 && !strcmp(argv[0], "rate")) { + } else if (argc == 3 && !strcmp(argv[1], "rate")) { /* set the driver to poll at i Hz */ - ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0)); if (ret) errx(ret,"pollrate could not be set"); - } else if (argc == 2 && !strcmp(argv[0], "range")) { + } else if (argc == 3 && !strcmp(argv[1], "range")) { /* set the range to i G */ - ret = ioctl(fd, ACCELIOCSRANGE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, ACCELIOCSRANGE, strtoul(argv[2], NULL, 0)); if (ret) errx(ret,"range could not be set"); - } else if(argc == 1 && !strcmp(argv[0], "check")) { + } else if(argc == 2 && !strcmp(argv[1], "check")) { ret = ioctl(fd, ACCELIOCSELFTEST, 0); if (ret) { From a3b2c99801a2cdc780cce77e04dd2d924c2c0560 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Tue, 27 Jan 2015 12:37:35 -0500 Subject: [PATCH 206/269] A huge developer time saver, J="" make archives This gives warnings about -j1 being forced some places, but it still successfully builds all archvies in parallel. The resulting archives have been tested on the board. It is disabled by default so no functional change unless someone adds J="" or J=8 in front of the make archvies. --- Makefile | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/Makefile b/Makefile index 64c468d31b..b178597056 100644 --- a/Makefile +++ b/Makefile @@ -169,16 +169,18 @@ ifneq ($(filter archives,$(MAKECMDGOALS)),) .NOTPARALLEL: endif +J?=1 + $(ARCHIVE_DIR)%.export: board = $(notdir $(basename $@)) $(ARCHIVE_DIR)%.export: configuration = nsh $(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) @$(ECHO) %% Configuring NuttX for $(board) $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) - $(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) distclean $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(board) .) $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(board)/$(configuration)) @$(ECHO) %% Exporting NuttX for $(board) - $(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) CONFIG_ARCH_BOARD=$(board) export + $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) CONFIG_ARCH_BOARD=$(board) export $(Q) $(MKDIR) -p $(dir $@) $(Q) $(COPY) $(NUTTX_SRC)nuttx-export.zip $@ $(Q) (cd $(NUTTX_SRC)/configs && $(RMDIR) $(board)) @@ -195,11 +197,11 @@ BOARD = $(BOARDS) menuconfig: $(NUTTX_SRC) @$(ECHO) %% Configuring NuttX for $(BOARD) $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) - $(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) distclean $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(BOARD) .) $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(BOARD)/nsh) @$(ECHO) %% Running menuconfig for $(BOARD) - $(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) menuconfig + $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) menuconfig @$(ECHO) %% Saving configuration file $(Q)$(COPY) $(NUTTX_SRC).config $(PX4_BASE)nuttx-configs/$(BOARD)/nsh/defconfig else From 27b2701340648e2fde1992b175abfa591e0eee01 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 Jan 2015 09:48:53 +0100 Subject: [PATCH 207/269] temporarily re-enable stack checking, disable some modules to make firmware fit --- makefiles/config_px4fmu-v2_default.mk | 10 +++++----- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 3abebd82fa..76457216bd 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -37,11 +37,11 @@ MODULES += drivers/hil # MODULES += drivers/hott/hott_sensors # MODULES += drivers/blinkm MODULES += drivers/airspeed -MODULES += drivers/ets_airspeed +# MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed -MODULES += drivers/frsky_telemetry +# MODULES += drivers/frsky_telemetry MODULES += modules/sensors -MODULES += drivers/mkblctrl +# MODULES += drivers/mkblctrl MODULES += drivers/px4flow # @@ -70,7 +70,7 @@ MODULES += modules/commander MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/gpio_led -MODULES += modules/uavcan +# MODULES += modules/uavcan MODULES += modules/land_detector # @@ -120,7 +120,7 @@ MODULES += lib/launchdetection # # OBC challenge # -MODULES += modules/bottle_drop +# MODULES += modules/bottle_drop # # Demo apps diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index dedebdfa03..9030a1f022 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -117,7 +117,7 @@ CONFIG_ARCH_HAVE_MPU=y # # CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y -CONFIG_ARMV7M_STACKCHECK=n +CONFIG_ARMV7M_STACKCHECK=y CONFIG_SERIAL_TERMIOS=y CONFIG_SDIO_DMA=y CONFIG_SDIO_DMAPRIO=0x00010000 From a25dbb3c8e795b90869c480ae5c62c84b8424b3c Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Wed, 28 Jan 2015 11:19:52 +0100 Subject: [PATCH 208/269] Add a compiler coloring tool to highlight warning and errors in the output. --- Tools/make_color.sh | 36 ++++++++++++++++++++++++++++++++++++ 1 file changed, 36 insertions(+) create mode 100755 Tools/make_color.sh diff --git a/Tools/make_color.sh b/Tools/make_color.sh new file mode 100755 index 0000000000..81316a9328 --- /dev/null +++ b/Tools/make_color.sh @@ -0,0 +1,36 @@ +#!/bin/sh +# make_color.sh +# +# Author: Simon Wilks (simon@uaventure.com) +# +# A compiler color coder. +# +# To invoke this script everytime you run make simply create the alias: +# +# alias make='/Tools/make_color.sh' +# +# Color codes: +# +# white "\033[1,37m" +# yellow "\033[1,33m" +# green "\033[1,32m" +# blue "\033[1,34m" +# cyan "\033[1,36m" +# red "\033[1,31m" +# magenta "\033[1,35m" +# black "\033[1,30m" +# darkwhite "\033[0,37m" +# darkyellow "\033[0,33m" +# darkgreen "\033[0,32m" +# darkblue "\033[0,34m" +# darkcyan "\033[0,36m" +# darkred "\033[0,31m" +# darkmagenta "\033[0,35m" +# off "\033[0,0m" +# +OFF="\o033[0m" +WARN="\o033[1;33m" +ERROR="\o033[1;31m" +INFO="\o033[0;37m" + +make ${@} 2>&1 | sed "s/make\[[0-9]\].*/$INFO & $OFF/;s/.*: warning: .*/$WARN & $OFF/;s/.*: error: .*/$ERROR & $OFF/" From 856b10cc1a5a36a7b7d2978477185155792366c2 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 Jan 2015 16:29:25 +0100 Subject: [PATCH 209/269] Revert "temporarily re-enable stack checking, disable some modules to make firmware fit" This reverts commit 27b2701340648e2fde1992b175abfa591e0eee01. --- makefiles/config_px4fmu-v2_default.mk | 10 +++++----- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 5d6beffd7e..2ce7388446 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -37,11 +37,11 @@ MODULES += drivers/gps # MODULES += drivers/hott/hott_sensors # MODULES += drivers/blinkm MODULES += drivers/airspeed -# MODULES += drivers/ets_airspeed +MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed -# MODULES += drivers/frsky_telemetry +MODULES += drivers/frsky_telemetry MODULES += modules/sensors -# MODULES += drivers/mkblctrl +MODULES += drivers/mkblctrl MODULES += drivers/px4flow # @@ -70,7 +70,7 @@ MODULES += modules/commander MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/gpio_led -# MODULES += modules/uavcan +MODULES += modules/uavcan MODULES += modules/land_detector # @@ -121,7 +121,7 @@ MODULES += platforms/nuttx # # OBC challenge # -# MODULES += modules/bottle_drop +MODULES += modules/bottle_drop # # Demo apps diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 9030a1f022..dedebdfa03 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -117,7 +117,7 @@ CONFIG_ARCH_HAVE_MPU=y # # CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y -CONFIG_ARMV7M_STACKCHECK=y +CONFIG_ARMV7M_STACKCHECK=n CONFIG_SERIAL_TERMIOS=y CONFIG_SDIO_DMA=y CONFIG_SDIO_DMAPRIO=0x00010000 From c67cb25f9a4cb4ec507029865dd70837417ef894 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 Jan 2015 07:54:58 +0100 Subject: [PATCH 210/269] port more uorb headers to msg --- msg/position_setpoint.msg | 35 ++++++ msg/position_setpoint_triplet.msg | 8 ++ msg/vehicle_global_velocity_setpoint.msg | 4 + msg/vehicle_local_position.msg | 36 ++++++ msg/vehicle_local_position_setpoint.msg | 7 ++ .../uORB/topics/position_setpoint_triplet.h | 116 ------------------ .../topics/vehicle_global_velocity_setpoint.h | 63 ---------- .../uORB/topics/vehicle_local_position.h | 96 --------------- .../topics/vehicle_local_position_setpoint.h | 67 ---------- 9 files changed, 90 insertions(+), 342 deletions(-) create mode 100644 msg/position_setpoint.msg create mode 100644 msg/position_setpoint_triplet.msg create mode 100644 msg/vehicle_global_velocity_setpoint.msg create mode 100644 msg/vehicle_local_position.msg create mode 100644 msg/vehicle_local_position_setpoint.msg delete mode 100644 src/modules/uORB/topics/position_setpoint_triplet.h delete mode 100644 src/modules/uORB/topics/vehicle_global_velocity_setpoint.h delete mode 100644 src/modules/uORB/topics/vehicle_local_position.h delete mode 100644 src/modules/uORB/topics/vehicle_local_position_setpoint.h diff --git a/msg/position_setpoint.msg b/msg/position_setpoint.msg new file mode 100644 index 0000000000..ae6756aa8b --- /dev/null +++ b/msg/position_setpoint.msg @@ -0,0 +1,35 @@ +# this file is only used in the position_setpoint triple as a dependency + +uint8 SETPOINT_TYPE_POSITION=0 # position setpoint +uint8 SETPOINT_TYPE_VELOCITY=1 # velocity setpoint +uint8 SETPOINT_TYPE_LOITER=2 # loiter setpoint +uint8 SETPOINT_TYPE_TAKEOFF=3 # takeoff setpoint +uint8 SETPOINT_TYPE_LAND=4 # land setpoint, altitude must be ignored, descend until landing +uint8 SETPOINT_TYPE_IDLE=5 # do nothing, switch off motors or keep at idle speed (MC) +uint8 SETPOINT_TYPE_OFFBOARD=6 # setpoint in NED frame (x, y, z, vx, vy, vz) set by offboard + +bool valid # true if setpoint is valid +uint8 type # setpoint type to adjust behavior of position controller +float32 x # local position setpoint in m in NED +float32 y # local position setpoint in m in NED +float32 z # local position setpoint in m in NED +bool position_valid # true if local position setpoint valid +float32 vx # local velocity setpoint in m/s in NED +float32 vy # local velocity setpoint in m/s in NED +float32 vz # local velocity setpoint in m/s in NED +bool velocity_valid # true if local velocity setpoint valid +float64 lat # latitude, in deg +float64 lon # longitude, in deg +float32 alt # altitude AMSL, in m +float32 yaw # yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw +bool yaw_valid # true if yaw setpoint valid +float32 yawspeed # yawspeed (only for multirotors, in rad/s) +bool yawspeed_valid # true if yawspeed setpoint valid +float32 loiter_radius # loiter radius (only for fixed wing), in m +int8 loiter_direction # loiter direction: 1 = CW, -1 = CCW +float32 pitch_min # minimal pitch angle for fixed wing takeoff waypoints +float32 a_x # acceleration x setpoint +float32 a_y # acceleration y setpoint +float32 a_z # acceleration z setpoint +bool acceleration_valid # true if acceleration setpoint is valid/should be used +bool acceleration_is_force # interprete acceleration as force diff --git a/msg/position_setpoint_triplet.msg b/msg/position_setpoint_triplet.msg new file mode 100644 index 0000000000..8717f65d0f --- /dev/null +++ b/msg/position_setpoint_triplet.msg @@ -0,0 +1,8 @@ +# Global position setpoint triplet in WGS84 coordinates. +# This are the three next waypoints (or just the next two or one). + +px4/position_setpoint previous +px4/position_setpoint current +px4/position_setpoint next + +uint8 nav_state # report the navigation state diff --git a/msg/vehicle_global_velocity_setpoint.msg b/msg/vehicle_global_velocity_setpoint.msg new file mode 100644 index 0000000000..ca7dcc826a --- /dev/null +++ b/msg/vehicle_global_velocity_setpoint.msg @@ -0,0 +1,4 @@ +# Velocity setpoint in NED frame +float32 vx # in m/s NED +float32 vy # in m/s NED +float32 vz # in m/s NED diff --git a/msg/vehicle_local_position.msg b/msg/vehicle_local_position.msg new file mode 100644 index 0000000000..4da027ae77 --- /dev/null +++ b/msg/vehicle_local_position.msg @@ -0,0 +1,36 @@ +# Fused local position in NED. + +uint64 timestamp # Time of this estimate, in microseconds since system start +bool xy_valid # true if x and y are valid +bool z_valid # true if z is valid +bool v_xy_valid # true if vy and vy are valid +bool v_z_valid # true if vz is valid + +# Position in local NED frame +float32 x # X position in meters in NED earth-fixed frame +float32 y # X position in meters in NED earth-fixed frame +float32 z # Z position in meters in NED earth-fixed frame (negative altitude) + +# Velocity in NED frame +float32 vx # Ground X Speed (Latitude), m/s in NED +float32 vy # Ground Y Speed (Longitude), m/s in NED +float32 vz # Ground Z Speed (Altitude), m/s in NED + +# Heading +float32 yaw + +# Reference position in GPS / WGS84 frame +bool xy_global # true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) +bool z_global # true if z is valid and has valid global reference (ref_alt) +uint64 ref_timestamp # Time when reference position was set +float64 ref_lat # Reference point latitude in degrees +float64 ref_lon # Reference point longitude in degrees +float32 ref_alt # Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level + +# Distance to surface +float32 dist_bottom # Distance to bottom surface (ground) +float32 dist_bottom_rate # Distance to bottom surface (ground) change rate +uint64 surface_bottom_timestamp # Time when new bottom surface found +bool dist_bottom_valid # true if distance to bottom surface is valid +float32 eph +float32 epv diff --git a/msg/vehicle_local_position_setpoint.msg b/msg/vehicle_local_position_setpoint.msg new file mode 100644 index 0000000000..a2ff8a4ae7 --- /dev/null +++ b/msg/vehicle_local_position_setpoint.msg @@ -0,0 +1,7 @@ +# Local position in NED frame + +uint64 timestamp # timestamp of the setpoint +float32 x # in meters NED +float32 y # in meters NED +float32 z # in meters NED +float32 yaw # in radians NED -PI..+PI diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h deleted file mode 100644 index cb2262534d..0000000000 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ /dev/null @@ -1,116 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file mission_item_triplet.h - * Definition of the global WGS84 position setpoint uORB topic. - * - * @author Thomas Gubler - * @author Julian Oes - * @author Lorenz Meier - */ - -#ifndef TOPIC_MISSION_ITEM_TRIPLET_H_ -#define TOPIC_MISSION_ITEM_TRIPLET_H_ - -#include -#include -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -enum SETPOINT_TYPE -{ - SETPOINT_TYPE_POSITION = 0, /**< position setpoint */ - SETPOINT_TYPE_VELOCITY, /**< velocity setpoint */ - SETPOINT_TYPE_LOITER, /**< loiter setpoint */ - SETPOINT_TYPE_TAKEOFF, /**< takeoff setpoint */ - SETPOINT_TYPE_LAND, /**< land setpoint, altitude must be ignored, descend until landing */ - SETPOINT_TYPE_IDLE, /**< do nothing, switch off motors or keep at idle speed (MC) */ - SETPOINT_TYPE_OFFBOARD, /**< setpoint in NED frame (x, y, z, vx, vy, vz) set by offboard */ -}; - -struct position_setpoint_s -{ - bool valid; /**< true if setpoint is valid */ - enum SETPOINT_TYPE type; /**< setpoint type to adjust behavior of position controller */ - float x; /**< local position setpoint in m in NED */ - float y; /**< local position setpoint in m in NED */ - float z; /**< local position setpoint in m in NED */ - bool position_valid; /**< true if local position setpoint valid */ - float vx; /**< local velocity setpoint in m/s in NED */ - float vy; /**< local velocity setpoint in m/s in NED */ - float vz; /**< local velocity setpoint in m/s in NED */ - bool velocity_valid; /**< true if local velocity setpoint valid */ - double lat; /**< latitude, in deg */ - double lon; /**< longitude, in deg */ - float alt; /**< altitude AMSL, in m */ - float yaw; /**< yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw */ - bool yaw_valid; /**< true if yaw setpoint valid */ - float yawspeed; /**< yawspeed (only for multirotors, in rad/s) */ - bool yawspeed_valid; /**< true if yawspeed setpoint valid */ - float loiter_radius; /**< loiter radius (only for fixed wing), in m */ - int8_t loiter_direction; /**< loiter direction: 1 = CW, -1 = CCW */ - float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ - float a_x; //**< acceleration x setpoint */ - float a_y; //**< acceleration y setpoint */ - float a_z; //**< acceleration z setpoint */ - bool acceleration_valid; //*< true if acceleration setpoint is valid/should be used */ - bool acceleration_is_force; //*< interprete acceleration as force */ -}; - -/** - * Global position setpoint triplet in WGS84 coordinates. - * - * This are the three next waypoints (or just the next two or one). - */ -struct position_setpoint_triplet_s -{ - struct position_setpoint_s previous; - struct position_setpoint_s current; - struct position_setpoint_s next; - - unsigned nav_state; /**< report the navigation state */ -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(position_setpoint_triplet); - -#endif diff --git a/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h b/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h deleted file mode 100644 index 5dac877d08..0000000000 --- a/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h +++ /dev/null @@ -1,63 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: @author Anton Babushkin - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file vehicle_global_velocity_setpoint.h - * Definition of the global velocity setpoint uORB topic. - */ - -#ifndef TOPIC_VEHICLE_GLOBAL_VELOCITY_SETPOINT_H_ -#define TOPIC_VEHICLE_GLOBAL_VELOCITY_SETPOINT_H_ - -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -struct vehicle_global_velocity_setpoint_s { - float vx; /**< in m/s NED */ - float vy; /**< in m/s NED */ - float vz; /**< in m/s NED */ -}; /**< Velocity setpoint in NED frame */ - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_global_velocity_setpoint); - -#endif diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h deleted file mode 100644 index 8b46c5a3f3..0000000000 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ /dev/null @@ -1,96 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file vehicle_local_position.h - * Definition of the local fused NED position uORB topic. - * - * @author Lorenz Meier - * @author Anton Babushkin - */ - -#ifndef TOPIC_VEHICLE_LOCAL_POSITION_H_ -#define TOPIC_VEHICLE_LOCAL_POSITION_H_ - -#include -#include -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -/** - * Fused local position in NED. - */ -struct vehicle_local_position_s { - uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */ - bool xy_valid; /**< true if x and y are valid */ - bool z_valid; /**< true if z is valid */ - bool v_xy_valid; /**< true if vy and vy are valid */ - bool v_z_valid; /**< true if vz is valid */ - /* Position in local NED frame */ - float x; /**< X position in meters in NED earth-fixed frame */ - float y; /**< X position in meters in NED earth-fixed frame */ - float z; /**< Z position in meters in NED earth-fixed frame (negative altitude) */ - /* Velocity in NED frame */ - float vx; /**< Ground X Speed (Latitude), m/s in NED */ - float vy; /**< Ground Y Speed (Longitude), m/s in NED */ - float vz; /**< Ground Z Speed (Altitude), m/s in NED */ - /* Heading */ - float yaw; - /* Reference position in GPS / WGS84 frame */ - bool xy_global; /**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */ - bool z_global; /**< true if z is valid and has valid global reference (ref_alt) */ - uint64_t ref_timestamp; /**< Time when reference position was set */ - double ref_lat; /**< Reference point latitude in degrees */ - double ref_lon; /**< Reference point longitude in degrees */ - float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */ - /* Distance to surface */ - float dist_bottom; /**< Distance to bottom surface (ground) */ - float dist_bottom_rate; /**< Distance to bottom surface (ground) change rate */ - uint64_t surface_bottom_timestamp; /**< Time when new bottom surface found */ - bool dist_bottom_valid; /**< true if distance to bottom surface is valid */ - float eph; - float epv; -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_local_position); - -#endif diff --git a/src/modules/uORB/topics/vehicle_local_position_setpoint.h b/src/modules/uORB/topics/vehicle_local_position_setpoint.h deleted file mode 100644 index 6766bb58a9..0000000000 --- a/src/modules/uORB/topics/vehicle_local_position_setpoint.h +++ /dev/null @@ -1,67 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler - * @author Julian Oes - * @author Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file vehicle_local_position_setpoint.h - * Definition of the local NED position setpoint uORB topic. - */ - -#ifndef TOPIC_VEHICLE_LOCAL_POSITION_SETPOINT_H_ -#define TOPIC_VEHICLE_LOCAL_POSITION_SETPOINT_H_ - -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -struct vehicle_local_position_setpoint_s { - uint64_t timestamp; /**< timestamp of the setpoint */ - float x; /**< in meters NED */ - float y; /**< in meters NED */ - float z; /**< in meters NED */ - float yaw; /**< in radians NED -PI..+PI */ -}; /**< Local position in NED frame */ - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_local_position_setpoint); - -#endif From 01835a51a8a3a0b0f7e7362cdc25475bd029a9a8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 Jan 2015 07:55:21 +0100 Subject: [PATCH 211/269] uorb constants are now defined inside class --- msg/templates/msg.h.template | 159 ------------------------------ msg/templates/uorb/msg.h.template | 23 ++++- 2 files changed, 21 insertions(+), 161 deletions(-) delete mode 100644 msg/templates/msg.h.template diff --git a/msg/templates/msg.h.template b/msg/templates/msg.h.template deleted file mode 100644 index cc128c1ea2..0000000000 --- a/msg/templates/msg.h.template +++ /dev/null @@ -1,159 +0,0 @@ -@############################################### -@# -@# PX4 ROS compatible message source code -@# generation for C++ -@# -@# EmPy template for generating .h files -@# Based on the original template for ROS -@# -@############################################### -@# Start of Template -@# -@# Context: -@# - file_name_in (String) Source file -@# - spec (msggen.MsgSpec) Parsed specification of the .msg file -@# - md5sum (String) MD5Sum of the .msg specification -@############################################### -/**************************************************************************** - * - * Copyright (C) 2013-2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - - /* Auto-generated by genmsg_cpp from file @file_name_in */ - -@{ -import genmsg.msgs -import gencpp - -cpp_namespace = '::%s::'%(spec.package) # TODO handle nested namespace -cpp_class = '%s_'%spec.short_name -cpp_full_name = '%s%s'%(cpp_namespace,cpp_class) -cpp_full_name_with_alloc = '%s'%(cpp_full_name) -cpp_msg_definition = gencpp.escape_message_definition(msg_definition) -}@ - -#pragma once - -@############################## -@# Generic Includes -@############################## -#include -#include "../uORB.h" - -@############################## -@# Includes for dependencies -@############################## -@{ -for field in spec.parsed_fields(): - if (not field.is_builtin): - if (not field.is_header): - (package, name) = genmsg.names.package_resource_name(field.base_type) - package = package or spec.package # convert '' to package - print('#include '%(name)) - -}@ -@# Constants -@[for constant in spec.constants]@ -#define @(constant.name) @(int(constant.val)) -@[end for] - -/** - * @@addtogroup topics - * @@{ - */ - -@############################## -@# Main struct of message -@############################## -@{ - -type_map = {'int8': 'int8_t', - 'int16': 'int16_t', - 'int32': 'int32_t', - 'int64': 'int64_t', - 'uint8': 'uint8_t', - 'uint16': 'uint16_t', - 'uint32': 'uint32_t', - 'uint64': 'uint64_t', - 'float32': 'float', - 'bool': 'bool', - 'fence_vertex': 'fence_vertex'} - -# Function to print a standard ros type -def print_field_def(field): - type = field.type - # detect embedded types - sl_pos = type.find('/') - type_appendix = '' - type_prefix = '' - if (sl_pos >= 0): - type = type[sl_pos + 1:] - type_prefix = 'struct ' - type_appendix = '_s' - - # detect arrays - a_pos = type.find('[') - array_size = '' - if (a_pos >= 0): - # field is array - array_size = type[a_pos:] - type = type[:a_pos] - - if type in type_map: - # need to add _t: int8 --> int8_t - type_px4 = type_map[type] - else: - raise Exception("Type {0} not supported, add to to template file!".format(type)) - - print('\t%s%s%s %s%s;'%(type_prefix, type_px4, type_appendix, field.name, array_size)) - -} -#ifdef __cplusplus -@#class @(spec.short_name)_s { -struct __EXPORT @(spec.short_name)_s { -@#public: -#else -struct @(spec.short_name)_s { -#endif -@{ -# loop over all fields and print the type and name -for field in spec.parsed_fields(): - if (not field.is_header): - print_field_def(field) -}@ -}; - -/** - * @@} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(@(spec.short_name)); diff --git a/msg/templates/uorb/msg.h.template b/msg/templates/uorb/msg.h.template index 622641617f..f16095c978 100644 --- a/msg/templates/uorb/msg.h.template +++ b/msg/templates/uorb/msg.h.template @@ -77,10 +77,13 @@ for field in spec.parsed_fields(): print('#include '%(name)) }@ -@# Constants + +@# Constants c style +#ifndef __cplusplus @[for constant in spec.constants]@ #define @(constant.name) @(int(constant.val)) @[end for] +#endif /** * @@addtogroup topics @@ -101,8 +104,10 @@ type_map = {'int8': 'int8_t', 'uint32': 'uint32_t', 'uint64': 'uint64_t', 'float32': 'float', + 'float64': 'double', 'bool': 'bool', - 'fence_vertex': 'fence_vertex'} + 'fence_vertex': 'fence_vertex', + 'position_setpoint': 'position_setpoint'} # Function to print a standard ros type def print_field_def(field): @@ -146,6 +151,20 @@ for field in spec.parsed_fields(): if (not field.is_header): print_field_def(field) }@ +#ifdef __cplusplus +@# Constants again c++-ified +@{ +for constant in spec.constants: + type = constant.type + if type in type_map: + # need to add _t: int8 --> int8_t + type_px4 = type_map[type] + else: + raise Exception("Type {0} not supported, add to to template file!".format(type)) + + print('\tstatic const %s %s = %s;'%(type_px4, constant.name, int(constant.val))) +} +#endif }; /** From 2d124852c1881d5b993b3c2ec9f7a79e1e03da1b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 Jan 2015 07:58:42 +0100 Subject: [PATCH 212/269] propagate uorb contants change through all modules/drivers --- src/drivers/blinkm/blinkm.cpp | 12 +- src/drivers/px4fmu/fmu.cpp | 16 +- src/modules/commander/commander.cpp | 296 +++++++++--------- src/modules/commander/commander_helper.cpp | 12 +- .../commander/state_machine_helper.cpp | 190 +++++------ .../ekf_att_pos_estimator_main.cpp | 4 +- .../fw_pos_control_l1_main.cpp | 18 +- src/modules/mavlink/mavlink_main.cpp | 2 +- src/modules/mavlink/mavlink_messages.cpp | 56 ++-- src/modules/mavlink/mavlink_receiver.cpp | 6 +- .../mc_pos_control/mc_pos_control_main.cpp | 8 +- src/modules/navigator/loiter.cpp | 2 +- src/modules/navigator/mission.cpp | 4 +- src/modules/navigator/mission_block.cpp | 10 +- src/modules/navigator/navigator_main.cpp | 30 +- src/modules/sensors/sensors.cpp | 86 ++--- 16 files changed, 376 insertions(+), 376 deletions(-) diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 6b14f5945a..d0253a8d10 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -571,7 +571,7 @@ BlinkM::led() printf(" cells found:%d\n", num_of_cells); } else { - if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { + if(vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) { /* LED Pattern for battery critical alerting */ led_color_1 = LED_RED; led_color_2 = LED_RED; @@ -595,7 +595,7 @@ BlinkM::led() led_color_8 = LED_BLUE; led_blink = LED_BLINK; - } else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_LOW) { + } else if(vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW) { /* LED Pattern for battery low warning */ led_color_1 = LED_YELLOW; led_color_2 = LED_YELLOW; @@ -647,14 +647,14 @@ BlinkM::led() if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) { /* indicate main control state */ - if (vehicle_status_raw.main_state == MAIN_STATE_POSCTL) + if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_POSCTL) led_color_4 = LED_GREEN; /* TODO: add other Auto modes */ - else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO_MISSION) + else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_AUTO_MISSION) led_color_4 = LED_BLUE; - else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTL) + else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_ALTCTL) led_color_4 = LED_YELLOW; - else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL) + else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_MANUAL) led_color_4 = LED_WHITE; else led_color_4 = LED_OFF; diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 2552e7e6a6..db6b34f999 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -142,11 +142,11 @@ private: uint32_t _groups_required; uint32_t _groups_subscribed; - int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS]; - actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS]; - orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS]; + int _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; + actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; + orb_id_t _control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; orb_id_t _actuator_output_topic; - pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS]; + pollfd _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; unsigned _poll_fds_num; pwm_limit_t _pwm_limit; @@ -514,7 +514,7 @@ PX4FMU::subscribe() uint32_t sub_groups = _groups_required & ~_groups_subscribed; uint32_t unsub_groups = _groups_subscribed & ~_groups_required; _poll_fds_num = 0; - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (sub_groups & (1 << i)) { warnx("subscribe to actuator_controls_%d", i); _control_subs[i] = orb_subscribe(_control_topics[i]); @@ -591,7 +591,7 @@ PX4FMU::task_main() } debug("adjusted actuator update interval to %ums", update_rate_in_ms); - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { orb_set_interval(_control_subs[i], update_rate_in_ms); } @@ -618,7 +618,7 @@ PX4FMU::task_main() /* get controls for required topics */ unsigned poll_id = 0; - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { if (_poll_fds[poll_id].revents & POLLIN) { orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); @@ -751,7 +751,7 @@ PX4FMU::task_main() } - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { ::close(_control_subs[i]); _control_subs[i] = -1; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a1c4e205d4..8f491acae7 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -368,31 +368,31 @@ void print_status() const char *armed_str; switch (state.arming_state) { - case ARMING_STATE_INIT: + case vehicle_status_s::ARMING_STATE_INIT: armed_str = "INIT"; break; - case ARMING_STATE_STANDBY: + case vehicle_status_s::ARMING_STATE_STANDBY: armed_str = "STANDBY"; break; - case ARMING_STATE_ARMED: + case vehicle_status_s::ARMING_STATE_ARMED: armed_str = "ARMED"; break; - case ARMING_STATE_ARMED_ERROR: + case vehicle_status_s::ARMING_STATE_ARMED_ERROR: armed_str = "ARMED_ERROR"; break; - case ARMING_STATE_STANDBY_ERROR: + case vehicle_status_s::ARMING_STATE_STANDBY_ERROR: armed_str = "STANDBY_ERROR"; break; - case ARMING_STATE_REBOOT: + case vehicle_status_s::ARMING_STATE_REBOOT: armed_str = "REBOOT"; break; - case ARMING_STATE_IN_AIR_RESTORE: + case vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE: armed_str = "IN_AIR_RESTORE"; break; @@ -415,7 +415,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char // Transition the armed state. By passing mavlink_fd to arming_state_transition it will // output appropriate error messages if the state cannot transition. - arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, + arming_res = arming_state_transition(&status, &safety, arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd_local); if (arming_res == TRANSITION_CHANGED && mavlink_fd) { @@ -452,7 +452,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s transition_result_t main_ret = TRANSITION_NOT_CHANGED; /* set HIL state */ - hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; + hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? vehicle_status_s::HIL_STATE_ON : vehicle_status_s::HIL_STATE_OFF; transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status_local, mavlink_fd); // Transition the arming state @@ -462,43 +462,43 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s /* use autopilot-specific mode */ if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { /* MANUAL */ - main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL); + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) { /* ALTCTL */ - main_ret = main_state_transition(status_local, MAIN_STATE_ALTCTL); + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ALTCTL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) { /* POSCTL */ - main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL); + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ - main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION); + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) { /* ACRO */ - main_ret = main_state_transition(status_local, MAIN_STATE_ACRO); + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ACRO); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) { /* OFFBOARD */ - main_ret = main_state_transition(status_local, MAIN_STATE_OFFBOARD); + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_OFFBOARD); } } else { /* use base mode */ if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { /* AUTO */ - main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION); + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION); } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { /* POSCTL */ - main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL); + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL); } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { /* MANUAL */ - main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL); + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL); } } } @@ -525,7 +525,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s // Flick to inair restore first if this comes from an onboard system if (cmd->source_system == status_local->system_id && cmd->source_component == status_local->component_id) { - status_local->arming_state = ARMING_STATE_IN_AIR_RESTORE; + status_local->arming_state = vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE; } transition_result_t arming_res = arm_disarm(cmd_arms, mavlink_fd, "arm/disarm component command"); @@ -550,12 +550,12 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s unsigned int mav_goto = (cmd->param1 + 0.5f); if (mav_goto == 0) { // MAV_GOTO_DO_HOLD - status_local->nav_state = NAVIGATION_STATE_AUTO_LOITER; + status_local->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; mavlink_log_critical(mavlink_fd, "Pause mission cmd"); cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE - status_local->nav_state = NAVIGATION_STATE_AUTO_MISSION; + status_local->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; mavlink_log_critical(mavlink_fd, "Continue mission cmd"); cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -668,14 +668,14 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s case VEHICLE_CMD_NAV_GUIDED_ENABLE: { transition_result_t res = TRANSITION_DENIED; - static main_state_t main_state_pre_offboard = MAIN_STATE_MANUAL; + static main_state_t main_state_pre_offboard =vehicle_status_s::MAIN_STATE_MANUAL; - if (status_local->main_state != MAIN_STATE_OFFBOARD) { + if (status_local->main_state !=vehicle_status_s::MAIN_STATE_OFFBOARD) { main_state_pre_offboard = status_local->main_state; } if (cmd->param1 > 0.5f) { - res = main_state_transition(status_local, MAIN_STATE_OFFBOARD); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD); if (res == TRANSITION_DENIED) { print_reject_mode(status_local, "OFFBOARD"); @@ -798,41 +798,41 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_ef_time_thres = param_find("COM_EF_TIME"); param_t _param_autostart_id = param_find("SYS_AUTOSTART"); - const char *main_states_str[MAIN_STATE_MAX]; - main_states_str[MAIN_STATE_MANUAL] = "MANUAL"; - main_states_str[MAIN_STATE_ALTCTL] = "ALTCTL"; - main_states_str[MAIN_STATE_POSCTL] = "POSCTL"; - main_states_str[MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION"; - main_states_str[MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER"; - main_states_str[MAIN_STATE_AUTO_RTL] = "AUTO_RTL"; - main_states_str[MAIN_STATE_ACRO] = "ACRO"; - main_states_str[MAIN_STATE_OFFBOARD] = "OFFBOARD"; + const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX]; + main_states_str[vehicle_status_s::MAIN_STATE_MANUAL] = "MANUAL"; + main_states_str[vehicle_status_s::MAIN_STATE_ALTCTL] = "ALTCTL"; + main_states_str[vehicle_status_s::MAIN_STATE_POSCTL] = "POSCTL"; + main_states_str[vehicle_status_s::MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION"; + main_states_str[vehicle_status_s::MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER"; + main_states_str[vehicle_status_s::MAIN_STATE_AUTO_RTL] = "AUTO_RTL"; + main_states_str[vehicle_status_s::MAIN_STATE_ACRO] = "ACRO"; + main_states_str[vehicle_status_s::MAIN_STATE_OFFBOARD] = "OFFBOARD"; - const char *arming_states_str[ARMING_STATE_MAX]; - arming_states_str[ARMING_STATE_INIT] = "INIT"; - arming_states_str[ARMING_STATE_STANDBY] = "STANDBY"; - arming_states_str[ARMING_STATE_ARMED] = "ARMED"; - arming_states_str[ARMING_STATE_ARMED_ERROR] = "ARMED_ERROR"; - arming_states_str[ARMING_STATE_STANDBY_ERROR] = "STANDBY_ERROR"; - arming_states_str[ARMING_STATE_REBOOT] = "REBOOT"; - arming_states_str[ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE"; + const char *arming_states_str[vehicle_status_s::ARMING_STATE_MAX]; + arming_states_str[vehicle_status_s::ARMING_STATE_INIT] = "INIT"; + arming_states_str[vehicle_status_s::ARMING_STATE_STANDBY] = "STANDBY"; + arming_states_str[vehicle_status_s::ARMING_STATE_ARMED] = "ARMED"; + arming_states_str[vehicle_status_s::ARMING_STATE_ARMED_ERROR] = "ARMED_ERROR"; + arming_states_str[vehicle_status_s::ARMING_STATE_STANDBY_ERROR] = "STANDBY_ERROR"; + arming_states_str[vehicle_status_s::ARMING_STATE_REBOOT] = "REBOOT"; + arming_states_str[vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE"; - const char *nav_states_str[NAVIGATION_STATE_MAX]; - nav_states_str[NAVIGATION_STATE_MANUAL] = "MANUAL"; - nav_states_str[NAVIGATION_STATE_ALTCTL] = "ALTCTL"; - nav_states_str[NAVIGATION_STATE_POSCTL] = "POSCTL"; - nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; - nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; - nav_states_str[NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL"; - nav_states_str[NAVIGATION_STATE_AUTO_RCRECOVER] = "AUTO_RCRECOVER"; - nav_states_str[NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS"; - nav_states_str[NAVIGATION_STATE_AUTO_LANDENGFAIL] = "AUTO_LANDENGFAIL"; - nav_states_str[NAVIGATION_STATE_AUTO_LANDGPSFAIL] = "AUTO_LANDGPSFAIL"; - nav_states_str[NAVIGATION_STATE_ACRO] = "ACRO"; - nav_states_str[NAVIGATION_STATE_LAND] = "LAND"; - nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND"; - nav_states_str[NAVIGATION_STATE_TERMINATION] = "TERMINATION"; - nav_states_str[NAVIGATION_STATE_OFFBOARD] = "OFFBOARD"; + const char *nav_states_str[vehicle_status_s::NAVIGATION_STATE_MAX]; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_MANUAL] = "MANUAL"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_ALTCTL] = "ALTCTL"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_POSCTL] = "POSCTL"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER] = "AUTO_RCRECOVER"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL] = "AUTO_LANDENGFAIL"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL] = "AUTO_LANDGPSFAIL"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_ACRO] = "ACRO"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_LAND] = "LAND"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_DESCEND] = "DESCEND"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_TERMINATION] = "TERMINATION"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_OFFBOARD] = "OFFBOARD"; /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -853,10 +853,10 @@ int commander_thread_main(int argc, char *argv[]) status.condition_landed = true; // initialize to safe value // We want to accept RC inputs as default status.rc_input_blocked = false; - status.main_state = MAIN_STATE_MANUAL; - status.nav_state = NAVIGATION_STATE_MANUAL; - status.arming_state = ARMING_STATE_INIT; - status.hil_state = HIL_STATE_OFF; + status.main_state =vehicle_status_s::MAIN_STATE_MANUAL; + status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; + status.arming_state = vehicle_status_s::ARMING_STATE_INIT; + status.hil_state = vehicle_status_s::HIL_STATE_OFF; status.failsafe = false; /* neither manual nor offboard control commands have been received */ @@ -869,7 +869,7 @@ int commander_thread_main(int argc, char *argv[]) status.data_link_lost = true; /* set battery warning flag */ - status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; + status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_NONE; status.condition_battery_voltage_valid = false; // XXX for now just set sensors as initialized @@ -1143,14 +1143,14 @@ int commander_thread_main(int argc, char *argv[]) } /* disable manual override for all systems that rely on electronic stabilization */ - if (status.system_type == VEHICLE_TYPE_COAXIAL || - status.system_type == VEHICLE_TYPE_HELICOPTER || - status.system_type == VEHICLE_TYPE_TRICOPTER || - status.system_type == VEHICLE_TYPE_QUADROTOR || - status.system_type == VEHICLE_TYPE_HEXAROTOR || - status.system_type == VEHICLE_TYPE_OCTOROTOR || - (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR && vtol_status.vtol_in_rw_mode) || - (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR && vtol_status.vtol_in_rw_mode)) { + if (status.system_type == vehicle_status_s::VEHICLE_TYPE_COAXIAL || + status.system_type == vehicle_status_s::VEHICLE_TYPE_HELICOPTER || + status.system_type == vehicle_status_s::VEHICLE_TYPE_TRICOPTER || + status.system_type == vehicle_status_s::VEHICLE_TYPE_QUADROTOR || + status.system_type == vehicle_status_s::VEHICLE_TYPE_HEXAROTOR || + status.system_type == vehicle_status_s::VEHICLE_TYPE_OCTOROTOR || + (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR && vtol_status.vtol_in_rw_mode) || + (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR && vtol_status.vtol_in_rw_mode)) { status.is_rotary_wing = true; @@ -1159,8 +1159,8 @@ int commander_thread_main(int argc, char *argv[]) } /* set vehicle_status.is_vtol flag */ - status.is_vtol = (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR) || - (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR); + status.is_vtol = (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR) || + (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR); /* check and update system / component ID */ param_get(_param_system_id, &(status.system_id)); @@ -1310,9 +1310,9 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(safety), safety_sub, &safety); /* disarm if safety is now on and still armed */ - if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) { - arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : - ARMING_STATE_STANDBY_ERROR); + if (status.hil_state == vehicle_status_s::HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) { + arming_state_t new_arming_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? vehicle_status_s::ARMING_STATE_STANDBY : + vehicle_status_s::ARMING_STATE_STANDBY_ERROR); if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd)) { @@ -1344,7 +1344,7 @@ int commander_thread_main(int argc, char *argv[]) status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab; /* Make sure that this is only adjusted if vehicle realy is of type vtol*/ - if ((status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR) || (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR)) { + if ((status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR) || (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR)) { status.is_rotary_wing = vtol_status.vtol_in_rw_mode; } } @@ -1516,7 +1516,7 @@ int commander_thread_main(int argc, char *argv[]) if (status.condition_battery_voltage_valid && status.battery_remaining < 0.18f && !low_battery_voltage_actions_done) { low_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED"); - status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; + status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW; status_changed = true; } else if (!on_usb_power && status.condition_battery_voltage_valid && status.battery_remaining < 0.09f @@ -1524,10 +1524,10 @@ int commander_thread_main(int argc, char *argv[]) /* critical battery voltage, this is rather an emergency, change state machine */ critical_battery_voltage_actions_done = true; mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY"); - status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; + status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL; if (armed.armed) { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */, + arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { @@ -1535,7 +1535,7 @@ int commander_thread_main(int argc, char *argv[]) } } else { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */, + arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { @@ -1549,9 +1549,9 @@ int commander_thread_main(int argc, char *argv[]) /* End battery voltage check */ /* If in INIT state, try to proceed to STANDBY state */ - if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { + if (status.arming_state == vehicle_status_s::ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { /* TODO: check for sensors */ - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, + arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { @@ -1662,14 +1662,14 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSIST mode and landed) -> disarm * do it only for rotary wings */ if (status.is_rotary_wing && - (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && - (status.main_state == MAIN_STATE_MANUAL || status.main_state == MAIN_STATE_ACRO || status.condition_landed) && + (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) && + (status.main_state == vehicle_status_s::MAIN_STATE_MANUAL || status.main_state == vehicle_status_s::MAIN_STATE_ACRO || status.condition_landed) && sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ - arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : - ARMING_STATE_STANDBY_ERROR); + arming_state_t new_arming_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? vehicle_status_s::ARMING_STATE_STANDBY : + vehicle_status_s::ARMING_STATE_STANDBY_ERROR); arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd); @@ -1688,7 +1688,7 @@ int commander_thread_main(int argc, char *argv[]) } /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ - if (status.arming_state == ARMING_STATE_STANDBY && + if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY && sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { @@ -1696,11 +1696,11 @@ int commander_thread_main(int argc, char *argv[]) * for being in manual mode only applies to manual arming actions. * the system can be armed in auto if armed via the GCS. */ - if (status.main_state != MAIN_STATE_MANUAL) { + if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL) { print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); } else { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, + arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { @@ -1719,7 +1719,7 @@ int commander_thread_main(int argc, char *argv[]) } if (arming_ret == TRANSITION_CHANGED) { - if (status.arming_state == ARMING_STATE_ARMED) { + if (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { mavlink_log_info(mavlink_fd, "ARMED by RC"); } else { @@ -1857,10 +1857,10 @@ int commander_thread_main(int argc, char *argv[]) /* At this point the data link and the gps system have been checked * If we are not in a manual (RC stick controlled mode) * and both failed we want to terminate the flight */ - if (status.main_state != MAIN_STATE_MANUAL && - status.main_state != MAIN_STATE_ACRO && - status.main_state != MAIN_STATE_ALTCTL && - status.main_state != MAIN_STATE_POSCTL && + if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL && + status.main_state !=vehicle_status_s::MAIN_STATE_ACRO && + status.main_state !=vehicle_status_s::MAIN_STATE_ALTCTL && + status.main_state !=vehicle_status_s::MAIN_STATE_POSCTL && ((status.data_link_lost && status.gps_failure) || (status.data_link_lost_cmd && status.gps_failure_cmd))) { armed.force_failsafe = true; @@ -1881,10 +1881,10 @@ int commander_thread_main(int argc, char *argv[]) /* At this point the rc signal and the gps system have been checked * If we are in manual (controlled with RC): * if both failed we want to terminate the flight */ - if ((status.main_state == MAIN_STATE_ACRO || - status.main_state == MAIN_STATE_MANUAL || - status.main_state == MAIN_STATE_ALTCTL || - status.main_state == MAIN_STATE_POSCTL) && + if ((status.main_state ==vehicle_status_s::MAIN_STATE_ACRO || + status.main_state ==vehicle_status_s::MAIN_STATE_MANUAL || + status.main_state ==vehicle_status_s::MAIN_STATE_ALTCTL || + status.main_state ==vehicle_status_s::MAIN_STATE_POSCTL) && ((status.rc_signal_lost && status.gps_failure) || (status.rc_signal_lost_cmd && status.gps_failure_cmd))) { armed.force_failsafe = true; @@ -1976,11 +1976,11 @@ int commander_thread_main(int argc, char *argv[]) set_tune(TONE_ARMING_WARNING_TUNE); arm_tune_played = true; - } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { + } else if (status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) { /* play tune on battery critical */ set_tune(TONE_BATTERY_WARNING_FAST_TUNE); - } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe) { + } else if (status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW || status.failsafe) { /* play tune on battery warning or failsafe */ set_tune(TONE_BATTERY_WARNING_SLOW_TUNE); @@ -2072,15 +2072,15 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu bool set_normal_color = false; /* set mode */ - if (status_local->arming_state == ARMING_STATE_ARMED) { + if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED) { rgbled_set_mode(RGBLED_MODE_ON); set_normal_color = true; - } else if (status_local->arming_state == ARMING_STATE_ARMED_ERROR) { + } else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) { rgbled_set_mode(RGBLED_MODE_BLINK_FAST); rgbled_set_color(RGBLED_COLOR_RED); - } else if (status_local->arming_state == ARMING_STATE_STANDBY) { + } else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { rgbled_set_mode(RGBLED_MODE_BREATHE); set_normal_color = true; @@ -2091,9 +2091,9 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu if (set_normal_color) { /* set color */ - if (status_local->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status_local->failsafe) { + if (status_local->battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW || status_local->failsafe) { rgbled_set_color(RGBLED_COLOR_AMBER); - /* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */ + /* vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL handled as vehicle_status_s::ARMING_STATE_ARMED_ERROR / vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ } else { if (status_local->condition_local_position_valid) { @@ -2149,12 +2149,12 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s /* if offboard is set allready by a mavlink command, abort */ if (status.offboard_control_set_by_command) { - return main_state_transition(status_local, MAIN_STATE_OFFBOARD); + return main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD); } /* offboard switch overrides main switch */ - if (sp_man->offboard_switch == SWITCH_POS_ON) { - res = main_state_transition(status_local, MAIN_STATE_OFFBOARD); + if (sp_man->offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD); if (res == TRANSITION_DENIED) { print_reject_mode(status_local, "OFFBOARD"); @@ -2166,24 +2166,24 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s /* offboard switched off or denied, check main mode switch */ switch (sp_man->mode_switch) { - case SWITCH_POS_NONE: + case manual_control_setpoint_s::SWITCH_POS_NONE: res = TRANSITION_NOT_CHANGED; break; - case SWITCH_POS_OFF: // MANUAL - if (sp_man->acro_switch == SWITCH_POS_ON) { - res = main_state_transition(status_local, MAIN_STATE_ACRO); + case manual_control_setpoint_s::SWITCH_POS_OFF: // MANUAL + if (sp_man->acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ACRO); } else { - res = main_state_transition(status_local, MAIN_STATE_MANUAL); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL); } // TRANSITION_DENIED is not possible here break; - case SWITCH_POS_MIDDLE: // ASSIST - if (sp_man->posctl_switch == SWITCH_POS_ON) { - res = main_state_transition(status_local, MAIN_STATE_POSCTL); + case manual_control_setpoint_s::SWITCH_POS_MIDDLE: // ASSIST + if (sp_man->posctl_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_POSCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -2193,24 +2193,24 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s } // fallback to ALTCTL - res = main_state_transition(status_local, MAIN_STATE_ALTCTL); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this mode } - if (sp_man->posctl_switch != SWITCH_POS_ON) { + if (sp_man->posctl_switch != manual_control_setpoint_s::SWITCH_POS_ON) { print_reject_mode(status_local, "ALTCTL"); } // fallback to MANUAL - res = main_state_transition(status_local, MAIN_STATE_MANUAL); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; - case SWITCH_POS_ON: // AUTO - if (sp_man->return_switch == SWITCH_POS_ON) { - res = main_state_transition(status_local, MAIN_STATE_AUTO_RTL); + case manual_control_setpoint_s::SWITCH_POS_ON: // AUTO + if (sp_man->return_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_RTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -2219,14 +2219,14 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s print_reject_mode(status_local, "AUTO_RTL"); // fallback to LOITER if home position not set - res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - } else if (sp_man->loiter_switch == SWITCH_POS_ON) { - res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); + } else if (sp_man->loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -2235,7 +2235,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s print_reject_mode(status_local, "AUTO_LOITER"); } else { - res = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_MISSION); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -2244,7 +2244,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s print_reject_mode(status_local, "AUTO_MISSION"); // fallback to LOITER if home position not set - res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -2252,21 +2252,21 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s } // fallback to POSCTL - res = main_state_transition(status_local, MAIN_STATE_POSCTL); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_POSCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } // fallback to ALTCTL - res = main_state_transition(status_local, MAIN_STATE_ALTCTL); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } // fallback to MANUAL - res = main_state_transition(status_local, MAIN_STATE_MANUAL); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; @@ -2283,11 +2283,11 @@ set_control_mode() /* set vehicle_control_mode according to set_navigation_state */ control_mode.flag_armed = armed.armed; control_mode.flag_external_manual_override_ok = (!status.is_rotary_wing && !status.is_vtol); - control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON; + control_mode.flag_system_hil_enabled = status.hil_state == vehicle_status_s::HIL_STATE_ON; control_mode.flag_control_offboard_enabled = false; switch (status.nav_state) { - case NAVIGATION_STATE_MANUAL: + case vehicle_status_s::NAVIGATION_STATE_MANUAL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = (status.is_rotary_wing || status.vtol_fw_permanent_stab); @@ -2299,7 +2299,7 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_ALTCTL: + case vehicle_status_s::NAVIGATION_STATE_ALTCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; @@ -2311,7 +2311,7 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_POSCTL: + case vehicle_status_s::NAVIGATION_STATE_POSCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; @@ -2323,12 +2323,12 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_AUTO_MISSION: - case NAVIGATION_STATE_AUTO_LOITER: - case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_RCRECOVER: - case NAVIGATION_STATE_AUTO_RTGS: - case NAVIGATION_STATE_AUTO_LANDENGFAIL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; @@ -2340,7 +2340,7 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_AUTO_LANDGPSFAIL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; @@ -2352,7 +2352,7 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_ACRO: + case vehicle_status_s::NAVIGATION_STATE_ACRO: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; @@ -2365,7 +2365,7 @@ set_control_mode() break; - case NAVIGATION_STATE_LAND: + case vehicle_status_s::NAVIGATION_STATE_LAND: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; @@ -2378,7 +2378,7 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_DESCEND: + case vehicle_status_s::NAVIGATION_STATE_DESCEND: /* TODO: check if this makes sense */ control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; @@ -2391,7 +2391,7 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_TERMINATION: + case vehicle_status_s::NAVIGATION_STATE_TERMINATION: /* disable all controllers on termination */ control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = false; @@ -2404,7 +2404,7 @@ set_control_mode() control_mode.flag_control_termination_enabled = true; break; - case NAVIGATION_STATE_OFFBOARD: + case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_offboard_enabled = true; @@ -2603,7 +2603,7 @@ void *commander_low_prio_loop(void *arg) int calib_ret = ERROR; /* try to go to INIT/PREFLIGHT arming state */ - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_INIT, &armed, true /* fRunPreArmChecks */, mavlink_fd)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; @@ -2667,7 +2667,7 @@ void *commander_low_prio_loop(void *arg) tune_negative(true); } - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); + arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); break; } diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 2022e99fb3..8a44511008 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -72,16 +72,16 @@ static const int ERROR = -1; bool is_multirotor(const struct vehicle_status_s *current_status) { - return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) || - (current_status->system_type == VEHICLE_TYPE_TRICOPTER)); + return ((current_status->system_type == vehicle_status_s::VEHICLE_TYPE_QUADROTOR) || + (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_HEXAROTOR) || + (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_OCTOROTOR) || + (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_TRICOPTER)); } bool is_rotary_wing(const struct vehicle_status_s *current_status) { - return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER) - || (current_status->system_type == VEHICLE_TYPE_COAXIAL); + return is_multirotor(current_status) || (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_HELICOPTER) + || (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_COAXIAL); } static int buzzer = -1; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 465f9cdc52..40da9c77b1 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -76,19 +76,19 @@ static const int ERROR = -1; // will be true for a valid transition or false for a invalid transition. In some cases even // though the transition is marked as true additional checks must be made. See arming_state_transition // code for those checks. -static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = { +static const bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX] = { // INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE - { /* ARMING_STATE_INIT */ true, true, false, false, false, false, false }, - { /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false }, - { /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true }, - { /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false }, - { /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false }, - { /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true }, - { /* ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI + { /* vehicle_status_s::ARMING_STATE_INIT */ true, true, false, false, false, false, false }, + { /* vehicle_status_s::ARMING_STATE_STANDBY */ true, true, true, true, false, false, false }, + { /* vehicle_status_s::ARMING_STATE_ARMED */ false, true, true, false, false, false, true }, + { /* vehicle_status_s::ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false }, + { /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false }, + { /* vehicle_status_s::ARMING_STATE_REBOOT */ true, true, false, false, true, true, true }, + { /* vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI }; // You can index into the array with an arming_state_t in order to get it's textual representation -static const char * const state_names[ARMING_STATE_MAX] = { +static const char * const state_names[vehicle_status_s::ARMING_STATE_MAX] = { "ARMING_STATE_INIT", "ARMING_STATE_STANDBY", "ARMING_STATE_ARMED", @@ -107,8 +107,8 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s const int mavlink_fd) ///< mavlink fd for error reporting, 0 for none { // Double check that our static arrays are still valid - ASSERT(ARMING_STATE_INIT == 0); - ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1); + ASSERT(vehicle_status_s::ARMING_STATE_INIT == 0); + ASSERT(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE == vehicle_status_s::ARMING_STATE_MAX - 1); transition_result_t ret = TRANSITION_DENIED; arming_state_t current_arming_state = status->arming_state; @@ -126,7 +126,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s int prearm_ret = OK; /* only perform the check if we have to */ - if (fRunPreArmChecks && new_arming_state == ARMING_STATE_ARMED) { + if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { prearm_ret = prearm_check(status, mavlink_fd); } @@ -136,7 +136,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s irqstate_t flags = irqsave(); /* enforce lockdown in HIL */ - if (status->hil_state == HIL_STATE_ON) { + if (status->hil_state == vehicle_status_s::HIL_STATE_ON) { armed->lockdown = true; } else { @@ -148,12 +148,12 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s if (valid_transition) { // We have a good transition. Now perform any secondary validation. - if (new_arming_state == ARMING_STATE_ARMED) { + if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { // Do not perform pre-arm checks if coming from in air restore - // Allow if HIL_STATE_ON - if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && - status->hil_state == HIL_STATE_OFF) { + // Allow if vehicle_status_s::HIL_STATE_ON + if (status->arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE && + status->hil_state == vehicle_status_s::HIL_STATE_OFF) { // Fail transition if pre-arm check fails if (prearm_ret) { @@ -200,18 +200,18 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s } - } else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) { - new_arming_state = ARMING_STATE_STANDBY_ERROR; + } else if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) { + new_arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; } } // HIL can always go to standby - if (status->hil_state == HIL_STATE_ON && new_arming_state == ARMING_STATE_STANDBY) { + if (status->hil_state == vehicle_status_s::HIL_STATE_ON && new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { valid_transition = true; } /* Sensors need to be initialized for STANDBY state */ - if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { + if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational."); feedback_provided = true; valid_transition = false; @@ -219,8 +219,8 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s // Finish up the state transition if (valid_transition) { - armed->armed = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_ARMED_ERROR; - armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY; + armed->armed = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR; + armed->ready_to_arm = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY; ret = TRANSITION_CHANGED; status->arming_state = new_arming_state; } @@ -264,12 +264,12 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta /* transition may be denied even if the same state is requested because conditions may have changed */ switch (new_main_state) { - case MAIN_STATE_MANUAL: - case MAIN_STATE_ACRO: + case vehicle_status_s::MAIN_STATE_MANUAL: + case vehicle_status_s::MAIN_STATE_ACRO: ret = TRANSITION_CHANGED; break; - case MAIN_STATE_ALTCTL: + case vehicle_status_s::MAIN_STATE_ALTCTL: /* need at minimum altitude estimate */ /* TODO: add this for fixedwing as well */ if (!status->is_rotary_wing || @@ -279,7 +279,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta } break; - case MAIN_STATE_POSCTL: + case vehicle_status_s::MAIN_STATE_POSCTL: /* need at minimum local position estimate */ if (status->condition_local_position_valid || status->condition_global_position_valid) { @@ -287,22 +287,22 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta } break; - case MAIN_STATE_AUTO_LOITER: + case vehicle_status_s::MAIN_STATE_AUTO_LOITER: /* need global position estimate */ if (status->condition_global_position_valid) { ret = TRANSITION_CHANGED; } break; - case MAIN_STATE_AUTO_MISSION: - case MAIN_STATE_AUTO_RTL: + case vehicle_status_s::MAIN_STATE_AUTO_MISSION: + case vehicle_status_s::MAIN_STATE_AUTO_RTL: /* need global position and home position */ if (status->condition_global_position_valid && status->condition_home_position_valid) { ret = TRANSITION_CHANGED; } break; - case MAIN_STATE_OFFBOARD: + case vehicle_status_s::MAIN_STATE_OFFBOARD: /* need offboard signal */ if (!status->offboard_control_signal_lost) { @@ -311,7 +311,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta break; - case MAIN_STATE_MAX: + case vehicle_status_s::MAIN_STATE_MAX: default: break; } @@ -338,16 +338,16 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, } else { switch (new_state) { - case HIL_STATE_OFF: + case vehicle_status_s::HIL_STATE_OFF: /* we're in HIL and unexpected things can happen if we disable HIL now */ mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)"); ret = TRANSITION_DENIED; break; - case HIL_STATE_ON: - if (current_status->arming_state == ARMING_STATE_INIT - || current_status->arming_state == ARMING_STATE_STANDBY - || current_status->arming_state == ARMING_STATE_STANDBY_ERROR) { + case vehicle_status_s::HIL_STATE_ON: + if (current_status->arming_state == vehicle_status_s::ARMING_STATE_INIT + || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY + || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { /* Disable publication of all attached sensors */ /* list directory */ @@ -448,55 +448,55 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en { navigation_state_t nav_state_old = status->nav_state; - bool armed = (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR); + bool armed = (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED || status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR); status->failsafe = false; /* evaluate main state to decide in normal (non-failsafe) mode */ switch (status->main_state) { - case MAIN_STATE_ACRO: - case MAIN_STATE_MANUAL: - case MAIN_STATE_ALTCTL: - case MAIN_STATE_POSCTL: + case vehicle_status_s::MAIN_STATE_ACRO: + case vehicle_status_s::MAIN_STATE_MANUAL: + case vehicle_status_s::MAIN_STATE_ALTCTL: + case vehicle_status_s::MAIN_STATE_POSCTL: /* require RC for all manual modes */ if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed) { status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; } else if (status->condition_local_position_valid) { - status->nav_state = NAVIGATION_STATE_LAND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { - status->nav_state = NAVIGATION_STATE_DESCEND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; } else { - status->nav_state = NAVIGATION_STATE_TERMINATION; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; } } else { switch (status->main_state) { - case MAIN_STATE_ACRO: - status->nav_state = NAVIGATION_STATE_ACRO; + case vehicle_status_s::MAIN_STATE_ACRO: + status->nav_state = vehicle_status_s::NAVIGATION_STATE_ACRO; break; - case MAIN_STATE_MANUAL: - status->nav_state = NAVIGATION_STATE_MANUAL; + case vehicle_status_s::MAIN_STATE_MANUAL: + status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; break; - case MAIN_STATE_ALTCTL: - status->nav_state = NAVIGATION_STATE_ALTCTL; + case vehicle_status_s::MAIN_STATE_ALTCTL: + status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL; break; - case MAIN_STATE_POSCTL: - status->nav_state = NAVIGATION_STATE_POSCTL; + case vehicle_status_s::MAIN_STATE_POSCTL: + status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; break; default: - status->nav_state = NAVIGATION_STATE_MANUAL; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; break; } } break; - case MAIN_STATE_AUTO_MISSION: + case vehicle_status_s::MAIN_STATE_AUTO_MISSION: /* go into failsafe * - if commanded to do so @@ -505,19 +505,19 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en /* first look at the commands */ if (status->engine_failure_cmd) { - status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; } else if (status->data_link_lost_cmd) { - status->nav_state = NAVIGATION_STATE_AUTO_RTGS; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; } else if (status->gps_failure_cmd) { - status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; } else if (status->rc_signal_lost_cmd) { - status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; /* finished handling commands which have priority, now handle failures */ } else if (status->gps_failure) { - status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; } else if (status->engine_failure) { - status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; /* datalink loss enabled: * check for datalink lost: this should always trigger RTGS */ @@ -525,13 +525,13 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_RTGS; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; } else if (status->condition_local_position_valid) { - status->nav_state = NAVIGATION_STATE_LAND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { - status->nav_state = NAVIGATION_STATE_DESCEND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; } else { - status->nav_state = NAVIGATION_STATE_TERMINATION; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; } /* datalink loss disabled: @@ -542,37 +542,37 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; } else if (status->condition_local_position_valid) { - status->nav_state = NAVIGATION_STATE_LAND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { - status->nav_state = NAVIGATION_STATE_DESCEND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; } else { - status->nav_state = NAVIGATION_STATE_TERMINATION; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; } /* stay where you are if you should stay in failsafe, otherwise everything is perfect */ } else if (!stay_in_failsafe){ - status->nav_state = NAVIGATION_STATE_AUTO_MISSION; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; } break; - case MAIN_STATE_AUTO_LOITER: + case vehicle_status_s::MAIN_STATE_AUTO_LOITER: /* go into failsafe on a engine failure */ if (status->engine_failure) { - status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; /* also go into failsafe if just datalink is lost */ } else if (status->data_link_lost && data_link_loss_enabled) { status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_RTGS; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; } else if (status->condition_local_position_valid) { - status->nav_state = NAVIGATION_STATE_LAND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { - status->nav_state = NAVIGATION_STATE_DESCEND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; } else { - status->nav_state = NAVIGATION_STATE_TERMINATION; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; } /* go into failsafe if RC is lost and datalink loss is not set up */ @@ -580,65 +580,65 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_RTGS; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; } else if (status->condition_local_position_valid) { - status->nav_state = NAVIGATION_STATE_LAND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { - status->nav_state = NAVIGATION_STATE_DESCEND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; } else { - status->nav_state = NAVIGATION_STATE_TERMINATION; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; } /* don't bother if RC is lost if datalink is connected */ } else if (status->rc_signal_lost) { /* this mode is ok, we don't need RC for loitering */ - status->nav_state = NAVIGATION_STATE_AUTO_LOITER; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; } else { /* everything is perfect */ - status->nav_state = NAVIGATION_STATE_AUTO_LOITER; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; } break; - case MAIN_STATE_AUTO_RTL: + case vehicle_status_s::MAIN_STATE_AUTO_RTL: /* require global position and home, also go into failsafe on an engine failure */ if (status->engine_failure) { - status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; } else if ((!status->condition_global_position_valid || !status->condition_home_position_valid)) { status->failsafe = true; if (status->condition_local_position_valid) { - status->nav_state = NAVIGATION_STATE_LAND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { - status->nav_state = NAVIGATION_STATE_DESCEND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; } else { - status->nav_state = NAVIGATION_STATE_TERMINATION; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; } } else { - status->nav_state = NAVIGATION_STATE_AUTO_RTL; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; } break; - case MAIN_STATE_OFFBOARD: + case vehicle_status_s::MAIN_STATE_OFFBOARD: /* require offboard control, otherwise stay where you are */ if (status->offboard_control_signal_lost && !status->rc_signal_lost) { status->failsafe = true; - status->nav_state = NAVIGATION_STATE_POSCTL; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; } else if (status->offboard_control_signal_lost && status->rc_signal_lost) { status->failsafe = true; if (status->condition_local_position_valid) { - status->nav_state = NAVIGATION_STATE_LAND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { - status->nav_state = NAVIGATION_STATE_DESCEND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; } else { - status->nav_state = NAVIGATION_STATE_TERMINATION; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; } } else { - status->nav_state = NAVIGATION_STATE_OFFBOARD; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_OFFBOARD; } default: break; diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index d51075b8c5..e33691b0c4 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -817,7 +817,7 @@ FixedwingEstimator::task_main() if (fds[1].revents & POLLIN) { /* check vehicle status for changes to publication state */ - bool prev_hil = (_vstatus.hil_state == HIL_STATE_ON); + bool prev_hil = (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON); vehicle_status_poll(); bool accel_updated; @@ -826,7 +826,7 @@ FixedwingEstimator::task_main() perf_count(_perf_gyro); /* Reset baro reference if switching to HIL, reset sensor states */ - if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) { + if (!prev_hil && (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON)) { /* system is in HIL now, wait for measurements to come in one last round */ usleep(60000); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 74249c9c53..0bdc285e70 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -816,7 +816,7 @@ void FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet) { - if (pos_sp_triplet.current.valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) { + if (pos_sp_triplet.current.valid && !(pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER)) { /* rotate ground speed vector with current attitude */ math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); @@ -963,7 +963,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } - if (pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION) { + if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { /* waypoint is a plain navigation waypoint */ _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); _att_sp.roll_body = _l1_control.nav_roll(); @@ -974,7 +974,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); - } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) { + } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { /* waypoint is a loiter waypoint */ _l1_control.navigate_loiter(curr_wp, current_position, pos_sp_triplet.current.loiter_radius, @@ -987,7 +987,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); - } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); @@ -1140,7 +1140,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi ground_speed); } - } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) { + } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { /* Perform launch detection */ if (launchDetector.launchDetectionEnabled() && @@ -1235,12 +1235,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } /* reset landing state */ - if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) { + if (pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_LAND) { reset_landing_state(); } /* reset takeoff/launch state */ - if (pos_sp_triplet.current.type != SETPOINT_TYPE_TAKEOFF) { + if (pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { reset_takeoff_state(); } @@ -1327,7 +1327,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Set thrust to 0 to minimize damage */ _att_sp.thrust = 0.0f; } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto - pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && + pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { /* making sure again that the correct thrust is used, * without depending on library calls for safety reasons */ @@ -1341,7 +1341,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* During a takeoff waypoint while waiting for launch the pitch sp is set * already (not by tecs) */ if (!(_control_mode_current == FW_POSCTRL_MODE_AUTO && - pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && + pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && launch_detection_state == LAUNCHDETECTION_RES_NONE)) { _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); } diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 9e4ab00df2..676b65adc1 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1431,7 +1431,7 @@ Mavlink::task_main(int argc, char *argv[]) if (status_sub->update(&status_time, &status)) { /* switch HIL mode if required */ - set_hil_enabled(status.hil_state == HIL_STATE_ON); + set_hil_enabled(status.hil_state == vehicle_status_s::HIL_STATE_ON); } /* check for requested subscriptions */ diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 6765100c70..6642fb2ac6 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -103,13 +103,13 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set *mavlink_custom_mode = 0; /* HIL */ - if (status->hil_state == HIL_STATE_ON) { + if (status->hil_state == vehicle_status_s::HIL_STATE_ON) { *mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED; } /* arming state */ - if (status->arming_state == ARMING_STATE_ARMED - || status->arming_state == ARMING_STATE_ARMED_ERROR) { + if (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED + || status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) { *mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } @@ -121,31 +121,31 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set switch (status->nav_state) { - case NAVIGATION_STATE_MANUAL: + case vehicle_status_s::NAVIGATION_STATE_MANUAL: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; break; - case NAVIGATION_STATE_ACRO: + case vehicle_status_s::NAVIGATION_STATE_ACRO: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO; break; - case NAVIGATION_STATE_ALTCTL: + case vehicle_status_s::NAVIGATION_STATE_ALTCTL: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL; break; - case NAVIGATION_STATE_POSCTL: + case vehicle_status_s::NAVIGATION_STATE_POSCTL: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL; break; - case NAVIGATION_STATE_AUTO_MISSION: + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; @@ -153,7 +153,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; break; - case NAVIGATION_STATE_AUTO_LOITER: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; @@ -161,9 +161,9 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; break; - case NAVIGATION_STATE_AUTO_RTL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: /* fallthrough */ - case NAVIGATION_STATE_AUTO_RCRECOVER: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; @@ -171,11 +171,11 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; break; - case NAVIGATION_STATE_LAND: - case NAVIGATION_STATE_AUTO_LANDENGFAIL: - case NAVIGATION_STATE_AUTO_LANDGPSFAIL: + case vehicle_status_s::NAVIGATION_STATE_LAND: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: /* fallthrough */ - case NAVIGATION_STATE_DESCEND: + case vehicle_status_s::NAVIGATION_STATE_DESCEND: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; @@ -183,7 +183,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; break; - case NAVIGATION_STATE_AUTO_RTGS: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; @@ -191,19 +191,19 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTGS; break; - case NAVIGATION_STATE_TERMINATION: + case vehicle_status_s::NAVIGATION_STATE_TERMINATION: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; break; - case NAVIGATION_STATE_OFFBOARD: + case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD; break; - case NAVIGATION_STATE_MAX: + case vehicle_status_s::NAVIGATION_STATE_MAX: /* this is an unused case, ignore */ break; @@ -212,21 +212,21 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set *mavlink_custom_mode = custom_mode.data; /* set system state */ - if (status->arming_state == ARMING_STATE_INIT - || status->arming_state == ARMING_STATE_IN_AIR_RESTORE - || status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review + if (status->arming_state == vehicle_status_s::ARMING_STATE_INIT + || status->arming_state == vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE + || status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { // TODO review *mavlink_state = MAV_STATE_UNINIT; - } else if (status->arming_state == ARMING_STATE_ARMED) { + } else if (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED) { *mavlink_state = MAV_STATE_ACTIVE; - } else if (status->arming_state == ARMING_STATE_ARMED_ERROR) { + } else if (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) { *mavlink_state = MAV_STATE_CRITICAL; - } else if (status->arming_state == ARMING_STATE_STANDBY) { + } else if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { *mavlink_state = MAV_STATE_STANDBY; - } else if (status->arming_state == ARMING_STATE_REBOOT) { + } else if (status->arming_state == vehicle_status_s::ARMING_STATE_REBOOT) { *mavlink_state = MAV_STATE_POWEROFF; } else { @@ -1438,7 +1438,7 @@ protected: updated |= _pos_sp_triplet_sub->update(&_pos_sp_triplet_time, &pos_sp_triplet); updated |= _status_sub->update(&_status_time, &status); - if (updated && (status.arming_state == ARMING_STATE_ARMED)) { + if (updated && (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)) { /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state; uint8_t mavlink_base_mode; @@ -2205,7 +2205,7 @@ protected: msg.param2 = 0; msg.param3 = 0; /* set camera capture ON/OFF depending on arming state */ - msg.param4 = (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) ? 1 : 0; + msg.param4 = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) ? 1 : 0; msg.param5 = 0; msg.param6 = 0; msg.param7 = 0; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 2e28a6d2c5..b1ba91cac9 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -617,7 +617,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t pos_sp_triplet.previous.valid = false; pos_sp_triplet.next.valid = false; pos_sp_triplet.current.valid = true; - pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others + pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; //XXX support others /* set the local pos values if the setpoint type is 'local pos' and none * of the local pos fields is set to 'ignore' */ @@ -986,10 +986,10 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) } else if (tsync.tc1 > 0) { - int64_t offset_ns = (tsync.ts1 + now_ns - tsync.tc1*2)/2 ; + int64_t offset_ns = (tsync.ts1 + now_ns - tsync.tc1*2)/2 ; int64_t dt = _time_offset - offset_ns; - if (dt > 10000000LL || dt < -10000000LL) { // 10 millisecond skew + if (dt > 10000000LL || dt < -10000000LL) { // 10 millisecond skew _time_offset = offset_ns; warnx("[timesync] Hard setting offset."); } else { diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 60682fb8ee..b9692ffbfc 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -768,7 +768,7 @@ MulticopterPositionControl::control_auto(float dt) /* by default use current setpoint as is */ math::Vector<3> pos_sp_s = curr_sp_s; - if (_pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.valid) { + if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.valid) { /* follow "previous - current" line */ math::Vector<3> prev_sp; map_projection_project(&_ref_pos, @@ -998,7 +998,7 @@ MulticopterPositionControl::task_main() } - if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) { + if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { /* idle state, don't run controller and set zero thrust */ R.identity(); memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); @@ -1037,7 +1037,7 @@ MulticopterPositionControl::task_main() } /* use constant descend rate when landing, ignore altitude setpoint */ - if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { _vel_sp(2) = _params.land_speed; } @@ -1124,7 +1124,7 @@ MulticopterPositionControl::task_main() /* adjust limits for landing mode */ if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && - _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { /* limit max tilt and min lift when landing */ tilt_max = _params.tilt_max_land; diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index f827e70c96..a744d58cf0 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -82,7 +82,7 @@ Loiter::on_activation() mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; - _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER); + _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER); _navigator->set_position_setpoint_triplet_updated(); } diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 9b0a092dad..b87bebd0cf 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -388,7 +388,7 @@ Mission::set_mission_items() pos_sp_triplet->next.valid = false; /* reuse setpoint for LOITER only if it's not IDLE */ - _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER); + _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER); set_mission_finished(); @@ -462,7 +462,7 @@ Mission::set_mission_items() mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); /* require takeoff after landing or idle */ - if (pos_sp_triplet->current.type == SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == SETPOINT_TYPE_IDLE) { + if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { _need_takeoff = true; } diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 723caec7ce..c936489d5e 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -193,25 +193,25 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite switch (item->nav_cmd) { case NAV_CMD_IDLE: - sp->type = SETPOINT_TYPE_IDLE; + sp->type = position_setpoint_s::SETPOINT_TYPE_IDLE; break; case NAV_CMD_TAKEOFF: - sp->type = SETPOINT_TYPE_TAKEOFF; + sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; break; case NAV_CMD_LAND: - sp->type = SETPOINT_TYPE_LAND; + sp->type = position_setpoint_s::SETPOINT_TYPE_LAND; break; case NAV_CMD_LOITER_TIME_LIMIT: case NAV_CMD_LOITER_TURN_COUNT: case NAV_CMD_LOITER_UNLIMITED: - sp->type = SETPOINT_TYPE_LOITER; + sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER; break; default: - sp->type = SETPOINT_TYPE_POSITION; + sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION; break; } } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 3f7670ec4a..e35b0bd6a1 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -217,7 +217,7 @@ Navigator::vehicle_status_update() { if (orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus) != OK) { /* in case the commander is not be running */ - _vstatus.arming_state = ARMING_STATE_STANDBY; + _vstatus.arming_state = vehicle_status_s::ARMING_STATE_STANDBY; } } @@ -419,25 +419,25 @@ Navigator::task_main() /* Do stuff according to navigation state set by commander */ switch (_vstatus.nav_state) { - case NAVIGATION_STATE_MANUAL: - case NAVIGATION_STATE_ACRO: - case NAVIGATION_STATE_ALTCTL: - case NAVIGATION_STATE_POSCTL: - case NAVIGATION_STATE_LAND: - case NAVIGATION_STATE_TERMINATION: - case NAVIGATION_STATE_OFFBOARD: + case vehicle_status_s::NAVIGATION_STATE_MANUAL: + case vehicle_status_s::NAVIGATION_STATE_ACRO: + case vehicle_status_s::NAVIGATION_STATE_ALTCTL: + case vehicle_status_s::NAVIGATION_STATE_POSCTL: + case vehicle_status_s::NAVIGATION_STATE_LAND: + case vehicle_status_s::NAVIGATION_STATE_TERMINATION: + case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: _navigation_mode = nullptr; _can_loiter_at_sp = false; break; - case NAVIGATION_STATE_AUTO_MISSION: + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_mission; break; - case NAVIGATION_STATE_AUTO_LOITER: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_loiter; break; - case NAVIGATION_STATE_AUTO_RCRECOVER: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER: _pos_sp_triplet_published_invalid_once = false; if (_param_rcloss_obc.get() != 0) { _navigation_mode = &_rcLoss; @@ -445,11 +445,11 @@ Navigator::task_main() _navigation_mode = &_rtl; } break; - case NAVIGATION_STATE_AUTO_RTL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_rtl; break; - case NAVIGATION_STATE_AUTO_RTGS: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: /* Use complex data link loss mode only when enabled via param * otherwise use rtl */ _pos_sp_triplet_published_invalid_once = false; @@ -459,11 +459,11 @@ Navigator::task_main() _navigation_mode = &_rtl; } break; - case NAVIGATION_STATE_AUTO_LANDENGFAIL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_engineFailure; break; - case NAVIGATION_STATE_AUTO_LANDGPSFAIL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_gpsFailure; break; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 6aa6b6bbda..82bb1eb8e2 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -813,28 +813,28 @@ Sensors::parameters_update() _parameters.rc_offboard_th = fabs(_parameters.rc_offboard_th); /* update RC function mappings */ - _rc.function[RC_CHANNELS_FUNCTION_THROTTLE] = _parameters.rc_map_throttle - 1; - _rc.function[RC_CHANNELS_FUNCTION_ROLL] = _parameters.rc_map_roll - 1; - _rc.function[RC_CHANNELS_FUNCTION_PITCH] = _parameters.rc_map_pitch - 1; - _rc.function[RC_CHANNELS_FUNCTION_YAW] = _parameters.rc_map_yaw - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE] = _parameters.rc_map_throttle - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ROLL] = _parameters.rc_map_roll - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PITCH] = _parameters.rc_map_pitch - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_YAW] = _parameters.rc_map_yaw - 1; - _rc.function[RC_CHANNELS_FUNCTION_MODE] = _parameters.rc_map_mode_sw - 1; - _rc.function[RC_CHANNELS_FUNCTION_RETURN] = _parameters.rc_map_return_sw - 1; - _rc.function[RC_CHANNELS_FUNCTION_POSCTL] = _parameters.rc_map_posctl_sw - 1; - _rc.function[RC_CHANNELS_FUNCTION_LOITER] = _parameters.rc_map_loiter_sw - 1; - _rc.function[RC_CHANNELS_FUNCTION_ACRO] = _parameters.rc_map_acro_sw - 1; - _rc.function[RC_CHANNELS_FUNCTION_OFFBOARD] = _parameters.rc_map_offboard_sw - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_MODE] = _parameters.rc_map_mode_sw - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_RETURN] = _parameters.rc_map_return_sw - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL] = _parameters.rc_map_posctl_sw - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_LOITER] = _parameters.rc_map_loiter_sw - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ACRO] = _parameters.rc_map_acro_sw - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD] = _parameters.rc_map_offboard_sw - 1; - _rc.function[RC_CHANNELS_FUNCTION_FLAPS] = _parameters.rc_map_flaps - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS] = _parameters.rc_map_flaps - 1; - _rc.function[RC_CHANNELS_FUNCTION_AUX_1] = _parameters.rc_map_aux1 - 1; - _rc.function[RC_CHANNELS_FUNCTION_AUX_2] = _parameters.rc_map_aux2 - 1; - _rc.function[RC_CHANNELS_FUNCTION_AUX_3] = _parameters.rc_map_aux3 - 1; - _rc.function[RC_CHANNELS_FUNCTION_AUX_4] = _parameters.rc_map_aux4 - 1; - _rc.function[RC_CHANNELS_FUNCTION_AUX_5] = _parameters.rc_map_aux5 - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1] = _parameters.rc_map_aux1 - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2] = _parameters.rc_map_aux2 - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3] = _parameters.rc_map_aux3 - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4] = _parameters.rc_map_aux4 - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5] = _parameters.rc_map_aux5 - 1; for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { - _rc.function[RC_CHANNELS_FUNCTION_PARAM_1 + i] = _parameters.rc_map_param[i] - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] = _parameters.rc_map_param[i] - 1; } /* gyro offsets */ @@ -1498,7 +1498,7 @@ Sensors::rc_parameter_map_poll(bool forced) orb_copy(ORB_ID(rc_parameter_map), _rc_parameter_map_sub, &_rc_parameter_map); /* update paramter handles to which the RC channels are mapped */ for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { - if (_rc.function[RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { + if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { /* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0) * or no request to map this channel to a param has been sent via mavlink */ @@ -1673,17 +1673,17 @@ Sensors::get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mi float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f; if (on_inv ? value < on_th : value > on_th) { - return SWITCH_POS_ON; + return manual_control_setpoint_s::SWITCH_POS_ON; } else if (mid_inv ? value < mid_th : value > mid_th) { - return SWITCH_POS_MIDDLE; + return manual_control_setpoint_s::SWITCH_POS_MIDDLE; } else { - return SWITCH_POS_OFF; + return manual_control_setpoint_s::SWITCH_POS_OFF; } } else { - return SWITCH_POS_NONE; + return manual_control_setpoint_s::SWITCH_POS_NONE; } } @@ -1694,14 +1694,14 @@ Sensors::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv) float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f; if (on_inv ? value < on_th : value > on_th) { - return SWITCH_POS_ON; + return manual_control_setpoint_s::SWITCH_POS_ON; } else { - return SWITCH_POS_OFF; + return manual_control_setpoint_s::SWITCH_POS_OFF; } } else { - return SWITCH_POS_NONE; + return manual_control_setpoint_s::SWITCH_POS_NONE; } } @@ -1710,14 +1710,14 @@ Sensors::set_params_from_rc() { static float param_rc_values[RC_PARAM_MAP_NCHAN] = {}; for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { - if (_rc.function[RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { + if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { /* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0) * or no request to map this channel to a param has been sent via mavlink */ continue; } - float rc_val = get_rc_value((RC_CHANNELS_FUNCTION_PARAM_1 + i), -1.0, 1.0); + float rc_val = get_rc_value( (rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i), -1.0, 1.0); /* Check if the value has changed, * maybe we need to introduce a more aggressive limit here */ if (rc_val > param_rc_values[i] + FLT_EPSILON || rc_val < param_rc_values[i] - FLT_EPSILON) { @@ -1847,24 +1847,24 @@ Sensors::rc_poll() manual.timestamp = rc_input.timestamp_last_signal; /* limit controls */ - manual.y = get_rc_value(RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0); - manual.x = get_rc_value(RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0); - manual.r = get_rc_value(RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0); - manual.z = get_rc_value(RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0); - manual.flaps = get_rc_value(RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0); - manual.aux1 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0); - manual.aux2 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0); - manual.aux3 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0); - manual.aux4 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0); - manual.aux5 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0); + manual.y = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0); + manual.x = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0); + manual.r = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0); + manual.z = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0); + manual.flaps = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0); + manual.aux1 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0); + manual.aux2 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0); + manual.aux3 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0); + manual.aux4 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0); + manual.aux5 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0); /* mode switches */ - manual.mode_switch = get_rc_sw3pos_position(RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv); - manual.posctl_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv); - manual.return_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th, _parameters.rc_return_inv); - manual.loiter_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv); - manual.acro_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv); - manual.offboard_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv); + manual.mode_switch = get_rc_sw3pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv); + manual.posctl_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv); + manual.return_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th, _parameters.rc_return_inv); + manual.loiter_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv); + manual.acro_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv); + manual.offboard_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv); /* publish manual_control_setpoint topic */ if (_manual_control_pub > 0) { From 719edf93e4c3994d3d61bff4435d5ac0760d912b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 Jan 2015 08:01:22 +0100 Subject: [PATCH 213/269] ported more geo functions to cpp --- src/platforms/ros/geo.cpp | 93 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 93 insertions(+) diff --git a/src/platforms/ros/geo.cpp b/src/platforms/ros/geo.cpp index 6fad681c99..04094de8bb 100644 --- a/src/platforms/ros/geo.cpp +++ b/src/platforms/ros/geo.cpp @@ -171,3 +171,96 @@ __EXPORT float _wrap_360(float bearing) return bearing; } + +__EXPORT bool map_projection_initialized(const struct map_projection_reference_s *ref) +{ + return ref->init_done; +} + +__EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref) +{ + return ref->timestamp; +} + +__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +{ + + ref->lat_rad = lat_0 * M_DEG_TO_RAD; + ref->lon_rad = lon_0 * M_DEG_TO_RAD; + ref->sin_lat = sin(ref->lat_rad); + ref->cos_lat = cos(ref->lat_rad); + + ref->timestamp = timestamp; + ref->init_done = true; + + return 0; +} + +__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +{ + return map_projection_init_timestamped(ref, lat_0, lon_0, px4::get_time_micros()); +} + +__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad) +{ + if (!map_projection_initialized(ref)) { + return -1; + } + + *ref_lat_rad = ref->lat_rad; + *ref_lon_rad = ref->lon_rad; + + return 0; +} + +__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y) +{ + if (!map_projection_initialized(ref)) { + return -1; + } + + double lat_rad = lat * M_DEG_TO_RAD; + double lon_rad = lon * M_DEG_TO_RAD; + + double sin_lat = sin(lat_rad); + double cos_lat = cos(lat_rad); + double cos_d_lon = cos(lon_rad - ref->lon_rad); + + double c = acos(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon); + double k = (fabs(c) < DBL_EPSILON) ? 1.0 : (c / sin(c)); + + *x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH; + *y = k * cos_lat * sin(lon_rad - ref->lon_rad) * CONSTANTS_RADIUS_OF_EARTH; + + return 0; +} + +__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon) +{ + if (!map_projection_initialized(ref)) { + return -1; + } + + double x_rad = x / CONSTANTS_RADIUS_OF_EARTH; + double y_rad = y / CONSTANTS_RADIUS_OF_EARTH; + double c = sqrtf(x_rad * x_rad + y_rad * y_rad); + double sin_c = sin(c); + double cos_c = cos(c); + + double lat_rad; + double lon_rad; + + if (fabs(c) > DBL_EPSILON) { + lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c); + lon_rad = (ref->lon_rad + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c)); + + } else { + lat_rad = ref->lat_rad; + lon_rad = ref->lon_rad; + } + + *lat = lat_rad * 180.0 / M_PI; + *lon = lon_rad * 180.0 / M_PI; + + return 0; +} From 6ba1912309fef5b7aa3fc2cda73a124b6b01a01f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 Jan 2015 08:02:08 +0100 Subject: [PATCH 214/269] add angle conversion defines for ros --- src/platforms/px4_defines.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index c8e2cf290b..fa4e1398e2 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -135,6 +135,8 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) #define M_IVLN10_F 0.43429448190325182765f /* 1 / log(10) */ #define M_LOG2_E_F _M_LN2_F #define M_INVLN2_F 1.4426950408889633870E0f /* 1 / log(2) */ +#define M_DEG_TO_RAD 0.01745329251994 +#define M_RAD_TO_DEG 57.2957795130823 #else /* From 19155372810e2f70d273b7d06069748d1e01071b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 Jan 2015 08:04:00 +0100 Subject: [PATCH 215/269] initial port of multiplatform version of mc_pos_control --- CMakeLists.txt | 15 + ROMFS/px4fmu_common/init.d/rc.mc_apps | 8 +- makefiles/config_px4fmu-v2_multiplatform.mk | 3 +- .../mc_pos_control.cpp | 947 ++++++++++++++++++ .../mc_pos_control.h | 219 ++++ .../mc_pos_control_main.cpp | 63 ++ .../mc_pos_control_params.c | 212 ++++ .../mc_pos_control_params.h | 61 ++ .../mc_pos_control_start_nuttx.cpp | 99 ++ .../mc_pos_control_multiplatform/module.mk | 43 + src/platforms/px4_includes.h | 8 + 11 files changed, 1676 insertions(+), 2 deletions(-) create mode 100644 src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp create mode 100644 src/modules/mc_pos_control_multiplatform/mc_pos_control.h create mode 100644 src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp create mode 100644 src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c create mode 100644 src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h create mode 100644 src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp create mode 100644 src/modules/mc_pos_control_multiplatform/module.mk diff --git a/CMakeLists.txt b/CMakeLists.txt index f76dbbf418..6a368d78fc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -69,6 +69,11 @@ add_message_files( actuator_armed.msg parameter_update.msg vehicle_status.msg + vehicle_local_position.msg + position_setpoint.msg + position_setpoint_triplet.msg + vehicle_local_position_setpoint.msg + vehicle_global_velocity_setpoint.msg ) ## Generate services in the 'srv' folder @@ -180,6 +185,16 @@ target_link_libraries(mc_att_control px4 ) +## MC Position Control +add_executable(mc_pos_control + src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp + src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp) +add_dependencies(mc_pos_control ${PROJECT_NAME}_generate_messages_cpp_cpp) +target_link_libraries(mc_pos_control + ${catkin_LIBRARIES} + px4 +) + ## Attitude Estimator dummy add_executable(attitude_estimator src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 7b29fb3a7b..2ecc104df7 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -14,7 +14,13 @@ else # try the multiplatform version mc_att_control_m start fi -mc_pos_control start + +if mc_pos_control start +then +else + # try the multiplatform version + mc_pos_control_m start +fi # # Start Land Detector diff --git a/makefiles/config_px4fmu-v2_multiplatform.mk b/makefiles/config_px4fmu-v2_multiplatform.mk index 76edade4b8..29eb680960 100644 --- a/makefiles/config_px4fmu-v2_multiplatform.mk +++ b/makefiles/config_px4fmu-v2_multiplatform.mk @@ -89,7 +89,8 @@ MODULES += modules/position_estimator_inav MODULES += modules/mc_att_control_multiplatform MODULES += examples/subscriber MODULES += examples/publisher -MODULES += modules/mc_pos_control +# MODULES += modules/mc_pos_control +MODULES += modules/mc_pos_control_multiplatform MODULES += modules/vtol_att_control # diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp new file mode 100644 index 0000000000..2685a99c32 --- /dev/null +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -0,0 +1,947 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_pos_control.cpp + * Multicopter position controller. + * + * @author Anton Babushkin + * @author Thomas Gubler + */ + +#include "mc_pos_control.h" +#include "mc_pos_control_params.h" + +#define TILT_COS_MAX 0.7f +#define SIGMA 0.000001f +#define MIN_DIST 0.01f + +MulticopterPositionControl::MulticopterPositionControl() : + + _task_should_exit(false), + _control_task(-1), + _mavlink_fd(-1), + +/* publications */ + _att_sp_pub(nullptr), + _local_pos_sp_pub(nullptr), + _global_vel_sp_pub(nullptr), + + _ref_alt(0.0f), + _ref_timestamp(0), + + _reset_pos_sp(true), + _reset_alt_sp(true), + _mode_auto(false), + _thrust_int(), + _R() +{ + memset(&_ref_pos, 0, sizeof(_ref_pos)); + + /* + * Do subscriptions + */ + _att = _n.subscribe(&MulticopterPositionControl::handle_vehicle_attitude, this, 0); + _v_att_sp = _n.subscribe(0); + _control_mode = _n.subscribe(0); + _parameter_update = _n.subscribe( + &MulticopterPositionControl::handle_parameter_update, this, 1000); + _manual_control_sp = _n.subscribe(0); + _armed = _n.subscribe(0); + _local_pos_sp = _n.subscribe(0); + _global_vel_sp = _n.subscribe(0); + + + + _params.pos_p.zero(); + _params.vel_p.zero(); + _params.vel_i.zero(); + _params.vel_d.zero(); + _params.vel_max.zero(); + _params.vel_ff.zero(); + _params.sp_offs_max.zero(); + + _pos.zero(); + _pos_sp.zero(); + _vel.zero(); + _vel_sp.zero(); + _vel_prev.zero(); + _vel_ff.zero(); + _sp_move_rate.zero(); + + _params_handles.thr_min = PX4_PARAM_INIT(MPC_THR_MIN); + _params_handles.thr_max = PX4_PARAM_INIT(MPC_THR_MAX); + _params_handles.z_p = PX4_PARAM_INIT(MPC_Z_P); + _params_handles.z_vel_p = PX4_PARAM_INIT(MPC_Z_VEL_P); + _params_handles.z_vel_i = PX4_PARAM_INIT(MPC_Z_VEL_I); + _params_handles.z_vel_d = PX4_PARAM_INIT(MPC_Z_VEL_D); + _params_handles.z_vel_max = PX4_PARAM_INIT(MPC_Z_VEL_MAX); + _params_handles.z_ff = PX4_PARAM_INIT(MPC_Z_FF); + _params_handles.xy_p = PX4_PARAM_INIT(MPC_XY_P); + _params_handles.xy_vel_p = PX4_PARAM_INIT(MPC_XY_VEL_P); + _params_handles.xy_vel_i = PX4_PARAM_INIT(MPC_XY_VEL_I); + _params_handles.xy_vel_d = PX4_PARAM_INIT(MPC_XY_VEL_D); + _params_handles.xy_vel_max = PX4_PARAM_INIT(MPC_XY_VEL_MAX); + _params_handles.xy_ff = PX4_PARAM_INIT(MPC_XY_FF); + _params_handles.tilt_max_air = PX4_PARAM_INIT(MPC_TILTMAX_AIR); + _params_handles.land_speed = PX4_PARAM_INIT(MPC_LAND_SPEED); + _params_handles.tilt_max_land = PX4_PARAM_INIT(MPC_TILTMAX_LND); + + /* fetch initial parameter values */ + parameters_update(); + + _R.identity(); +} + +MulticopterPositionControl::~MulticopterPositionControl() +{ +} + +int +MulticopterPositionControl::parameters_update() +{ + PX4_PARAM_GET(_params_handles.thr_min, &_params.thr_min); + PX4_PARAM_GET(_params_handles.thr_max, &_params.thr_max); + PX4_PARAM_GET(_params_handles.tilt_max_air, &_params.tilt_max_air); + _params.tilt_max_air = math::radians(_params.tilt_max_air); + PX4_PARAM_GET(_params_handles.land_speed, &_params.land_speed); + PX4_PARAM_GET(_params_handles.tilt_max_land, &_params.tilt_max_land); + _params.tilt_max_land = math::radians(_params.tilt_max_land); + + float v; + PX4_PARAM_GET(_params_handles.xy_p, &v); + _params.pos_p(0) = v; + _params.pos_p(1) = v; + PX4_PARAM_GET(_params_handles.z_p, &v); + _params.pos_p(2) = v; + PX4_PARAM_GET(_params_handles.xy_vel_p, &v); + _params.vel_p(0) = v; + _params.vel_p(1) = v; + PX4_PARAM_GET(_params_handles.z_vel_p, &v); + _params.vel_p(2) = v; + PX4_PARAM_GET(_params_handles.xy_vel_i, &v); + _params.vel_i(0) = v; + _params.vel_i(1) = v; + PX4_PARAM_GET(_params_handles.z_vel_i, &v); + _params.vel_i(2) = v; + PX4_PARAM_GET(_params_handles.xy_vel_d, &v); + _params.vel_d(0) = v; + _params.vel_d(1) = v; + PX4_PARAM_GET(_params_handles.z_vel_d, &v); + _params.vel_d(2) = v; + PX4_PARAM_GET(_params_handles.xy_vel_max, &v); + _params.vel_max(0) = v; + _params.vel_max(1) = v; + PX4_PARAM_GET(_params_handles.z_vel_max, &v); + _params.vel_max(2) = v; + PX4_PARAM_GET(_params_handles.xy_ff, &v); + v = math::constrain(v, 0.0f, 1.0f); + _params.vel_ff(0) = v; + _params.vel_ff(1) = v; + PX4_PARAM_GET(_params_handles.z_ff, &v); + v = math::constrain(v, 0.0f, 1.0f); + _params.vel_ff(2) = v; + + _params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f; + + return OK; +} + + +float +MulticopterPositionControl::scale_control(float ctl, float end, float dz) +{ + if (ctl > dz) { + return (ctl - dz) / (end - dz); + + } else if (ctl < -dz) { + return (ctl + dz) / (end - dz); + + } else { + return 0.0f; + } +} + +void +MulticopterPositionControl::update_ref() +{ + if (_local_pos->data().ref_timestamp != _ref_timestamp) { + double lat_sp, lon_sp; + float alt_sp = 0.0f; + + if (_ref_timestamp != 0) { + /* calculate current position setpoint in global frame */ + map_projection_reproject(&_ref_pos, _pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp); + alt_sp = _ref_alt - _pos_sp(2); + } + + /* update local projection reference */ + map_projection_init(&_ref_pos, _local_pos->data().ref_lat, _local_pos->data().ref_lon); + _ref_alt = _local_pos->data().ref_alt; + + if (_ref_timestamp != 0) { + /* reproject position setpoint to new reference */ + map_projection_project(&_ref_pos, lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]); + _pos_sp(2) = -(alt_sp - _ref_alt); + } + + _ref_timestamp = _local_pos->data().ref_timestamp; + } +} + +void +MulticopterPositionControl::reset_pos_sp() +{ + if (_reset_pos_sp) { + _reset_pos_sp = false; + /* shift position setpoint to make attitude setpoint continuous */ + _pos_sp(0) = _pos(0) + (_vel(0) - PX4_R(_att_sp_msg.data().R_body, 0, 2) * _att_sp_msg.data().thrust / _params.vel_p(0) + - _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0); + _pos_sp(1) = _pos(1) + (_vel(1) - PX4_R(_att_sp_msg.data().R_body, 1, 2) * _att_sp_msg.data().thrust / _params.vel_p(1) + - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1); + + //XXX: port this once a mavlink like interface is available + // mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1)); + PX4_INFO("[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1)); + } +} + +void +MulticopterPositionControl::reset_alt_sp() +{ + if (_reset_alt_sp) { + _reset_alt_sp = false; + _pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2); + + //XXX: port this once a mavlink like interface is available + // mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2)); + PX4_INFO("[mpc] reset alt sp: %d", -(int)_pos_sp(2)); + } +} + +void +MulticopterPositionControl::limit_pos_sp_offset() +{ + math::Vector<3> pos_sp_offs; + pos_sp_offs.zero(); + + if (_control_mode->data().flag_control_position_enabled) { + pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); + pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); + } + + if (_control_mode->data().flag_control_altitude_enabled) { + pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); + } + + float pos_sp_offs_norm = pos_sp_offs.length(); + + if (pos_sp_offs_norm > 1.0f) { + pos_sp_offs /= pos_sp_offs_norm; + _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); + } +} + +void +MulticopterPositionControl::control_manual(float dt) +{ + _sp_move_rate.zero(); + + if (_control_mode->data().flag_control_altitude_enabled) { + /* move altitude setpoint with throttle stick */ + _sp_move_rate(2) = -scale_control(_manual_control_sp->data().z - 0.5f, 0.5f, alt_ctl_dz); + } + + if (_control_mode->data().flag_control_position_enabled) { + /* move position setpoint with roll/pitch stick */ + _sp_move_rate(0) = _manual_control_sp->data().x; + _sp_move_rate(1) = _manual_control_sp->data().y; + } + + /* limit setpoint move rate */ + float sp_move_norm = _sp_move_rate.length(); + + if (sp_move_norm > 1.0f) { + _sp_move_rate /= sp_move_norm; + } + + /* _sp_move_rate scaled to 0..1, scale it to max speed and rotate around yaw */ + math::Matrix<3, 3> R_yaw_sp; + R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp_msg.data().yaw_body); + _sp_move_rate = R_yaw_sp * _sp_move_rate.emult(_params.vel_max); + + if (_control_mode->data().flag_control_altitude_enabled) { + /* reset alt setpoint to current altitude if needed */ + reset_alt_sp(); + } + + if (_control_mode->data().flag_control_position_enabled) { + /* reset position setpoint to current position if needed */ + reset_pos_sp(); + } + + /* feed forward setpoint move rate with weight vel_ff */ + _vel_ff = _sp_move_rate.emult(_params.vel_ff); + + /* move position setpoint */ + _pos_sp += _sp_move_rate * dt; + + /* check if position setpoint is too far from actual position */ + math::Vector<3> pos_sp_offs; + pos_sp_offs.zero(); + + if (_control_mode->data().flag_control_position_enabled) { + pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); + pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); + } + + if (_control_mode->data().flag_control_altitude_enabled) { + pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); + } + + float pos_sp_offs_norm = pos_sp_offs.length(); + + if (pos_sp_offs_norm > 1.0f) { + pos_sp_offs /= pos_sp_offs_norm; + _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); + } +} + +void +MulticopterPositionControl::control_offboard(float dt) +{ + if (_pos_sp_triplet->data().current.valid) { + if (_control_mode->data().flag_control_position_enabled && _pos_sp_triplet->data().current.position_valid) { + /* control position */ + _pos_sp(0) = _pos_sp_triplet->data().current.x; + _pos_sp(1) = _pos_sp_triplet->data().current.y; + } else if (_control_mode->data().flag_control_velocity_enabled && _pos_sp_triplet->data().current.velocity_valid) { + /* control velocity */ + /* reset position setpoint to current position if needed */ + reset_pos_sp(); + + /* set position setpoint move rate */ + _sp_move_rate(0) = _pos_sp_triplet->data().current.vx; + _sp_move_rate(1) = _pos_sp_triplet->data().current.vy; + } + + if (_pos_sp_triplet->data().current.yaw_valid) { + _att_sp_msg.data().yaw_body = _pos_sp_triplet->data().current.yaw; + } else if (_pos_sp_triplet->data().current.yawspeed_valid) { + _att_sp_msg.data().yaw_body = _att_sp_msg.data().yaw_body + _pos_sp_triplet->data().current.yawspeed * dt; + } + + if (_control_mode->data().flag_control_altitude_enabled && _pos_sp_triplet->data().current.position_valid) { + /* Control altitude */ + _pos_sp(2) = _pos_sp_triplet->data().current.z; + } else if (_control_mode->data().flag_control_climb_rate_enabled && _pos_sp_triplet->data().current.velocity_valid) { + /* reset alt setpoint to current altitude if needed */ + reset_alt_sp(); + + /* set altitude setpoint move rate */ + _sp_move_rate(2) = _pos_sp_triplet->data().current.vz; + } + + /* feed forward setpoint move rate with weight vel_ff */ + _vel_ff = _sp_move_rate.emult(_params.vel_ff); + + /* move position setpoint */ + _pos_sp += _sp_move_rate * dt; + + } else { + reset_pos_sp(); + reset_alt_sp(); + } +} + +bool +MulticopterPositionControl::cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r, + const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res) +{ + /* project center of sphere on line */ + /* normalized AB */ + math::Vector<3> ab_norm = line_b - line_a; + ab_norm.normalize(); + math::Vector<3> d = line_a + ab_norm * ((sphere_c - line_a) * ab_norm); + float cd_len = (sphere_c - d).length(); + + /* we have triangle CDX with known CD and CX = R, find DX */ + if (sphere_r > cd_len) { + /* have two roots, select one in A->B direction from D */ + float dx_len = sqrtf(sphere_r * sphere_r - cd_len * cd_len); + res = d + ab_norm * dx_len; + return true; + + } else { + /* have no roots, return D */ + res = d; + return false; + } +} + +void +MulticopterPositionControl::control_auto(float dt) +{ + if (!_mode_auto) { + _mode_auto = true; + /* reset position setpoint on AUTO mode activation */ + reset_pos_sp(); + reset_alt_sp(); + } + + if (_pos_sp_triplet->data().current.valid) { + /* in case of interrupted mission don't go to waypoint but stay at current position */ + _reset_pos_sp = true; + _reset_alt_sp = true; + + /* project setpoint to local frame */ + math::Vector<3> curr_sp; + map_projection_project(&_ref_pos, + _pos_sp_triplet->data().current.lat, _pos_sp_triplet->data().current.lon, + &curr_sp.data[0], &curr_sp.data[1]); + curr_sp(2) = -(_pos_sp_triplet->data().current.alt - _ref_alt); + + /* scaled space: 1 == position error resulting max allowed speed, L1 = 1 in this space */ + math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max); // TODO add mult param here + + /* convert current setpoint to scaled space */ + math::Vector<3> curr_sp_s = curr_sp.emult(scale); + + /* by default use current setpoint as is */ + math::Vector<3> pos_sp_s = curr_sp_s; + + if (_pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_POSITION && _pos_sp_triplet->data().previous.valid) { + /* follow "previous - current" line */ + math::Vector<3> prev_sp; + map_projection_project(&_ref_pos, + _pos_sp_triplet->data().previous.lat, _pos_sp_triplet->data().previous.lon, + &prev_sp.data[0], &prev_sp.data[1]); + prev_sp(2) = -(_pos_sp_triplet->data().previous.alt - _ref_alt); + + if ((curr_sp - prev_sp).length() > MIN_DIST) { + + /* find X - cross point of L1 sphere and trajectory */ + math::Vector<3> pos_s = _pos.emult(scale); + math::Vector<3> prev_sp_s = prev_sp.emult(scale); + math::Vector<3> prev_curr_s = curr_sp_s - prev_sp_s; + math::Vector<3> curr_pos_s = pos_s - curr_sp_s; + float curr_pos_s_len = curr_pos_s.length(); + if (curr_pos_s_len < 1.0f) { + /* copter is closer to waypoint than L1 radius */ + /* check next waypoint and use it to avoid slowing down when passing via waypoint */ + if (_pos_sp_triplet->data().next.valid) { + math::Vector<3> next_sp; + map_projection_project(&_ref_pos, + _pos_sp_triplet->data().next.lat, _pos_sp_triplet->data().next.lon, + &next_sp.data[0], &next_sp.data[1]); + next_sp(2) = -(_pos_sp_triplet->data().next.alt - _ref_alt); + + if ((next_sp - curr_sp).length() > MIN_DIST) { + math::Vector<3> next_sp_s = next_sp.emult(scale); + + /* calculate angle prev - curr - next */ + math::Vector<3> curr_next_s = next_sp_s - curr_sp_s; + math::Vector<3> prev_curr_s_norm = prev_curr_s.normalized(); + + /* cos(a) * curr_next, a = angle between current and next trajectory segments */ + float cos_a_curr_next = prev_curr_s_norm * curr_next_s; + + /* cos(b), b = angle pos - curr_sp - prev_sp */ + float cos_b = -curr_pos_s * prev_curr_s_norm / curr_pos_s_len; + + if (cos_a_curr_next > 0.0f && cos_b > 0.0f) { + float curr_next_s_len = curr_next_s.length(); + /* if curr - next distance is larger than L1 radius, limit it */ + if (curr_next_s_len > 1.0f) { + cos_a_curr_next /= curr_next_s_len; + } + + /* feed forward position setpoint offset */ + math::Vector<3> pos_ff = prev_curr_s_norm * + cos_a_curr_next * cos_b * cos_b * (1.0f - curr_pos_s_len) * + (1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f)); + pos_sp_s += pos_ff; + } + } + } + + } else { + bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, pos_sp_s); + if (near) { + /* L1 sphere crosses trajectory */ + + } else { + /* copter is too far from trajectory */ + /* if copter is behind prev waypoint, go directly to prev waypoint */ + if ((pos_sp_s - prev_sp_s) * prev_curr_s < 0.0f) { + pos_sp_s = prev_sp_s; + } + + /* if copter is in front of curr waypoint, go directly to curr waypoint */ + if ((pos_sp_s - curr_sp_s) * prev_curr_s > 0.0f) { + pos_sp_s = curr_sp_s; + } + + pos_sp_s = pos_s + (pos_sp_s - pos_s).normalized(); + } + } + } + } + + /* move setpoint not faster than max allowed speed */ + math::Vector<3> pos_sp_old_s = _pos_sp.emult(scale); + + /* difference between current and desired position setpoints, 1 = max speed */ + math::Vector<3> d_pos_m = (pos_sp_s - pos_sp_old_s).edivide(_params.pos_p); + float d_pos_m_len = d_pos_m.length(); + if (d_pos_m_len > dt) { + pos_sp_s = pos_sp_old_s + (d_pos_m / d_pos_m_len * dt).emult(_params.pos_p); + } + + /* scale result back to normal space */ + _pos_sp = pos_sp_s.edivide(scale); + + /* update yaw setpoint if needed */ + if (isfinite(_pos_sp_triplet->data().current.yaw)) { + _att_sp_msg.data().yaw_body = _pos_sp_triplet->data().current.yaw; + } + + } else { + /* no waypoint, do nothing, setpoint was already reset */ + } +} + +void MulticopterPositionControl::handle_parameter_update(const px4_parameter_update &msg) +{ + parameters_update(); +} + +void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_attitude &msg) +{ + static bool reset_int_z = true; + static bool reset_int_z_manual = false; + static bool reset_int_xy = true; + static bool was_armed = false; + static uint64_t t_prev = 0; + + uint64_t t = get_time_micros(); + float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.0f; + t_prev = t; + + if (_control_mode->data().flag_armed && !was_armed) { + /* reset setpoints and integrals on arming */ + _reset_pos_sp = true; + _reset_alt_sp = true; + reset_int_z = true; + reset_int_xy = true; + } + + was_armed = _control_mode->data().flag_armed; + + update_ref(); + + if (_control_mode->data().flag_control_altitude_enabled || + _control_mode->data().flag_control_position_enabled || + _control_mode->data().flag_control_climb_rate_enabled || + _control_mode->data().flag_control_velocity_enabled) { + + _pos(0) = _local_pos->data().x; + _pos(1) = _local_pos->data().y; + _pos(2) = _local_pos->data().z; + + _vel(0) = _local_pos->data().vx; + _vel(1) = _local_pos->data().vy; + _vel(2) = _local_pos->data().vz; + + _vel_ff.zero(); + _sp_move_rate.zero(); + + /* select control source */ + if (_control_mode->data().flag_control_manual_enabled) { + /* manual control */ + control_manual(dt); + _mode_auto = false; + + } else if (_control_mode->data().flag_control_offboard_enabled) { + /* offboard control */ + control_offboard(dt); + _mode_auto = false; + + } else { + /* AUTO */ + control_auto(dt); + } + + /* fill local position setpoint */ + _local_pos_sp_msg.data().timestamp = get_time_micros(); + _local_pos_sp_msg.data().x = _pos_sp(0); + _local_pos_sp_msg.data().y = _pos_sp(1); + _local_pos_sp_msg.data().z = _pos_sp(2); + _local_pos_sp_msg.data().yaw = _att_sp_msg.data().yaw_body; + + /* publish local position setpoint */ + if (_local_pos_sp_pub != nullptr) { + _local_pos_sp_pub->publish(_local_pos_sp_msg); + + } else { + _local_pos_sp_pub = _n.advertise(); + } + + + if (!_control_mode->data().flag_control_manual_enabled && _pos_sp_triplet->data().current.valid && _pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_IDLE) { + /* idle state, don't run controller and set zero thrust */ + _R.identity(); + memcpy(&_att_sp_msg.data().R_body[0], _R.data, sizeof(_att_sp_msg.data().R_body)); + _att_sp_msg.data().R_valid = true; + + _att_sp_msg.data().roll_body = 0.0f; + _att_sp_msg.data().pitch_body = 0.0f; + _att_sp_msg.data().yaw_body = _att->data().yaw; + _att_sp_msg.data().thrust = 0.0f; + + _att_sp_msg.data().timestamp = get_time_micros(); + + /* publish attitude setpoint */ + if (_att_sp_pub != nullptr) { + _att_sp_pub->publish(_att_sp_msg); + + } else { + _att_sp_pub = _n.advertise(); + } + + } else { + /* run position & altitude controllers, calculate velocity setpoint */ + math::Vector<3> pos_err = _pos_sp - _pos; + + _vel_sp = pos_err.emult(_params.pos_p) + _vel_ff; + + if (!_control_mode->data().flag_control_altitude_enabled) { + _reset_alt_sp = true; + _vel_sp(2) = 0.0f; + } + + if (!_control_mode->data().flag_control_position_enabled) { + _reset_pos_sp = true; + _vel_sp(0) = 0.0f; + _vel_sp(1) = 0.0f; + } + + /* use constant descend rate when landing, ignore altitude setpoint */ + if (!_control_mode->data().flag_control_manual_enabled && _pos_sp_triplet->data().current.valid && _pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_LAND) { + _vel_sp(2) = _params.land_speed; + } + + _global_vel_sp_msg.data().vx = _vel_sp(0); + _global_vel_sp_msg.data().vy = _vel_sp(1); + _global_vel_sp_msg.data().vz = _vel_sp(2); + + /* publish velocity setpoint */ + if (_global_vel_sp_pub != nullptr) { + _global_vel_sp_pub->publish(_global_vel_sp_msg); + + } else { + _global_vel_sp_pub = _n.advertise(); + } + + if (_control_mode->data().flag_control_climb_rate_enabled || _control_mode->data().flag_control_velocity_enabled) { + /* reset integrals if needed */ + if (_control_mode->data().flag_control_climb_rate_enabled) { + if (reset_int_z) { + reset_int_z = false; + float i = _params.thr_min; + + if (reset_int_z_manual) { + i = _manual_control_sp->data().z; + + if (i < _params.thr_min) { + i = _params.thr_min; + + } else if (i > _params.thr_max) { + i = _params.thr_max; + } + } + + _thrust_int(2) = -i; + } + + } else { + reset_int_z = true; + } + + if (_control_mode->data().flag_control_velocity_enabled) { + if (reset_int_xy) { + reset_int_xy = false; + _thrust_int(0) = 0.0f; + _thrust_int(1) = 0.0f; + } + + } else { + reset_int_xy = true; + } + + /* velocity error */ + math::Vector<3> vel_err = _vel_sp - _vel; + + /* derivative of velocity error, not includes setpoint acceleration */ + math::Vector<3> vel_err_d = (_sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt; + _vel_prev = _vel; + + /* thrust vector in NED frame */ + math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + _thrust_int; + + if (!_control_mode->data().flag_control_velocity_enabled) { + thrust_sp(0) = 0.0f; + thrust_sp(1) = 0.0f; + } + + if (!_control_mode->data().flag_control_climb_rate_enabled) { + thrust_sp(2) = 0.0f; + } + + /* limit thrust vector and check for saturation */ + bool saturation_xy = false; + bool saturation_z = false; + + /* limit min lift */ + float thr_min = _params.thr_min; + + if (!_control_mode->data().flag_control_velocity_enabled && thr_min < 0.0f) { + /* don't allow downside thrust direction in manual attitude mode */ + thr_min = 0.0f; + } + + float tilt_max = _params.tilt_max_air; + + /* adjust limits for landing mode */ + if (!_control_mode->data().flag_control_manual_enabled && _pos_sp_triplet->data().current.valid && + _pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_LAND) { + /* limit max tilt and min lift when landing */ + tilt_max = _params.tilt_max_land; + + if (thr_min < 0.0f) { + thr_min = 0.0f; + } + } + + /* limit min lift */ + if (-thrust_sp(2) < thr_min) { + thrust_sp(2) = -thr_min; + saturation_z = true; + } + + if (_control_mode->data().flag_control_velocity_enabled) { + /* limit max tilt */ + if (thr_min >= 0.0f && tilt_max < M_PI_F / 2 - 0.05f) { + /* absolute horizontal thrust */ + float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); + + if (thrust_sp_xy_len > 0.01f) { + /* max horizontal thrust for given vertical thrust*/ + float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max); + + if (thrust_sp_xy_len > thrust_xy_max) { + float k = thrust_xy_max / thrust_sp_xy_len; + thrust_sp(0) *= k; + thrust_sp(1) *= k; + saturation_xy = true; + } + } + } + + } else { + /* thrust compensation for altitude only control mode */ + float att_comp; + + if (PX4_R(_att->data().R, 2, 2) > TILT_COS_MAX) { + att_comp = 1.0f / PX4_R(_att->data().R, 2, 2); + + } else if (PX4_R(_att->data().R, 2, 2) > 0.0f) { + att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * PX4_R(_att->data().R, 2, 2) + 1.0f; + saturation_z = true; + + } else { + att_comp = 1.0f; + saturation_z = true; + } + + thrust_sp(2) *= att_comp; + } + + /* limit max thrust */ + float thrust_abs = thrust_sp.length(); + + if (thrust_abs > _params.thr_max) { + if (thrust_sp(2) < 0.0f) { + if (-thrust_sp(2) > _params.thr_max) { + /* thrust Z component is too large, limit it */ + thrust_sp(0) = 0.0f; + thrust_sp(1) = 0.0f; + thrust_sp(2) = -_params.thr_max; + saturation_xy = true; + saturation_z = true; + + } else { + /* preserve thrust Z component and lower XY, keeping altitude is more important than position */ + float thrust_xy_max = sqrtf(_params.thr_max * _params.thr_max - thrust_sp(2) * thrust_sp(2)); + float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); + float k = thrust_xy_max / thrust_xy_abs; + thrust_sp(0) *= k; + thrust_sp(1) *= k; + saturation_xy = true; + } + + } else { + /* Z component is negative, going down, simply limit thrust vector */ + float k = _params.thr_max / thrust_abs; + thrust_sp *= k; + saturation_xy = true; + saturation_z = true; + } + + thrust_abs = _params.thr_max; + } + + /* update integrals */ + if (_control_mode->data().flag_control_velocity_enabled && !saturation_xy) { + _thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt; + _thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt; + } + + if (_control_mode->data().flag_control_climb_rate_enabled && !saturation_z) { + _thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt; + + /* protection against flipping on ground when landing */ + if (_thrust_int(2) > 0.0f) { + _thrust_int(2) = 0.0f; + } + } + + /* calculate attitude setpoint from thrust vector */ + if (_control_mode->data().flag_control_velocity_enabled) { + /* desired body_z axis = -normalize(thrust_vector) */ + math::Vector<3> body_x; + math::Vector<3> body_y; + math::Vector<3> body_z; + + if (thrust_abs > SIGMA) { + body_z = -thrust_sp / thrust_abs; + + } else { + /* no thrust, set Z axis to safe value */ + body_z.zero(); + body_z(2) = 1.0f; + } + + /* vector of desired yaw direction in XY plane, rotated by PI/2 */ + math::Vector<3> y_C(-sinf(_att_sp_msg.data().yaw_body), cosf(_att_sp_msg.data().yaw_body), 0.0f); + + if (fabsf(body_z(2)) > SIGMA) { + /* desired body_x axis, orthogonal to body_z */ + body_x = y_C % body_z; + + /* keep nose to front while inverted upside down */ + if (body_z(2) < 0.0f) { + body_x = -body_x; + } + + body_x.normalize(); + + } else { + /* desired thrust is in XY plane, set X downside to construct correct matrix, + * but yaw component will not be used actually */ + body_x.zero(); + body_x(2) = 1.0f; + } + + /* desired body_y axis */ + body_y = body_z % body_x; + + /* fill rotation matrix */ + for (int i = 0; i < 3; i++) { + _R(i, 0) = body_x(i); + _R(i, 1) = body_y(i); + _R(i, 2) = body_z(i); + } + + /* copy rotation matrix to attitude setpoint topic */ + memcpy(&_att_sp_msg.data().R_body[0], _R.data, sizeof(_att_sp_msg.data().R_body)); + _att_sp_msg.data().R_valid = true; + + /* calculate euler angles, for logging only, must not be used for control */ + math::Vector<3> euler = _R.to_euler(); + _att_sp_msg.data().roll_body = euler(0); + _att_sp_msg.data().pitch_body = euler(1); + /* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */ + + } else if (!_control_mode->data().flag_control_manual_enabled) { + /* autonomous altitude control without position control (failsafe landing), + * force level attitude, don't change yaw */ + _R.from_euler(0.0f, 0.0f, _att_sp_msg.data().yaw_body); + + /* copy rotation matrix to attitude setpoint topic */ + memcpy(&_att_sp_msg.data().R_body[0], _R.data, sizeof(_att_sp_msg.data().R_body)); + _att_sp_msg.data().R_valid = true; + + _att_sp_msg.data().roll_body = 0.0f; + _att_sp_msg.data().pitch_body = 0.0f; + } + + _att_sp_msg.data().thrust = thrust_abs; + + _att_sp_msg.data().timestamp = get_time_micros(); + + /* publish attitude setpoint */ + if (_att_sp_pub != nullptr) { + _att_sp_pub->publish(_att_sp_msg); + + } else { + _att_sp_pub = _n.advertise(); + } + + } else { + reset_int_z = true; + } + } + + } else { + /* position controller disabled, reset setpoints */ + _reset_alt_sp = true; + _reset_pos_sp = true; + _mode_auto = false; + reset_int_z = true; + reset_int_xy = true; + } + + /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ + reset_int_z_manual = _control_mode->data().flag_armed && _control_mode->data().flag_control_manual_enabled && !_control_mode->data().flag_control_climb_rate_enabled; +} diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h new file mode 100644 index 0000000000..05bd1387be --- /dev/null +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h @@ -0,0 +1,219 @@ +/**************************************************************************** + * + * Copyright (c) 2013 - 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_pos_control.h + * Multicopter position controller. + * + * @author Anton Babushkin + * @author Thomas Gubler + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +// #include +// #include +// #include +// #include +#include +#include +// #include + +using namespace px4; + +class MulticopterPositionControl +{ +public: + /** + * Constructor + */ + MulticopterPositionControl(); + + /** + * Destructor, also kills task. + */ + ~MulticopterPositionControl(); + + /* Callbacks for topics */ + void handle_vehicle_attitude(const px4_vehicle_attitude &msg); + void handle_parameter_update(const px4_parameter_update &msg); + + void spin() { _n.spin(); } + +protected: + const float alt_ctl_dz = 0.2f; + + bool _task_should_exit; /**< if true, task should exit */ + int _control_task; /**< task handle for task */ + int _mavlink_fd; /**< mavlink fd */ + + Publisher *_att_sp_pub; /**< attitude setpoint publication */ + Publisher *_local_pos_sp_pub; /**< vehicle local position setpoint publication */ + Publisher *_global_vel_sp_pub; /**< vehicle global velocity setpoint publication */ + + + Subscriber *_att; /**< vehicle attitude */ + Subscriber *_v_att_sp; /**< vehicle attitude setpoint */ + Subscriber *_control_mode; /**< vehicle control mode */ + Subscriber *_parameter_update; /**< parameter update */ + Subscriber *_manual_control_sp; /**< manual control setpoint */ + Subscriber *_armed; /**< actuator arming status */ + Subscriber *_local_pos; /**< local position */ + Subscriber *_pos_sp_triplet; /**< local position */ + Subscriber *_local_pos_sp; /**< local position */ + Subscriber *_global_vel_sp; /**< local position */ + + px4_vehicle_attitude_setpoint _att_sp_msg; + px4_vehicle_local_position_setpoint _local_pos_sp_msg; + px4_vehicle_global_velocity_setpoint _global_vel_sp_msg; + + px4::NodeHandle _n; + + + struct { + px4_param_t thr_min; + px4_param_t thr_max; + px4_param_t z_p; + px4_param_t z_vel_p; + px4_param_t z_vel_i; + px4_param_t z_vel_d; + px4_param_t z_vel_max; + px4_param_t z_ff; + px4_param_t xy_p; + px4_param_t xy_vel_p; + px4_param_t xy_vel_i; + px4_param_t xy_vel_d; + px4_param_t xy_vel_max; + px4_param_t xy_ff; + px4_param_t tilt_max_air; + px4_param_t land_speed; + px4_param_t tilt_max_land; + } _params_handles; /**< handles for interesting parameters */ + + struct { + float thr_min; + float thr_max; + float tilt_max_air; + float land_speed; + float tilt_max_land; + + math::Vector<3> pos_p; + math::Vector<3> vel_p; + math::Vector<3> vel_i; + math::Vector<3> vel_d; + math::Vector<3> vel_ff; + math::Vector<3> vel_max; + math::Vector<3> sp_offs_max; + } _params; + + struct map_projection_reference_s _ref_pos; + float _ref_alt; + uint64_t _ref_timestamp; + + bool _reset_pos_sp; + bool _reset_alt_sp; + bool _mode_auto; + + math::Vector<3> _pos; + math::Vector<3> _pos_sp; + math::Vector<3> _vel; + math::Vector<3> _vel_sp; + math::Vector<3> _vel_prev; /**< velocity on previous step */ + math::Vector<3> _vel_ff; + math::Vector<3> _sp_move_rate; + + math::Vector<3> _thrust_int; + math::Matrix<3, 3> _R; + + /** + * Update our local parameter cache. + */ + int parameters_update(); + + /** + * Update control outputs + */ + void control_update(); + + static float scale_control(float ctl, float end, float dz); + + /** + * Update reference for local position projection + */ + void update_ref(); + /** + * Reset position setpoint to current position + */ + void reset_pos_sp(); + + /** + * Reset altitude setpoint to current altitude + */ + void reset_alt_sp(); + + /** + * Check if position setpoint is too far from current position and adjust it if needed. + */ + void limit_pos_sp_offset(); + + /** + * Set position setpoint using manual control + */ + void control_manual(float dt); + + /** + * Set position setpoint using offboard control + */ + void control_offboard(float dt); + + bool cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r, + const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res); + + /** + * Set position setpoint for AUTO + */ + void control_auto(float dt); + + /** + * Select between barometric and global (AMSL) altitudes + */ + void select_alt(bool global); +}; diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp new file mode 100644 index 0000000000..0b5775736e --- /dev/null +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp @@ -0,0 +1,63 @@ +/**************************************************************************** + * + * Copyright (c) 2013 - 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_pos_control_main.cpp + * Multicopter position controller. + * + * The controller has two loops: P loop for position error and PID loop for velocity error. + * Output of velocity controller is thrust vector that splitted to thrust direction + * (i.e. rotation matrix for multicopter orientation) and thrust module (i.e. multicopter thrust itself). + * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging. + * + * @author Anton Babushkin + * @author Thomas Gubler + */ + +#include "mc_pos_control.h" + +bool thread_running = false; /**< Deamon status flag */ + +int main(int argc, char **argv) +{ + px4::init(argc, argv, "mc_pos_control_m"); + + PX4_INFO("starting"); + MulticopterPositionControl posctl; + thread_running = true; + posctl.spin(); + + PX4_INFO("exiting."); + thread_running = false; + return 0; +} diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c new file mode 100644 index 0000000000..c741a7f0a6 --- /dev/null +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c @@ -0,0 +1,212 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Anton Babushkin + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_pos_control_params.c + * Multicopter position controller parameters. + * + * @author Anton Babushkin + */ + +#include +#include "mc_pos_control_params.h" +#include + +/** + * Minimum thrust + * + * Minimum vertical thrust. It's recommended to set it > 0 to avoid free fall with zero thrust. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_THR_MIN); + +/** + * Maximum thrust + * + * Limit max allowed thrust. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_THR_MAX); + +/** + * Proportional gain for vertical position error + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_Z_P); + +/** + * Proportional gain for vertical velocity error + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_P); + +/** + * Integral gain for vertical velocity error + * + * Non zero value allows hovering thrust estimation on stabilized or autonomous takeoff. + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_I); + +/** + * Differential gain for vertical velocity error + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_D); + +/** + * Maximum vertical velocity + * + * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTCTRL). + * + * @unit m/s + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX); + +/** + * Vertical velocity feed forward + * + * Feed forward weight for altitude control in stabilized modes (ALTCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_Z_FF); + +/** + * Proportional gain for horizontal position error + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_XY_P); + +/** + * Proportional gain for horizontal velocity error + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_P); + +/** + * Integral gain for horizontal velocity error + * + * Non-zero value allows to resist wind. + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_I); + +/** + * Differential gain for horizontal velocity error. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_D); + +/** + * Maximum horizontal velocity + * + * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSCTRL). + * + * @unit m/s + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX); + +/** + * Horizontal velocity feed forward + * + * Feed forward weight for position control in position control mode (POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_XY_FF); + +/** + * Maximum tilt angle in air + * + * Limits maximum tilt in AUTO and POSCTRL modes during flight. + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR); + +/** + * Maximum tilt during landing + * + * Limits maximum tilt angle on landing. + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND); + +/** + * Landing descend rate + * + * @unit m/s + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_LAND_SPEED); + diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h new file mode 100644 index 0000000000..fec3bcb7cc --- /dev/null +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h @@ -0,0 +1,61 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Anton Babushkin + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_pos_control_params.h + * Multicopter position controller parameters. + * + * @author Anton Babushkin + */ + +#pragma once + +#define PARAM_MPC_THR_MIN_DEFAULT 0.1f +#define PARAM_MPC_THR_MAX_DEFAULT 1.0f +#define PARAM_MPC_Z_P_DEFAULT 1.0f +#define PARAM_MPC_Z_VEL_P_DEFAULT 0.1f +#define PARAM_MPC_Z_VEL_I_DEFAULT 0.02f +#define PARAM_MPC_Z_VEL_D_DEFAULT 0.0f +#define PARAM_MPC_Z_VEL_MAX_DEFAULT 5.0f +#define PARAM_MPC_Z_FF_DEFAULT 0.5f +#define PARAM_MPC_XY_P_DEFAULT 1.0f +#define PARAM_MPC_XY_VEL_P_DEFAULT 0.1f +#define PARAM_MPC_XY_VEL_I_DEFAULT 0.02f +#define PARAM_MPC_XY_VEL_D_DEFAULT 0.01f +#define PARAM_MPC_XY_VEL_MAX_DEFAULT 5.0f +#define PARAM_MPC_XY_FF_DEFAULT 0.5f +#define PARAM_MPC_TILTMAX_AIR_DEFAULT 45.0f +#define PARAM_MPC_TILTMAX_LND_DEFAULT 15.0f +#define PARAM_MPC_LAND_SPEED_DEFAULT 1.0f + diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp new file mode 100644 index 0000000000..87996d93ba --- /dev/null +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * Copyright (C) 2013 - 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_pos_control_m_start_nuttx.cpp + * + * @author Thomas Gubler + */ +#include +#include +#include +#include + +extern bool thread_running; +int daemon_task; /**< Handle of deamon task / thread */ +namespace px4 +{ +bool task_should_exit = false; +} +using namespace px4; + +extern int main(int argc, char **argv); + +extern "C" __EXPORT int mc_pos_control_m_main(int argc, char *argv[]); +int mc_pos_control_m_main(int argc, char *argv[]) +{ + if (argc < 1) { + errx(1, "usage: mc_pos_control_m {start|stop|status}"); + } + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("already running"); + /* this is not an error */ + exit(0); + } + + task_should_exit = false; + + daemon_task = task_spawn_cmd("mc_pos_control_m", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 3000, + main, + (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + task_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("is running"); + + } else { + warnx("not started"); + } + + exit(0); + } + + warnx("unrecognized command"); + return 1; +} diff --git a/src/modules/mc_pos_control_multiplatform/module.mk b/src/modules/mc_pos_control_multiplatform/module.mk new file mode 100644 index 0000000000..2852ebbec8 --- /dev/null +++ b/src/modules/mc_pos_control_multiplatform/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Build multicopter position controller +# + +MODULE_COMMAND = mc_pos_control_m + +SRCS = mc_pos_control_main.cpp \ + mc_pos_control_start_nuttx.cpp \ + mc_pos_control.cpp \ + mc_pos_control_params.c diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index 1bd4509caf..f8561fa3b7 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -62,6 +62,10 @@ #include #include #include +#include +#include +#include +#include #endif #else @@ -85,6 +89,10 @@ #include #include #include +#include +#include +#include +#include #endif #include #include From 8e7974e2e29a75daf5a55e723f6d6a4b416c252f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 Jan 2015 08:25:42 +0100 Subject: [PATCH 216/269] fix uorb constants for test functions --- .../state_machine_helper_test.cpp | 346 +++++++++--------- 1 file changed, 173 insertions(+), 173 deletions(-) diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 3cfa8b4c6c..4ddb4e0fbf 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -73,7 +73,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) bool armed; // actuator_armed_s.armed bool ready_to_arm; // actuator_armed_s.ready_to_arm } ArmingTransitionVolatileState_t; - + // This structure represents a test case for arming_state_transition. It contains the machine // state prior to transtion, the requested state to transition to and finally the expected // machine state after transition. @@ -88,7 +88,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) ArmingTransitionVolatileState_t expected_state; // Expected machine state after transition transition_result_t expected_transition_result; // Expected result from arming_state_transition } ArmingTransitionTest_t; - + // We use these defines so that our test cases are more readable #define ATT_ARMED true #define ATT_DISARMED false @@ -100,182 +100,182 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) #define ATT_SAFETY_NOT_AVAILABLE true #define ATT_SAFETY_OFF true #define ATT_SAFETY_ON false - + // These are test cases for arming_state_transition static const ArmingTransitionTest_t rgArmingTransitionTests[] = { // TRANSITION_NOT_CHANGED tests - + { "no transition: identical states", - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_INIT, - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_NOT_CHANGED }, - + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_INIT, + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_NOT_CHANGED }, + // TRANSITION_CHANGED tests - + // Check all basic valid transitions, these don't require special state in vehicle_status_t or safety_s - + { "transition: init to standby", - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY, - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY, + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: init to standby error", - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY_ERROR, - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY_ERROR, + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: init to reboot", - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_REBOOT, - { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_REBOOT, + { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: standby to init", - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_INIT, - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_INIT, + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: standby to standby error", - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY_ERROR, - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY_ERROR, + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: standby to reboot", - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_REBOOT, - { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_REBOOT, + { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: armed to standby", - { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY, - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY, + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: armed to armed error", - { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_ARMED_ERROR, - { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_ARMED_ERROR, + { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: armed error to standby error", - { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY_ERROR, - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY_ERROR, + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: standby error to reboot", - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_REBOOT, - { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_REBOOT, + { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: in air restore to armed", - { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_ARMED, - { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_ARMED, + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: in air restore to reboot", - { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_REBOOT, - { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_REBOOT, + { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + // hil on tests, standby error to standby not normally allowed - + { "transition: standby error to standby, hil on", - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY, - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY, + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + // Safety switch arming tests - + { "transition: standby to armed, no safety switch", - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF, - ARMING_STATE_ARMED, - { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF, + vehicle_status_s::ARMING_STATE_ARMED, + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: standby to armed, safety switch off", - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF, - ARMING_STATE_ARMED, - { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF, + vehicle_status_s::ARMING_STATE_ARMED, + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + // standby error { "transition: armed error to standby error requested standby", - { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY, - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY, + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + // TRANSITION_DENIED tests - + // Check some important basic invalid transitions, these don't require special state in vehicle_status_t or safety_s - + { "no transition: init to armed", - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_ARMED, - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_ARMED, + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + { "no transition: standby to armed error", - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_ARMED_ERROR, - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_ARMED_ERROR, + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, + { "no transition: armed to init", - { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_INIT, - { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_INIT, + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, + { "no transition: armed to reboot", - { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_REBOOT, - { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_REBOOT, + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, + { "no transition: armed error to armed", - { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_ARMED, - { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_ARMED, + { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + { "no transition: armed error to reboot", - { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_REBOOT, - { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_REBOOT, + { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + { "no transition: standby error to armed", - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_ARMED, - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_ARMED, + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + { "no transition: standby error to standby", - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY, - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY, + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + { "no transition: reboot to armed", - { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_ARMED, - { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_ARMED, + { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + { "no transition: in air restore to standby", - { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY, - { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY, + { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + // Sensor tests - + { "no transition: init to standby - sensors not initialized", - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY, - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY, + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + // Safety switch arming tests - + { "no transition: init to standby, safety switch on", - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_ARMED, - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_ARMED, + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, }; - + struct vehicle_status_s status; struct safety_s safety; struct actuator_armed_s armed; - + size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]); for (size_t i=0; icurrent_state.arming_state; status.condition_system_sensors_initialized = test->condition_system_sensors_initialized; @@ -286,10 +286,10 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) safety.safety_off = test->safety_off; armed.armed = test->current_state.armed; armed.ready_to_arm = test->current_state.ready_to_arm; - + // Attempt transition transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, false /* no pre-arm checks */, 0 /* no mavlink_fd */); - + // Validate result of transition ut_assert(test->assertMsg, test->expected_transition_result == result); ut_assert(test->assertMsg, status.arming_state == test->expected_state.arming_state); @@ -310,7 +310,7 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) main_state_t to_state; // State to transition to transition_result_t expected_transition_result; // Expected result from main_state_transition call } MainTransitionTest_t; - + // Bits for condition_bits #define MTT_ALL_NOT_VALID 0 #define MTT_ROTARY_WING 1 << 0 @@ -318,108 +318,108 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) #define MTT_LOC_POS_VALID 1 << 2 #define MTT_HOME_POS_VALID 1 << 3 #define MTT_GLOBAL_POS_VALID 1 << 4 - + static const MainTransitionTest_t rgMainTransitionTests[] = { - + // TRANSITION_NOT_CHANGED tests - + { "no transition: identical states", MTT_ALL_NOT_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED }, - + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED }, + // TRANSITION_CHANGED tests - + { "transition: MANUAL to ACRO", MTT_ALL_NOT_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_ACRO, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ACRO, TRANSITION_CHANGED }, { "transition: ACRO to MANUAL", MTT_ALL_NOT_VALID, - MAIN_STATE_ACRO, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_ACRO, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, { "transition: MANUAL to AUTO_MISSION - global position valid, home position valid", MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED }, { "transition: AUTO_MISSION to MANUAL - global position valid, home position valid", MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, - MAIN_STATE_AUTO_MISSION, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_AUTO_MISSION, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, { "transition: MANUAL to AUTO_LOITER - global position valid", MTT_GLOBAL_POS_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED }, { "transition: AUTO_LOITER to MANUAL - global position valid", MTT_GLOBAL_POS_VALID, - MAIN_STATE_AUTO_LOITER, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_AUTO_LOITER, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, { "transition: MANUAL to AUTO_RTL - global position valid, home position valid", MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED }, { "transition: AUTO_RTL to MANUAL - global position valid, home position valid", MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, - MAIN_STATE_AUTO_RTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_AUTO_RTL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, { "transition: MANUAL to ALTCTL - not rotary", MTT_ALL_NOT_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, { "transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude valid", MTT_ROTARY_WING | MTT_LOC_ALT_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, - + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, + { "transition: MANUAL to ALTCTL - rotary, global position valid, local altitude not valid", MTT_ROTARY_WING | MTT_GLOBAL_POS_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, - + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, + { "transition: ALTCTL to MANUAL - local altitude valid", MTT_LOC_ALT_VALID, - MAIN_STATE_ALTCTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_ALTCTL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, { "transition: MANUAL to POSCTL - local position not valid, global position valid", MTT_GLOBAL_POS_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED }, { "transition: MANUAL to POSCTL - local position valid, global position not valid", MTT_LOC_POS_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED }, { "transition: POSCTL to MANUAL - local position valid, global position valid", MTT_LOC_POS_VALID, - MAIN_STATE_POSCTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_POSCTL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, // TRANSITION_DENIED tests { "no transition: MANUAL to AUTO_MISSION - global position not valid", MTT_ALL_NOT_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED }, - + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED }, + { "no transition: MANUAL to AUTO_LOITER - global position not valid", MTT_ALL_NOT_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED }, - + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED }, + { "no transition: MANUAL to AUTO_RTL - global position not valid, home position not valid", MTT_ALL_NOT_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, - + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, + { "no transition: MANUAL to AUTO_RTL - global position not valid, home position valid", MTT_HOME_POS_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, { "no transition: MANUAL to AUTO_RTL - global position valid, home position not valid", MTT_GLOBAL_POS_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, - + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, + { "no transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude not valid", MTT_ROTARY_WING, - MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_DENIED }, - + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_DENIED }, + { "no transition: MANUAL to POSCTL - local position not valid, global position not valid", MTT_ALL_NOT_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_DENIED }, + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_POSCTL, TRANSITION_DENIED }, }; - + size_t cMainTransitionTests = sizeof(rgMainTransitionTests) / sizeof(rgMainTransitionTests[0]); for (size_t i=0; icondition_bits & MTT_LOC_POS_VALID; current_state.condition_home_position_valid = test->condition_bits & MTT_HOME_POS_VALID; current_state.condition_global_position_valid = test->condition_bits & MTT_GLOBAL_POS_VALID; - + // Attempt transition transition_result_t result = main_state_transition(¤t_state, test->to_state); - + // Validate result of transition ut_assert(test->assertMsg, test->expected_transition_result == result); if (test->expected_transition_result == result) { @@ -495,8 +495,8 @@ bool StateMachineHelperTest::run_tests(void) ut_run_test(armingStateTransitionTest); ut_run_test(mainStateTransitionTest); ut_run_test(isSafeTest); - + return (_tests_failed == 0); } -ut_declare_test(stateMachineHelperTest, StateMachineHelperTest) \ No newline at end of file +ut_declare_test(stateMachineHelperTest, StateMachineHelperTest) From 66007d56ef35ebc1f11ac83f2347bfc22b9664f9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 Jan 2015 09:08:58 +0100 Subject: [PATCH 217/269] fix uorb constants in uavcan module --- src/modules/uavcan/uavcan_main.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 4dc03b61b7..b93a95f965 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -381,7 +381,7 @@ int UavcanNode::run() } else { // get controls for required topics bool controls_updated = false; - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { if (_poll_fds[_poll_ids[i]].revents & POLLIN) { controls_updated = true; @@ -393,14 +393,14 @@ int UavcanNode::run() /* see if we have any direct actuator updates */ - if (_actuator_direct_sub != -1 && + if (_actuator_direct_sub != -1 && (_poll_fds[_actuator_direct_poll_fd_num].revents & POLLIN) && orb_copy(ORB_ID(actuator_direct), _actuator_direct_sub, &_actuator_direct) == OK && !_test_in_progress) { if (_actuator_direct.nvalues > NUM_ACTUATOR_OUTPUTS) { _actuator_direct.nvalues = NUM_ACTUATOR_OUTPUTS; } - memcpy(&_outputs.output[0], &_actuator_direct.values[0], + memcpy(&_outputs.output[0], &_actuator_direct.values[0], _actuator_direct.nvalues*sizeof(float)); _outputs.noutputs = _actuator_direct.nvalues; new_output = true; @@ -476,7 +476,7 @@ int UavcanNode::run() if (updated) { orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); - // Update the armed status and check that we're not locked down and motor + // Update the armed status and check that we're not locked down and motor // test is not running bool set_armed = _armed.armed && !_armed.lockdown && !_test_in_progress; @@ -502,7 +502,7 @@ UavcanNode::control_callback(uintptr_t handle, uint8_t control_group, uint8_t co int UavcanNode::teardown() { - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { ::close(_control_subs[i]); _control_subs[i] = -1; @@ -526,7 +526,7 @@ UavcanNode::subscribe() uint32_t sub_groups = _groups_required & ~_groups_subscribed; uint32_t unsub_groups = _groups_subscribed & ~_groups_required; // the first fd used by CAN - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (sub_groups & (1 << i)) { warnx("subscribe to actuator_controls_%d", i); _control_subs[i] = orb_subscribe(_control_topics[i]); From a6ca4c2796a825ac1878c1d570afc0afc4fed74d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 Jan 2015 11:17:23 +0100 Subject: [PATCH 218/269] initialize all subscribers --- src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp index 2685a99c32..aa23e0a60d 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -78,6 +78,8 @@ MulticopterPositionControl::MulticopterPositionControl() : &MulticopterPositionControl::handle_parameter_update, this, 1000); _manual_control_sp = _n.subscribe(0); _armed = _n.subscribe(0); + _local_pos = _n.subscribe(0); + _pos_sp_triplet = _n.subscribe(0); _local_pos_sp = _n.subscribe(0); _global_vel_sp = _n.subscribe(0); From 9245f28fb84e87505c6024121720bbd0776e64a0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 Jan 2015 17:46:22 +0100 Subject: [PATCH 219/269] mc attctl multiplatform: fix memcpy --- .../mc_att_control_multiplatform/mc_att_control_base.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp index fcfee28dc7..5577c5f03e 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp @@ -97,7 +97,8 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) if (_v_control_mode->data().flag_control_velocity_enabled || _v_control_mode->data().flag_control_climb_rate_enabled) { /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ - memcpy(&_v_att_sp_mod, &_v_att_sp->data(), sizeof(_v_att_sp_mod)); + //XXX get rid of memcpy + memcpy(&(_v_att_sp_mod.data()), &_v_att_sp->data(), sizeof(_v_att_sp_mod.data())); } if (!_v_control_mode->data().flag_control_climb_rate_enabled) { @@ -156,7 +157,8 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) } else { /* in non-manual mode use 'vehicle_attitude_setpoint' topic */ - memcpy(&_v_att_sp_mod, &_v_att_sp->data(), sizeof(_v_att_sp_mod)); + //XXX get rid of memcpy + memcpy(&(_v_att_sp_mod.data()), &_v_att_sp->data(), sizeof(_v_att_sp_mod.data())); /* reset yaw setpoint after non-manual control mode */ _reset_yaw_sp = true; From 8a6b94adbfa58245d13354216168de86e888c782 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 29 Jan 2015 10:28:56 +0100 Subject: [PATCH 220/269] attitude estimator dummy node: add timestamp --- .../ros/nodes/attitude_estimator/attitude_estimator.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp index bcee0b4799..f744aa11c2 100644 --- a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp +++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp @@ -43,6 +43,7 @@ #include #include #include +#include AttitudeEstimator::AttitudeEstimator() : _n(), @@ -92,6 +93,7 @@ void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstP // msg_v_att.pitchspeed = -(float)msg->twist[index].angular.y; // msg_v_att.yawspeed = -(float)msg->twist[index].angular.z; + msg_v_att.timestamp = px4::get_time_micros(); _vehicle_attitude_pub.publish(msg_v_att); } From 6f4f5d637de51272a2bc9fd3a8db0a2755d763c6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 29 Jan 2015 10:29:15 +0100 Subject: [PATCH 221/269] first version of position estimator dummy node --- CMakeLists.txt | 9 ++ .../position_estimator/position_estimator.cpp | 107 ++++++++++++++++++ .../position_estimator/position_estimator.h | 62 ++++++++++ 3 files changed, 178 insertions(+) create mode 100644 src/platforms/ros/nodes/position_estimator/position_estimator.cpp create mode 100644 src/platforms/ros/nodes/position_estimator/position_estimator.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 6a368d78fc..0c81607b00 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -204,6 +204,15 @@ target_link_libraries(attitude_estimator px4 ) +## Position Estimator dummy +add_executable(position_estimator + src/platforms/ros/nodes/position_estimator/position_estimator.cpp) +add_dependencies(position_estimator ${PROJECT_NAME}_generate_messages_cpp_cpp) +target_link_libraries(position_estimator + ${catkin_LIBRARIES} + px4 +) + ## Manual input add_executable(manual_input src/platforms/ros/nodes/manual_input/manual_input.cpp) diff --git a/src/platforms/ros/nodes/position_estimator/position_estimator.cpp b/src/platforms/ros/nodes/position_estimator/position_estimator.cpp new file mode 100644 index 0000000000..231e9763a8 --- /dev/null +++ b/src/platforms/ros/nodes/position_estimator/position_estimator.cpp @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file position_estimator.cpp + * + * @author Thomas Gubler + * @author Roman Bapst +*/ + +#include "position_estimator.h" + +#include +#include +#include +#include +#include +#include +#include + +PositionEstimator::PositionEstimator() : + _n(), + _sub_modelstates(_n.subscribe("/gazebo/model_states", 1, &PositionEstimator::model_states_callback, this)), + _vehicle_position_pub(_n.advertise("vehicle_local_position", 1)), + _startup_time(px4::get_time_micros()) +{ +} + +void PositionEstimator::model_states_callback(const gazebo_msgs::ModelStatesConstPtr &msg) +{ + //XXX: use a proper sensor topic + + px4::vehicle_local_position msg_v_l_pos; + + /* Fill px4 position topic with contents from modelstates topic */ + int index = 0; + //XXX: maybe a more clever approach would be to do this not on every loop, need to check if and when + //gazebo rearranges indexes. + for(std::vector::const_iterator it = msg->name.begin(); it != msg->name.end(); ++it) { + if (*it == "iris" || *it == "ardrone") { + index = it - msg->name.begin(); + break; + } + } + msg_v_l_pos.xy_valid = true; + msg_v_l_pos.z_valid = true; + msg_v_l_pos.v_xy_valid = true; + msg_v_l_pos.v_z_valid = true; + + msg_v_l_pos.x = msg->pose[index].position.x; + msg_v_l_pos.y = -msg->pose[index].position.y; + msg_v_l_pos.z = -msg->pose[index].position.z; + msg_v_l_pos.vx = msg->twist[index].linear.x; + msg_v_l_pos.vy = -msg->twist[index].linear.y; + msg_v_l_pos.vz = -msg->twist[index].linear.z; + + msg_v_l_pos.xy_global = true; + msg_v_l_pos.z_global = true; + msg_v_l_pos.ref_timestamp = _startup_time; + msg_v_l_pos.ref_lat = 47.378301; + msg_v_l_pos.ref_lon = 8.538777; + msg_v_l_pos.ref_alt = 1200.0f; + + + msg_v_l_pos.timestamp = px4::get_time_micros(); + _vehicle_position_pub.publish(msg_v_l_pos); +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "position_estimator"); + PositionEstimator m; + + ros::spin(); + + return 0; +} diff --git a/src/platforms/ros/nodes/position_estimator/position_estimator.h b/src/platforms/ros/nodes/position_estimator/position_estimator.h new file mode 100644 index 0000000000..40c4baa235 --- /dev/null +++ b/src/platforms/ros/nodes/position_estimator/position_estimator.h @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file position_estimator.h + * Dummy position estimator that forwards attitude from gazebo to px4 topic + * + * @author Thomas Gubler +*/ + +#include "ros/ros.h" +#include +#include + +class PositionEstimator +{ +public: + PositionEstimator(); + + ~PositionEstimator() {} + +protected: + void model_states_callback(const gazebo_msgs::ModelStatesConstPtr &msg); + + ros::NodeHandle _n; + ros::Subscriber _sub_modelstates; + ros::Publisher _vehicle_position_pub; + + uint64_t _startup_time; + + +}; From 8b62e003f0edbd8fe82286eaead83f0b3ef88a8c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 29 Jan 2015 10:30:07 +0100 Subject: [PATCH 222/269] add dummy position estimator to launch file --- launch/multicopter.launch | 1 + 1 file changed, 1 insertion(+) diff --git a/launch/multicopter.launch b/launch/multicopter.launch index 96ff3ad99c..5d1a351262 100644 --- a/launch/multicopter.launch +++ b/launch/multicopter.launch @@ -6,6 +6,7 @@ + From 9a5ea31784bacb57b9a86134fb612119609fc08c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 29 Jan 2015 10:33:12 +0100 Subject: [PATCH 223/269] add pos controller to launch file --- launch/multicopter.launch | 1 + 1 file changed, 1 insertion(+) diff --git a/launch/multicopter.launch b/launch/multicopter.launch index 5d1a351262..0f8cc5132d 100644 --- a/launch/multicopter.launch +++ b/launch/multicopter.launch @@ -8,6 +8,7 @@ + From 5b5a4b73864bb3331830d232fc3bb1bf7372d844 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 29 Jan 2015 13:36:53 +0100 Subject: [PATCH 224/269] function rename --- .../ros/nodes/position_estimator/position_estimator.cpp | 4 ++-- .../ros/nodes/position_estimator/position_estimator.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/platforms/ros/nodes/position_estimator/position_estimator.cpp b/src/platforms/ros/nodes/position_estimator/position_estimator.cpp index 231e9763a8..fae304bf49 100644 --- a/src/platforms/ros/nodes/position_estimator/position_estimator.cpp +++ b/src/platforms/ros/nodes/position_estimator/position_estimator.cpp @@ -50,13 +50,13 @@ PositionEstimator::PositionEstimator() : _n(), - _sub_modelstates(_n.subscribe("/gazebo/model_states", 1, &PositionEstimator::model_states_callback, this)), + _sub_modelstates(_n.subscribe("/gazebo/model_states", 1, &PositionEstimator::ModelStatesCallback, this)), _vehicle_position_pub(_n.advertise("vehicle_local_position", 1)), _startup_time(px4::get_time_micros()) { } -void PositionEstimator::model_states_callback(const gazebo_msgs::ModelStatesConstPtr &msg) +void PositionEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg) { //XXX: use a proper sensor topic diff --git a/src/platforms/ros/nodes/position_estimator/position_estimator.h b/src/platforms/ros/nodes/position_estimator/position_estimator.h index 40c4baa235..4d396f39a9 100644 --- a/src/platforms/ros/nodes/position_estimator/position_estimator.h +++ b/src/platforms/ros/nodes/position_estimator/position_estimator.h @@ -50,7 +50,7 @@ public: ~PositionEstimator() {} protected: - void model_states_callback(const gazebo_msgs::ModelStatesConstPtr &msg); + void ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg); ros::NodeHandle _n; ros::Subscriber _sub_modelstates; From 3c79b2a586286615ef00e1584c7c2f74887e38cd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 29 Jan 2015 13:37:37 +0100 Subject: [PATCH 225/269] manual input dummy node: extend to support switching between modes also fixing thrust input --- .../ros/nodes/manual_input/manual_input.cpp | 80 ++++++++++++++----- .../ros/nodes/manual_input/manual_input.h | 23 +++++- 2 files changed, 82 insertions(+), 21 deletions(-) diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp index 688df50e0c..7e1ae7a174 100644 --- a/src/platforms/ros/nodes/manual_input/manual_input.cpp +++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp @@ -40,13 +40,14 @@ #include "manual_input.h" -#include #include ManualInput::ManualInput() : _n(), _joy_sub(_n.subscribe("joy", 1, &ManualInput::JoyCallback, this)), - _man_ctrl_sp_pub(_n.advertise("manual_control_setpoint", 1)) + _man_ctrl_sp_pub(_n.advertise("manual_control_setpoint", 1)), + _buttons_state(), + _msg_mc_sp() { double dz_default = 0.2; /* Load parameters, default values work for Microsoft XBox Controller */ @@ -63,40 +64,46 @@ ManualInput::ManualInput() : _n.param("map_z", _param_axes_map[2], 2); _n.param("scale_z", _param_axes_scale[2], -0.5); _n.param("off_z", _param_axes_offset[2], -1.0); - _n.param("dz_z", _param_axes_dz[2], dz_default); + _n.param("dz_z", _param_axes_dz[2], 0.0); _n.param("map_r", _param_axes_map[3], 0); _n.param("scale_r", _param_axes_scale[3], -1.0); _n.param("off_r", _param_axes_offset[3], 0.0); _n.param("dz_r", _param_axes_dz[3], dz_default); + _n.param("map_manual", _param_buttons_map[0], 0); + _n.param("map_altctl", _param_buttons_map[1], 1); + _n.param("map_posctl", _param_buttons_map[2], 2); + _n.param("map_auto_mission", _param_buttons_map[3], 3); + _n.param("map_auto_loiter", _param_buttons_map[4], 4); + _n.param("map_auto_rtl", _param_buttons_map[5], 4); + + /* Default to manual */ + _msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + _msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + _msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + _msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + _msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + } void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr &msg) { - px4::manual_control_setpoint msg_out; /* Fill px4 manual control setpoint topic with contents from ros joystick */ /* Map sticks to x, y, z, r */ - MapAxis(msg, _param_axes_map[0], _param_axes_scale[0], _param_axes_offset[0], _param_axes_dz[0], msg_out.x); - MapAxis(msg, _param_axes_map[1], _param_axes_scale[1], _param_axes_offset[1], _param_axes_dz[1], msg_out.y); - MapAxis(msg, _param_axes_map[2], _param_axes_scale[2], _param_axes_offset[2], _param_axes_dz[2], msg_out.z); - MapAxis(msg, _param_axes_map[3], _param_axes_scale[3], _param_axes_offset[3], _param_axes_dz[3], msg_out.r); - //ROS_INFO("x: %1.4f y: %1.4f z: %1.4f r: %1.4f", msg_out.x, msg_out.y, msg_out.z, msg_out.r); + MapAxis(msg, _param_axes_map[0], _param_axes_scale[0], _param_axes_offset[0], _param_axes_dz[0], _msg_mc_sp.x); + MapAxis(msg, _param_axes_map[1], _param_axes_scale[1], _param_axes_offset[1], _param_axes_dz[1], _msg_mc_sp.y); + MapAxis(msg, _param_axes_map[2], _param_axes_scale[2], _param_axes_offset[2], _param_axes_dz[2], _msg_mc_sp.z); + MapAxis(msg, _param_axes_map[3], _param_axes_scale[3], _param_axes_offset[3], _param_axes_dz[3], _msg_mc_sp.r); + //ROS_INFO("x: %1.4f y: %1.4f z: %1.4f r: %1.4f", msg_mc_sp.x, msg_out.y, msg_out.z, msg_out.r); /* Map buttons to switches */ - //XXX todo - /* for now just publish switches in position for manual flight */ - msg_out.mode_switch = 0; - msg_out.return_switch = 0; - msg_out.posctl_switch = 0; - msg_out.loiter_switch = 0; - msg_out.acro_switch = 0; - msg_out.offboard_switch = 0; + MapButtons(msg, _msg_mc_sp); - msg_out.timestamp = px4::get_time_micros(); + _msg_mc_sp.timestamp = px4::get_time_micros(); - _man_ctrl_sp_pub.publish(msg_out); + _man_ctrl_sp_pub.publish(_msg_mc_sp); } void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, double scale, double offset, @@ -106,6 +113,41 @@ void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, do if (val + offset > deadzone || val + offset < -deadzone) { out = (float)((val + offset)) * scale; + } else { + out = 0.0f; + } +} + +void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_control_setpoint &msg_mc_sp) { + msg_mc_sp.acro_switch = px4::manual_control_setpoint::SWITCH_POS_NONE; + msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_NONE; + + if (_buttons_state[MAIN_STATE_MANUAL] != msg->buttons[_param_buttons_map[MAIN_STATE_MANUAL]] == true) { + msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + return; + + } else if (_buttons_state[MAIN_STATE_ALTCTL] != msg->buttons[_param_buttons_map[MAIN_STATE_ALTCTL]] == true) { + msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_MIDDLE; + msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + return; + + } else if (_buttons_state[MAIN_STATE_POSCTL] != msg->buttons[_param_buttons_map[MAIN_STATE_POSCTL]] == true) { + msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_MIDDLE; + msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_ON; + msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + return; + + } else { + ROS_WARN("requested mode via joystick that is not implemented"); } } diff --git a/src/platforms/ros/nodes/manual_input/manual_input.h b/src/platforms/ros/nodes/manual_input/manual_input.h index 93e0abe64d..bf704f6757 100644 --- a/src/platforms/ros/nodes/manual_input/manual_input.h +++ b/src/platforms/ros/nodes/manual_input/manual_input.h @@ -39,6 +39,7 @@ */ #include "ros/ros.h" +#include #include class ManualInput @@ -55,20 +56,38 @@ protected: void JoyCallback(const sensor_msgs::JoyConstPtr &msg); /** - * Helper function to map and scale joystick input + * Helper function to map and scale joystick axis */ void MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, double scale, double offset, double deadzone, float &out); + /** + * Helper function to map joystick buttons + */ + void MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_control_setpoint &msg_mc_sp); ros::NodeHandle _n; ros::Subscriber _joy_sub; ros::Publisher _man_ctrl_sp_pub; /* Parameters */ + enum MAIN_STATE { + MAIN_STATE_MANUAL = 0, + MAIN_STATE_ALTCTL, + MAIN_STATE_POSCTL, + MAIN_STATE_AUTO_MISSION, + MAIN_STATE_AUTO_LOITER, + MAIN_STATE_AUTO_RTL, + MAIN_STATE_MAX + }; + + int _param_buttons_map[MAIN_STATE_MAX]; /**< joystick button mapping, order according to MAIN_STATE */ + bool _buttons_state[MAIN_STATE_MAX]; /**< joystick button state of last iteration, + order according to MAIN_STATE */ + int _param_axes_map[4]; double _param_axes_scale[4]; double _param_axes_offset[4]; double _param_axes_dz[4]; - + px4::manual_control_setpoint _msg_mc_sp; }; From 5237364a5a6a5e765bbb30cb20eb399dcd14489a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 29 Jan 2015 13:38:39 +0100 Subject: [PATCH 226/269] commander dummy node: extend to support switching between modes --- .../ros/nodes/commander/commander.cpp | 64 +++++++++++++++---- src/platforms/ros/nodes/commander/commander.h | 9 +++ 2 files changed, 61 insertions(+), 12 deletions(-) diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp index b9fc296f9d..fee32b55f9 100644 --- a/src/platforms/ros/nodes/commander/commander.cpp +++ b/src/platforms/ros/nodes/commander/commander.cpp @@ -40,9 +40,6 @@ #include "commander.h" -#include -#include -#include #include Commander::Commander() : @@ -62,12 +59,9 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon px4::vehicle_control_mode msg_vehicle_control_mode; px4::vehicle_status msg_vehicle_status; - /* fill vehicle control mode */ - //XXX hardcoded + /* fill vehicle control mode based on (faked) stick positions*/ + EvalSwitches(msg, msg_vehicle_status, msg_vehicle_control_mode); msg_vehicle_control_mode.timestamp = px4::get_time_micros(); - msg_vehicle_control_mode.flag_control_manual_enabled = true; - msg_vehicle_control_mode.flag_control_rates_enabled = true; - msg_vehicle_control_mode.flag_control_attitude_enabled = true; /* fill actuator armed */ float arm_th = 0.95; @@ -77,21 +71,19 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon /* Check for disarm */ if (msg->r < -arm_th && msg->z < (1 - arm_th)) { _msg_actuator_armed.armed = false; + msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_STANDBY; } } else { /* Check for arm */ if (msg->r > arm_th && msg->z < (1 - arm_th)) { _msg_actuator_armed.armed = true; + msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_ARMED; } } /* fill vehicle status */ - //XXX hardcoded msg_vehicle_status.timestamp = px4::get_time_micros(); - msg_vehicle_status.main_state = msg_vehicle_status.MAIN_STATE_MANUAL; - msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_MANUAL; - msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_ARMED; msg_vehicle_status.hil_state = msg_vehicle_status.HIL_STATE_OFF; msg_vehicle_status.hil_state = msg_vehicle_status.VEHICLE_TYPE_QUADROTOR; msg_vehicle_status.is_rotary_wing = true; @@ -107,6 +99,54 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon } } +void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, + px4::vehicle_status &msg_vehicle_status, + px4::vehicle_control_mode &msg_vehicle_control_mode) { + // XXX this is a minimal implementation. If more advanced functionalities are + // needed consider a full port of the commander + + switch (msg->mode_switch) { + case px4::manual_control_setpoint::SWITCH_POS_NONE: + ROS_WARN("Joystick button mapping error, main mode not set"); + break; + + case px4::manual_control_setpoint::SWITCH_POS_OFF: // MANUAL + msg_vehicle_status.main_state = msg_vehicle_status.MAIN_STATE_MANUAL; + msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_MANUAL; + msg_vehicle_control_mode.flag_control_manual_enabled = true; + msg_vehicle_control_mode.flag_control_rates_enabled = true; + msg_vehicle_control_mode.flag_control_attitude_enabled = true; + msg_vehicle_control_mode.flag_control_altitude_enabled = false; + msg_vehicle_control_mode.flag_control_climb_rate_enabled = false; + msg_vehicle_control_mode.flag_control_position_enabled = false; + + break; + + case px4::manual_control_setpoint::SWITCH_POS_MIDDLE: // ASSIST + if (msg->posctl_switch == px4::manual_control_setpoint::SWITCH_POS_ON) { + msg_vehicle_status.main_state = msg_vehicle_status.MAIN_STATE_POSCTL; + msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_POSCTL; + msg_vehicle_control_mode.flag_control_manual_enabled = true; + msg_vehicle_control_mode.flag_control_rates_enabled = true; + msg_vehicle_control_mode.flag_control_attitude_enabled = true; + msg_vehicle_control_mode.flag_control_altitude_enabled = true; + msg_vehicle_control_mode.flag_control_climb_rate_enabled = true; + msg_vehicle_control_mode.flag_control_position_enabled = true; + } else { + msg_vehicle_status.main_state = msg_vehicle_status.MAIN_STATE_ALTCTL; + msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_ALTCTL; + msg_vehicle_control_mode.flag_control_manual_enabled = true; + msg_vehicle_control_mode.flag_control_rates_enabled = true; + msg_vehicle_control_mode.flag_control_attitude_enabled = true; + msg_vehicle_control_mode.flag_control_altitude_enabled = true; + msg_vehicle_control_mode.flag_control_climb_rate_enabled = true; + msg_vehicle_control_mode.flag_control_position_enabled = false; + } + break; + } + +} + int main(int argc, char **argv) { ros::init(argc, argv, "commander"); diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h index bd4092b3a2..f251f7c1a0 100644 --- a/src/platforms/ros/nodes/commander/commander.h +++ b/src/platforms/ros/nodes/commander/commander.h @@ -40,6 +40,8 @@ #include "ros/ros.h" #include +#include +#include #include #include @@ -56,6 +58,13 @@ protected: */ void ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg); + /** + * Set control mode flags based on stick positions (equiv to code in px4 commander) + */ + void EvalSwitches(const px4::manual_control_setpointConstPtr &msg, + px4::vehicle_status &msg_vehicle_status, + px4::vehicle_control_mode &msg_vehicle_control_mode); + ros::NodeHandle _n; ros::Subscriber _man_ctrl_sp_sub; ros::Publisher _vehicle_control_mode_pub; From 6dead1d576b91811ba4bcd988ed4f4151eacafcc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 29 Jan 2015 14:09:25 +0100 Subject: [PATCH 227/269] commander dummy node: set control velocity enabled flag --- src/platforms/ros/nodes/commander/commander.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp index fee32b55f9..3829cbc32b 100644 --- a/src/platforms/ros/nodes/commander/commander.cpp +++ b/src/platforms/ros/nodes/commander/commander.cpp @@ -119,6 +119,7 @@ void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, msg_vehicle_control_mode.flag_control_altitude_enabled = false; msg_vehicle_control_mode.flag_control_climb_rate_enabled = false; msg_vehicle_control_mode.flag_control_position_enabled = false; + msg_vehicle_control_mode.flag_control_velocity_enabled = false; break; @@ -132,6 +133,7 @@ void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, msg_vehicle_control_mode.flag_control_altitude_enabled = true; msg_vehicle_control_mode.flag_control_climb_rate_enabled = true; msg_vehicle_control_mode.flag_control_position_enabled = true; + msg_vehicle_control_mode.flag_control_velocity_enabled = true; } else { msg_vehicle_status.main_state = msg_vehicle_status.MAIN_STATE_ALTCTL; msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_ALTCTL; @@ -141,6 +143,7 @@ void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, msg_vehicle_control_mode.flag_control_altitude_enabled = true; msg_vehicle_control_mode.flag_control_climb_rate_enabled = true; msg_vehicle_control_mode.flag_control_position_enabled = false; + msg_vehicle_control_mode.flag_control_velocity_enabled = false; } break; } From a0ae5aeebb9eea7e90cf365d931c9a29790ebba1 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 29 Jan 2015 14:23:03 +0100 Subject: [PATCH 228/269] commander dummy node: small fix for vehicle_control_mode --- src/platforms/ros/nodes/commander/commander.cpp | 12 +++++++----- src/platforms/ros/nodes/commander/commander.h | 1 + 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp index 3829cbc32b..2673122c70 100644 --- a/src/platforms/ros/nodes/commander/commander.cpp +++ b/src/platforms/ros/nodes/commander/commander.cpp @@ -50,18 +50,18 @@ Commander::Commander() : _vehicle_status_pub(_n.advertise("vehicle_status", 10)), _parameter_update_pub(_n.advertise("parameter_update", 10)), _msg_parameter_update(), - _msg_actuator_armed() + _msg_actuator_armed(), + _msg_vehicle_control_mode() { } void Commander::ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg) { - px4::vehicle_control_mode msg_vehicle_control_mode; px4::vehicle_status msg_vehicle_status; /* fill vehicle control mode based on (faked) stick positions*/ - EvalSwitches(msg, msg_vehicle_status, msg_vehicle_control_mode); - msg_vehicle_control_mode.timestamp = px4::get_time_micros(); + EvalSwitches(msg, msg_vehicle_status, _msg_vehicle_control_mode); + _msg_vehicle_control_mode.timestamp = px4::get_time_micros(); /* fill actuator armed */ float arm_th = 0.95; @@ -71,6 +71,7 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon /* Check for disarm */ if (msg->r < -arm_th && msg->z < (1 - arm_th)) { _msg_actuator_armed.armed = false; + _msg_vehicle_control_mode.flag_armed = false; msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_STANDBY; } @@ -78,6 +79,7 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon /* Check for arm */ if (msg->r > arm_th && msg->z < (1 - arm_th)) { _msg_actuator_armed.armed = true; + _msg_vehicle_control_mode.flag_armed = true; msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_ARMED; } } @@ -88,7 +90,7 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon msg_vehicle_status.hil_state = msg_vehicle_status.VEHICLE_TYPE_QUADROTOR; msg_vehicle_status.is_rotary_wing = true; - _vehicle_control_mode_pub.publish(msg_vehicle_control_mode); + _vehicle_control_mode_pub.publish(_msg_vehicle_control_mode); _actuator_armed_pub.publish(_msg_actuator_armed); _vehicle_status_pub.publish(msg_vehicle_status); diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h index f251f7c1a0..58b7257b72 100644 --- a/src/platforms/ros/nodes/commander/commander.h +++ b/src/platforms/ros/nodes/commander/commander.h @@ -74,5 +74,6 @@ protected: px4::parameter_update _msg_parameter_update; px4::actuator_armed _msg_actuator_armed; + px4::vehicle_control_mode _msg_vehicle_control_mode; }; From 211b58cd1a0b8903a4f1c7a6f9e4e51deff8e7e2 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 29 Jan 2015 14:40:28 +0100 Subject: [PATCH 229/269] fix typo in comment --- .../mc_att_control_multiplatform/mc_att_control_base.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp index 5577c5f03e..1f0d88bcd7 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp @@ -138,7 +138,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) // _publish_att_sp = true; } - /* reset yaw setpint to current position if needed */ + /* reset yaw setpoint to current position if needed */ if (_reset_yaw_sp) { _reset_yaw_sp = false; _v_att_sp_mod.data().yaw_body = _v_att->data().yaw; From 6606b5636434c2198ba76221fab58e943eecb253 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Thu, 29 Jan 2015 04:49:39 -1000 Subject: [PATCH 230/269] Updated NuttX submodule with memcpy fix, disabled run time stack checking and added modules back in --- NuttX | 2 +- makefiles/config_px4fmu-v2_default.mk | 10 +++++----- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- nuttx-configs/px4io-v1/nsh/defconfig | 2 +- nuttx-configs/px4io-v2/nsh/defconfig | 2 +- 6 files changed, 10 insertions(+), 10 deletions(-) diff --git a/NuttX b/NuttX index e4c914e261..e1e56f2254 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit e4c914e261d2647e44d05222afa7aa3cc90d3c67 +Subproject commit e1e56f2254559c1e71e97d423cb282ec82feece4 diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 76457216bd..3abebd82fa 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -37,11 +37,11 @@ MODULES += drivers/hil # MODULES += drivers/hott/hott_sensors # MODULES += drivers/blinkm MODULES += drivers/airspeed -# MODULES += drivers/ets_airspeed +MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed -# MODULES += drivers/frsky_telemetry +MODULES += drivers/frsky_telemetry MODULES += modules/sensors -# MODULES += drivers/mkblctrl +MODULES += drivers/mkblctrl MODULES += drivers/px4flow # @@ -70,7 +70,7 @@ MODULES += modules/commander MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/gpio_led -# MODULES += modules/uavcan +MODULES += modules/uavcan MODULES += modules/land_detector # @@ -120,7 +120,7 @@ MODULES += lib/launchdetection # # OBC challenge # -# MODULES += modules/bottle_drop +MODULES += modules/bottle_drop # # Demo apps diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index a467fa605e..539634e3da 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -92,7 +92,7 @@ CONFIG_ARCH_HAVE_MPU=y # # CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y -CONFIG_ARMV7M_STACKCHECK=y +CONFIG_ARMV7M_STACKCHECK=n CONFIG_SERIAL_TERMIOS=y # diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 9030a1f022..dedebdfa03 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -117,7 +117,7 @@ CONFIG_ARCH_HAVE_MPU=y # # CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y -CONFIG_ARMV7M_STACKCHECK=y +CONFIG_ARMV7M_STACKCHECK=n CONFIG_SERIAL_TERMIOS=y CONFIG_SDIO_DMA=y CONFIG_SDIO_DMAPRIO=0x00010000 diff --git a/nuttx-configs/px4io-v1/nsh/defconfig b/nuttx-configs/px4io-v1/nsh/defconfig index 7c76be7ec0..385b8d6a8f 100755 --- a/nuttx-configs/px4io-v1/nsh/defconfig +++ b/nuttx-configs/px4io-v1/nsh/defconfig @@ -93,7 +93,7 @@ CONFIG_ARCH_DMA=y CONFIG_ARCH_MATH_H=y CONFIG_ARMV7M_CMNVECTOR=y -# CONFIG_ARMV7M_STACKCHECK is not set +CONFIG_ARMV7M_STACKCHECK=n # # JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): diff --git a/nuttx-configs/px4io-v2/nsh/defconfig b/nuttx-configs/px4io-v2/nsh/defconfig index 02b51e3d73..a9b8477948 100755 --- a/nuttx-configs/px4io-v2/nsh/defconfig +++ b/nuttx-configs/px4io-v2/nsh/defconfig @@ -89,7 +89,7 @@ CONFIG_ARCH_DMA=y CONFIG_ARCH_MATH_H=y CONFIG_ARMV7M_CMNVECTOR=y - +CONFIG_ARMV7M_STACKCHECK=n # # JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): # From 869d4115faaf6120715289b0e81ead14a2737553 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 29 Jan 2015 16:02:58 +0100 Subject: [PATCH 231/269] ros mixer: remove debug output --- src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp index 54f5fa78b6..0749c8e926 100644 --- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -223,19 +223,15 @@ void MultirotorMixer::actuatorControlsCallback(const PX4_TOPIC_T(actuator_contro _n.getParamCached("mixer", mixer_name); if (mixer_name == "x") { _rotors = _config_index[0]; - ROS_WARN("using x"); } else if (mixer_name == "+") { _rotors = _config_index[1]; } else if (mixer_name == "e") { _rotors = _config_index[2]; } else if (mixer_name == "w") { _rotors = _config_index[3]; - ROS_WARN("using w"); } else if (mixer_name == "i") { _rotors = _config_index[4]; - ROS_WARN("using i"); } - ROS_WARN("mixer_name %s", mixer_name.c_str()); // mix mix(); From 2c644715002516ebe998bd952aa97baf9b80d64f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 29 Jan 2015 16:31:21 +0100 Subject: [PATCH 232/269] Updated NuttX submodule --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index e1e56f2254..37cbc3e8ae 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit e1e56f2254559c1e71e97d423cb282ec82feece4 +Subproject commit 37cbc3e8ae6b75e9b7e79996d30fe8ed0beb9704 From 8de411619a0ce05cc9f34f5a9f756908dbd21db8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 24 Jan 2015 15:50:53 +0100 Subject: [PATCH 233/269] Initial stab at supporting multiple publications on the same base name and auto-enumeration of additional publications. --- ROMFS/px4fmu_test/init.d/rcS | 7 + src/drivers/drv_orb_dev.h | 9 +- src/modules/uORB/Publication.cpp | 2 +- src/modules/uORB/Publication.hpp | 2 +- src/modules/uORB/Subscription.cpp | 2 +- src/modules/uORB/Subscription.hpp | 2 +- src/modules/uORB/module.mk | 5 +- src/modules/uORB/objects_common.cpp | 2 +- src/modules/uORB/uORB.cpp | 270 ++++++++++++++++++++-------- src/modules/uORB/uORB.h | 65 ++++++- 10 files changed, 278 insertions(+), 88 deletions(-) diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS index 40b3500e0e..4b9a9b68a0 100644 --- a/ROMFS/px4fmu_test/init.d/rcS +++ b/ROMFS/px4fmu_test/init.d/rcS @@ -97,6 +97,13 @@ else set unit_test_failure_list "${unit_test_failure_list} commander_tests" fi +if uorb test +then +else + set unit_test_failure 1 + set unit_test_failure_list "${unit_test_failure_list} uorb_tests" +fi + if hmc5883 -I start then # This is an FMUv3 diff --git a/src/drivers/drv_orb_dev.h b/src/drivers/drv_orb_dev.h index 713618545d..e0b16fa5cd 100644 --- a/src/drivers/drv_orb_dev.h +++ b/src/drivers/drv_orb_dev.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,7 +35,9 @@ #define _DRV_UORB_H /** - * @file uORB published object driver. + * @file drv_orb_dev.h + * + * uORB published object driver. */ #include @@ -84,4 +86,7 @@ /** Get the global advertiser handle for the topic */ #define ORBIOCGADVERTISER _ORBIOC(13) +/** Get the priority for the topic */ +#define ORBIOCGPRIORITY _ORBIOC(14) + #endif /* _DRV_UORB_H */ diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index 71757e1f42..d33f930602 100644 --- a/src/modules/uORB/Publication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/uORB/Publication.hpp b/src/modules/uORB/Publication.hpp index 8650b3df89..b64559734c 100644 --- a/src/modules/uORB/Publication.hpp +++ b/src/modules/uORB/Publication.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index 44b6debc7e..8884e5a3a1 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp index 34e9a83e03..75cf362546 100644 --- a/src/modules/uORB/Subscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/uORB/module.mk b/src/modules/uORB/module.mk index 9385ce253c..71ad09130c 100644 --- a/src/modules/uORB/module.mk +++ b/src/modules/uORB/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -37,8 +37,7 @@ MODULE_COMMAND = uorb -# XXX probably excessive, 2048 should be sufficient -MODULE_STACKSIZE = 4096 +MODULE_STACKSIZE = 2048 SRCS = uORB.cpp \ objects_common.cpp \ diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 78fdf4de75..20a9bcc43c 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index 3373aac837..cfea12f044 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (Cc) 2012-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -51,6 +51,7 @@ #include #include #include +#include #include #include @@ -68,6 +69,7 @@ namespace { +/* internal use only */ static const unsigned orb_maxpath = 64; /* oddly, ERROR is not defined for c++ */ @@ -81,17 +83,30 @@ enum Flavor { PARAM }; +struct orb_advertdata { + const struct orb_metadata *meta; + int *instance; + int priority; +}; + int -node_mkpath(char *buf, Flavor f, const struct orb_metadata *meta) +node_mkpath(char *buf, Flavor f, const struct orb_metadata *meta, int *instance = nullptr) { unsigned len; - len = snprintf(buf, orb_maxpath, "/%s/%s", - (f == PUBSUB) ? "obj" : "param", - meta->o_name); + unsigned index = 0; - if (len >= orb_maxpath) + if (instance != nullptr) { + index = *instance; + } + + len = snprintf(buf, orb_maxpath, "/%s/%s%d", + (f == PUBSUB) ? "obj" : "param", + meta->o_name, index); + + if (len >= orb_maxpath) { return -ENAMETOOLONG; + } return OK; } @@ -104,7 +119,7 @@ node_mkpath(char *buf, Flavor f, const struct orb_metadata *meta) class ORBDevNode : public device::CDev { public: - ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path); + ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path, int priority); ~ORBDevNode(); virtual int open(struct file *filp); @@ -126,6 +141,7 @@ private: struct hrt_call update_call; /**< deferred wakeup call if update_period is nonzero */ void *poll_priv; /**< saved copy of fds->f_priv while poll is active */ bool update_reported; /**< true if we have reported the update via poll/check */ + int priority; /**< priority of publisher */ }; const struct orb_metadata *_meta; /**< object metadata information */ @@ -133,6 +149,7 @@ private: hrt_abstime _last_update; /**< time the object was last updated */ volatile unsigned _generation; /**< object generation count */ pid_t _publisher; /**< if nonzero, current publisher */ + const int _priority; /**< priority of topic */ SubscriberData *filp_to_sd(struct file *filp) { SubscriberData *sd = (SubscriberData *)(filp->f_priv); @@ -160,13 +177,14 @@ private: bool appears_updated(SubscriberData *sd); }; -ORBDevNode::ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path) : +ORBDevNode::ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path, int priority) : CDev(name, path), _meta(meta), _data(nullptr), _last_update(0), _generation(0), - _publisher(0) + _publisher(0), + _priority(priority) { // enable debug() calls _debug_enabled = true; @@ -176,6 +194,7 @@ ORBDevNode::~ORBDevNode() { if (_data != nullptr) delete[] _data; + } int @@ -225,6 +244,9 @@ ORBDevNode::open(struct file *filp) /* default to no pending update */ sd->generation = _generation; + /* set priority */ + sd->priority = _priority; + filp->f_priv = (void *)sd; ret = CDev::open(filp); @@ -283,6 +305,9 @@ ORBDevNode::read(struct file *filp, char *buffer, size_t buflen) /* track the last generation that the file has seen */ sd->generation = _generation; + /* set priority */ + sd->priority = _priority; + /* * Clear the flag that indicates that an update has been reported, as * we have just collected it. @@ -364,6 +389,10 @@ ORBDevNode::ioctl(struct file *filp, int cmd, unsigned long arg) *(uintptr_t *)arg = (uintptr_t)this; return OK; + case ORBIOCGPRIORITY: + *(int *)arg = sd->priority; + return OK; + default: /* give it to the superclass */ return CDev::ioctl(filp, cmd, arg); @@ -556,40 +585,73 @@ ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg) switch (cmd) { case ORBIOCADVERTISE: { - const struct orb_metadata *meta = (const struct orb_metadata *)arg; + const struct orb_advertdata *adv = (const struct orb_advertdata *)arg; + const struct orb_metadata *meta = adv->meta; const char *objname; + const char *devpath; char nodepath[orb_maxpath]; ORBDevNode *node; /* construct a path to the node - this also checks the node name */ - ret = node_mkpath(nodepath, _flavor, meta); + ret = node_mkpath(nodepath, _flavor, meta, adv->instance); - if (ret != OK) + if (ret != OK) { return ret; + } /* driver wants a permanent copy of the node name, so make one here */ objname = strdup(meta->o_name); - if (objname == nullptr) + if (objname == nullptr) { return -ENOMEM; - - /* construct the new node */ - node = new ORBDevNode(meta, objname, nodepath); - - /* initialise the node - this may fail if e.g. a node with this name already exists */ - if (node != nullptr) - ret = node->init(); - - /* if we didn't get a device, that's bad */ - if (node == nullptr) - return -ENOMEM; - - /* if init failed, discard the node and its name */ - if (ret != OK) { - delete node; - free((void *)objname); } + /* ensure that only one advertiser runs through this critical section */ + lock(); + + ret = ERROR; + + /* try for topic groups */ + const unsigned max_group_tries = (adv->instance != nullptr) ? ORB_MULTI_MAX_INSTANCES : 1; + unsigned group_tries = 0; + do { + /* if path is modifyable change try index */ + if (adv->instance != nullptr) { + /* replace the number at the end of the string */ + nodepath[strlen(nodepath) - 1] = '0' + group_tries; + *(adv->instance) = group_tries; + } + + /* driver wants a permanent copy of the path, so make one here */ + devpath = strdup(nodepath); + + /* construct the new node */ + node = new ORBDevNode(meta, objname, devpath, adv->priority); + + /* if we didn't get a device, that's bad */ + if (node == nullptr) { + unlock(); + return -ENOMEM; + } + + /* initialise the node - this may fail if e.g. a node with this name already exists */ + ret = node->init(); + + /* if init failed, discard the node and its name */ + if (ret != OK) { + delete node; + free((void *)objname); + } + + } while (ret != OK && (group_tries++ < max_group_tries)); + + if (group_tries >= max_group_tries) { + ret = -ENOMEM; + } + + /* the file handle for the driver has been created, unlock */ + unlock(); + return ret; } @@ -614,6 +676,7 @@ struct orb_test { }; ORB_DEFINE(orb_test, struct orb_test); +ORB_DEFINE(orb_multitest, struct orb_test); int test_fail(const char *fmt, ...) @@ -643,8 +706,6 @@ test_note(const char *fmt, ...) return OK; } -ORB_DECLARE(sensor_combined); - int test() { @@ -700,48 +761,65 @@ test() orb_unsubscribe(sfd); close(pfd); -#if 0 - /* this is a hacky test that exploits the sensors app to test rate-limiting */ + /* this routine tests the multi-topic support */ + test_note("try multi-topic support"); - sfd = orb_subscribe(ORB_ID(sensor_combined)); + int instance0; + int pfd0 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance0, ORB_PRIO_MAX); - hrt_abstime start, end; - unsigned count; + test_note("advertised"); + usleep(300000); - start = hrt_absolute_time(); - count = 0; + int instance1; + int pfd1 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance1, ORB_PRIO_MIN); - do { - orb_check(sfd, &updated); + if (instance0 != 0) + return test_fail("mult. id0: %d", instance0); - if (updated) { - orb_copy(ORB_ID(sensor_combined), sfd, nullptr); - count++; - } - } while (count < 100); + if (instance1 != 1) + return test_fail("mult. id1: %d", instance1); - end = hrt_absolute_time(); - test_note("full-speed, 100 updates in %llu", end - start); + t.val = 103; + if (OK != orb_publish(ORB_ID(orb_multitest), pfd0, &t)) + return test_fail("mult. pub0 fail"); - orb_set_interval(sfd, 10); + test_note("published"); + usleep(300000); - start = hrt_absolute_time(); - count = 0; + t.val = 203; + if (OK != orb_publish(ORB_ID(orb_multitest), pfd1, &t)) + return test_fail("mult. pub1 fail"); - do { - orb_check(sfd, &updated); + /* subscribe to both topics and ensure valid data is received */ + int sfd0 = orb_subscribe_multi(ORB_ID(orb_multitest), 0); - if (updated) { - orb_copy(ORB_ID(sensor_combined), sfd, nullptr); - count++; - } - } while (count < 100); + if (OK != orb_copy(ORB_ID(orb_multitest), sfd0, &t)) + return test_fail("sub #0 copy failed: %d", errno); - end = hrt_absolute_time(); - test_note("100Hz, 100 updates in %llu", end - start); + if (t.val != 103) + return test_fail("sub #0 val. mismatch: %d", t.val); - orb_unsubscribe(sfd); -#endif + int sfd1 = orb_subscribe_multi(ORB_ID(orb_multitest), 1); + + if (OK != orb_copy(ORB_ID(orb_multitest), sfd1, &t)) + return test_fail("sub #1 copy failed: %d", errno); + + if (t.val != 203) + return test_fail("sub #1 val. mismatch: %d", t.val); + + /* test priorities */ + int prio; + if (OK != orb_priority(sfd0, &prio)) + return test_fail("prio #0"); + + if (prio != ORB_PRIO_MAX) + return test_fail("prio: %d", prio); + + if (OK != orb_priority(sfd1, &prio)) + return test_fail("prio #1"); + + if (prio != ORB_PRIO_MIN) + return test_fail("prio: %d", prio); return test_note("PASS"); } @@ -771,7 +849,7 @@ uorb_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (g_dev != nullptr) { - fprintf(stderr, "[uorb] already loaded\n"); + warnx("already loaded"); /* user wanted to start uorb, its already running, no error */ return 0; } @@ -780,18 +858,17 @@ uorb_main(int argc, char *argv[]) g_dev = new ORBDevMaster(PUBSUB); if (g_dev == nullptr) { - fprintf(stderr, "[uorb] driver alloc failed\n"); + warnx("driver alloc failed"); return -ENOMEM; } if (OK != g_dev->init()) { - fprintf(stderr, "[uorb] driver init failed\n"); + warnx("driver init failed"); delete g_dev; g_dev = nullptr; return -EIO; } - printf("[uorb] ready\n"); return OK; } @@ -807,8 +884,7 @@ uorb_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) return info(); - fprintf(stderr, "unrecognised command, try 'start', 'test' or 'status'\n"); - return -EINVAL; + errx(-EINVAL, "unrecognized command, try 'start', 'test' or 'status'"); } /* @@ -825,11 +901,14 @@ namespace * we tried to advertise. */ int -node_advertise(const struct orb_metadata *meta) +node_advertise(const struct orb_metadata *meta, int *instance = nullptr, int priority = ORB_PRIO_DEFAULT) { int fd = -1; int ret = ERROR; + /* fill advertiser data */ + const struct orb_advertdata adv = { meta, instance, priority }; + /* open the control device */ fd = open(TOPIC_MASTER_DEVICE_PATH, 0); @@ -837,11 +916,12 @@ node_advertise(const struct orb_metadata *meta) goto out; /* advertise the object */ - ret = ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)meta); + ret = ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)&adv); /* it's OK if it already exists */ - if ((OK != ret) && (EEXIST == errno)) + if ((OK != ret) && (EEXIST == errno)) { ret = OK; + } out: @@ -858,7 +938,7 @@ out: * advertisers. */ int -node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool advertiser) +node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool advertiser, int *instance = nullptr, int priority = ORB_PRIO_DEFAULT) { char path[orb_maxpath]; int fd, ret; @@ -883,7 +963,7 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve /* * Generate the path to the node and try to open it. */ - ret = node_mkpath(path, f, meta); + ret = node_mkpath(path, f, meta, instance); if (ret != OK) { errno = -ret; @@ -893,15 +973,34 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve /* open the path as either the advertiser or the subscriber */ fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY); + /* if we want to advertise and the node existed, we have to re-try again */ + if ((fd >= 0) && (instance != nullptr) && (advertiser)) { + /* close the fd, we want a new one */ + close(fd); + /* the node_advertise call will automatically go for the next free entry */ + fd = -1; + } + /* we may need to advertise the node... */ if (fd < 0) { /* try to create the node */ - ret = node_advertise(meta); + ret = node_advertise(meta, instance, priority); + + if (ret == OK) { + /* update the path, as it might have been updated during the node_advertise call */ + ret = node_mkpath(path, f, meta, instance); + + if (ret != OK) { + errno = -ret; + return ERROR; + } + } /* on success, try the open again */ - if (ret == OK) + if (ret == OK) { fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY); + } } if (fd < 0) { @@ -917,12 +1016,18 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) +{ + return orb_advertise_multi(meta, data, nullptr, ORB_PRIO_DEFAULT); +} + +orb_advert_t +orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority) { int result, fd; orb_advert_t advertiser; /* open the node as an advertiser */ - fd = node_open(PUBSUB, meta, data, true); + fd = node_open(PUBSUB, meta, data, true, instance, priority); if (fd == ERROR) return ERROR; @@ -933,7 +1038,7 @@ orb_advertise(const struct orb_metadata *meta, const void *data) return ERROR; /* the advertiser must perform an initial publish to initialise the object */ - result= orb_publish(meta, advertiser, data); + result = orb_publish(meta, advertiser, data); if (result == ERROR) return ERROR; @@ -946,6 +1051,13 @@ orb_subscribe(const struct orb_metadata *meta) return node_open(PUBSUB, meta, nullptr, false); } +int +orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) +{ + int inst = instance; + return node_open(PUBSUB, meta, nullptr, false, &inst); +} + int orb_unsubscribe(int handle) { @@ -988,6 +1100,12 @@ orb_stat(int handle, uint64_t *time) return ioctl(handle, ORBIOCLASTUPDATE, (unsigned long)(uintptr_t)time); } +int +orb_priority(int handle, int *priority) +{ + return ioctl(handle, ORBIOCGPRIORITY, (unsigned long)(uintptr_t)priority); +} + int orb_set_interval(int handle, unsigned interval) { diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index beb23f61df..a0ad752735 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -56,6 +56,25 @@ struct orb_metadata { typedef const struct orb_metadata *orb_id_t; +/** + * Maximum number of multi topic instances + */ +#define ORB_MULTI_MAX_INSTANCES 4 + +/** + * Topic priority. + * Relevant for multi-topics / topic groups + */ +enum ORB_PRIO { + ORB_PRIO_MIN = 0, + ORB_PRIO_VERY_LOW = 25, + ORB_PRIO_LOW = 50, + ORB_PRIO_DEFAULT = 75, + ORB_PRIO_HIGH = 100, + ORB_PRIO_VERY_HIGH = 125, + ORB_PRIO_MAX = 255 +}; + /** * Generates a pointer to the uORB metadata structure for * a given topic. @@ -128,7 +147,7 @@ typedef const struct orb_metadata *orb_id_t; #define ORB_DEFINE(_name, _struct) \ const struct orb_metadata __orb_##_name = { \ #_name, \ - sizeof(_struct) \ + sizeof(_struct), \ }; struct hack __BEGIN_DECLS @@ -167,6 +186,34 @@ typedef intptr_t orb_advert_t; */ extern orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) __EXPORT; +/** + * Advertise as the publisher of a topic. + * + * This performs the initial advertisement of a topic; it creates the topic + * node in /obj if required and publishes the initial data. + * + * Any number of advertisers may publish to a topic; publications are atomic + * but co-ordination between publishers is not provided by the ORB. + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @param data A pointer to the initial data to be published. + * For topics updated by interrupt handlers, the advertisement + * must be performed from non-interrupt context. + * @param instance Pointer to an integer which will yield the instance ID (0-based) + * of the publication. + * @param priority The priority of the instance. If a subscriber subscribes multiple + * instances, the priority allows the subscriber to prioritize the best + * data source as long as its available. + * @return ERROR on error, otherwise returns a handle + * that can be used to publish to the topic. + * If the topic in question is not known (due to an + * ORB_DEFINE with no corresponding ORB_DECLARE) + * this function will return -1 and set errno to ENOENT. + */ +extern orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority) __EXPORT; + + /** * Publish new data to a topic. * @@ -210,6 +257,8 @@ extern int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, con */ extern int orb_subscribe(const struct orb_metadata *meta) __EXPORT; +extern int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) __EXPORT; + /** * Unsubscribe from a topic. * @@ -266,6 +315,18 @@ extern int orb_check(int handle, bool *updated) __EXPORT; */ extern int orb_stat(int handle, uint64_t *time) __EXPORT; +/** + * Return the priority of the topic + * + * @param handle A handle returned from orb_subscribe. + * @param priority Returns the priority of this topic. This is only relevant for + * topics which are published by multiple publishers (e.g. mag0, mag1, etc.) + * and allows a subscriber to automatically pick the topic with the highest + * priority, independent of the startup order of the associated publishers. + * @return OK on success, ERROR otherwise with errno set accordingly. + */ +extern int orb_priority(int handle, int *priority) __EXPORT; + /** * Set the minimum interval between which updates are seen for a subscription. * From 7932e2eda238b1d480fdc9de71bb1b5fcaa3e373 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 16:16:15 +0100 Subject: [PATCH 234/269] Add top to test build --- makefiles/config_px4fmu-v2_test.mk | 1 + src/modules/uORB/uORB.cpp | 6 +++++- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index bc6723e5f5..91fc2a7512 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -32,6 +32,7 @@ MODULES += systemcmds/tests MODULES += systemcmds/nshterm MODULES += systemcmds/mtd MODULES += systemcmds/ver +MODULES += systemcmds/top # # Testing modules diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index cfea12f044..c4de996bb9 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -641,9 +641,13 @@ ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg) if (ret != OK) { delete node; free((void *)objname); + free((void *)devpath); } - } while (ret != OK && (group_tries++ < max_group_tries)); + /* try with next larger index */ + group_tries++; + + } while (ret != OK && (group_tries < max_group_tries)); if (group_tries >= max_group_tries) { ret = -ENOMEM; From 4f9a6273cb28e50fdabdf1f60c39da6fe0b4fcd6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 17:01:39 +0100 Subject: [PATCH 235/269] uORB: correct pub creation for multi-topics --- src/modules/uORB/uORB.cpp | 21 ++++++++++++--------- src/modules/uORB/uORB.h | 2 +- 2 files changed, 13 insertions(+), 10 deletions(-) diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index c4de996bb9..b3a9bedb1e 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -599,13 +599,6 @@ ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg) return ret; } - /* driver wants a permanent copy of the node name, so make one here */ - objname = strdup(meta->o_name); - - if (objname == nullptr) { - return -ENOMEM; - } - /* ensure that only one advertiser runs through this critical section */ lock(); @@ -622,9 +615,20 @@ ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg) *(adv->instance) = group_tries; } + /* driver wants a permanent copy of the node name, so make one here */ + objname = strdup(meta->o_name); + + if (objname == nullptr) { + return -ENOMEM; + } + /* driver wants a permanent copy of the path, so make one here */ devpath = strdup(nodepath); + if (devpath == nullptr) { + return -ENOMEM; + } + /* construct the new node */ node = new ORBDevNode(meta, objname, devpath, adv->priority); @@ -644,12 +648,11 @@ ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg) free((void *)devpath); } - /* try with next larger index */ group_tries++; } while (ret != OK && (group_tries < max_group_tries)); - if (group_tries >= max_group_tries) { + if (group_tries > max_group_tries) { ret = -ENOMEM; } diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index a0ad752735..30cd598809 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -59,7 +59,7 @@ typedef const struct orb_metadata *orb_id_t; /** * Maximum number of multi topic instances */ -#define ORB_MULTI_MAX_INSTANCES 4 +#define ORB_MULTI_MAX_INSTANCES 3 /** * Topic priority. From 50a58db7e605c61bb50cfb154cbc43bf9c4c67ae Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 17:50:57 +0100 Subject: [PATCH 236/269] uORB: Ensure correct instance initialization, port complete mag API to new interface --- src/modules/uORB/objects_common.cpp | 4 +--- src/modules/uORB/uORB.cpp | 5 +++++ 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 20a9bcc43c..a8305ebeaf 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -46,9 +46,7 @@ #include #include -ORB_DEFINE(sensor_mag0, struct mag_report); -ORB_DEFINE(sensor_mag1, struct mag_report); -ORB_DEFINE(sensor_mag2, struct mag_report); +ORB_DEFINE(sensor_mag, struct mag_report); #include ORB_DEFINE(sensor_accel0, struct accel_report); diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index b3a9bedb1e..6f021459ce 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -592,6 +592,11 @@ ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg) char nodepath[orb_maxpath]; ORBDevNode *node; + /* set instance to zero - we could allow selective multi-pubs later based on value */ + if (adv->instance != nullptr) { + *(adv->instance) = 0; + } + /* construct a path to the node - this also checks the node name */ ret = node_mkpath(nodepath, _flavor, meta, adv->instance); From f30b02272beee4ad5c137a25c726ec158f0135de Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 17:51:33 +0100 Subject: [PATCH 237/269] Move HMC5883 to new interface --- src/drivers/drv_mag.h | 6 ++---- src/drivers/hmc5883/hmc5883.cpp | 35 ++++++++------------------------- 2 files changed, 10 insertions(+), 31 deletions(-) diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index d341e89472..d8fe1ae7a3 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -81,9 +81,7 @@ struct mag_scale { /* * ObjDev tag for raw magnetometer data. */ -ORB_DECLARE(sensor_mag0); -ORB_DECLARE(sensor_mag1); -ORB_DECLARE(sensor_mag2); +ORB_DECLARE(sensor_mag); /* * mag device types, for _device_id diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index a06be72d9a..b1605a5b09 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -69,7 +69,6 @@ #include #include -#include #include #include @@ -157,10 +156,9 @@ private: float _range_ga; bool _collect_phase; int _class_instance; + int _orb_class_instance; orb_advert_t _mag_topic; - orb_advert_t _subsystem_pub; - orb_id_t _mag_orb_id; perf_counter_t _sample_perf; perf_counter_t _comms_errors; @@ -348,9 +346,8 @@ HMC5883::HMC5883(device::Device *interface, const char *path, enum Rotation rota _range_ga(1.3f), _collect_phase(false), _class_instance(-1), + _orb_class_instance(-1), _mag_topic(-1), - _subsystem_pub(-1), - _mag_orb_id(nullptr), _sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")), _comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")), @@ -419,7 +416,6 @@ HMC5883::init() reset(); _class_instance = register_class_devname(MAG_DEVICE_PATH); - _mag_orb_id = ORB_ID_TRIPLE(sensor_mag, _class_instance); ret = OK; /* sensor is ok, but not calibrated */ @@ -850,6 +846,7 @@ HMC5883::collect() perf_begin(_sample_perf); struct mag_report new_report; + bool sensor_is_onboard = false; /* this should be fairly close to the end of the measurement, so the best approximation of the time */ new_report.timestamp = hrt_absolute_time(); @@ -902,7 +899,8 @@ HMC5883::collect() // XXX revisit for SPI part, might require a bus type IOCTL unsigned dummy; - if (!_interface->ioctl(MAGIOCGEXTERNAL, dummy)) { + sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy); + if (sensor_is_onboard) { // convert onboard so it matches offboard for the // scaling below report.y = -report.y; @@ -925,9 +923,10 @@ HMC5883::collect() if (_mag_topic != -1) { /* publish it */ - orb_publish(_mag_orb_id, _mag_topic, &new_report); + orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); } else { - _mag_topic = orb_advertise(_mag_orb_id, &new_report); + _mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_report, + &_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX); if (_mag_topic < 0) debug("ADVERT FAIL"); @@ -1185,24 +1184,6 @@ int HMC5883::check_calibration() warnx("mag cal status changed %s%s", (scale_valid) ? "" : "scale invalid ", (offset_valid) ? "" : "offset invalid"); _calibrated = (offset_valid && scale_valid); - - - // XXX Change advertisement - - /* notify about state change */ - struct subsystem_info_s info = { - true, - true, - _calibrated, - SUBSYSTEM_TYPE_MAG}; - - if (!(_pub_blocked)) { - if (_subsystem_pub > 0) { - orb_publish(ORB_ID(subsystem_info), _subsystem_pub, &info); - } else { - _subsystem_pub = orb_advertise(ORB_ID(subsystem_info), &info); - } - } } /* return 0 if calibrated, 1 else */ From 114465aba4b65ecdc9ffe3f4125afb2391fbdc2b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 17:51:50 +0100 Subject: [PATCH 238/269] Move LSM303D mag to new multi-pub interface --- src/drivers/lsm303d/lsm303d.cpp | 22 ++++------------------ 1 file changed, 4 insertions(+), 18 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 57754e4c0d..6b65965b49 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -507,7 +507,7 @@ private: LSM303D *_parent; orb_advert_t _mag_topic; - orb_id_t _mag_orb_id; + int _mag_orb_class_instance; int _mag_class_instance; void measure(); @@ -641,21 +641,7 @@ LSM303D::init() _mag_reports->get(&mrp); /* measurement will have generated a report, publish */ - switch (_mag->_mag_class_instance) { - case CLASS_DEVICE_PRIMARY: - _mag->_mag_orb_id = ORB_ID(sensor_mag0); - break; - - case CLASS_DEVICE_SECONDARY: - _mag->_mag_orb_id = ORB_ID(sensor_mag1); - break; - - case CLASS_DEVICE_TERTIARY: - _mag->_mag_orb_id = ORB_ID(sensor_mag2); - break; - } - - _mag->_mag_topic = orb_advertise(_mag->_mag_orb_id, &mrp); + _mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp, &_mag->_mag_orb_class_instance, ORB_PRIO_LOW); if (_mag->_mag_topic < 0) { warnx("ADVERT ERR"); @@ -1641,7 +1627,7 @@ LSM303D::mag_measure() if (!(_pub_blocked)) { /* publish it */ - orb_publish(_mag->_mag_orb_id, _mag->_mag_topic, &mag_report); + orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report); } _mag_read++; @@ -1742,7 +1728,7 @@ LSM303D_mag::LSM303D_mag(LSM303D *parent) : CDev("LSM303D_mag", LSM303D_DEVICE_PATH_MAG), _parent(parent), _mag_topic(-1), - _mag_orb_id(nullptr), + _mag_orb_class_instance(-1), _mag_class_instance(-1) { } From 801e9ed4fbc66d0aa359184be9f4ed3899f10096 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 17:52:36 +0100 Subject: [PATCH 239/269] Commander: move sensor interface for mag to new multi-sub --- src/modules/commander/mag_calibration.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 53013fdb97..2afb9a4408 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -149,7 +149,7 @@ int do_mag_calibration(int mavlink_fd) } if (res == OK) { - int sub_mag = orb_subscribe(ORB_ID(sensor_mag0)); + int sub_mag = orb_subscribe_multi(ORB_ID(sensor_mag), 0); if (sub_mag < 0) { mavlink_log_critical(mavlink_fd, "No mag found, abort"); @@ -179,7 +179,7 @@ int do_mag_calibration(int mavlink_fd) int poll_ret = poll(fds, 1, 1000); if (poll_ret > 0) { - orb_copy(ORB_ID(sensor_mag0), sub_mag, &mag); + orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); x[calibration_counter] = mag.x; y[calibration_counter] = mag.y; From f641a26feee76f4a811fec34834cc818a8e8f76a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 17:53:04 +0100 Subject: [PATCH 240/269] Move MAVLink to new mag interface --- src/modules/mavlink/mavlink_receiver.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e5a21651d2..5aa623e955 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1086,10 +1086,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) mag.z = imu.zmag; if (_mag_pub < 0) { - _mag_pub = orb_advertise(ORB_ID(sensor_mag0), &mag); + /* publish to the first mag topic */ + _mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag); } else { - orb_publish(ORB_ID(sensor_mag0), _mag_pub, &mag); + orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag); } } From c3eb10560ba255fdda2454e6044bb4efac35b38f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 17:53:15 +0100 Subject: [PATCH 241/269] Move sensors to new mag interface --- src/modules/sensors/sensors.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 793b7c2b65..3f66d7995e 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1249,7 +1249,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) if (mag_updated) { struct mag_report mag_report; - orb_copy(ORB_ID(sensor_mag0), _mag_sub, &mag_report); + orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); @@ -1278,7 +1278,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) if (mag_updated) { struct mag_report mag_report; - orb_copy(ORB_ID(sensor_mag1), _mag1_sub, &mag_report); + orb_copy(ORB_ID(sensor_mag), _mag1_sub, &mag_report); raw.magnetometer1_raw[0] = mag_report.x_raw; raw.magnetometer1_raw[1] = mag_report.y_raw; @@ -1292,7 +1292,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) if (mag_updated) { struct mag_report mag_report; - orb_copy(ORB_ID(sensor_mag2), _mag2_sub, &mag_report); + orb_copy(ORB_ID(sensor_mag), _mag2_sub, &mag_report); raw.magnetometer2_raw[0] = mag_report.x_raw; raw.magnetometer2_raw[1] = mag_report.y_raw; @@ -1945,13 +1945,13 @@ Sensors::task_main() */ _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro0)); _accel_sub = orb_subscribe(ORB_ID(sensor_accel0)); - _mag_sub = orb_subscribe(ORB_ID(sensor_mag0)); + _mag_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 0); _gyro1_sub = orb_subscribe(ORB_ID(sensor_gyro1)); _accel1_sub = orb_subscribe(ORB_ID(sensor_accel1)); - _mag1_sub = orb_subscribe(ORB_ID(sensor_mag1)); + _mag1_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 1); _gyro2_sub = orb_subscribe(ORB_ID(sensor_gyro2)); _accel2_sub = orb_subscribe(ORB_ID(sensor_accel2)); - _mag2_sub = orb_subscribe(ORB_ID(sensor_mag2)); + _mag2_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 2); _rc_sub = orb_subscribe(ORB_ID(input_rc)); _baro_sub = orb_subscribe(ORB_ID(sensor_baro0)); _baro1_sub = orb_subscribe(ORB_ID(sensor_baro1)); From cc7a00b96e658f5d36763ef90ebac7fa813b55af Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 17:54:09 +0100 Subject: [PATCH 242/269] Disable UAVCAN build until sensors use all new-style API and UAVCAN sensors base class can be reworked to use it consistently --- makefiles/config_px4fmu-v2_default.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 3abebd82fa..68a65efed1 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -70,7 +70,7 @@ MODULES += modules/commander MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/gpio_led -MODULES += modules/uavcan +#MODULES += modules/uavcan MODULES += modules/land_detector # From d851a630d874f639e0861dc8411405f83ee23769 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 21:05:38 +0100 Subject: [PATCH 243/269] Move all drivers to multi pub/sub API --- src/drivers/drv_accel.h | 6 ++-- src/drivers/drv_baro.h | 6 ++-- src/drivers/drv_blinkm.h | 2 +- src/drivers/drv_gyro.h | 6 ++-- src/drivers/l3gd20/l3gd20.cpp | 40 ++++++++++----------- src/drivers/lsm303d/lsm303d.cpp | 42 +++++++++++----------- src/drivers/mpu6000/mpu6000.cpp | 63 +++++++++++++-------------------- src/drivers/ms5611/ms5611.cpp | 21 +++++++---- 8 files changed, 87 insertions(+), 99 deletions(-) diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h index 1f98d966bd..1eca6155b0 100644 --- a/src/drivers/drv_accel.h +++ b/src/drivers/drv_accel.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -81,9 +81,7 @@ struct accel_scale { /* * ObjDev tag for raw accelerometer data. */ -ORB_DECLARE(sensor_accel0); -ORB_DECLARE(sensor_accel1); -ORB_DECLARE(sensor_accel2); +ORB_DECLARE(sensor_accel); /* * ioctl() definitions diff --git a/src/drivers/drv_baro.h b/src/drivers/drv_baro.h index 3e28d3d3d9..3d275d6198 100644 --- a/src/drivers/drv_baro.h +++ b/src/drivers/drv_baro.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -63,9 +63,7 @@ struct baro_report { /* * ObjDev tag for raw barometer data. */ -ORB_DECLARE(sensor_baro0); -ORB_DECLARE(sensor_baro1); -ORB_DECLARE(sensor_baro2); +ORB_DECLARE(sensor_baro); /* * ioctl() definitions diff --git a/src/drivers/drv_blinkm.h b/src/drivers/drv_blinkm.h index 9c278f6c50..b757da5459 100644 --- a/src/drivers/drv_blinkm.h +++ b/src/drivers/drv_blinkm.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h index 41b013a443..5e0334a187 100644 --- a/src/drivers/drv_gyro.h +++ b/src/drivers/drv_gyro.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -81,9 +81,7 @@ struct gyro_scale { /* * ObjDev tag for raw gyro data. */ -ORB_DECLARE(sensor_gyro0); -ORB_DECLARE(sensor_gyro1); -ORB_DECLARE(sensor_gyro2); +ORB_DECLARE(sensor_gyro); /* * ioctl() definitions diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 08bc1fead8..bd1bd9f86d 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,7 +33,7 @@ /** * @file l3gd20.cpp - * Driver for the ST L3GD20 MEMS gyro connected via SPI. + * Driver for the ST L3GD20 MEMS and L3GD20H mems gyros connected via SPI. * * Note: With the exception of the self-test feature, the ST L3G4200D is * also supported by this driver. @@ -179,6 +179,12 @@ static const int ERROR = -1; #define L3GD20_DEFAULT_FILTER_FREQ 30 #define L3GD20_TEMP_OFFSET_CELSIUS 40 +#ifdef PX4_SPI_BUS_EXT +#define EXTERNAL_BUS PX4_SPI_BUS_EXT +#else +#define EXTERNAL_BUS 0 +#endif + #ifndef SENSOR_BOARD_ROTATION_DEFAULT #define SENSOR_BOARD_ROTATION_DEFAULT SENSOR_BOARD_ROTATION_270_DEG #endif @@ -221,7 +227,7 @@ private: float _gyro_range_scale; float _gyro_range_rad_s; orb_advert_t _gyro_topic; - orb_id_t _orb_id; + int _orb_class_instance; int _class_instance; unsigned _current_rate; @@ -273,6 +279,13 @@ private: */ void disable_i2c(); + /** + * Get the internal / external state + * + * @return true if the sensor is not on the main MCU board + */ + bool is_external() { return (_bus == EXTERNAL_BUS); } + /** * Static trampoline from the hrt_call context; because we don't have a * generic hrt wrapper yet. @@ -391,7 +404,7 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), _gyro_topic(-1), - _orb_id(nullptr), + _orb_class_instance(-1), _class_instance(-1), _current_rate(0), _orientation(SENSOR_BOARD_ROTATION_DEFAULT), @@ -456,20 +469,6 @@ L3GD20::init() _class_instance = register_class_devname(GYRO_DEVICE_PATH); - switch (_class_instance) { - case CLASS_DEVICE_PRIMARY: - _orb_id = ORB_ID(sensor_gyro0); - break; - - case CLASS_DEVICE_SECONDARY: - _orb_id = ORB_ID(sensor_gyro1); - break; - - case CLASS_DEVICE_TERTIARY: - _orb_id = ORB_ID(sensor_gyro2); - break; - } - reset(); measure(); @@ -478,7 +477,8 @@ L3GD20::init() struct gyro_report grp; _reports->get(&grp); - _gyro_topic = orb_advertise(_orb_id, &grp); + _gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, + &_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); if (_gyro_topic < 0) { debug("failed to create sensor_gyro publication"); @@ -1050,7 +1050,7 @@ L3GD20::measure() /* publish for subscribers */ if (!(_pub_blocked)) { /* publish it */ - orb_publish(_orb_id, _gyro_topic, &report); + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report); } _read++; diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 6b65965b49..ff7068936a 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -211,6 +211,12 @@ static const int ERROR = -1; #define LSM303D_ONE_G 9.80665f +#ifdef PX4_SPI_BUS_EXT +#define EXTERNAL_BUS PX4_SPI_BUS_EXT +#else +#define EXTERNAL_BUS 0 +#endif + extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); } @@ -275,7 +281,7 @@ private: unsigned _mag_samplerate; orb_advert_t _accel_topic; - orb_id_t _accel_orb_id; + int _accel_orb_class_instance; int _accel_class_instance; unsigned _accel_read; @@ -329,6 +335,13 @@ private: */ void disable_i2c(); + /** + * Get the internal / external state + * + * @return true if the sensor is not on the main MCU board + */ + bool is_external() { return (_bus == EXTERNAL_BUS); } + /** * Static trampoline from the hrt_call context; because we don't have a * generic hrt wrapper yet. @@ -539,7 +552,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota _mag_range_scale(0.0f), _mag_samplerate(0), _accel_topic(-1), - _accel_orb_id(nullptr), + _accel_orb_class_instance(-1), _accel_class_instance(-1), _accel_read(0), _mag_read(0), @@ -618,7 +631,6 @@ LSM303D::init() if (_accel_reports == nullptr) goto out; - /* advertise accel topic */ _mag_reports = new RingBuffer(2, sizeof(mag_report)); if (_mag_reports == nullptr) @@ -641,7 +653,8 @@ LSM303D::init() _mag_reports->get(&mrp); /* measurement will have generated a report, publish */ - _mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp, &_mag->_mag_orb_class_instance, ORB_PRIO_LOW); + _mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp, + &_mag->_mag_orb_class_instance, ORB_PRIO_LOW); if (_mag->_mag_topic < 0) { warnx("ADVERT ERR"); @@ -654,21 +667,8 @@ LSM303D::init() _accel_reports->get(&arp); /* measurement will have generated a report, publish */ - switch (_accel_class_instance) { - case CLASS_DEVICE_PRIMARY: - _accel_orb_id = ORB_ID(sensor_accel0); - break; - - case CLASS_DEVICE_SECONDARY: - _accel_orb_id = ORB_ID(sensor_accel1); - break; - - case CLASS_DEVICE_TERTIARY: - _accel_orb_id = ORB_ID(sensor_accel2); - break; - } - - _accel_topic = orb_advertise(_accel_orb_id, &arp); + _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, + &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); if (_accel_topic < 0) { warnx("ADVERT ERR"); @@ -1556,7 +1556,7 @@ LSM303D::measure() if (!(_pub_blocked)) { /* publish it */ - orb_publish(_accel_orb_id, _accel_topic, &accel_report); + orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); } _accel_read++; diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 2be7582442..e4e9824902 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -175,6 +175,12 @@ #define MPU6000_ONE_G 9.80665f +#ifdef PX4_SPI_BUS_EXT +#define EXTERNAL_BUS PX4_SPI_BUS_EXT +#else +#define EXTERNAL_BUS 0 +#endif + /* the MPU6000 can only handle high SPI bus speeds on the sensor and interrupt status registers. All other registers have a maximum 1MHz @@ -234,7 +240,7 @@ private: float _accel_range_scale; float _accel_range_m_s2; orb_advert_t _accel_topic; - orb_id_t _accel_orb_id; + int _accel_orb_class_instance; int _accel_class_instance; RingBuffer *_gyro_reports; @@ -360,6 +366,13 @@ private: */ uint16_t swap16(uint16_t val) { return (val >> 8) | (val << 8); } + /** + * Get the internal / external state + * + * @return true if the sensor is not on the main MCU board + */ + bool is_external() { return (_bus == EXTERNAL_BUS); } + /** * Measurement self test * @@ -457,7 +470,7 @@ protected: private: MPU6000 *_parent; orb_advert_t _gyro_topic; - orb_id_t _gyro_orb_id; + int _gyro_orb_class_instance; int _gyro_class_instance; /* do not allow to copy this class due to pointer data members */ @@ -479,7 +492,7 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), _accel_topic(-1), - _accel_orb_id(nullptr), + _accel_orb_class_instance(-1), _accel_class_instance(-1), _gyro_reports(nullptr), _gyro_scale{}, @@ -613,22 +626,8 @@ MPU6000::init() _accel_reports->get(&arp); /* measurement will have generated a report, publish */ - switch (_accel_class_instance) { - case CLASS_DEVICE_PRIMARY: - _accel_orb_id = ORB_ID(sensor_accel0); - break; - - case CLASS_DEVICE_SECONDARY: - _accel_orb_id = ORB_ID(sensor_accel1); - break; - - case CLASS_DEVICE_TERTIARY: - _accel_orb_id = ORB_ID(sensor_accel2); - break; - - } - - _accel_topic = orb_advertise(_accel_orb_id, &arp); + _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, + &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH); if (_accel_topic < 0) { warnx("ADVERT FAIL"); @@ -639,22 +638,8 @@ MPU6000::init() struct gyro_report grp; _gyro_reports->get(&grp); - switch (_gyro->_gyro_class_instance) { - case CLASS_DEVICE_PRIMARY: - _gyro->_gyro_orb_id = ORB_ID(sensor_gyro0); - break; - - case CLASS_DEVICE_SECONDARY: - _gyro->_gyro_orb_id = ORB_ID(sensor_gyro1); - break; - - case CLASS_DEVICE_TERTIARY: - _gyro->_gyro_orb_id = ORB_ID(sensor_gyro2); - break; - - } - - _gyro->_gyro_topic = orb_advertise(_gyro->_gyro_orb_id, &grp); + _gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, + &_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH); if (_gyro->_gyro_topic < 0) { warnx("ADVERT FAIL"); @@ -1763,12 +1748,12 @@ MPU6000::measure() perf_begin(_controller_latency_perf); perf_begin(_system_latency_perf); /* publish it */ - orb_publish(_accel_orb_id, _accel_topic, &arb); + orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); } if (!(_pub_blocked)) { /* publish it */ - orb_publish(_gyro->_gyro_orb_id, _gyro->_gyro_topic, &grb); + orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); } /* stop measuring */ @@ -1818,7 +1803,7 @@ MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) : CDev("MPU6000_gyro", path), _parent(parent), _gyro_topic(-1), - _gyro_orb_id(nullptr), + _gyro_orb_class_instance(-1), _gyro_class_instance(-1) { } diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 0a793cbc97..75b1f65fdb 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -57,6 +57,7 @@ #include #include +#include #include #include @@ -134,8 +135,7 @@ protected: unsigned _msl_pressure; /* in Pa */ orb_advert_t _baro_topic; - orb_id_t _orb_id; - + int _orb_class_instance; int _class_instance; perf_counter_t _sample_perf; @@ -171,6 +171,13 @@ protected: */ void cycle(); + /** + * Get the internal / external state + * + * @return true if the sensor is not on the main MCU board + */ + bool is_external() { return (_orb_class_instance == 0); /* XXX put this into the interface class */ } + /** * Static trampoline from the workq context; because we don't have a * generic workq wrapper yet. @@ -210,6 +217,7 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char* _SENS(0), _msl_pressure(101325), _baro_topic(-1), + _orb_class_instance(-1), _class_instance(-1), _sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")), _measure_perf(perf_alloc(PC_ELAPSED, "ms5611_measure")), @@ -263,7 +271,6 @@ MS5611::init() /* register alternate interfaces if we have to */ _class_instance = register_class_devname(BARO_DEVICE_PATH); - _orb_id = ORB_ID_TRIPLE(sensor_baro, _class_instance); struct baro_report brp; /* do a first measurement cycle to populate reports with valid data */ @@ -303,7 +310,9 @@ MS5611::init() ret = OK; - _baro_topic = orb_advertise(_orb_id, &brp); + _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp, + &_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT); + if (_baro_topic < 0) { warnx("failed to create sensor_baro publication"); @@ -725,7 +734,7 @@ MS5611::collect() /* publish it */ if (!(_pub_blocked)) { /* publish it */ - orb_publish(_orb_id, _baro_topic, &report); + orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); } if (_reports->force(&report)) { From 7f299ea0cc5aedbf1f7913930d592bdc696e0ca9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 21:06:03 +0100 Subject: [PATCH 244/269] Move commander to multi pub/sub API --- src/modules/commander/gyro_calibration.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 8ab14dd52b..2be0e881ee 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -95,7 +95,7 @@ int do_gyro_calibration(int mavlink_fd) unsigned poll_errcount = 0; /* subscribe to gyro sensor topic */ - int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro0)); + int sub_sensor_gyro = orb_subscribe_multi(ORB_ID(sensor_gyro), 0); struct gyro_report gyro_report; while (calibration_counter < calibration_count) { @@ -107,7 +107,7 @@ int do_gyro_calibration(int mavlink_fd) int poll_ret = poll(fds, 1, 1000); if (poll_ret > 0) { - orb_copy(ORB_ID(sensor_gyro0), sub_sensor_gyro, &gyro_report); + orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report); gyro_scale.x_offset += gyro_report.x; gyro_scale.y_offset += gyro_report.y; gyro_scale.z_offset += gyro_report.z; From 777eda1a89222b9b7b91fd12c92475291ed56ce7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 21:06:27 +0100 Subject: [PATCH 245/269] Move MATLAB example to multi pub/sub API --- .../matlab_csv_serial/matlab_csv_serial.c | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/examples/matlab_csv_serial/matlab_csv_serial.c b/src/examples/matlab_csv_serial/matlab_csv_serial.c index a95f45d1ad..145cf99cc4 100644 --- a/src/examples/matlab_csv_serial/matlab_csv_serial.c +++ b/src/examples/matlab_csv_serial/matlab_csv_serial.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -190,10 +190,10 @@ int matlab_csv_serial_thread_main(int argc, char *argv[]) struct gyro_report gyro1; /* subscribe to parameter changes */ - int accel0_sub = orb_subscribe(ORB_ID(sensor_accel0)); - int accel1_sub = orb_subscribe(ORB_ID(sensor_accel1)); - int gyro0_sub = orb_subscribe(ORB_ID(sensor_gyro0)); - int gyro1_sub = orb_subscribe(ORB_ID(sensor_gyro1)); + int accel0_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0); + int accel1_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 1); + int gyro0_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 0); + int gyro1_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 1); thread_running = true; @@ -224,10 +224,10 @@ int matlab_csv_serial_thread_main(int argc, char *argv[]) /* accel0 update available? */ if (fds[0].revents & POLLIN) { - orb_copy(ORB_ID(sensor_accel0), accel0_sub, &accel0); - orb_copy(ORB_ID(sensor_accel1), accel1_sub, &accel1); - orb_copy(ORB_ID(sensor_gyro0), gyro0_sub, &gyro0); - orb_copy(ORB_ID(sensor_gyro1), gyro1_sub, &gyro1); + orb_copy(ORB_ID(sensor_accel), accel0_sub, &accel0); + orb_copy(ORB_ID(sensor_accel), accel1_sub, &accel1); + orb_copy(ORB_ID(sensor_gyro), gyro0_sub, &gyro0); + orb_copy(ORB_ID(sensor_gyro), gyro1_sub, &gyro1); // write out on accel 0, but collect for all other sensors as they have updates dprintf(serial_fd, "%llu,%d,%d,%d,%d,%d,%d\n", accel0.timestamp, (int)accel0.x_raw, (int)accel0.y_raw, (int)accel0.z_raw, From 83a0f8e5ce2041494f57643246a612f2cd836752 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 21:06:48 +0100 Subject: [PATCH 246/269] Move EKF to multi pub/sub API --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 923aa28614..d0f5fb6f8a 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -719,7 +719,7 @@ FixedwingEstimator::task_main() * do subscriptions */ _distance_sub = orb_subscribe(ORB_ID(sensor_range_finder)); - _baro_sub = orb_subscribe(ORB_ID(sensor_baro0)); + _baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); @@ -1087,7 +1087,7 @@ FixedwingEstimator::task_main() if (baro_updated) { - orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro); + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); float baro_elapsed = (_baro.timestamp - baro_last) / 1e6f; baro_last = _baro.timestamp; @@ -1216,7 +1216,7 @@ FixedwingEstimator::task_main() initVelNED[2] = _gps.vel_d_m_s; // Set up height correctly - orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro); + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); _baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame // init filtered gps and baro altitudes From c4cf28fa9529ed12a1c78f0e297863f34644167a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 21:07:06 +0100 Subject: [PATCH 247/269] Move FW att control to multi pub sub API --- src/modules/fw_att_control/fw_att_control_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 6f225bb48f..4cb6c1aef2 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -549,7 +549,7 @@ FixedwingAttitudeControl::vehicle_accel_poll() orb_check(_accel_sub, &accel_updated); if (accel_updated) { - orb_copy(ORB_ID(sensor_accel0), _accel_sub, &_accel); + orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); } } @@ -619,7 +619,7 @@ FixedwingAttitudeControl::task_main() */ _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - _accel_sub = orb_subscribe(ORB_ID(sensor_accel0)); + _accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); From 165e5f5a62eedf1a4776b921b50fb84a3acdd3db Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 21:07:31 +0100 Subject: [PATCH 248/269] Move MAVLink to multi pub/sub API (to first index) --- src/modules/mavlink/mavlink_receiver.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 5aa623e955..cbd24f0d4a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1043,10 +1043,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) gyro.temperature = imu.temperature; if (_gyro_pub < 0) { - _gyro_pub = orb_advertise(ORB_ID(sensor_gyro0), &gyro); + _gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro); } else { - orb_publish(ORB_ID(sensor_gyro0), _gyro_pub, &gyro); + orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro); } } @@ -1065,10 +1065,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) accel.temperature = imu.temperature; if (_accel_pub < 0) { - _accel_pub = orb_advertise(ORB_ID(sensor_accel0), &accel); + _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); } else { - orb_publish(ORB_ID(sensor_accel0), _accel_pub, &accel); + orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); } } @@ -1105,10 +1105,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) baro.temperature = imu.temperature; if (_baro_pub < 0) { - _baro_pub = orb_advertise(ORB_ID(sensor_baro0), &baro); + _baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro); } else { - orb_publish(ORB_ID(sensor_baro0), _baro_pub, &baro); + orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro); } } @@ -1395,10 +1395,10 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) accel.temperature = 25.0f; if (_accel_pub < 0) { - _accel_pub = orb_advertise(ORB_ID(sensor_accel0), &accel); + _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); } else { - orb_publish(ORB_ID(sensor_accel0), _accel_pub, &accel); + orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); } } From 9190d597912d90b6d5fbf7a606f8e9d083797534 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 21:08:07 +0100 Subject: [PATCH 249/269] Move sensors app to multi pub/sub --- src/modules/sensors/sensors.cpp | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 3f66d7995e..630c54335a 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1113,7 +1113,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) if (accel_updated) { struct accel_report accel_report; - orb_copy(ORB_ID(sensor_accel0), _accel_sub, &accel_report); + orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); vect = _board_rotation * vect; @@ -1134,7 +1134,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) if (accel_updated) { struct accel_report accel_report; - orb_copy(ORB_ID(sensor_accel1), _accel_sub, &accel_report); + orb_copy(ORB_ID(sensor_accel), _accel1_sub, &accel_report); math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); vect = _board_rotation * vect; @@ -1155,7 +1155,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) if (accel_updated) { struct accel_report accel_report; - orb_copy(ORB_ID(sensor_accel2), _accel_sub, &accel_report); + orb_copy(ORB_ID(sensor_accel), _accel2_sub, &accel_report); math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); vect = _board_rotation * vect; @@ -1181,7 +1181,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) if (gyro_updated) { struct gyro_report gyro_report; - orb_copy(ORB_ID(sensor_gyro0), _gyro_sub, &gyro_report); + orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); vect = _board_rotation * vect; @@ -1202,7 +1202,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) if (gyro_updated) { struct gyro_report gyro_report; - orb_copy(ORB_ID(sensor_gyro1), _gyro1_sub, &gyro_report); + orb_copy(ORB_ID(sensor_gyro), _gyro1_sub, &gyro_report); math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); vect = _board_rotation * vect; @@ -1223,7 +1223,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) if (gyro_updated) { struct gyro_report gyro_report; - orb_copy(ORB_ID(sensor_gyro2), _gyro_sub, &gyro_report); + orb_copy(ORB_ID(sensor_gyro), _gyro2_sub, &gyro_report); math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); vect = _board_rotation * vect; @@ -1310,7 +1310,7 @@ Sensors::baro_poll(struct sensor_combined_s &raw) if (baro_updated) { - orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_barometer); + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_barometer); raw.baro_pres_mbar = _barometer.pressure; // Pressure in mbar raw.baro_alt_meter = _barometer.altitude; // Altitude in meters @@ -1325,7 +1325,7 @@ Sensors::baro_poll(struct sensor_combined_s &raw) struct baro_report baro_report; - orb_copy(ORB_ID(sensor_baro1), _baro1_sub, &baro_report); + orb_copy(ORB_ID(sensor_baro), _baro1_sub, &baro_report); raw.baro1_pres_mbar = baro_report.pressure; // Pressure in mbar raw.baro1_alt_meter = baro_report.altitude; // Altitude in meters @@ -1943,18 +1943,18 @@ Sensors::task_main() /* * do subscriptions */ - _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro0)); - _accel_sub = orb_subscribe(ORB_ID(sensor_accel0)); + _gyro_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 0); + _accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0); _mag_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 0); - _gyro1_sub = orb_subscribe(ORB_ID(sensor_gyro1)); - _accel1_sub = orb_subscribe(ORB_ID(sensor_accel1)); + _gyro1_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 1); + _accel1_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 1); _mag1_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 1); - _gyro2_sub = orb_subscribe(ORB_ID(sensor_gyro2)); - _accel2_sub = orb_subscribe(ORB_ID(sensor_accel2)); + _gyro2_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 2); + _accel2_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 2); _mag2_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 2); _rc_sub = orb_subscribe(ORB_ID(input_rc)); - _baro_sub = orb_subscribe(ORB_ID(sensor_baro0)); - _baro1_sub = orb_subscribe(ORB_ID(sensor_baro1)); + _baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0); + _baro1_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 1); _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); From 95462c5b7c357b343d39da3c5e6a49ae2055264c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 21:08:22 +0100 Subject: [PATCH 250/269] uORB: Remove duplicate topics --- src/modules/uORB/objects_common.cpp | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index a8305ebeaf..4a41aa4655 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -49,19 +49,13 @@ ORB_DEFINE(sensor_mag, struct mag_report); #include -ORB_DEFINE(sensor_accel0, struct accel_report); -ORB_DEFINE(sensor_accel1, struct accel_report); -ORB_DEFINE(sensor_accel2, struct accel_report); +ORB_DEFINE(sensor_accel, struct accel_report); #include -ORB_DEFINE(sensor_gyro0, struct gyro_report); -ORB_DEFINE(sensor_gyro1, struct gyro_report); -ORB_DEFINE(sensor_gyro2, struct gyro_report); +ORB_DEFINE(sensor_gyro, struct gyro_report); #include -ORB_DEFINE(sensor_baro0, struct baro_report); -ORB_DEFINE(sensor_baro1, struct baro_report); -ORB_DEFINE(sensor_baro2, struct baro_report); +ORB_DEFINE(sensor_baro, struct baro_report); #include ORB_DEFINE(sensor_range_finder, struct range_finder_report); From 1cc4c808a88787b4e1156896453fa9369841bcde Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 21:56:56 +0100 Subject: [PATCH 251/269] Upgrade UAVCAN to multi pub/sub A API --- makefiles/config_px4fmu-v2_default.mk | 2 +- src/modules/uavcan/module.mk | 2 +- src/modules/uavcan/sensors/baro.cpp | 9 ++------- src/modules/uavcan/sensors/baro.hpp | 2 +- src/modules/uavcan/sensors/gnss.cpp | 2 +- src/modules/uavcan/sensors/gnss.hpp | 2 +- src/modules/uavcan/sensors/mag.cpp | 10 ++-------- src/modules/uavcan/sensors/mag.hpp | 2 +- src/modules/uavcan/sensors/sensor_bridge.cpp | 6 ++---- src/modules/uavcan/sensors/sensor_bridge.hpp | 11 +++++------ 10 files changed, 17 insertions(+), 31 deletions(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 68a65efed1..3abebd82fa 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -70,7 +70,7 @@ MODULES += modules/commander MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/gpio_led -#MODULES += modules/uavcan +MODULES += modules/uavcan MODULES += modules/land_detector # diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 600cb47f39..4f63629a0c 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (C) 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. # Author: Pavel Kirienko # # Redistribution and use in source and binary forms, with or without diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index 8741ae20dd..ad09dfcac4 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -38,15 +38,10 @@ #include "baro.hpp" #include -static const orb_id_t BARO_TOPICS[2] = { - ORB_ID(sensor_baro0), - ORB_ID(sensor_baro1) -}; - const char *const UavcanBarometerBridge::NAME = "baro"; UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode& node) : -UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_DEVICE_PATH, BARO_TOPICS), +UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_DEVICE_PATH, ORB_ID(sensor_baro)), _sub_air_data(node) { } diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp index 9d470219ed..c7bbc5af8d 100644 --- a/src/modules/uavcan/sensors/baro.hpp +++ b/src/modules/uavcan/sensors/baro.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index 571a6f1cd7..3ae07367fa 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/uavcan/sensors/gnss.hpp b/src/modules/uavcan/sensors/gnss.hpp index 2111ff80b6..96ff9404f5 100644 --- a/src/modules/uavcan/sensors/gnss.hpp +++ b/src/modules/uavcan/sensors/gnss.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 35ebee5426..ee278aaf53 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -39,16 +39,10 @@ #include -static const orb_id_t MAG_TOPICS[3] = { - ORB_ID(sensor_mag0), - ORB_ID(sensor_mag1), - ORB_ID(sensor_mag2) -}; - const char *const UavcanMagnetometerBridge::NAME = "mag"; UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) : -UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, MAG_TOPICS), +UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, ORB_ID(sensor_mag)), _sub_mag(node) { _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; diff --git a/src/modules/uavcan/sensors/mag.hpp b/src/modules/uavcan/sensors/mag.hpp index 74077d883e..db38aee1d3 100644 --- a/src/modules/uavcan/sensors/mag.hpp +++ b/src/modules/uavcan/sensors/mag.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index 0999938fc3..b370764440 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -62,7 +62,6 @@ UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase() (void)unregister_class_devname(_class_devname, _channels[i].class_instance); } } - delete [] _orb_topics; delete [] _channels; } @@ -116,11 +115,10 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) } // Publish to the appropriate topic, abort on failure - channel->orb_id = _orb_topics[class_instance]; channel->node_id = node_id; channel->class_instance = class_instance; - channel->orb_advert = orb_advertise(channel->orb_id, report); + channel->orb_advert = orb_advertise_multi(_orb_topic, report, &channel->orb_instance, ORB_PRIO_HIGH); if (channel->orb_advert < 0) { log("ADVERTISE FAILED"); (void)unregister_class_devname(_class_devname, class_instance); @@ -132,7 +130,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) } assert(channel != nullptr); - (void)orb_publish(channel->orb_id, channel->orb_advert, report); + (void)orb_publish(_orb_topic, channel->orb_advert, report); } unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp index e319605370..94e52dbe50 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.hpp +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -90,28 +90,27 @@ class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CD struct Channel { int node_id = -1; - orb_id_t orb_id = nullptr; orb_advert_t orb_advert = -1; int class_instance = -1; + int orb_instance = -1; }; const unsigned _max_channels; const char *const _class_devname; - orb_id_t *const _orb_topics; + const orb_id_t _orb_topic; Channel *const _channels; bool _out_of_channels = false; protected: template UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname, - const orb_id_t (&orb_topics)[MaxChannels]) : + const orb_id_t orb_topic_sensor) : device::CDev(name, devname), _max_channels(MaxChannels), _class_devname(class_devname), - _orb_topics(new orb_id_t[MaxChannels]), + _orb_topic(orb_topic_sensor), _channels(new Channel[MaxChannels]) { - memcpy(_orb_topics, orb_topics, sizeof(orb_id_t) * MaxChannels); _device_id.devid_s.bus_type = DeviceBusType_UAVCAN; _device_id.devid_s.bus = 0; } From 2f7a9eaf6553d7da6c5d6e9b3edf6e710b4dc292 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 26 Jan 2015 02:34:03 +0300 Subject: [PATCH 252/269] Fix for a compilation failure --- src/modules/uavcan/sensors/sensor_bridge.hpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp index 94e52dbe50..de130b0788 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.hpp +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -75,8 +75,7 @@ public: /** * Sensor bridge factory. - * Creates a bridge object by its ASCII name, e.g. "gnss", "mag". - * @return nullptr if such bridge can't be created. + * Creates all known sensor bridges and puts them in the linked list. */ static void make_all(uavcan::INode &node, List &list); }; @@ -102,14 +101,16 @@ class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CD bool _out_of_channels = false; protected: - template + static constexpr unsigned DEFAULT_MAX_CHANNELS = 5; // 640 KB ought to be enough for anybody + UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname, - const orb_id_t orb_topic_sensor) : + const orb_id_t orb_topic_sensor, + const unsigned max_channels = DEFAULT_MAX_CHANNELS) : device::CDev(name, devname), - _max_channels(MaxChannels), + _max_channels(max_channels), _class_devname(class_devname), _orb_topic(orb_topic_sensor), - _channels(new Channel[MaxChannels]) + _channels(new Channel[max_channels]) { _device_id.devid_s.bus_type = DeviceBusType_UAVCAN; _device_id.devid_s.bus = 0; From cbe3783d5eeeb6597b1f02f38abf16737f6d4d64 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Jan 2015 09:49:30 +0100 Subject: [PATCH 253/269] Support topic groups in MAVLink subscription handling --- src/modules/mavlink/mavlink_main.cpp | 6 +++--- src/modules/mavlink/mavlink_main.h | 2 +- src/modules/mavlink/mavlink_messages.cpp | 11 ++--------- src/modules/mavlink/mavlink_orb_subscription.cpp | 11 +++++++++-- src/modules/mavlink/mavlink_orb_subscription.h | 4 +++- 5 files changed, 18 insertions(+), 16 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 9e4ab00df2..d6e9982de0 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -904,20 +904,20 @@ Mavlink::send_statustext(unsigned char severity, const char *string) mavlink_logbuffer_write(&_logbuffer, &logmsg); } -MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic) +MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, int instance) { /* check if already subscribed to this topic */ MavlinkOrbSubscription *sub; LL_FOREACH(_subscriptions, sub) { - if (sub->get_topic() == topic) { + if (sub->get_topic() == topic && sub->get_instance() == instance) { /* already subscribed */ return sub; } } /* add new subscription */ - MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic); + MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic, instance); LL_APPEND(_subscriptions, sub_new); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index ad5e5001b9..baaa7bc139 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -171,7 +171,7 @@ public: void handle_message(const mavlink_message_t *msg); - MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); + MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, int instance=0); int get_instance_id(); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 6765100c70..4a095a765e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1342,14 +1342,7 @@ protected: _act_sub(nullptr), _act_time(0) { - orb_id_t act_topics[] = { - ORB_ID(actuator_outputs_0), - ORB_ID(actuator_outputs_1), - ORB_ID(actuator_outputs_2), - ORB_ID(actuator_outputs_3) - }; - - _act_sub = _mavlink->add_orb_subscription(act_topics[N]); + _act_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_outputs), N); } void send(const hrt_abstime t) @@ -1424,7 +1417,7 @@ protected: _status_time(0), _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))), _pos_sp_triplet_time(0), - _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0))), + _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_outputs))), _act_time(0) {} diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 734f0903a5..315776e297 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -46,10 +46,11 @@ #include "mavlink_orb_subscription.h" -MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) : +MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic, int instance) : next(nullptr), _topic(topic), - _fd(orb_subscribe(_topic)), + _instance(instance), + _fd(orb_subscribe_multi(_topic, instance)), _published(false) { } @@ -65,6 +66,12 @@ MavlinkOrbSubscription::get_topic() const return _topic; } +int +MavlinkOrbSubscription::get_instance() const +{ + return _instance; +} + bool MavlinkOrbSubscription::update(uint64_t *time, void* data) { diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index 7af454df65..5394e5097b 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -50,7 +50,7 @@ class MavlinkOrbSubscription public: MavlinkOrbSubscription *next; ///< pointer to next subscription in list - MavlinkOrbSubscription(const orb_id_t topic); + MavlinkOrbSubscription(const orb_id_t topic, int instance); ~MavlinkOrbSubscription(); /** @@ -77,9 +77,11 @@ public: */ bool is_published(); orb_id_t get_topic() const; + int get_instance() const; private: const orb_id_t _topic; ///< topic metadata + const int _instance; ///< get topic instance int _fd; ///< subscription handle bool _published; ///< topic was ever published From 455f6abfcf25e48edd4d707fdf9b278e02a81772 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Jan 2015 09:49:42 +0100 Subject: [PATCH 254/269] Support topic groups in sdlog2 --- src/modules/sdlog2/sdlog2.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index a3407e42c4..99632ef0bc 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1096,7 +1096,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); subs.rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); - subs.act_outputs_sub = orb_subscribe(ORB_ID_VEHICLE_CONTROLS); + subs.act_outputs_sub = orb_subscribe(ORB_ID(actuator_outputs)); subs.act_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); subs.act_controls_1_sub = orb_subscribe(ORB_ID(actuator_controls_1)); subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); @@ -1473,7 +1473,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- ACTUATOR OUTPUTS --- */ - if (copy_if_updated(ORB_ID(actuator_outputs_0), subs.act_outputs_sub, &buf.act_outputs)) { + if (copy_if_updated(ORB_ID(actuator_outputs), subs.act_outputs_sub, &buf.act_outputs)) { log_msg.msg_type = LOG_OUT0_MSG; memcpy(log_msg.body.log_OUT0.output, buf.act_outputs.output, sizeof(log_msg.body.log_OUT0.output)); LOGBUFFER_WRITE_AND_COUNT(OUT0); From 678d5c24fb978f0c18bb87a6894ed2922fe227ca Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Jan 2015 09:49:57 +0100 Subject: [PATCH 255/269] FMU driver: Move to topic groups --- src/drivers/px4fmu/fmu.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 112c015136..363d2cdcfe 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -141,7 +141,7 @@ private: int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS]; actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS]; orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS]; - orb_id_t _actuator_output_topic; + int _actuator_output_topic_instance; pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS]; unsigned _poll_fds_num; @@ -256,7 +256,7 @@ PX4FMU::PX4FMU() : _groups_required(0), _groups_subscribed(0), _control_subs{-1}, - _actuator_output_topic(nullptr), + _actuator_output_topic_instance(-1), _poll_fds_num(0), _pwm_limit{}, _failsafe_pwm{0}, @@ -327,8 +327,6 @@ PX4FMU::init() log("default PWM output device"); } - _actuator_output_topic = ORB_ID_DOUBLE(actuator_outputs_, _class_instance); - /* reset GPIOs */ gpio_reset(); @@ -679,10 +677,10 @@ PX4FMU::task_main() /* publish mixed control outputs */ if (_outputs_pub < 0) { - _outputs_pub = orb_advertise(_actuator_output_topic, &outputs); + _outputs_pub = orb_advertise_multi(ORB_ID(actuator_outputs), &outputs, &_actuator_output_topic_instance, ORB_PRIO_DEFAULT); } else { - orb_publish(_actuator_output_topic, _outputs_pub, &outputs); + orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs); } } } From 51088026c79aa6dfa237ecfd8276d7e4997bbb9e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Jan 2015 09:50:09 +0100 Subject: [PATCH 256/269] IO driver: move to topic groups --- src/drivers/px4io/px4io.cpp | 13 ++++--------- 1 file changed, 4 insertions(+), 9 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 556eebab6d..653d0d5d7f 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1681,17 +1681,12 @@ PX4IO::io_publish_pwm_outputs() /* lazily advertise on first publication */ if (_to_outputs == 0) { - _to_outputs = orb_advertise((_primary_pwm_device ? - ORB_ID_VEHICLE_CONTROLS : - ORB_ID(actuator_outputs_1)), - &outputs); + int instance; + _to_outputs = orb_advertise_multi(ORB_ID(actuator_outputs), + &outputs, &instance, ORB_PRIO_MAX); } else { - orb_publish((_primary_pwm_device ? - ORB_ID_VEHICLE_CONTROLS : - ORB_ID(actuator_outputs_1)), - _to_outputs, - &outputs); + orb_publish(ORB_ID(actuator_outputs), _to_outputs, &outputs); } return OK; From d9d9a59ce440fab4bb1177bd888dc99c77a82350 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Jan 2015 09:50:23 +0100 Subject: [PATCH 257/269] MKBL: Move to topic groups --- src/drivers/mkblctrl/mkblctrl.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 1055487cb9..15412984ba 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -456,8 +456,9 @@ MK::task_main() actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); /* advertise the mixed control outputs */ - _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), - &outputs); + int dummy; + _t_outputs = orb_advertise_multi(ORB_ID(actuator_outputs), + &outputs, &dummy, ORB_PRIO_HIGH); /* advertise the blctrl status */ esc_status_s esc; From 8d37d58da732a621695d39c886ff4877151064cc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Jan 2015 09:50:36 +0100 Subject: [PATCH 258/269] HIL: Move to topic groups --- src/drivers/hil/hil.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index 9b5c8133b4..0584256762 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -330,8 +330,9 @@ HIL::task_main() actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); /* advertise the mixed control outputs */ - _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), - &outputs); + int dummy; + _t_outputs = orb_advertise_multi(ORB_ID(actuator_outputs), + &outputs, &dummy, ORB_PRIO_LOW); pollfd fds[2]; fds[0].fd = _t_actuators; @@ -423,7 +424,7 @@ HIL::task_main() } /* and publish for anyone that cares to see */ - orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &outputs); + orb_publish(ORB_ID(actuator_outputs), _t_outputs, &outputs); } } From aadbcb42a91d5158330c7e1f63103ee64f3fbd7c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Jan 2015 09:50:50 +0100 Subject: [PATCH 259/269] ARDrone driver: Move to topic groups --- src/drivers/ardrone_interface/ardrone_motor_control.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c index 4fa24275f1..0f45b65366 100644 --- a/src/drivers/ardrone_interface/ardrone_motor_control.c +++ b/src/drivers/ardrone_interface/ardrone_motor_control.c @@ -339,7 +339,8 @@ int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor outputs.output[3] = motor4; static orb_advert_t pub = 0; if (pub == 0) { - pub = orb_advertise(ORB_ID_VEHICLE_CONTROLS, &outputs); + /* advertise to channel 0 / primary */ + pub = orb_advertise(ORB_ID(actuator_outputs), &outputs); } if (hrt_absolute_time() - last_motor_time > min_motor_interval) { @@ -350,7 +351,7 @@ int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor fsync(ardrone_fd); /* publish just written values */ - orb_publish(ORB_ID_VEHICLE_CONTROLS, pub, &outputs); + orb_publish(ORB_ID(actuator_outputs), pub, &outputs); if (ret == sizeof(buf)) { return OK; From e532b81ac1f3356a3f2771d605b8d92d88d19d67 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Jan 2015 09:51:36 +0100 Subject: [PATCH 260/269] uORB: Remove last remnants of ORB_ID_DOUBLE/TRIPLE and migrate actuator outputs groups to new style interface --- src/modules/uORB/objects_common.cpp | 5 +- src/modules/uORB/topics/actuator_outputs.h | 8 +-- src/modules/uORB/uORB.h | 58 +++++++++++----------- 3 files changed, 32 insertions(+), 39 deletions(-) diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 4a41aa4655..ba1ac0350e 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -196,10 +196,7 @@ ORB_DEFINE(actuator_controls_virtual_fw, struct actuator_controls_s); ORB_DEFINE(actuator_armed, struct actuator_armed_s); #include "topics/actuator_outputs.h" -ORB_DEFINE(actuator_outputs_0, struct actuator_outputs_s); -ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s); -ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s); -ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s); +ORB_DEFINE(actuator_outputs, struct actuator_outputs_s); #include "topics/actuator_direct.h" ORB_DEFINE(actuator_direct, struct actuator_direct_s); diff --git a/src/modules/uORB/topics/actuator_outputs.h b/src/modules/uORB/topics/actuator_outputs.h index 4461404230..5f8f6d83ec 100644 --- a/src/modules/uORB/topics/actuator_outputs.h +++ b/src/modules/uORB/topics/actuator_outputs.h @@ -68,12 +68,6 @@ struct actuator_outputs_s { */ /* actuator output sets; this list can be expanded as more drivers emerge */ -ORB_DECLARE(actuator_outputs_0); -ORB_DECLARE(actuator_outputs_1); -ORB_DECLARE(actuator_outputs_2); -ORB_DECLARE(actuator_outputs_3); - -/* output sets with pre-defined applications */ -#define ORB_ID_VEHICLE_CONTROLS ORB_ID(actuator_outputs_0) +ORB_DECLARE(actuator_outputs); #endif \ No newline at end of file diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index 30cd598809..9c33c8a3ef 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -86,33 +86,6 @@ enum ORB_PRIO { */ #define ORB_ID(_name) &__orb_##_name -/** - * Generates a pointer to the uORB metadata structure for - * a given topic. - * - * The topic must have been declared previously in scope - * with ORB_DECLARE(). - * - * @param _name The name of the topic. - * @param _count The class ID of the topic - */ -#define ORB_ID_DOUBLE(_name, _count) ((_count == CLASS_DEVICE_PRIMARY) ? &__orb_##_name##0 : &__orb_##_name##1) - -/** - * Generates a pointer to the uORB metadata structure for - * a given topic. - * - * The topic must have been declared previously in scope - * with ORB_DECLARE(). - * - * @param _name The name of the topic. - * @param _count The class ID of the topic - */ -#define ORB_ID_TRIPLE(_name, _count) \ - ((_count == CLASS_DEVICE_PRIMARY) ? &__orb_##_name##0 : \ - ((_count == CLASS_DEVICE_SECONDARY) ? &__orb_##_name##1 : \ - (((_count == CLASS_DEVICE_TERTIARY) ? &__orb_##_name##2 : 0)))) - /** * Declare (prototype) the uORB metadata for a topic. * @@ -147,7 +120,7 @@ enum ORB_PRIO { #define ORB_DEFINE(_name, _struct) \ const struct orb_metadata __orb_##_name = { \ #_name, \ - sizeof(_struct), \ + sizeof(_struct) \ }; struct hack __BEGIN_DECLS @@ -257,6 +230,35 @@ extern int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, con */ extern int orb_subscribe(const struct orb_metadata *meta) __EXPORT; +/** + * Subscribe to a multi-instance of a topic. + * + * The returned value is a file descriptor that can be passed to poll() + * in order to wait for updates to a topic, as well as topic_read, + * orb_check and orb_stat. + * + * Subscription will succeed even if the topic has not been advertised; + * in this case the topic will have a timestamp of zero, it will never + * signal a poll() event, checking will always return false and it cannot + * be copied. When the topic is subsequently advertised, poll, check, + * stat and copy calls will react to the initial publication that is + * performed as part of the advertisement. + * + * Subscription will fail if the topic is not known to the system, i.e. + * there is nothing in the system that has declared the topic and thus it + * can never be published. + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @param instance The instance of the topic. Instance 0 matches the + * topic of the orb_subscribe() call, higher indices + * are for topics created with orb_publish_multi(). + * @return ERROR on error, otherwise returns a handle + * that can be used to read and update the topic. + * If the topic in question is not known (due to an + * ORB_DEFINE_OPTIONAL with no corresponding ORB_DECLARE) + * this function will return -1 and set errno to ENOENT. + */ extern int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) __EXPORT; /** From a381ce37323b08597df637e901099fcdd69c6ec1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Jan 2015 09:56:15 +0100 Subject: [PATCH 261/269] Fix newline --- src/modules/uORB/topics/actuator_outputs.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uORB/topics/actuator_outputs.h b/src/modules/uORB/topics/actuator_outputs.h index 5f8f6d83ec..c6fbaaed5a 100644 --- a/src/modules/uORB/topics/actuator_outputs.h +++ b/src/modules/uORB/topics/actuator_outputs.h @@ -70,4 +70,4 @@ struct actuator_outputs_s { /* actuator output sets; this list can be expanded as more drivers emerge */ ORB_DECLARE(actuator_outputs); -#endif \ No newline at end of file +#endif From 990573aef3f875bb122f524c5f563248c2e96343 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 29 Jan 2015 17:11:01 +0100 Subject: [PATCH 262/269] fix logic error in manual input node --- src/platforms/ros/nodes/manual_input/manual_input.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp index 7e1ae7a174..72f6e252f9 100644 --- a/src/platforms/ros/nodes/manual_input/manual_input.cpp +++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp @@ -145,10 +145,8 @@ void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_co msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; return; - - } else { - ROS_WARN("requested mode via joystick that is not implemented"); } + } int main(int argc, char **argv) From 76d2417cc53c493a90689582ebae9057a3987641 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 29 Jan 2015 19:02:43 +0100 Subject: [PATCH 263/269] fix comment --- src/platforms/ros/nodes/position_estimator/position_estimator.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/ros/nodes/position_estimator/position_estimator.h b/src/platforms/ros/nodes/position_estimator/position_estimator.h index 4d396f39a9..da1fc2c5a8 100644 --- a/src/platforms/ros/nodes/position_estimator/position_estimator.h +++ b/src/platforms/ros/nodes/position_estimator/position_estimator.h @@ -33,7 +33,7 @@ /** * @file position_estimator.h - * Dummy position estimator that forwards attitude from gazebo to px4 topic + * Dummy position estimator that forwards position from gazebo to px4 topic * * @author Thomas Gubler */ From 6a122ab643e8a541329803117fc481c04aed902b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 30 Jan 2015 12:24:33 +0100 Subject: [PATCH 264/269] ros att estimator dummy node: publish timestamp --- .../ros/nodes/attitude_estimator/attitude_estimator.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp index f744aa11c2..1d36e3d99c 100644 --- a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp +++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp @@ -136,6 +136,7 @@ void AttitudeEstimator::ImuCallback(const sensor_msgs::ImuConstPtr &msg) msg_v_att.pitchspeed = -(float)msg->angular_velocity.y; msg_v_att.yawspeed = -(float)msg->angular_velocity.z; + msg_v_att.timestamp = px4::get_time_micros(); _vehicle_attitude_pub.publish(msg_v_att); } From e9bcc0a2624c77c6f71bb926347e85b8c7592e34 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Thu, 29 Jan 2015 14:05:48 +0100 Subject: [PATCH 265/269] Add yaw modes that define multirotor heading behaviour during missions. --- src/modules/navigator/mission.cpp | 72 +++++++++++++++++++++++-- src/modules/navigator/mission.h | 16 +++++- src/modules/navigator/mission_block.cpp | 3 +- src/modules/navigator/mission_params.c | 16 ++++++ 4 files changed, 102 insertions(+), 5 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 9b0a092dad..b52b12ce7c 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -39,6 +39,7 @@ * @author Thomas Gubler * @author Anton Babushkin * @author Ban Siesta + * @author Simon Wilks */ #include @@ -68,6 +69,7 @@ Mission::Mission(Navigator *navigator, const char *name) : _param_takeoff_alt(this, "MIS_TAKEOFF_ALT", false), _param_dist_1wp(this, "MIS_DIST_1WP", false), _param_altmode(this, "MIS_ALTMODE", false), + _param_yawmode(this, "MIS_YAWMODE", false), _onboard_mission({0}), _offboard_mission({0}), _current_onboard_mission_index(-1), @@ -80,6 +82,7 @@ Mission::Mission(Navigator *navigator, const char *name) : _missionFeasiblityChecker(), _min_current_sp_distance_xy(FLT_MAX), _mission_item_previous_alt(NAN), + _on_arrival_yaw(NAN), _distance_current_previous(0.0f) { /* load initial params */ @@ -166,6 +169,13 @@ Mission::on_active() _navigator->set_can_loiter_at_sp(true); } } + + /* see if we need to update the current yaw heading for rotary wing types */ + if (_navigator->get_vstatus()->is_rotary_wing + && _param_yawmode.get() != MISSION_YAWMODE_NONE + && _mission_type != MISSION_TYPE_NONE) { + heading_sp_update(); + } } void @@ -275,7 +285,7 @@ Mission::check_dist_1wp() &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) { /* check only items with valid lat/lon */ - if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT || + if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT || mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || @@ -362,7 +372,6 @@ Mission::set_mission_items() mavlink_log_critical(_navigator->get_mavlink_fd(), "offboard mission now running"); } _mission_type = MISSION_TYPE_OFFBOARD; - } else { /* no mission available or mission finished, switch to loiter */ if (_mission_type != MISSION_TYPE_NONE) { @@ -396,6 +405,10 @@ Mission::set_mission_items() return; } + if (pos_sp_triplet->current.valid) { + _on_arrival_yaw = _mission_item.yaw; + } + /* do takeoff on first waypoint for rotary wing vehicles */ if (_navigator->get_vstatus()->is_rotary_wing) { /* force takeoff if landed (additional protection) */ @@ -442,6 +455,7 @@ Mission::set_mission_items() _mission_item.nav_cmd = NAV_CMD_TAKEOFF; _mission_item.lat = _navigator->get_global_position()->lat; _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.yaw = NAN; _mission_item.altitude = takeoff_alt; _mission_item.altitude_is_relative = false; _mission_item.autocontinue = true; @@ -481,7 +495,6 @@ Mission::set_mission_items() if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) { /* got next mission item, update setpoint triplet */ mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next); - } else { /* next mission item is not available */ pos_sp_triplet->next.valid = false; @@ -503,6 +516,59 @@ Mission::set_mission_items() _navigator->set_position_setpoint_triplet_updated(); } +void +Mission::heading_sp_update() +{ + if (_takeoff) { + /* we don't want to be yawing during takeoff */ + return; + } + + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + /* Don't change setpoint if last and current waypoint are not valid */ + if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid || + !isfinite(_on_arrival_yaw)) { + return; + } + + /* Don't do FOH for landing and takeoff waypoints, the ground may be near + * and the FW controller has a custom landing logic */ + if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) { + return; + } + + /* set yaw angle for the waypoint iff a loiter time has been specified */ + if (_waypoint_position_reached && _mission_item.time_inside > 0.0f) { + _mission_item.yaw = _on_arrival_yaw; + /* always keep the front of the rotary wing pointing to the next waypoint */ + } else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_WAYPOINT) { + _mission_item.yaw = get_bearing_to_next_waypoint( + _navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, + _mission_item.lat, + _mission_item.lon); + /* always keep the back of the rotary wing pointing towards home */ + } else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_HOME) { + _mission_item.yaw = get_bearing_to_next_waypoint( + _navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, + _navigator->get_home_position()->lat, + _navigator->get_home_position()->lon); + /* always keep the back of the rotary wing pointing towards home */ + } else if (_param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) { + _mission_item.yaw = _wrap_pi(get_bearing_to_next_waypoint( + _navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, + _navigator->get_home_position()->lat, + _navigator->get_home_position()->lon) + M_PI_F); + } + + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + _navigator->set_position_setpoint_triplet_updated(); +} + + void Mission::altitude_sp_foh_update() { diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index a8a644b0f5..e9f78e8fdc 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -83,6 +83,13 @@ public: MISSION_ALTMODE_FOH = 1 }; + enum mission_yaw_mode { + MISSION_YAWMODE_NONE = 0, + MISSION_YAWMODE_FRONT_TO_WAYPOINT = 1, + MISSION_YAWMODE_FRONT_TO_HOME = 2, + MISSION_YAWMODE_BACK_TO_HOME = 3 + }; + private: /** * Update onboard mission topic @@ -110,6 +117,11 @@ private: */ void set_mission_items(); + /** + * Updates the heading of the vehicle. Rotary wings only. + */ + void heading_sp_update(); + /** * Updates the altitude sp to follow a foh */ @@ -155,6 +167,7 @@ private: control::BlockParamFloat _param_takeoff_alt; control::BlockParamFloat _param_dist_1wp; control::BlockParamInt _param_altmode; + control::BlockParamInt _param_yawmode; struct mission_s _onboard_mission; struct mission_s _offboard_mission; @@ -177,7 +190,8 @@ private: float _min_current_sp_distance_xy; /**< minimum distance which was achieved to the current waypoint */ float _mission_item_previous_alt; /**< holds the altitude of the previous mission item, - can be replaced by a full copy of the previous mission item if needed*/ + can be replaced by a full copy of the previous mission item if needed */ + float _on_arrival_yaw; /**< holds the yaw value that should be applied when the current waypoint is reached */ float _distance_current_previous; /**< distance from previous to current sp in pos_sp_triplet, only use if current and previous are valid */ }; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 723caec7ce..e39fb32164 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -134,6 +134,7 @@ MissionBlock::is_mission_item_reached() } } + /* Check if the waypoint and the requested yaw setpoint. */ if (_waypoint_position_reached && !_waypoint_yaw_reached) { /* TODO: removed takeoff, why? */ @@ -151,7 +152,7 @@ MissionBlock::is_mission_item_reached() } } - /* check if the current waypoint was reached */ + /* Once the waypoint and yaw setpoint have been reached we can start the loiter time countdown */ if (_waypoint_position_reached && _waypoint_yaw_reached) { if (_time_first_inside_orbit == 0) { diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 04c01fe51c..6310cf6de4 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -95,3 +95,19 @@ PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 500); * @group Mission */ PARAM_DEFINE_INT32(MIS_ALTMODE, 0); + +/** + * Multirotor only. Yaw setpoint mode. + * + * 0: Set the yaw heading to the yaw value specified for the destination waypoint. + * 1: Maintain a yaw heading pointing towards the next waypoint. + * 2: Maintain a yaw heading that always points to the home location. + * 3: Maintain a yaw heading that always points away from the home location (ie: back always faces home). + * + * The values are defined in the enum mission_altitude_mode + * + * @min 0 + * @max 3 + * @group Mission + */ +PARAM_DEFINE_INT32(MIS_YAWMODE, 0); From 0b784c20c8bf69cee281f6717f055b0309d331b1 Mon Sep 17 00:00:00 2001 From: hauptmech Date: Wed, 28 Jan 2015 15:45:00 +1300 Subject: [PATCH 266/269] Save and check device id for acc and gyro calibration parameters. Fix config utility to work with all devices of each type. Accel, gyro and mag devices correctly set their device_id devtype. Combo devices (mpu6000 lsm303d) now correctly return their devtype. config util shows device id for all sensor types. Add, save during calibration and check during preflight ID parameters for accelerometer and gyro --- src/drivers/drv_mag.h | 6 -- src/drivers/drv_sensor.h | 18 ++++++ src/drivers/l3gd20/l3gd20.cpp | 16 +++--- src/drivers/lsm303d/lsm303d.cpp | 32 ++++++++--- src/drivers/mpu6000/mpu6000.cpp | 56 ++++++++++++------- .../commander/accelerometer_calibration.cpp | 8 +++ src/modules/commander/gyro_calibration.cpp | 7 +++ src/modules/sensors/sensor_params.c | 15 ++++- src/systemcmds/config/config.c | 17 ++++-- .../preflight_check/preflight_check.c | 38 ++++++++++--- 10 files changed, 156 insertions(+), 57 deletions(-) diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index d8fe1ae7a3..193c816e0e 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -41,7 +41,6 @@ #include #include -#include "drv_device.h" #include "drv_sensor.h" #include "drv_orb_dev.h" @@ -83,11 +82,6 @@ struct mag_scale { */ ORB_DECLARE(sensor_mag); -/* - * mag device types, for _device_id - */ -#define DRV_MAG_DEVTYPE_HMC5883 1 -#define DRV_MAG_DEVTYPE_LSM303D 2 /* * ioctl() definitions diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 5e4598de86..467dace082 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -43,6 +43,24 @@ #include #include +#include "drv_device.h" + +/** + * Sensor type definitions. + * + * Used to create a unique device id for redundant and combo sensors + */ + +#define DRV_MAG_DEVTYPE_HMC5883 0x01 +#define DRV_MAG_DEVTYPE_LSM303D 0x02 +#define DRV_ACC_DEVTYPE_LSM303D 0x11 +#define DRV_ACC_DEVTYPE_BMA180 0x12 +#define DRV_ACC_DEVTYPE_MPU6000 0x13 +#define DRV_GYR_DEVTYPE_MPU6000 0x21 +#define DRV_GYR_DEVTYPE_L3GD20 0x22 +#define DRV_RNG_DEVTYPE_MB12XX 0x31 +#define DRV_RNG_DEVTYPE_LL40LS 0x32 + /* * ioctl() definitions * diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index bd1bd9f86d..f583bced4f 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -220,7 +220,7 @@ private: struct hrt_call _call; unsigned _call_interval; - + RingBuffer *_reports; struct gyro_scale _gyro_scale; @@ -424,6 +424,8 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati // enable debug() calls _debug_enabled = true; + _device_id.devid_s.devtype = DRV_GYR_DEVTYPE_L3GD20; + // default scale factors _gyro_scale.x_offset = 0; _gyro_scale.x_scale = 1.0f; @@ -639,7 +641,7 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) return -ENOMEM; } irqrestore(flags); - + return OK; } @@ -867,7 +869,7 @@ L3GD20::reset() disable_i2c(); /* set default configuration */ - write_checked_reg(ADDR_CTRL_REG1, + write_checked_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); write_checked_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ write_checked_reg(ADDR_CTRL_REG3, 0x08); /* DRDY enable */ @@ -911,7 +913,7 @@ L3GD20::check_registers(void) if we get the wrong value then we know the SPI bus or sensor is very sick. We set _register_wait to 20 and wait until we have seen 20 good values in a row - before we consider the sensor to be OK again. + before we consider the sensor to be OK again. */ perf_count(_bad_registers); @@ -974,7 +976,7 @@ L3GD20::measure() we waited for DRDY, but did not see DRDY on all axes when we captured. That means a transfer error of some sort */ - perf_count(_errors); + perf_count(_errors); return; } #endif @@ -994,7 +996,7 @@ L3GD20::measure() */ report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_bad_registers); - + switch (_orientation) { case SENSOR_BOARD_ROTATION_000_DEG: @@ -1072,7 +1074,7 @@ L3GD20::print_info() for (uint8_t i=0; i_device_id.devid = _device_id.devid; + _mag->_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D; + + // default scale factors _accel_scale.x_offset = 0.0f; _accel_scale.x_scale = 1.0f; @@ -660,6 +667,7 @@ LSM303D::init() warnx("ADVERT ERR"); } + _accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH); /* advertise sensor topic, measure manually to initialize valid report */ @@ -698,7 +706,7 @@ LSM303D::reset() disable_i2c(); /* enable accel*/ - write_checked_reg(ADDR_CTRL_REG1, + write_checked_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A); /* enable mag */ @@ -732,7 +740,7 @@ LSM303D::probe() /* verify that the device is attached and functioning */ bool success = (read_reg(ADDR_WHO_AM_I) == WHO_I_AM); - + if (success) { _checked_values[0] = WHO_I_AM; return OK; @@ -1005,7 +1013,7 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) return SENSOR_POLLRATE_MANUAL; return 1000000 / _call_mag_interval; - + case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ if ((arg < 1) || (arg > 100)) @@ -1410,7 +1418,7 @@ LSM303D::check_registers(void) if we get the wrong value then we know the SPI bus or sensor is very sick. We set _register_wait to 20 and wait until we have seen 20 good values in a row - before we consider the sensor to be OK again. + before we consider the sensor to be OK again. */ perf_count(_bad_registers); @@ -1534,7 +1542,7 @@ LSM303D::measure() perf_count(_bad_values); _constant_accel_count = 0; } - + _last_accel[0] = x_in_new; _last_accel[1] = y_in_new; _last_accel[2] = z_in_new; @@ -1652,7 +1660,7 @@ LSM303D::print_info() for (uint8_t i=0; imag_ioctl(filp, cmd, arg); + switch (cmd) { + case DEVIOCGDEVICEID: + return (int)CDev::ioctl(filp, cmd, arg); + break; + default: + return _parent->mag_ioctl(filp, cmd, arg); + } } void diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index e4e9824902..e322e8b3a5 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -446,7 +446,7 @@ const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPU MPUREG_INT_ENABLE, MPUREG_INT_PIN_CFG }; - + /** * Helper class implementing the gyro driver node. @@ -523,6 +523,12 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev // disable debug() calls _debug_enabled = false; + _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU6000; + + /* Prime _gyro with parents devid. */ + _gyro->_device_id.devid = _device_id.devid; + _gyro->_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_MPU6000; + // default accel scale factors _accel_scale.x_offset = 0; _accel_scale.x_scale = 1.0f; @@ -609,6 +615,7 @@ MPU6000::init() _gyro_scale.z_offset = 0; _gyro_scale.z_scale = 1.0f; + /* do CDev init for the gyro device node, keep it optional */ ret = _gyro->init(); /* if probe/setup failed, bail now */ @@ -668,7 +675,7 @@ int MPU6000::reset() // for it to come out of sleep write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); up_udelay(1000); - + // Disable I2C bus (recommended on datasheet) write_checked_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS); irqrestore(state); @@ -726,7 +733,7 @@ int MPU6000::reset() case MPU6000_REV_D9: case MPU6000_REV_D10: // default case to cope with new chip revisions, which - // presumably won't have the accel scaling bug + // presumably won't have the accel scaling bug default: // Accel scale 8g (4096 LSB/g) write_checked_reg(MPUREG_ACCEL_CONFIG, 2 << 3); @@ -804,7 +811,7 @@ MPU6000::_set_dlpf_filter(uint16_t frequency_hz) { uint8_t filter; - /* + /* choose next highest filter frequency available */ if (frequency_hz == 0) { @@ -906,7 +913,7 @@ MPU6000::gyro_self_test() if (self_test()) return 1; - /* + /* * Maximum deviation of 20 degrees, according to * http://www.invensense.com/mems/gyro/documents/PS-MPU-6000A-00v3.4.pdf * Section 6.1, initial ZRO tolerance @@ -967,7 +974,7 @@ MPU6000::factory_self_test() // gyro self test has to be done at 250DPS write_reg(MPUREG_GYRO_CONFIG, BITS_FS_250DPS); - struct MPUReport mpu_report; + struct MPUReport mpu_report; float accel_baseline[3]; float gyro_baseline[3]; float accel[3]; @@ -997,10 +1004,10 @@ MPU6000::factory_self_test() } #if 1 - write_reg(MPUREG_GYRO_CONFIG, - BITS_FS_250DPS | - BITS_GYRO_ST_X | - BITS_GYRO_ST_Y | + write_reg(MPUREG_GYRO_CONFIG, + BITS_FS_250DPS | + BITS_GYRO_ST_X | + BITS_GYRO_ST_Y | BITS_GYRO_ST_Z); // accel 8g, self-test enabled all axes @@ -1070,7 +1077,7 @@ MPU6000::factory_self_test() ::printf("FAIL\n"); ret = -EIO; } - } + } for (uint8_t i=0; i<3; i++) { float diff = gyro[i]-gyro_baseline[i]; float err = 100*(diff - gyro_ftrim[i]) / gyro_ftrim[i]; @@ -1085,7 +1092,7 @@ MPU6000::factory_self_test() ::printf("FAIL\n"); ret = -EIO; } - } + } write_reg(MPUREG_GYRO_CONFIG, saved_gyro_config); write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config); @@ -1232,14 +1239,14 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) /* lower bound is mandatory, upper bound is a sanity check */ if ((arg < 1) || (arg > 100)) return -EINVAL; - + irqstate_t flags = irqsave(); if (!_accel_reports->resize(arg)) { irqrestore(flags); return -ENOMEM; } irqrestore(flags); - + return OK; } @@ -1521,13 +1528,13 @@ MPU6000::check_registers(void) the data registers. */ uint8_t v; - if ((v=read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) != + if ((v=read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) != _checked_values[_checked_next]) { /* if we get the wrong value then we know the SPI bus or sensor is very sick. We set _register_wait to 20 and wait until we have seen 20 good values in a row - before we consider the sensor to be OK again. + before we consider the sensor to be OK again. */ perf_count(_bad_registers); @@ -1640,7 +1647,7 @@ MPU6000::measure() _register_wait--; return; } - + /* * Swap axes and negate y @@ -1701,7 +1708,7 @@ MPU6000::measure() float x_in_new = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; float y_in_new = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; float z_in_new = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - + arb.x = _accel_filter_x.apply(x_in_new); arb.y = _accel_filter_y.apply(y_in_new); arb.z = _accel_filter_z.apply(z_in_new); @@ -1722,7 +1729,7 @@ MPU6000::measure() float x_gyro_in_new = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; float y_gyro_in_new = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; float z_gyro_in_new = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; - + grb.x = _gyro_filter_x.apply(x_gyro_in_new); grb.y = _gyro_filter_y.apply(y_gyro_in_new); grb.z = _gyro_filter_z.apply(z_gyro_in_new); @@ -1776,7 +1783,7 @@ MPU6000::print_info() for (uint8_t i=0; igyro_ioctl(filp, cmd, arg); + + switch (cmd) { + case DEVIOCGDEVICEID: + return (int)CDev::ioctl(filp, cmd, arg); + break; + default: + return _parent->gyro_ioctl(filp, cmd, arg); + } } /** diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index d4cd97be6a..13ab966aba 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -159,6 +159,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo int do_accel_calibration(int mavlink_fd) { int fd; + int32_t device_id; mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); @@ -180,6 +181,9 @@ int do_accel_calibration(int mavlink_fd) /* reset all offsets to zero and all scales to one */ fd = open(ACCEL_DEVICE_PATH, 0); + + device_id = ioctl(fd, DEVIOCGDEVICEID, 0); + res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); close(fd); @@ -226,6 +230,10 @@ int do_accel_calibration(int mavlink_fd) mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); res = ERROR; } + + if (param_set(param_find("SENS_ACC_ID"), &(device_id))) { + res = ERROR; + } } if (res == OK) { diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 2be0e881ee..8410297efc 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -62,6 +62,7 @@ static const char *sensor_name = "gyro"; int do_gyro_calibration(int mavlink_fd) { + int32_t device_id; mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); mavlink_log_info(mavlink_fd, "HOLD STILL"); @@ -81,6 +82,9 @@ int do_gyro_calibration(int mavlink_fd) /* reset all offsets to zero and all scales to one */ int fd = open(GYRO_DEVICE_PATH, 0); + + device_id = ioctl(fd, DEVIOCGDEVICEID, 0); + res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale); close(fd); @@ -277,6 +281,9 @@ int do_gyro_calibration(int mavlink_fd) mavlink_log_critical(mavlink_fd, "ERROR: failed to set scale params"); res = ERROR; } + if (param_set(param_find("SENS_GYRO_ID"), &(device_id))) { + res = ERROR; + } } if (res == OK) { diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index bf5708e0b7..67b7feef7b 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -44,6 +44,13 @@ #include #include +/** + * ID of the Gyro that the calibration is for. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(SENS_GYRO_ID, 0); + /** * Gyro X-axis offset * @@ -153,6 +160,12 @@ PARAM_DEFINE_FLOAT(SENS_MAG_YSCALE, 1.0f); */ PARAM_DEFINE_FLOAT(SENS_MAG_ZSCALE, 1.0f); +/** + * ID of the Accelerometer that the calibration is for. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(SENS_ACC_ID, 0); /** * Accelerometer X-axis offset @@ -270,7 +283,7 @@ PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); /** * PX4Flow board rotation * - * This parameter defines the rotation of the PX4FLOW board relative to the platform. + * This parameter defines the rotation of the PX4FLOW board relative to the platform. * Zero rotation is defined as Y on flow board pointing towards front of vehicle * Possible values are: * 0 = No rotation diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 9f13edb184..f54342f06a 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -81,7 +81,7 @@ config_main(int argc, char *argv[]) do_device(argc - 1, argv + 1); } } - + errx(1, "expected a device, try '/dev/gyro', '/dev/accel', '/dev/mag'"); } @@ -192,8 +192,12 @@ do_gyro(int argc, char *argv[]) int srate = ioctl(fd, GYROIOCGSAMPLERATE, 0); int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); int range = ioctl(fd, GYROIOCGRANGE, 0); + int id = ioctl(fd, DEVIOCGDEVICEID,0); + int32_t calibration_id = 0; - warnx("gyro: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d dps", srate, prate, range); + param_get(param_find("SENS_GYRO_ID"), &(calibration_id)); + + warnx("gyro: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d dps", id, calibration_id, srate, prate, range); close(fd); } @@ -267,9 +271,10 @@ do_mag(int argc, char *argv[]) int range = ioctl(fd, MAGIOCGRANGE, 0); int id = ioctl(fd, DEVIOCGDEVICEID,0); int32_t calibration_id = 0; + param_get(param_find("SENS_MAG_ID"), &(calibration_id)); - warnx("mag: \n\tdevice id:\t%x\t(calibration is for device id %x)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", id, calibration_id, srate, prate, range); + warnx("mag: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", id, calibration_id, srate, prate, range); close(fd); } @@ -341,8 +346,12 @@ do_accel(int argc, char *argv[]) int srate = ioctl(fd, ACCELIOCGSAMPLERATE, 0); int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); int range = ioctl(fd, ACCELIOCGRANGE, 0); + int id = ioctl(fd, DEVIOCGDEVICEID,0); + int32_t calibration_id = 0; - warnx("accel: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G", srate, prate, range); + param_get(param_find("SENS_ACC_ID"), &(calibration_id)); + + warnx("accel: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G", id, calibration_id, srate, prate, range); close(fd); } diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index bbd90b961f..3e1f76714b 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -84,7 +84,7 @@ int preflight_check_main(int argc, char *argv[]) /* open text message output path */ int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); int ret; - int32_t mag_devid,mag_calibration_devid; + int32_t devid, calibration_devid; /* give the system some time to sample the sensors in the background */ usleep(150000); @@ -98,9 +98,9 @@ int preflight_check_main(int argc, char *argv[]) goto system_eval; } - mag_devid = ioctl(fd, DEVIOCGDEVICEID,0); - param_get(param_find("SENS_MAG_ID"), &(mag_calibration_devid)); - if (mag_devid != mag_calibration_devid){ + devid = ioctl(fd, DEVIOCGDEVICEID,0); + param_get(param_find("SENS_MAG_ID"), &(calibration_devid)); + if (devid != calibration_devid){ warnx("magnetometer calibration is for a different device - calibrate magnetometer first"); mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CAL ID"); system_ok = false; @@ -108,7 +108,7 @@ int preflight_check_main(int argc, char *argv[]) } ret = ioctl(fd, MAGIOCSELFTEST, 0); - + if (ret != OK) { warnx("magnetometer calibration missing or bad - calibrate magnetometer first"); mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CHECK/CAL"); @@ -120,8 +120,18 @@ int preflight_check_main(int argc, char *argv[]) close(fd); fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + + devid = ioctl(fd, DEVIOCGDEVICEID,0); + param_get(param_find("SENS_ACC_ID"), &(calibration_devid)); + if (devid != calibration_devid){ + warnx("accelerometer calibration is for a different device - calibrate accelerometer first"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACC CAL ID"); + system_ok = false; + goto system_eval; + } + ret = ioctl(fd, ACCELIOCSELFTEST, 0); - + if (ret != OK) { warnx("accel self test failed"); mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL CHECK/CAL"); @@ -156,8 +166,18 @@ int preflight_check_main(int argc, char *argv[]) close(fd); fd = open(GYRO_DEVICE_PATH, 0); + + devid = ioctl(fd, DEVIOCGDEVICEID,0); + param_get(param_find("SENS_GYRO_ID"), &(calibration_devid)); + if (devid != calibration_devid){ + warnx("gyro calibration is for a different device - calibrate gyro first"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CAL ID"); + system_ok = false; + goto system_eval; + } + ret = ioctl(fd, GYROIOCSELFTEST, 0); - + if (ret != OK) { warnx("gyro self test failed"); mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CHECK/CAL"); @@ -183,10 +203,10 @@ int preflight_check_main(int argc, char *argv[]) system_ok &= rc_ok; - + system_eval: - + if (system_ok) { /* all good, exit silently */ exit(0); From a2a244584e36a0e9ffdb93a0dda8473baf8344d3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 31 Jan 2015 23:09:48 +0100 Subject: [PATCH 267/269] Update block diagram --- Documentation/px4_block_diagram.odg | Bin 91712 -> 94510 bytes 1 file changed, 0 insertions(+), 0 deletions(-) diff --git a/Documentation/px4_block_diagram.odg b/Documentation/px4_block_diagram.odg index e66e1b1cdd704ebe713a2127004488aed7fb2218..84566844252805e31620a7313d29aa34d1b36029 100644 GIT binary patch delta 50391 zcmaG{Q+H-fw|!#Uwr#89j%_;~+fUSS$9B@OZQJPBw(aE0dw#&VsIhNq)UI8%$69mF zHCN9BMAa??f}#u2oWdcY5*GEW4h$6c3woU3hA$0 zP9Xrnpy0iR$?r>h@L)B^u{9G!olnmz-qd`;62c-q-c;wsJD~6>_(rC!J$dhjvmmIu zCe~p{m&)77hAMtALSZilitwn(t;=lC-MWMC?2-Pt1EIzybt21RK<3?}`QEmHoI=rK z;0EH-P|cHaa?1A`3E#{au95n=3wqkqkLyihnQg-Wusmm1)*y4@%*wkR$Edl6Gg$eI z0YMO-@yiF$04)D(=HuAj8W6%Wa8XZ1r;`y}9;` z-lb4O-C~`;x_4ec=MXo^?I~&xC-`$qNK}YD3TAb1Hq0DR0%R!vi+^7=Y5u1&TKZ*J z0FQxX?f&~{&g&Q3`=?FFr>3A<)MI%6Oo+C>_vi37Fo8T^F5u7OT3Db|b*d>nx;W&4 z0F-NB4#<}cg9kBa7dPak+>GQZiR%ebc5X~n;03H1w-yl)EDP3^;JT9LdM#u0E@hhz z=i0k@CLe)wP-L-y=mNYS!iya0)0A(9C6y+SP29l(?fD;c*qA^4)1Bv6_#Ho(e}nOi zz5&2+I!E|<2l3hs@^i@RdI-Oz8co(rm>#?jbsYujFQp}1K&Vb{0Su(_b}y%cA>p|V zgeO>m-qZl_EXZ&OlIRS;W_CY8{%p#%f2HrIR5{gJ6u?9%!ZRKp{2oltZk7r6Erj@(K!2*)n0} zW}oCv`n*wZ;X~{afE3(>$tD1!1bCuV`~e$1^vML$2l#`3x*?PQFtowdrFA4+AbB1V zcc=sS6(ad~r^2fmwj(N2Qm-~h2QW=J*?!8166Xh|f>Y8F=LHS7_uP11FXBrMYT~Rf zz*B?f=s|q$(k~|fpezFN{R8_eK+E7nAv`Wu@oEuTSx74U9LmG=e`4-kOI$JCRTAgkP;*Gy0?AL))eH=bTq&}utr-oDux^~j9a7OKpDnNwdJ zN$I!0Ec_uU5@(KM;sK=3PN?iMvIc==Vzt}IoMGUtop1ZpOYP{jyY*x?vxa*D978dg zV=5Uv3@qs^{`4YO-g;rsttgy-`)Uf}Q=5QbGYBuMFN4rvRU0p8?S7vRwJra!?FuK4 zd?{~G*@c5*iKFy>>QKbG_oM?%Y=%ZvuSrV$auEZLEUK3*k-A#BA#$oX(i}TX16fop*n-K>=8udFT$>xg zm&>`G0^0c5V60PUUz3aZQ)UUrwDCYk4Eey55mNcPfyRElAiMJ^1bIctw`p`aERviz zoZ)3g!ysntmEf|8vJzf9+t7M+JWZo=C@p`Gc5-y({3m;`puNMvg2)?VxAMt98x^Fo zN{=g!rKDsog){VxS=S-*t}4|XSGs~K63?%d8jtWRz76La{5XjH7&{=lss=!SA$%xB z?&{++`J9I74Q8-Mk!t*P8>cI~))BLr|JsvtCwmO2_HC}DV6TsEJkHM z78lmXy|?dRWn%AO$o+LbtLslk1-DnD)N(i=kx)BOK=V*YX7Z3PBIxX%VYBj49$229 zSl5dQpHEGl)!60DY^&5l4j51~2`3$pFsYCR2c(uTpl8LbZ~EC+zpuXhSSp+0B_P6O z3i+|@+Is`{(%G(~qYBJD^)N2GJ^yU&ZTLH;J8KMx1uUsz_t(xY10%I&+gDuA&52|> zI2AjbMUQtY9FJD7b9`KnM+$IMXUr-_C#~p7)v;NmOkjcvRdtkYQVhnD+v%#vH#HM? zpz<)tK6n<$0KqhX;E26$*vEgmp|a)|t{ZteOQF+p$?A*tw*>Fn;GS z2Hd6VOW2S#l%8K+f$kM>g6_Qk^a>^W#@bpkCsR-OGb*(mPJ)_;9tq@lf~_NEQ`}k9S0+(I$z@E3PkUZy{xU@C z^QNgKy3g5eiUTd4T8K;rJn%e%%A~B7&qdZxSrsDGh#bH z-?b67%-E1QhzyY+8g@BHGnF3#teT1FlyU)s_l`uMx)7vI{SRcrW?rC%`Ejwd#ll65 zozIIl?vc0ipFCZ}%iAeLvK^khZ2a~~^nLRfluRil!a&Xpc%wVV6-zaYFofGZG-tCQ zd_o#PsJaa*qSRpgq`ywBF8v-Ag(XXuhSIEq88}0v-q=+*gY7wyTSnC$z~W{A)9#U} z+ct4bzUw?Bs703~01ywTwQb973)`o0`*jYvvM_;J+qwucwK9 zR;(&_#Oc~Cz@HlT@sX`#nkKO@;X2b%Giyun<5#L3!Mi`zqpgsC20N}E;DV++$bne7 z>SX6fcP0BoD^wKC3?qZ#EiI%Tj8p{1R@KRn0*#S}&TzRfgjJSxrdm$=JXr7-?&9em z8r-H1!2dD2gMc{JJBeAvI8&dpnDYiLjwD4H4kOr~rn?(C+p{S+x3$6U-@f#OAyy?H zO(2d@L#LAsXg#90Kzx5u%~BsX9np*^k{z1-co}-v`ELeTu?5{^na)4!D%ycHHlXfu zKn+7Vs)u;FUtnPgU<)kE0X5j6U3=|5c=)~iMiua)Nc6JjdV0?(!(-Mcbd1j^^xVi4 zyF0^nDkBGf>9g=cXLk1@yM@ILgi(a&KunL?W~Lf7GH3ZdiZ!-BPi>3RT#c)9JGx&% z{hR68TGK6LSi~Yh3$G4*b%(^))B9t!ft)3Pxi0-KloD(z(nOY?D$WW6B;2a;a9wHBKLLuD_Z(fwF*T*?&^5JPP0#q#*_ z&}Q)4;Z3wknM|R{d8*}c)<VPR|8B{+Y*Gw5e@@%NeT8eTJ7 zg4%AOp1mxR1|5>=ULF#zx1V+wSR!OA2<|9HanEJj%`Q1FL2TnGh2ABc*5%%K1V$?5 zA1>@|H}Cm5UB`$tt3GB%E{!TtWW>6rF{c^=f+BucMHdx2oE0rYZj=gZP(VH&zt^Uu zcXGV`IZf5Q`S3W0!4oKUuT{X&GGEIxPMQV_3mpC0AMd zQ8wgC?FgD^cFKFM0DXs~82?c zM%oA+`Y3V`$@i6wEk!;DNRYPjme*Oya`8ZPrQxF2r%X?HWo6Ifec92!;M;MsC^uR= zJFly~5LD>HnZZOX&4}CTs=TTON`vm)h)v9R;5&c8kpi`jBlaGb1wZ0g%^9*N8$C>OU4^I16F$-Ur>pCeNut}>nDOVjIFF2mC z-PPiCMka3(X;{6vIQo*TDX5WhJGtTqs0h<6>-HDhvp?E5$MtPDP3b8WM&QDFK zQ#gPg$Te2GuCA-tYe@NX!qM(fBnD_)S71sRw5$V*^bk3_L>IJ<&nN0ZftSOMg2phw zQBQz8r(s>lu_Txl7^it=5+j7D;-1;TU3XJ8nWXzTrRQF$y(Hvfgx)gf#?j{cRvCe* zQ(xuI%4MZSSaVcY`}iEE;deh!zlAhn6x5S*Ci?Zr4NJ_)8re3IYS4o>E@tn~<>cfL_GaBGXJ*N_DZDB}YUQwWC8ae~j1fBRK;b)P5i;n4|V#mJdwG@qaDJTFs&g zkPF57{JCkcnutDP?BGf1P#iUl5FO4NzE8K_Xe;Y zQQZ`(oU{EN;E!delql6r%{#fhhCu%#bLv>EtFEn`rk}_IL>TnG`SJXwPB$K z#k&J2{&-KO=o}<8wJ^kXCHwn^&Iv7!k9|CqQoRM2MPM#`I(jTiZxqaTBJP;z9?A1* zoF&F)=?PsEVMS6%vYZMzriFppbQ&mNX9eCOLnicFP^B^|L^J=+>`kJ13a&(v%^c^? zdS#y5AaQ{~ao^ub^6eqjlXR3~DFBGtn{`7n6rmbT1gVk=CgD0BwnW%xk}i}$n6CP~ zh8~f`v#X8!d{uj5H0XFnw=m3Rb4M2U$yvqqbolFaKT!UGjC8G zkjCi>fC|*Y<4-j#4!8k*KDXvoJ0!q2W@X^Hc8QiyOK0q5aI6uOC8M2(uC|DwZm{fa z0vP6VY3D zNq8MZo=}He&v-@5s&?ZkY5UD+a`GD&>~bkd_ikaJX{kYvB+>{(&%R`j5~$(}(nI0z zc!7)Xp0c;iwHQ!}YRao13(AvFOq-%7%zQlma+c1;4Y_P>2-4%S%yq;K7HrC_5i*Ss zq@&$i0YY>FZWADsVN?zRPX`bAUL7Rv&_2dQ#2Azc3R$;3NPavk5{f8~j)_MgBNbc{ zkQWR|^g(78PIQAS&8+)d8Mu-!dC+CB_4s97lYZku0A6jkt9#bbL9W%5yP`NRSpj{6 zbcz5_)dc-|$7KY9h2dyt!{2#SaJ$;f1E z8v8sfqDit{X>}RMpB@+jCz}E^pYifaJo}M=alJs|hRiJGk<@E53jB((?+=qz%boz+ z92Lu+&KjPFE;LiV;gy&@gW2@@^kBTMMJHyYQQ3pEj{03J>u@P&X)*$(NMc}Gku?92 z-vt0bz_Bv$p@0m_Tq_3P^%|rCG%@Y9K>s4TTc9-^n#3AODfX01su@JRDEpf#V%CkL zj`fy|dKX^fVqMWF1DS^$@^4_05&E18+E71}U{% zN_Aiu1zIfGQDEGtok}BFls&O(&dQ^(l+&F%f2oAX23P6p`>gC&ai^C+=L>^R`rcBl zUxWJki+KjP=W0e5)aR;;9-PeSyDsMomk@PUtN#(%kwecj1>SVw#ODXNkfY+|h?8aw z&sFQ49=*&*jb}G1Ix-`J{&}R}(n{a)%7mvNA&(k#$*@aPgF? zaT!VqQRM>CY+7{9|1y-_=tARZq+qvPL#RDels$(T=oxcKI>p6WVmff35z@=v(N3I) z5)NL6EuH3;0TDL8w;4Nt4FNXvk-(JGBz_$42)AREs#-v(Xdiz491i0*Y$OvP96G*& zC?n+}xc}&H!c(~+M-a2c33M`LOrshwkt3#=sr4k_?}N)vfmSFFC4eVZSfJu($@Yya zBkl^(oGOdCLS+AKZXl#l6PBVgS?)?8M1y;Mi=B-HoIE71Js2tSLFBJQ9m$XObd+;o zub>ocU&78H^z#O%jW=t_em2`q6wBwB4nYkHV~6`@B&9QxFIXr@-ia~it9D1Li!*=u z?hqwU7Lrkg{@LKovsQJj*EBAG?50`#W8|DHUySzkSG52jKM+U#Zk=PyO)Xm6C~StB zNw3-l7MA`9kczQ^?R27DTCXTPq+=6r=Agxrksr;+YhxdJ6_x&L(@Ig*Uw4MG=dAxP zI#zsE^2(MBg9^QGODNvc)V)JIba?0|PXhDvGUJ7v8gpJyy}F_}MFxKZ1EZ3cUHr@kLG^q-n$Yh~jmNv~V4Cl)z8HLPgzlT3A+{CT(S9TLiPHyr#*B15& z$;?`7rPYX5wYe_S47C+z<>?hI#1SIgz_4A0)1A9C`;IiYk$IP$YW__W;(NH2v1@v! z+goswvcq_m(#*+~X8!rjz;!Y#C!M7b1$YfX7mI|5tX>{>y-+9Pn%i+Ad%D7tJE3%5 zhz>)*JuG5N*QNXRk>lA0UIu4#AAK$e9_14FxY=7DH4H=`(F!3e%W3?H{a3#w&?3@n z?X_wPAy>v3lC6K~@i-_UNr;fqsWI!%BU%paoB5hpZ%Kn690!a6=`Um7US~R|Q?c3A zs?OX_7*0^5aS>8j+2HbEE1JTHyG~IWSt(YFtb`3HPRQ>2{?beH!MYM7J~SM@irobQ zZO$vxe~XK9aRH;S-}!V)q0p2Th}MhiwI$28krhDn z2NOGamF%{3=Y7Yho+)pw=;8j?U12<#XvOFZM=2NXk{kGwViomVO0;=jm@HOief?sU z>(0?;>Y1h58t8V<%TEJ%;ND=X%i<8D4Qy_Q7Tr%1b^o&k%hnD8THn(L;C>l3S4hks z>~?i1D9WGZ8W8CS`S8Umxcq z1*Q&ZYNFQ#v%zj0IFmdEa1KJ!0*a2Y7Qc{UADOnE%W6e5C=KqYB&UbI;VZrM#e^UQ z-rVf^98OxI47~fL4;;NO+ZW3OTuWrxtu{FAwF=Zwm0rJjY61)zim8Z)c%V9{-*cK; z1D%~$%S&{A@n`B&OR^Xfqyhvf<7|IfcnyB6S;$nK>%G`P4Ra3#5C#$HXnC|4%6VOP z$5S4jQaE|$PKAQDYS>5oZ#ChLd)}=WisYOze_HestpUYNMv%JktH8Z=zzeg6HXDC} z6u$wAip2JiHp3=*851bF99mlZ=oV^OZ3o$40=o$d+f;RAlBoj$k(aq)@iQ*^V%- z(AUF%X=>`u0_FuNzt@x83eUSegLI;y#7p;G4iz)huG30CPoewD4mDsq*)lur9M`QH zRVlZxQCL;h6y9SG-sg8BlF+6s#hVu>{uR*z9^hH0hEBEuSn9;?|DLF@MRVfVbfiS$ z^AOsg-G3&o>1vemI>rAJ!F(H4`-gIY0Aldc2-;n)SJ$)Pc2XP@H{I{iFnZY8 zf!#uS#Dr#Lb}4a21k~bIzmWbP2XIvz;SQ~@Feo(ips9iR9s$>TmlZ1b3z0Jh+YW@m=MZMky=OcW41hX_`Yn zZd@93vPYPy7}bmU+`L~r>4LwJ$-C`vv2rWd6efXT1R_P*c`LMMjL74o45Se)$eYb_ zaPu^39o9{!QA_l8Big?5oTIk4PP$+R7y&^VFPo7wP=PYRKkUy?DiWFDYCd9Q*gvGr zn+Va!HA3jRE3?S(iFOpHAw^SYhT?-yh7tySk59u%1Z&L!H!ERrj)$SWDeX*<+p}kz zIRDya-4?`fPADD1ZUT6@iA-Fm1t%Og3_N6L0}B~eix5sNO7E0Hk0HWrMu{>8(6JXpG;<0oW1 z!^bSbiNf5Ef%ax9e#rz}XLLpx-GB@lB?%2|O|Z+Y9xrT~jk7E+;~MKCaI2K1VYXgD^6KJ~_;!KndsH zoPQw_C7Z;iW3c?L*aS3tCt0cCKHXa3euucy*mG0n&9-ck&i)wv+^M~1ldK~_U}?{U zGw<#uHDFj)Xd+dkz5l!jkGYrrt3`*ZG8L1wmT^#8ftRSL5xHy3%c*Z=tKR*xd;huP zB>X$uc%)vK`t=!I%O4{ANZMOV7D+{R6*G=)6-)$=b--N25uZK%JjvI)aZTH zlthgHT_i6ye8HYNLYnOvfpupq>Z=H)a zr>-KnO8wzy!25}ezZFJ=bZNApyT#lY6&sfF;cKv)Q-`H~ogNnDt}^dgSQNYvM*vHk z6;BXNeYI%L_?pO{Ol%s7jOF+B6V_G>+f?MI>p>rjP&-XCI&uuSp<0xpax#Ur-0W2d zEGIPX26mrapSH|}rY9%~;NgVLU>1HI(dy!Ui;G(mtAS%0gtt|`53dz9?EP|wS1U8K zHRqohM{=>~jz)W5_XL^Bs~p1hmYd`_I4UGU&+@5pPy&a=8i3-TfGmuQhkyy?|5v1aV>q(FFq|BlQ!_1jyX^?0^~P8q!DkR*=c#Y3T9Z<#qnTYLxp+g zn;)@qrB@k`8jXNe>~)c%?sEF4AAY_G?wrp^h#C!fWt;CGCFTy!cVyKMUN{vk>bh#! zqcXVi7+_U^_Ckg+_)}$6EOOLVbv6FM*bbuX=Gc|n{u~VHm%}+d zjCM;1DGPD=NH)}X5gfBYU?{Qk4d$iP)#Y*qiWgJmb(U|Wr8yInFEO^$D1LhhKr7D` zXK`SYr+pV!q9G{6X{4{P_Xhg8Vb_7E>Nu{}Wty*Ra6> zc-`hcWIMAkLee0&!6%JuGY<|l++36S}WoZ@!)rHlA(NYpcB|4;#(ZW<9x8>IC$V1;+;eZLWTK`P=`sjJnmx zQ|rv?OKy?YR!}Q+NH=F&E0>7=?;vvm2$mu;oJ};Y+tZQu410QTuaLt31a?SMaxC$< z-W9HD06_MU4Hill3%Y1ZLV)zlkrAg%E;{9mvUa1y?F z%F?>ridjj%|fHxF{hTdX>n3Z*tZitv4jlx{DUTp9-M=NNJ5 z-#goeXoT(*cxQkqZ88b7*4bt@in*43ZIDTuPk|QT8(}bE`u!(ke1_?;3;~Sk?p{O* z$?u~=0?S6EI^fDqDe<_N7b}WnDG?!9W8{xw^$^=SL#T;^?6{V^xFmD zF&rZ(-I{J6Ic~;FF`?W$40v=}6kQwv=e$?Y9okn7?rAYOSCFN9g{dGFj5yXT%a}hA zst;=(IJ-zIS27DM0H&<{YNo>Mp?Vv z``X2>CtC)fj$M21=>}mQoV1Cb{`OS9xAteK{8ly#%WWhI;-yM zwY@uX{y76Dq0yOIOS_^DbBm*i-!`6$yM+;Oxcm;DCWPC(`g16o#!A3B@Ej3Ct&SRfHOxJ5 z^`8Y6{WjhPbI-LEfkw7XyFRPy>)I1$w`Ndqme=VfFz)2(k-udJT`(X}cInOK)(UkV z%_&0Pu6O1MQeUX(=qm$ly4Xp8_q8%Oz$-e?~V zhMWD$ybvchIOvTIryWEt~~TL011S^{12#DWJkt)7vBe^8xVPTl(>0{Pdu!+{zIW z0c}o>f#5*b?%`F({j%fN#_8nmc*p1YJTdym!_O<_`m-Hx!TY0}uc!DZ=}IqK6qYK6 zq{$fIZu8>MyuH9vVI`lCb%Eq=ZStXmj5Bj$Z=Ra1FT;&p2DsnjTnuxNxeq@MaWbLp zCGhOb>+aYYYSRnn23yM2TyEqup3b);6~OE;KqDPA)1-jbjrrxTS;MqjWBWG-~pF9;@pb3Y(`^ zv)m{#V6Vv8ImBdCYRb%fn2GS`G~#%~GVl#om2X^GxT9&_R0Pg~3Ow-NtSD0~gLgKzS9kXdQJ$iun;+!Q(;|)x#*UBolZ0w>y5xc8=So`D^B-ME*p38!cC_d){4WDQ z3mH*&$tSGt>$kf!8y#;2XLeS8r&ZG17c?PrG*0@vky)%I`}M4JT-!e!1U&WCG{EH4 zY@8j2$~007G^Y=p#R`}2!u_*=BgQ%N#;O}MO_!&~*N=Mkk;ns>Gy$OjgHFrYiI{7` z5af>g)tF@qx7K`sG-wk{ABvh;R0V6?m9e?BDi=~y=I-?|pc!OcB<$U21-iQHla(mv z33@jq;AFLH^U?vROk8$LDhh#J1k_uz*3R5M_r7zLhRlYX`V~1Sq2EttvTm2hwr*L9 zFlYo8iwFwBuwKElZb^fLh)}l&hQ(ONMge2al7e(0_S4 zvVYGeH#BSgq&0RM9ch*@_626FFbVTSZ3z(#hqfA|ajyBFv3UHDrz?$dO;RfS?q%36l zX;%&>9+7m1WfqAn7El0l*_}lu@4oiu*JCtS4q+m=Nu3Nn3(@dRs4?0$pD~(!vA|SU z1Uc@fp(2YvDcF`bxoYn6RcOexTsDvc!Hn9g14MHJkhztWO2TJwZ-3-`mxFXQLE*=W zFuC7lC?@=ECaMC)0UqU=KvP+q*+Sd(|1;YJCWdRu1!+qGcOsx2^YGtiM03hbho<1O z1EpXg`w1$;Z$^zk8&ffv0UWn?au;XXe~%A*nT&L8?~fD*z$#UhOd0%_Ef4Z*eUAk6 zCgUOy0=*Q2rbc@s+wE6szbdgZb_vYN=LygFx$fQUw;oqYBQu(}a^$ml93K*yfhek!T}^YW>i%s44tz0n9T`cKbK=JK4aulp8tp2qA>`i>p)dDI>8aim56V)c=j z@tO&Gtogoez7MzRYN6_hBSglD;(MQ6@_F<2^?FR7FQ3GPlhBt=PKGP9=y1}O1xIk| zZ>+L2`{eO#+Muoen?G%HW98Xw_qw=#KJN4uHUDn!_t7m%V{I;}!BvTyfdi_|l__}B_q#q%Q%B+JQo!4e#;m>? zp9deYET7DV!}`?Tn>yX-cbm}Lr~yse9Bf4Lzd4;IblQ{IF13uS^*c7r=BDb-GdzV) zEYXW_p!I6g8ljxjlj-5t+Vw<56>T=oHjU0Y*?>mb%urWl*{081mV3cbvHgZu&R(pB zzUS@bn^1FS^`6hIvHNf|)#r|aPMgkt*lpgM9}S8a@NOTArdl8QqluZciXyaK=&Al| zRef^(T@oz*PwEWPzgd}4O-+U)a|7;cYxiIIK#Pm$4R68BtSs2UpjyS_(dw`1WR73- z*lll*)0>U8_D=5#Hm`(SBhOUQ572--T~UQn4GvH1`+XOM9LNh_C_lqwxcZZq&#Eq& zAd5Gy%Q5*(g7Jf^RmZ16nNYTuo!U|swVT%6~EJYTH24#&g zjJEJ=twLYp_K^L0HYIR-f894euU|~2JFkOG-QUWvZnvh3hrwzW7yaMPW4hDR1^V5` zkVTcc+S|*m^GfrA6nxf5=ZCnl|CtqC0+CY~kY-r_mCDj4OjDo|6l4Mq4vGm|-fm`l zn;)kqEa03E;cz>E}sPQF^#|z>WdG zxB+J~T+X@r7$QE{XZfh@FlQ3e}|Y?R70J5HC%mEqm?xM*3>U1@|1! ze{=IR1g4@af4<&V;Ld+=K9?p~Jevd0M}D&qPhfM}%0Wci-bw{RvddATPeuLl16s>A zl$D2^hHaGuc$=fNc=(NYgAT`B0>>w&4}L}$vD}$FPg?)wmJ-frP@YfvKQ3_oXSJx_ zXwABdD6T$M6tesef`BB+ZbmE0EVZJI$Ekh#fV+q0K7fPYK-!?-1>}?17Febb4*8G!fy;)syS@Vy- zB!pTt9z>MBBXro8Cb`R7XS1SxdHb1Hsb65RsO{Rx>CgKLyXvN>&geYUDgUKSxBr~h_*Q?8(Vu)zYUO;Y>GdiRf>Z%7OP1*?*=8Sd;oq-sP? zvSrH}>|*)6Hf15|Wv{?coLc?>NWl+Y*l7 zWK4Ca)DNvs82A|yrm;R!FLK4?DOfegyIA*aIq+9ps$pfU=6(TM>5O5OtR5MlyH)k9 z?fV8SM_yOn9TdHS04yN%VC=O%tY&d%g-#b{Uj6V|MqbhVlE$3|Fx~!^T#U8%jf}gJ zMT4n9&|15`&Q~`+<%5gu3)N&ebP68k=d^iyR%HNVc3IZ<{g8uCtzD|P>rn?3zsa%p zzf^G418?Fj&OzWBDINnOf^nz!a&rAyEXtPmrutP8Arv@#LW1G7ZB#3SL(mH^JnCj10(O z0P#oBFQuc1b_AcE?}n6;#U=F+rf(5}X8_FdgK)>5a( z%*af4qA78M#$xpOp(=nplcZZ!^F#f5Jgl}LSA@yV@F zfpDPqQOeu&{r6GE(>lz=`7)b5q_;ZXZ)F?NuYt2_5bC$IJYy(Z<25hIW7jnqHdV=sNzr7C;g~H!lzT0Kqx&;YS*Y+o z=>y3}n4S>yq7`tr;ET?#W@sF$@CMNkTxqkp1XK7qRqHHAFxM$EpuQR&YfOWAT@2A^+>e3zL5 z_sMBo&K8Jr-S4vF7w%CX+o7_1Oi}U<0@I{}H?t1YWn-#!tO7x%UDZo+Ednh+f`Ol? z-(|yVa%Of;r*Kt1+1f9a6V=HP(RFM3`fl{den0Bb>WN(|P&T%A+Uk0iQcP!ti#|_* zhL-HC4+oe|yR%r0J7_+FpH5aj12m0_-gV5>MGWI)U(ZPV`}BM3g(7w@$J&PzR$7@-L7)+$2`GnUiSFY79mQdoKhq4a=R9+ zwE)#SS)&|nnMDM<<_*>r81UAZeBplitiLq{9WR|rf9-L+4|zxbHDcu zaykn;OX{>YwrXqwC^&RJ9iMM?JMcU()qZ(lZ_nd#?O?37)>Fz>hikg4HrMd7k`^(z zw&opXEXfZ5kkHuD`d+H!$SLUmoGJ$G6%(#iK+?5|-~u!JmA9CEIMcf~jPEXD=slWT zs66{SRj0b`{DAWB6C>pZHfLbRM$Br%2cfWR0VK#)C;NT@aOe=JyUeW;gP`89#GZi7 z`C?tn)2qvKTC}H>xHg07pXk)O<>@j=r9~DG8%bjYY{S6VXu~2p!yf~1%}BeIY`)qp zj>@x*bqE%#8V~8tDiJ%@1pY`&iq0ZZu`NJN_HPA!)Ls`P`V|ci&j;hhW?~)VrymUD;~<^YT^B zn{x};zMjr}OU}V|clGIOVa=H&SGX@_Dd+6X`vD{%z=pFt-ThS?w(3O|`0GMKO1xaM zplS2Ys#8D-haaiGe3!3j`Gg)m)QVMI{%G~;OB_K12$}QlGoBSg!jR1e+}$-7D4euMJrqeug(5M0bf9 ziPgc88N7h}+w1LmPYhR`X9bc$(Al-37S~4x!?_j77drd?E}j z+z#n5P7 z8He~XtGWZO>E+G)>ZS*cZ^ElT2rxd8rIU&KE_?s#DoX$B{jisYwKwwXaZLR=(6ZnN z#E+3n>m!pkeKYrWPe(PQ72$$}-hT z^-1I=U#JROH@ke$B!b@+zRppP?e3txr|^!{ti0Ognfxf{@fVUuFiIVxP$3b610RHulLZy;@jT=x&#mD-V!qhAi^~s4-F3lV{nkHL+}V zK5crZ2`ItA&6B@kXgr>dtT$9;_4vX3{_(W#{sh0MzVWehd=5VNukj`&73Tq{-6Mhw zExGP8^s~6-=Jds_^HV4B^*etJD@_9LUdtk(@egzm#5!GwtNX#@(8q`eyq{3p=Ks3d zqU|fe3OJVzJSl6Uyb2Y6w_8%_AKPAao-s$nGNN@yq*Mbjn19WtWh?yVG`>wpwYf=M@KIy@yFx6{T6b{i9jV~! zeLL^$L=Gyxb-$WP;9wi5A?M8fjsMLi&pr`W+V)?}0GddMv*JqMTO zRwx%o?+6g4Mtx1Gwe)`~(5Ly2BmA&o%QmOSg9f*zf!DPPky4wly4?K1%abuSM2fOj z8@OXhkII-b7Bv}9%GWl;F8i3n>uP?O5dgqRy2fM4IfuxyxtWqP4PG08^>(gjzv0LR=l5^wzY$`^FnY9Xl;G{Q zKpk#P#0it%%Ng?C;Cr8hzt6Pv`JY3iXvI9u_4=ngpycJIdQ~DM6yr)TWIqXIHQQb5 zqZnL&UxI>QS~a&a$12=lm1H)a>Y*`OP>yQ7y|$f(_i+{DhTABaJ0}js*^I+M^g|pL z(p04Zny+D{pV8*!ujKCAf9DKmKUi?S0wLpM6?DrguI9%%ak47--Py~_o;Nml^zStw z(NXfHfF-gsSMR+<6w?GaY1QkE&ck%o@4|m5Ic;^Z;@1Nr$to`x>KmE_U^E)#irmJga zs{7P*Rb6w90=(w5E&bHm6sN=C*4i^+5xRU365+{u?RS66`l}Xy+Q>~ndvwHbs!d~P zAJ$M%?rF~AvlXRcw)4qCCO*;W=@H}U%JurIRK9^Iw6+h%S{k@QqRuz-y0hRq|BD5O zK8MY+j3qo{d&yBX=P+hwYxB+wn(SzJ%SqfNyY%X?^F{O!;hjqO_Zx$`*FkMX^RzK_ zhm^poX6x0F2budPg6s!CVA6rel7}`s6@EsS}P?om^4?yO7$^|x5h9-HRSyr zhn2adnPuc4nrox`_}RJF`6vIWd%~4xokB!V5J7fdYd0_+7K!#J;0;!^v8=33V`LHW zoFUosj3YH%?r^55t4cA8#A6I1OIY!cN+4Lw=2RXlQNO54m~?BPy5=qC3NADYKkG)m+@m;VKHN?YuzJIRZNE)u4m9%ca=pYYn#Z za|pe0W@G{DZCo4B6|}YrO-xqAhfdBPG%`aw!=>G`Puy<5@x#K;O{R%A6s^vxuKgL^ zd}3aiFM=42ZzNY1jUN2%BZ99{v!wXjvN{PSYV}w_%w= zQJ&njfJI{qa)Vxyvx9>_Uv_zU`Qzi`5me8cNdn`vpWO%mCPf}6!S6exck(v+@og#J zw&>m6uek-4@C84#m#?ropH=JYw+D({+=sygk7vDPmEZ1%V!Y6Dn2KB3O1;ivmXQ~y zYKxz~<_gsCy^exDQ1iD(9j)3;{SwhE{%ncXxDY>hW^$Z7VyKsm?ANNv0#<#38%R=} zhCz+C92w9JSY+D_k%i|8P?H;BZ;=QowZ4IgF=HYGq~d3W*4okk6AqA{ZK%FnZ+U!p zs=1YS-C|A4hK0qM_nl{ZYR=?jK0lwl|32$!rr@mo`OJ9`PxHa4iU95RO*NChAXObX zDr&~l_NOcI7y4h!*w7Pa(kF znlvzB2)IB7Gi!34>;U1ulyR8kk~X@ z8E5lmsI|n=^u*)2yWo-^ERtcV@_6(QhcCf3gT`Oy2 z|K49zRa2AVOBi;(Qf0sO5|Xs;a$F_->Y)tJeeai)Q#ChMjtHGmRHQ12)JOUhk{c96 zZj3Roswi={Tt40A^-TBv@@L&;8^LJ3M9muyhFty2uX4&F1-H>B1j~@U!yOL|U_L67 zb7^MI5GTNt$FyU-J)wu%0I#OWCo7*XhFH^EvPt zHTshjLC>h!WDKy~05G}Qp0o>d&vY31ndfP*-%qW-zfOOP3!7KRzV(U0+R>|lhvdrz z-)~sG+|cB3(qLn%HXfhT`D`Ww=o^sSTLZ^y8dht>U+~W#7RTT8zTlp!&N8!S9jVXq z*c^F)>>^Mx(KCbgeI(Dw%r9o_8lvF2AGV$4_Skwz9eYzzK0eIFa(5>Mu6H{W3neR^ z=Igm?nCv@lsNUJ}EoWsmA1-drZeM$n#v&xK3>xjkx;%=7L4wN&e^GP*hN`0DQUsip z>(ogKnyy>t9dgW?N6EaxAx-E7dfbvKA;qMs6_x$>3m2*E!d<1bLiM==6vLBc}DFLV_sj@VChD@FX#2c81H7 z-wYw9ny7rmxUd7$*Ne&=98-&-BT16h6k`|=zHSiS3nYUV-1OX1kw~8U2dxc^_P1GW zpZ&l7xc>-$67tU`WkJ)K^DZtViAb?F7!BXJyK_>lMbJ3IbNj>qELtza2sR;y7>qK? zFaOD>2qX`gpPa;?;Y}&$VPVbvs^;Vota^*pFC;l^M?p~)4W|N8zc?DF^NS1J>|0{a zq)EPtL&)vi;PwfntQPmTQS|;^r|%Y?oo1ie=smi~S~ucEk?HK?U%!dF+@1W$x0l0! z_oviWb$g)#hDB6n*`0n*^S7^3i!o}JC zKIF6Djy&d8V{vkHz8=-|?!bBA!>@lzQ8=}xg)4p*P$cc`)`k0-rVF*V_pZsD8rf?* zo6_$ zlzneF|MC$h22lBM9uq>J?I%Pdua-*2TCG{iErE10MysqfZLc$fYRO8tGlftY1rAC` zH)vEdnVxl_PcCBc=P>r~CO+7W_ZGN;dKcsshEC0PNU8C%ER0eN*YY((IiH@27!>Ff>P;UmJ)$n6l4Ska6fHu4Ra+S`Ex(R0B`F<dAcjb6D5b?iOQn`eoco=(GGW_iZ3R3FC z^;oq37C^m|;8s9o7Wh#V>ia?H|0G`%@c%En)BmLE|544r|67)+|F`$?^bShAi=AJk z6WQ|!@^KRtloBZC)A9XJ`Yvc_=zr38#aZy-0mD|y^vHtuudw{5N?ZInG$_^8l%ce^ zc|Y+#bNG={(5i=^i?B<*x4Ws7uV4oxk&KVDUH=+tX~@@V^}Sw7bre9wr|A?asa>qd zEf`H=i#jVt`t9jDiP~6wHQ^0o3(ia3EVKRSQWuFhW<%9_w-OA+i4?nxIfCB3&rIas z55zU7rD-1@2t(5dAGmOm|6#7OiV~4UH)+g}Asg3lx4HdE8S*PotBW)inYpU1M_uyv zWJT_{)472JUb2bi52sP)u$nu|YET3McBy?D(JAsQKW2Xmql zKRi3d^}~GxGHp@YtlcPzZ0C<$DlatV7mAs0epzF}E=W+#Cdb9rZu=X$g}8>IsX(Q_ zS2NbtM^2>Ybh?IN3kK5qvKk&Z+VCNY^ zfc#$vU+ACH0t6(^PXHTOx8Gtx_Bz*&xxC5t{2Z-4?+3Gj?4^-R>JQa;_e6dIIWwRZ z^(E+HU73-5w-ZXZKSL@l$Uskg{ob^0O|#7N`Eb_QnC|lzK`m$6F{&{f8!WD&eMe)) zvqWz8EL@(IEej#dLf3;pZX3neU-5(_<2Mep=Euv96zHvi0&=RrcJC)uc`F3)jWpA` z)5*GE3(wsI**6VmOfkqhJJ%+SOuC9O*^yzaS+hF))m3-|2oVH9g)eM)n|JA!YFLlq zV>LOY!q(%$y{wZ)-EO)u7sFz^&8=?$liE|f;Vz@TIPH_q9H-&>VyMlLaR7tSqB(1LHU0=g1B|-bzh7d)}zRDB8m4op&>Jn0V;3cd$KY%%N zJh(u}aUm}N<K#JaF`!+AZ5a8MNnu1UkUIX}-yPl{j%M&E`F@WitaKLBfO|?Bm&(AM zJ^U#i4U!))C>gNw^yK}Hpsm;2UZh}=R5PyF8@h#X7?&XB-tr*O9dyh=!&jOv^y7N` zPjg0MHeyWZi~7gBpC{CLWBiqEAt*~i<6R)af7sKJHBcu5PrR8LqJj+6E>u8D|N=*TtxZH;?PPC=x{F8*Oqjqwt#Z zKHz8CcuKHwjRwmi?I8D}(Z=0xEHS>Al_VozQ)b8qFWjbo-BEiQg>v&Yqq9#3n>XNv zM+=*Oal8jETW$1BFGltu#ix_O>H`EsL=*wps?jIFufTjfXl*v&n0WIs8mYKGh-KF7 z%M)<#!+i~xBZEFL2^A^3o6v0s7HR$7$=;4p6PX!atjRyUnXe+wHMaZJ4Bib?@Rlg} zja2$8LbZuhW8*4@NDBcgZtN!!nMFLYW4P?#>N|IK@zKvZB_R-#0OTsZn(T&C}+UDB4LIY$2Rs+3!F-ugHZqhWw)Gw_CvSS%vBZn%-Ym&G4tbuCZ{}0 z60EuOOfrfQk%9Ppf}(i6ttl2#=NC#BP0rcJ?`@v*f(*YiKfR2yUa?+3H7g*ZjiFu& zxg?HD!14EF`7WKQ%yA3IRL{4E$ugzOpX*39nOdrnIy5@lyv2N0JJ159)2krQeLn=W z2qazpJn+D6$s!mL9Pc?Lc2Eq}AyvXHS0BPbuf@BK2N63fx?G%OR2-TunK%cJ!-pR3 zSfj0=+&2Xa^|Acvaz3LdL@_j|cjPo3dd*LrPK zrzi*wvQ>>}hx`lJ-9b_G!+!>}2nXJ`6XV2SJv6^vY{fKS50PZf#)xC1Ph*7e{z77P zJn^RRDY%Xs^M1pA@g^8hJe1|GyOvpdZv35o_b9(j_fKc4)k<=J4+#SDlkES~nZ}KZ z5&^NK!ii)L+}gURT;F3)q?qP4C4cmZz(6JC8L;7C5E>ZHi^GChokf!1Ux029qP&E- zroV+Jx}Fsr-MNq@)hH0-bKBL;j#Y4M%tx)>n5wSUO+p z^=G*F zqc_vJpT!6-3cF#mzx`8}1f;_o4SunA1x|Ng*D5>WA0FDpOl$}`ys5f(U|L@6kh4GM ze`gI2yScgAI&?hEefN6YD+ZEQ?v=2RShVpGg{D=lYhy=lyyz`fU~McVfr8TrsE8IY zEp=RWv=jI`)OoD99bL{88oY!2A26L2!N0@LjP85fpnGb<#H}gj=M$3n9Csi)alAzz zm_@kX3b1}SsjV|;X6w5Zt~-Qvd>w`~x867bt5EHRahS!hm9$D)abxnr*DN@VhNXYJ ziI#wJDQ3NC@XT=MtFH^(jx&0Y3pD)kjbKN7Ip#s6>)-GR#sI7%t6?G%%D|zeTZQKR>AyNU|`;XlML)&OV z{k9I*OEbm;gxxTZMtxhz@PYAbG&dm$eyQl5s#y#Eoykck8;T8pMo9^Xh+@+^=e9M+D{V{kJ*X@QD{!cuwr%@0Em~~gb zk~I@G*^u|#d-}|`tbpUG?pLK#E{8eO3;Qbu7gj*QaTa!-OL*3!=OROm`V`p`P`BVB zEcXVfaF08FRXV%+_F&5iwBX&8_{Y_cT*X2m%rg5JJ7loY< zTO+()b0TB*cia9fV$aTb9c0zynZ2i!9N&HfNG!A_+UIh59IP4vT6G@Br+E$^_xtEO zFzuI#v|rl2ME-}8(~y}T>!2NE9w3(FDI-vwFe;!AZG#Wajk?T)KJ15KR!}|J?-9qK zoGkL#v_pG5SKBWaM82;*V-@@npNgm9&imuKi1ZZ-y4P%M;MB%Dc%+Q|(#HGXvMSkM zXASScWva0pd0~K_@kpLP=fRCT#Q3tbH5*uC>wfKZB%xtB!jwj!gZhoFQK%+CSd5Rd z-ax4NSbzqR_~_7ut#NFqiH9(fV!ic2+B5K-P_0@`S=33qYY6S2=Vc=Yb12?$lw^Oz zX-Y|WHX`MHf1-M~TCU!<^|!?><&A-tiK+8!+-S_LfgxaRg-Ekp?pDq9ZIPS^K52fR zWcdNdAPvJ%4O8nrUuIo9s7CbMh+YgCuH7$Zhg~slUztWb%++i>U6r|iRI=lITL#1w(RN6P>Q5m;W5sFK*M~#hcYe z^Xof(<|r_6t!-q6$oG#{m zOFM?b*?s(JX-P^^vckhICuXq-J-!cC9BnoBNfwB;vI<{)09b|z+|A0^jN|@Pm+nuA zk0%HvIJELjf!#>g{m#o8f0ZXb#23hbNViqjy|Sdd(!Ygm1Kl{3M0Vq*=-GVO=II^f z;r5mh`h@A`9zxX1q(FA52BN*6jkYz>w#F!Xm_5C`^#r;iBD|}1CKErt`JH{8do))? zxe2f#8LYoqyWF=aV|I>Mqkp-d=&qAcgh@K4nMJ$A*9zF{=7J^2`%Vx_Fmj;H{o+1# zPkO(t;}ir4>%RzGZfGMv7TZ9F_6i+VplI{pJX##l~M@$kScDiMF2fsl?T<(9q zznp!E?D9Nkf%${f_NH^^dMREMefB91=8k?Hqrr4&U!0Fkl)vaz#T8LJ-j`_{IXxzZ zT`4Qh1(EcH+EYqHVAk^Vm2{c8>l$a50wyoUL9v~S@X$p);cQ`w*-aF!#D*%v01vRm zS2hxyKC5H3<*ZQ^N6EZ1rJoIC#u65*AGfHGM=~#)sA?ox>WZv!AM|sfo_pN2i*9GY ztL4={hkM@TC8wLY^W1*_@rw)MLpgW&w z^uJx1kkr%#ZnB?vF*Qo{G!VJPi82D5wOU^_4fBRA(%?z5dURHD;iS}^N3kN~Ud=5u z81fOhNJ{LLBiQ9KETvgo1dR{YzuXD2nhznNzMqy-@-C>?=SB)7On-`Y6n{J$EVQOt zS5ci^HJB;i!4qBv+sUL}O5Ach>Zv;$<7sVknZ=KTa{~=qNo4wRZ!lFCPp1zMZ6Q)5 z6Uel?S3gT>wv&Xm#GKn>$D;SF0Pxd7Pb&P$N4?uS+BxcVMrMJWOWb$(nTXw0#shsD zFMLhYEN4> zjr-I)fBwJ-|3I1x>RDV6uu zosjQdOzkr4(_?-_dcv>U`n{Iy-zY8fWbibOjpFBZF<0W3mZXP%w2m&tu}U%V>SybE zh&oXOENbaAq&_0%=X>^u@ot6gnAmFBTpMtu8C}oyUDv8#| zMLmBY`0hZ^=I_7H79EcP2KT0V@j|o2^V(IlzgDEgP~(=Z3LUm!uC;~UMq>K-PD6}4 z0i2NBKXaxgrY*P8R>k=YV_20-LSydLTbaiGn}o}si@%df8=Wm1$4+Gj4PWD=k2>|5 zm^bpoaV!?n6#QP=uhd#@j!0IPYg}g0X^BQ5;X-lVc{bLgd=Y$fveU6`IW3C(o04r%0lkDkS_S`RzI{Qu; z`{3!xA{OY^!Bb6;P$0$bC)c3VJVn?T#xp zqTd&3FrrVp)pvDt%feYT8#STIP)BHZnyk0vkS`uW9p%A(~oLMke`@ zL~*ytvgP%wF|dQYSS0=DGLiBjUAB1$F_tA}&506!45?cDwWxpZ2M=xT3-XU^^^oJw zVbJJGX4(Z9!?$Wx7G@iSo*kZuHC1DsqL#R|R&T^x0O#i1&Kc3MTB+HS=A7R|J;S(* zq30MSh5?u3w_%Qyx}hI9aU3FQf=0t^U-myq#T91gDFZMTUW2>k6Q^ld=ETy8pXu5@ z3s$}O$}Hl%nd?-)h;VAU=?DgKJ#k+YUcWVZZJA)z(;`t+%t{*N5WQpYdtS4Qb)-)m z+r^JHod;6CAc-P{PIc_6lMhNhfRLmHWNowqYR+Bz2w3P%=JdUB4>g+)dq$}`eQ%U1 z^ovX_7A3Z%?pH?i;D`tXemt>5XcK5g+kZsIU{;myPhLra1+N{O$gn#f}2 zg`qHwxxtn?3sVsKN%5hG30Xt1A6RZyzo;Qhz5s*c2^3Vk`Pe}sK_K;4^KB?MrGmFM z@q6Mu$6a3z3-;yvzottQo6x5smMcrDHXFTEZpQXa= z?7ci}@&;R&|5ovalP~E;@V5%90t!9szmi{>B+|kS^pfNButRUqdXPVGGWFD^nDN5f z%fZJj=Otk8dBabeMWo38iuN3#B7yxO2b=vvgJXmmHHO7bUG&@(aei};#VWaQskZM< ztnFkfyFfH_|D*F>%qM2GT#d`x{dQM-AiAp*hLb|b_ohC$`%652$J=7t%@-%}M>o#( zRwJh2Xy0~}j7F>-Kdzs9^TUYxlKC%_zl?^5HM6Wt~UjRGkt zLPAJPxX2SVaR;+MLi*$GfaxU(JMgc)MeM+OX*T0P;*TQ#_@@~8KgDjZJA!rp<8GY3 zRut@ix33nq(4@cvU7)UskL-UM#@GlumGr%j9)9tkcBKb;>BmRtnFHR!bdz7`KmYd# zaP&;TKY#2t@b43@MuFKd4}TBE-`$>du~W0&AsOIlTmgJ|`!(^_iP(E>!ib5R7rH

!1yI#m)-qqqfC zK3$Q(?39cB5&B-z|9IXVzhL#!*WPT~(vP@GJLdf+ z>fpr9#<|vpkC@Qv`ol+RP#B$k^_xCC8|GFUzDLH5;co+tFvYp6NopbWOx$?@ zCBM?cORoI)Oe79m{Mg*<`G0L5J^Vj5!}Tn%*gvIBiF_M@y&2bCb8~6Ee77KMHSL)R zA6(q2iX&Gi0rtVBjloLFQtL+jtMx7BO2?A$d_uZ_qCU$s7(#P3kMXkQMKzZ{3tuc5 zbJ7s`YCVcAvj1MtLos{R0&+bj=lr*&cr}t&n^+~W1HnvfWqHeAHfs|dq^Nj^MXv|2 z+drOw$nAg-<(Lh~TTgVbh&CS|wxsMc6jh46XaOFoA>l3GTFA&MPtWu3AM8JJ6(B$g zvg)7wSo7@M?Xgh2v!z`_+TNTZXl!LWk^Iy)xs^CFcvIeYc;x=P{T!rwRrGeY3;Fi8 z=SzL4^a;a2K1OjM?~y9WA?b>$qNx37yW{s!jpJ+uXHzlva`6*cJ-19)pZQwj)yh@C zb{VkN(3=vGdJKX=BRi(_YF5>2A-LM`k<$v#GJy6w>p3j#Z#PLYkRf@PoCE?anOtC-K35W-?54)}t7F>C^pdkV?4Mo;SeZ zrxlqLLTun7M>W{I(sj#}vL|LKwU-xH4PIQ_la}bk&GYth7WadS%?bObd2w}g-vRVI z$>>9C({b1DI6CMRLYD;OH8^fc^sP+IeObvX<8(xKLJd)E>JGbk+Zi z!;a|AufMAc6pAT&`E%1>o+r9rl8Dk~D!UplC1M}og%kdXnr>amO4Ue+vsz8C-+alG z5Yl2RVo-{{&}-Dzg5-%b#=&zeBLrHJ&$d>GvbbraM*CXX;u1E*wn}PLe<&8hV_-d~ z(b*n&o?CgWtAA2ZoYl%KD}1=pkep@OKmMj|A7b6SL+DL>+W&j)`{{k)AXo%rRK}Mc zUORc)p6Z4fgq~f<>+Cb4{f=iT_xs09TAW*>VcT}doQ4_$Os&xFPRwKMqN|UDaoK%N z;WM4_s`Bpv`xiF1);g)kxh(oK7P&eZ9}CM>=tfC9v60b{3#>Gr>3Y37{*09ttM=sWs#`z>|+!9p&Sw$-h?$u8|eha#5%!E z6itq;)WsWTc`VK4a|%AOG30m-?;cne(}!!yuIuaGga-e8<51izEY1dz0df~^Hcq1N zm}nREP+C`PL_a+02jIvaR+J+R(x86l&CBog$V){V3L90V1MtLYN^}4tum6ykJbE_G z8LLPH*$Tx91?wy&D5oIG7-VLaHR`o)K2-3k5sh{7=SPcMK--Q~&oFJ~g)>RiMf(cQ zfjBDPdd;ZL@16|YWjU3H=n}K&_UbjRL7N;ffMz~@56Y~3ZSRdarQc6U>~hi8YtNtM z?!lm6OWN~432YZMpe}*x)79i7Tf~q5M*n;3OcSOkex~iWNb0*BWn3lGCM%Z1Z|a7L z9=+qHc@6^H?&;I_I`|UQ8h+`E>0f6 z9Gq(wJmVs5tLInF8k?Ebt`xL%o})l)lj(fE5_sPJuJ_f+`{~s-_n$5s4CU&7o}hlh<+XEM(6PCC(VfO3)u-_#Fpj1Z~9eEbHo+OxR~iw?@m4K+4HG+20xnk; z%z!;Enp6C7;cUk~pN+tooY*m+?UX%!!gY(;I3q+P3sc5eYJB!E?C+%T0}z34Q_|$+ zruTG}wNJqvUcp0$WHbJW-dFprX8mOow6xO!FE}798(>W)dtKcAqI#Bh?b@{2_c6P5 z=EWo!hkKrt)uLt@$nZ`Xeab zv`NPaN*I(G5|pPgxC}FX0z}3vS8ktfja1&_G{6V&Yzs5%WLkA(Haytj%?R$TR)d`2 zbdk2O^+v!%oD*JqJy#Z5F*YAZJPkU!q=lcORsMdxmG;}&ZqQ#LWzg~22pr|X0wa(h z-~LD@7uAheba>cD&ifdL)Q01hMLt*xG^|v`)wn!fTtQ}CW4VZK0zxDc%s^92m$nu9 zOU35=luM+^Amsb_?`+EX8qtWy2 zsGT*&flX8oVPw4_a9`Z&gJu1&u6mJG;5V7|X7@2|cthW~!SH-ka*Kr07xdH8@12^L z9SieI?VFKmE6%m}`>?i^K;;{^VyiWamx)ap?b;rba?dDh>Q;m)iMbz#f3GtUYBnO9J&-s%0Z+Cd`V38DmVHb-tUgr< z{#krFxm~Z@uV%v=TaRl6Hy-S+TFfiPTB>aO9nn{3u^Spd>Lo%TEX z{mI66IJ$9g-W#~0+VIANDcfCiQ05OevS1EsPHB&FkM}z5>P5Vym^cS>D9W0LJ;D&l zJ3eFsUj{f~1}q?g-S0cW=-GeUcYYNxMqB_$`Go9pP#IAfGm#jNlm-?$L%vD&`VSN5 z8tm|&#vmogB*@%K$j;8>mgE{zIiq?$~|8nX2KN5@+ALLrH72;xoDktF_8Mw zwSz>iK&J4jCo?;jW2XG3;~mEJ;HN$R=m(QM=qUam@$j*3qOsk)>c3!1qc^)lD4vFY8>H<}vnFntjG{4n1?A)ONxh)#qbK=-AfJOJd545}__0RW6Lm)*F(dI`H z`DWRmMQ@xMtVriiZ`?k-z1c!U$H^Fe2ViQQhD*U))>H~slzlpk4#s_;#-mZd_OqT8 z#mO7NY^3_T43X4y>6^&}$p5tl7UNo+>+Cf>tR{v(Mw@$8NX%`$S2~LAP5KlF ztS)C|z}nAQ)S3;ilgjIz@{s4L{?+S-IXPVTe$WK-<#6>`yS%-4PKgfowowJ9^wz)o zsgIr29w7{ZE=;M&LW|frhK8Ejx{i&%d6Exg-2`TQj=bLYA@IYV%cBt~W4-D^u;56- z3TadK59WT_sEDcwP2Fw$2H0T5Zqim)P->v*4@5}^<&Z{tGP7kbI?s1grRhh8iAGF; z?f-(@noKnDu%7yYdmi6UMuGcZbjVT60^9C!F4-u z%h(^%_a9-cJ>RPy*797JsM}ZhBwD@bFxq5+_EtUO{1d5}=1SN&FOI%5Fme=9Dj9{^E466eW8F+)7Vb7)&r~|#qz>f&I)lyV zl9DZ(e2jRx{b?oQgFDXt zWB)-BITn%Ycl%t+%xkJFV2WbcFdLc6x1d-Xa0mxx_}8vH#BPL+Mu!7{Mqf`xKjy}C z^;2Tm2J2Tm0~*Xe1lCet=&$$>H~!fQGZl^NUtQbM<&z=h$8uI)nC3B3*AR)SAme;Y z7=WHya>}cD!B?@Yi%=tbGt<-5kQ^_=bMlr-b44}5M8Ty)O{{w-6k{8E?~uZWg9^-O zp2qW4@~(nSotx)*=eQlHBi4g$a@J|M&tLijkS~HvG1OnERPdM26CPRSXnmL3lGjs1 z-Pjv}pJw92`Oe!>pWi-~Hg9tZKSc#Tc>vdXoNi2Kn#S%YeZ}mf#ZZoyK(fbc+52!A z%HA?jdrhJsQf|GXx{_)8(01Tnxi-TBi|JXTUZZ)5&n2VUdEV#HK=lc{qgG zMvW*6$8lgRe&0;Ia`<=@O6n*<3nb8&ecjN~#!gO|d=$5nBjA;N#m`DfRmeVY>8Z4> z7TOOxQG&@OU6m!qotxobQS(U}-!b?aye5i-L|a2#> z*B8u&7^y}T$tkUgkg+oBZbK*Eu&Zh4d1S#>pRalvy81T{ln?5RIAlHLlzNEkkQ=mJ zjBBl(^DFwkv+L*y>D`uD_P%q~dg|`}^=L)+FO?38I1aeUMU~s%N^%dPqbQJvfj%sXD{TF-T}MIv!s%Wv6jc< zBnt5FZ_}?A$DoC7e#e4zgG6PdEo!+=eNKyz@ZPZM<>Y>}0-swhznH8Hptkhxb}+XS zdXc!eyx1^!z94&W;Jiz9c~l1vl8}czOkmoQ7FyS6)rYF!jN3C48HTL6-B<_hcD$m$ z*m03Jkor)6I4SZ5N{iREt3ZdWj4aqhM(}m?!8dU>C z{eO93?T>Zc?Hklb9fCsC{0BuMn1)EG%&8{uX?xS=g`rgfS_kH@j*kd5Hu$YVuHHST z@3}YHd}<$-rhhIO+l+^f)gWO~nW2oK{wtqt81Cn=npLxujQC&fdNS%AXWA5Ney!ZY zxpyd)3HzU2n0~qHLDc<+U?E1MIN$MU&r4>re~o_)@Xv#P&6@l#c4bpKP+e#Pn7^bU z4eqlzOW2VDG)Md6)@XK}P)1NcWI~M0S-(}`7?sTNYuDtcU_n10m#k}bJf-aU{^xyV zB9EOd8vkl>8r5olIvv%y;c!KOHiG%9joX|Z$!8Qp%~$nbz+&w3uP~4WI;6paTWkNy z|Df=$9B13F_rqz3iYwS*qTvv~5c}}d*JHkhw@j#|eNWz?52pOrJRQCU+_9_kuauH!`mY?L|Fe;Ii~Xi~dB06iP>b_6fSIu7C@ z|D_&NI~$<%y!@=ahCG1!FC}jatf!OX%U3Ru5F=NVe$>B?zMkRg17}#-75bz z?+9VR9d-9D1>TLnTlKF+I(%cYFqZ;V4OxM56zucDuNMxh8|iw z&N|{#tqN#1&0m&-`V)hCkXc@?_m&qGace!`kR8y*FdwABjd$78UmbW}+(6Fh8?|n3 zUM6v&*D7P3yHX|uHBPQ(tbD5^ds(5%~QZ7ld7G7J7)rP<>zPd zg(fXcTE>9IFx4mh3G^g6TM$g}t=8#qE?cq%&94_F`q#r(GNm|3{^;U6fC@{uT%{yL znW{N&*w;Bu3X>SD+OYT`7`0u@cqEb$B6U=n`dLX1uO5AKbM8 zJT$kzbwUm%OQpu;ExZQZJHIuY+{zy99h#%s7{mJ+L79s5H^?INJURRTG24H-{Qx%~ z?HcR1`L&%VQg6Nsv#Ace5uFJhsaUdx($3Q-T?Umi_i0r|m`mPkdu0&x`3nFaJAFGX zjhE+AE*#kMx!FLLpKJ&*|hU9e(&^)kwSI(2O*P-Rd3JJ=OH7`9a(Cs z9I}+|AGH0|-r-f8{1&zFZI+=h;6y@f-9jJw(fWmEVeuju3@t{`^B2B`kLu@MLT2b} zzv?jJNO!kf8eIHwVLnDk@`^e+EHGdMFX;emu7Yh8msEt7L{QX={K+%oY!Ym4<8AA> z^Xq?b1PQxg(5r8n<vYgP%rAtQm{#N&Jr z8|^kPvLB$Vf88&FJL`|C@A{~J2RS?aZcT);bLx10?)7Cg?Q{BH9*~=iNSkR)v1z=G zk#>kq^DzD$PiL^eWyrq;UV4dN>M!nX1_uK^{+^jp;%iBYcY^n=fY0rP>L_$!Q>hV3 z)md!nID`@w26HwT5qT|>`icQpZKx`!Dwz-}EPmau&sw7^^UYZ6 z$0)p{kJ2!>Fu3E;xW@(J4!bxn#4vO6^zbkAhQK2_`twL?6{Ah&>=i{qe!C#6wUs% zrWK&v@vQq|>a&9PGxd?6W9{YUdY!gh%af?RS#CDiQ`Y67PYgayKu-lPrgL~Pf@G7r zV~8_@cslW>q}L%k~W_7I`ruR4i5`3$D#8loXlLWG;=YE9)mvIl@lQ5bikS ziSb|_c=!Xa5BCglBdn87yY5V|WvJBDp$Di9wNjAW#flj=mN#laT`)2}7(ojZtlAn9 zqY5C?CM+V#^TIE9s`8OO+QvgL`m;P>+Y+N{Z_`Ls_KlvN=)9n&t6&35XWsUrM<@1U zlzgmB+wp!w-$kR%9sUtkN3fvZV%V-WDLlx6^#d@@WF43kjpzShZxt_}<=l}1*eRfk zH^ta8vJH{U2ikFdlQcG6?gGx;i2?_yU7p>Psdg93(6DCXY$Xz5CLuobZkpuK3S;~+ zCF7oxrdtS)B9|+V&l(~*bpBO(3j{E3g_=`@C$c-SoP&muo=H&uU4r|&KaQw#~aYwwEDoH*~GR7g1J~tZ1eukzLXhoP0D8o0;>wcq?U3X8Qr>oUdrZ*u`WKiWkB?(w|B*IC{%PEzS9)6(^A?T_ZE8=Uv_G zG9+_|-AMjAip^Ydd2lq43T^u*`Tg;-#C%DA_7*r>3$Z;JqX*A&dJDe&%$)o#>UNk!C^J= zk%MMGPyduY2CXfWzCU=Nr`m98zi;Vz(<*A^lX$y0nE>j5C)#G8-{^LEbt(NZTh+RHxxzp|8k*{q`mS1d@3s;SDY7V?PPF;uqq-ijp&b~HLItzj#_Ya_byV@lVrYL64N@QLtA7m@G;5bY z=Qv=Ly{+2*?)pg*6Q^W|Y->jIzJ4@p$i|8Guj1{oWah$%_!r15K;V$?h z^_LgKitd3DN## zO~BkgNKU|ewX4J4SMS9|I7+@v?8Er4E2m%7D!8O)CQ?%s=U&k5?-0QF`FMZc*Z&M) zv$)8U=CThyqqT>*X!*Y*L!b8cwXKQ6fi}Ue2-8;Fj$th3)5O=(y0hF8R?`Y+v2KHskzt)BJQ>K4D`v!+Emx zZI&1))GX2X(EoM8Tc1OKC^Y-Ib$aY~R^z;8idOd5#72`Q!x;+MX1)#XIoaLr1C=v!E8dM&k}DcVpD$rLw2=?uFz^)3$ub z_H55^Pteup1#CC9bg`#pLtRZfd?E3}UGRcag_oDQkV?k-FKY8?HXgEq(`I7A?EIQq z9PKEI9c_|IvJYIal^<83Fk?bR0!tZA1A;B&kyk)GeM(U@KEFo5RsyrfKa6r>M;+Je# zTF1-eV_af>n3KIw5=N#rK=kc1GG0hTUJ=Zkoh@Z`Pe+A9`|E$T^%hWZEYYGigS)%C zTL|tFT!On>aQ6U%yK91b2=4Cg!QC~uyS+)?IrpCTuYbM;R99E;>fTk<4aARO-J7LZRCI=EGqGqV%NS|6W87^2EVF_G%(?yCBG+ z6Jhpf4RK3~OVM;S#>z2BL&CXdS_Kv={Wh5JSA2(F6_aeZZp;>hF>e-Aiy> z<>&jNAE^ubaRTehPO>o^%lFNqQB3RvckFKYGtIQPoyAP-{b7sYGf^^Q6|~hLV<15n zmb`KLAlZ05V2i0x3x{5xLY_Hug;b$!*WqfFv#X=QJx$wF-8^#cc7h|)<72m_p((^!!ds2S58Np$o6l0gagca*AeX`^rXAfo+-!{!>y*Sas%&senX zn6=rkdrDz=#(2MQ%CvUG#~3FuBE7rY?IfKD6%N!N{`AlRxt00^cbHBmnadaS{!X|@ zZGr2L8R-4dbin#!xdJ5faUAiZCLJ0@8qH_YunHVY=Cz=M8Tp5O7mVf)Ym}Yna$Kdh zdVk)kKMTD_gTmR74F^$TT*1l>$-@#xn!_@BAp7^@53FGM^J&%Y9?jivl`@FK%KgO~ z9Cp>^i0s#SU3a=oeKi$$se*cjk1N*8)z$`}3rL$JN-GD#8=iJm6J?$UQ=iz4^HSWE z^jYh2!lo88c&M)QoR|QTwe9g0okuP8kjVSem~a4tiw0>O(~yYSPP^SirrGCT;kkL<#u$vSO_K}v5+7cgQ1uFN4m1A zMnmS)Xv%}LsdUK9teIT29Dz;Dt-k6uzA|O{5Lu&|x-%2Yh9hnfS-#3;*(N7Y+Dwv6 zWhCz~=jCNUlAxT0{5RraDTKt+=HtS(jFN_AHibtnxmm{6lfbMx^yEr{k@?WSVlcy&5{qd5qC{feSGrtdH-&fAunOFnxmCC62 zb7I?4WdeWL|A@kw1X_d*+|$bET~=5SHpy$| zIsVJ{Pzkz!P9a-n;QgDOcL{#!pupZCX^9@~F115X{a;I03dqux1hRC6vg=1_qDEu< zgQPmzpWFH&x8n)gxqmT0@>e;RUjS+;@gIV=pb{2?7%42ow@uK+_B0dTkz8VRA&o|61i#Lgk3qTw@ky zdCI(!Xn4C0IN_%o+rxo+iU8ocLc-yVV}eWdTi)2Y8RO$rc_*z0rIQZ&sp~7OUn+y8{C36^8Z*9$E?D6{`UkK3I*rCtuo)F z{I9w4(+G+ZhR}@mL+8O7vYy5@yGj`fJI9|= zy!o`?EywY-dMsPDQf{5AZQ2R7+DTK(&dO+Oy~xCgr1(>FnaP`^wbOIDI~B=zU7lVmlfxrpeEWdSY2AW-+sX;2ij29!y-# z3M~KlgL>TLb_{1`?Oz*gc>gHea^H3)e4?he{nFXLis{?ld$BRTR0x)4>}WJ~H+KAo zOD8YRYL6d1l-L^@6@2mB|KP#HmoC+&VwA-e&b1~_R)KS}?;<8>(CgFcBDTc2cJb%? znR~ntX49CIJ~46Tm5LmEQo(E7o!ByprBK0Z($ylRrH~%{by9tHzR9JxGHEsUKX~xO zMr(p3qeq@IC6Ef#skYE!>@cOojTgNZP+bze+vXB?OG$~sdKb?HF(Y|H~dbCpk!eo+Fg& ztTdC=UfrA{aO{p+H3UxJt}7W`jrJ^j!*IGezppNSyJQYv;Dku*>w{dlPS z?MK-QAyMRk^M7gsj{Z+|_B{UoX)boof14{q_&?2sPw;PZDUklBxuT>0X|98%HX-8Z z%|_6qG5o;_JQou|lp*_DmiNoPI4!xkusR^NPIC|9%3goS!u16&<`I}MDg?*f$`1vz z+B2@nT4`+0Cc6QTao04Oyn#%DM&lQoo14MB0YM3^!!(rgFA`V@oTXhA} z5Bs<=bP<_aqN=aJ1cDGPIAEQM+2^9|esEJinC9 zWvs6ln*-^gS1kv!%Hm!o!2FZb$7nISET_mj*PYAZEX)MN*D|{$bA3f}1TZR=l8;?J(0uOm!Gd z2i%i^sk{366GGReW|Fj{yQgdjjS}XQk-k!1wS&^=x_5r%(D^G?mzlx)CEvok!4q_Q zaUNvT7%VFanzJhQOXQiz#X`0AX0Q7_=m;xzxSM6PHMdR;&=3rO5e!JdOhsU*Sf-RH%#ybO0viq z93;=daP4(VVJ7ApGjm6BF~$r@-F@$2^6hm;pj}5IOe7^xkEAJ1OEOsBCA-H+zycmS z+53{;m>waGI>I>z4vOuE{MkOY@AVVggC(T4=Y<>F8O@C4g_k$*og4qg=Qxq%`{o}W zAvc#wTrb)H{tOgfogo!STFe|ii=%Jxc2QXsSd(miy+3Q+WFl3*a9bpZPJjb^{L3{R z81Q;IDKdGz3|p_=K{Ix_ke$KLH2I!=C=a0lq405G>K1I>Qxu;K@BzeSJlJJJBAp9? zq`kD>M2iR21p7F%Tmc0$pYvswEOyWzO95C|ydec@8#)N$4{dfqGn~#d9+ROE-}#Wg(=4wgcRS~vZ$KM;n7PXUt#0mXuK>X9xPcs4>edi8r)p z5XwhKimq$!thu2(*kT9l#m`}7^{nnvV8Aguouj!oKFxfm6%T+@jKEY;7#vs@-yp9K znziOhTZ=TTMwljdenDK6<4-4qj6O86Q%+$pb+*3VUc zFKDBgZV+0w_2?R`A*_z2Tsu7l_C>*^-nCIsK62;%Q$2Ac!#&n;s}9fxqnh>m zzq%19+5? zi3tY8HlL+TC5Jt;^}^2&1PN0MHc`rW4Q&S6 z6|An0Y^m;geCtE5dL-7$kD7@<-VsdxM z`Sen6@-1rJqY7NE=k0OI2(I|=pUkHZze$JQ3u0}{`;{1qqn0FGhyQU;{G@cto%wlu zTOPLSrLXNz;=%mvWm{fsoW}5bs6+%2 z0+VG`hI9))Gp^S)kIeALQ##x1(09l(ChyE&J&wqsI8XMAwbVN=YBV{>;=s`ZvgZFmIh8g+q-+zaykF zQNL@U{9DT-9q)~{a%-rc?*+*_u>A=QC%CayQ@WLWx^Y#|+|wf}UeM7vUa%rY&_+E% zgb4I-%Gf_}4p%hyh$wt+2_7#6f}bFRpJcG(sSs{m7-yraS|o;-X0$^R2!Xo~ zf5#IeO#y{LgWh_?y+hA8{0cMLUu^R*m2;M5_R;c9mHmj}*MFe+^9!Z++ZLR%Z*4!~ z9uM^MX88ogB8>veA`LIfT|a>}eBwYHDKJu4oUy5MgX8WV5sA#yXgdA4fJ~L|Q19P9v3P>Xl42XI1#m010wR{iRM{PCapsIaUszhXj_Lu( zkj{eh%x?HitR@%N+$jMcje<=0-FUK+l%iVv!FuV;p*21tv)h@FnlW;GnKuT8U6taP znV_Gc146T9e)%hS9!73zJQ)!>5IA8-VZq~B5V!ad7?6tPf{*V*amJBj@+vYz2Exl` zbkYoJ?ywFW@#DbO%g;V&W9zaaHXv{#0X>qvjZ184dri?;6zR%A z#akd(42i}5)qSs%z+3vI0%t?Kcx$gZgw*kY>^PEL##0~tIiX2)I~@fmlfh!B33d)K z-i-$@exb;R8M^78ite84Z)`yYg@h3bZO{H$-=rxK$)ZUmN)KhEW_m zq1{6s@;m(}x;7GgNTX3%2_ye&sCgXRGDyq^{EY-ygDvLI z8;&8}JZ~s(0PtGF0T|o~EBlfgB(bxoc3~X8d6aGSsxp4%;?P}_1?f;Hj2O*LlIuKgkUoWus|c97%wEfD7@9B*T+SNPVGNO=1bJ_4ir3u ziUqIs2>FF#vVoA>eSvt_^q6ey8Vm=8X@kP;goJ74Yt0}yanQGsI7@Gm?f@65uV>0uBLYZ9?i#P( z{54UNiS9&-v!kkSH4O>*v7n1w6rB;m1~SAFUh-{laz(2@uYr$ctg#~y1yYliMbvYO zj|VX13j=lhV%v;!$)PH(l9RQijG8qVqfo~NCh)6HqkpP11{t^IXcma87dQ}o-K>QT z67?Jlx&zc<5woD(V{La_d0YE!^3y_hAfesMZ-&kg-l-%t7K7my7`OFI)Q8+)?<35% zk($BpAnePbJ?~)QXadLDQveQuhW6lHR1YPW48RU(a!)TCfeVV_UOIty=rn1e$VckH z$FT(uU)mY55HLR+iW9NV64RnCsoYWf8<@V=i2A!h)C!|gs0EWH=zfc)z?u* z@i^Q%XyPZ;Bx<;(kf%mw$lKJxw;oW(#2<6Ip1{u#PT&uMVSN3NyLx#h28j>ki~7d> z4S-(1FC1W@s$qOj5kA2m!ajH*KU`4Vp;`+Zm3oStAW+h04S@Z$B+P;hB;=5Tsq90F zQbm3ucJWYD+T@drMsLRU+OiXCGy^9?tcS<>o^@?>RV7p2bOg~FIh9DHhc)Sdby=vEjT`Q0u>fR;N*C!Vfjy=N2P$4hY9m06 z96C&?n&X2Try?N8KVVU&Fbbog=-poeFixsYqzL~j%rGOrC&MZZUJHL2SsvlHDmHJH z2nPz5eI*H`MTkkMDA{6dfJsuS2%ZZ?_&q_DMEn&1OEEaq7sF(*>kI*DL=j6e7(5y> zTo3XqTtcR3$YA1_Sdt-h)u0HFWCvC*<{X;oboukAHmgpuLazGkIf;Re+cFL)bU$-8 zFn&U5(t$QqtbQABT8ig!lHR>AOj*b|~8Fjl?3f@S&HC4fn?AK6H)D57u!1CVfecBek#*X-YNJ114bCQs@-25O^1^= zrdr!Ev!9WsD!-)^bO@iLV6Eqg1oAPVt!Jl@7u~+~)b%~0w_lLToQupxw({QqNM)kw zTZEWE5j1Z*gm;L1thR_u*8_*zVB*a({KvkcF1@7962MZCPc*}{4?25dJi)dre8I(c zUz|n8H20yPqB$d}Q(j$g;ml9d6 z1d|~?Kvg0|(J^Iz{ap?wZDB&V$arY(gT*+~_MHA5F`BJ`u*c~`kkXGph0sv+!^v#3 zYoqc27sQ9H0EkbFp`7@iat=1}P9@vjXP-=J@rw_CYeqW?0mEjDj$jbzfeyA$dRxtN z3%w7g;&wR3zY3j=kYs!MR(A2$cQZzANvHFT$_P{{YGj}Ht(`v+4*Xz zdQetf!W;<$fFq?{s|-`z4pi z4Evw~LLAc#+zV-5grhV?S7_Wje4xdLN0x9h$@*MqO{TG)hyuC$ zkY|#UfBqh^$;=0|ve6F4NK*tm77dANe<(#A%V)FHe5t<1a_zo*p2a2xfx$AmrSEhzGBrVi@ zDe$ql4idC45QdQ-mJ#)UgtNuRF;`7iO78=a9kRR0!`BYWsKLe}u?_M76|V`hN7^XL zcSpE}b`_t)Q_0Adk?9j*Su{@wE6C&qe2Dxv@iwPnCgt^t5osS{jl?Kto%K(t;u5yM zaTp~~Yw_{%v@5>}8FtlyIpZTj3k~Ct#sDqD=|C~xL92+5e2t8D@skg6@He}_SVSHD zkb>x-C;8D~YQ-ywKdbrN%u%r9uE5Fr`Z3Moe((sEVY`6{_9_s4uXlf&D3|^Xk1tt` zQt1gq%nNpB^1>mrYDH%4CPU959`8=5^EcuG|qG^uEz?#{fC^+eSK&-t2+o9`0gbtl8 z_Q{nSRxM)6Le=O#)LeWaBRC#b06qz}@@|o5P!o~acohD;E|rW|1Cuj97N)??5H}DO zjNioq_UT(uKksQVcb#qNqS_uoWHhkiBd%u7904vZRH4Bq$1+2?57Z_V!BVg1+JQtU z^0*&8*0kUGvjcx^eDKPO!B>1I6gvSIgMk0= z2GNAlwVzKzjx_PKi5(;TVF7tEM3{iowS??|oF)(n-S7cJf=(4xMd;h20Q54jp;|5& zfmq^c!cL)T;2X4!q|d}-l17vZJ_M4r@elSL*PW4^Us`6f{4S1K=amXOKC3HO#f#j( zG}q}Nh*l`_^{#NDM9;fIlncb<#*{y3Le7WEx0Q4{aD5f{^>y4|IXZLz7bZnGbJP%a z>c>>oWWNP}&d9#rms{I&{&|Rb3}U zYDb7=l4AEr$4@H77Vuc|xg!|@DRim?TXnv8%hcBb+clYS401+lbeVx_{t@uqR?zB9 z5c0^3UyL?;VI@H`dZ0PIo~jbvp$m-piG=Js_>pp}5ho$_(DgT}Q z>!}*iYHy*(-z=%n-r`c>9t15gb(}NDQKtJ$1Sz$_T7S_$T3UK;O`VP|#lS>iSLmY4 zv|vdc_6I@cu!_q~J8)Z_N(8Z3JthKX9HJ73ChBmj-lv|7)D-L3OzY4+@G9?_76d-Q zyWEA(0i|0xK!fXxweQw+9Jtube+VC$0q66P8IS+58!)b#4AcPjQaQiX&8ut7O_I#P5 zO@Jq;^F$;aS)%bE>O5jaYOuOWrhmFG===|oQRh2TuoBlE%1~z+r%Bci;XC_{j?fuA z^Nt+=Yp-zHU`6q}G*7cXp7lDv@aQ#s`Ml+(=Z>}psK#`^R4ja-KACd@a;(tByzXi} z%<<*om1+k|?DWKf{ErwR8^4G+B_}G^Q{JW|%<9NY+oVuxIMK=I#^zYn;PF+_?m*|c z=tNB0(8;U_CtXfO))?w*L@0+biQ_FqDBDqujFtF{R(!&1NZzCVBIyHeq=2Q4u!N1w zhp3VFn{P6BgUTnTDqZ9qxGEMI@ts>)!3oI#Q(l0wJh^V3Mai1!0kgz%+(z2kPF6dT zU^4AAZvL*&wmFY+>RM;AVLOjeV?lLTx%$-f=YzSHlP<3yDww?_qwJb+ij}O^6_Xhq z{*W?BCF4v@H>~2x{q7DIizs&ZTt|cch<9z$wHp)s_i?V066~5qpz=_Sf1IcKkYT-G zupdmB`sR8-FvEn@zEg*{1Q(kI->`RLAHi@dL!z;NWN!hUrkzl#*DP+4p3AMjsUBa%!YR^L?*+!)r8;x;B$ zXg&Tq3U0Cf4F3T{=fwDj1>@QY!`c(Dbe2JjtylIms>JkltzWQIy3`;9iF7O0%(Fh~ zMGjd}kn41fNA&+5w>lO6M2lgz(B_2ZY_;%uFbA^5KD|OF=?h;RZyqhU3qf*8+uw0Z z7Xb%c!{47^Uf}dQUpaoq(s01GdGbAQY_pXE`wC0v&sf44CiueqdiSlD�>zHuFsh zSAXg800;I$+A}mSC}5B1T;&8~plc5+z*o|N`bpE74?Rq6H@_nD2ogtKD|R@fd9^;j zj6{E5Do;VkUg+Dp{G2y{6f4Wf<)Bn?Xma~3W^-l&-=fm54-6IzX_dN=BZ0tUTQXM# zcJxnRF?O+nNu?us^>&k3qZ8ualprk^Sl3ptyDM25xlcmWmdc75>@?@X63MzWw%wo| zI`=%Bk|zII?JWB}fmiZq)w0Cd5#cP10#CzMPI>xs(ztV|WnuR33#I^_3o8%4EJSnn zdqCJDGSltbxxT@a-l40We5vYVu+l7$7+XT?i1kz#o%f<@Qoe7;O z3z^9c`utBZP^m-M_4~TAt^1j!&9_zO8Mw7Se1=`iwkOYtfLvimbMPE*<9t*dW}*Qv z*~b82y_{4}=$C$j)xvg1Kh$GK~S)t}=CtFD>ioEl%iCw+LOFML2&CJ!XZME#3}L6o|8 z#Jy~^M05~mE%?eyz*TlMLl2EevY#zldu2`^C^NQb{bbKXnQjti)z`aI;o0VTASL=> z*!N{?^u~wumaiBS+z_^yzic&NRnWp}uof?6f}sfoEAxbGbEsiReXj=SPWp#jy$nk_ z7VuD!prvAf$QDcD=yWQ#>U6SQ+QE}73T(cutoVvj+dTs$1}{H9ryjns<8Kpm9;6A@ zhTDeQ?&JFX;SB$oV6d`>pxl;9Ypa z+UE1G$p~%|rsgx^4TH}EGnNlt{NE{i!xXdr@`kJU_J~fcSHO9eQPpOypYCm%u@^LK0?NYF9QnD#hFN^Pz-NmG z-xq~X0)Hc$wH^%LX+5x%S+dS3zSZ;N0@&&brCV;3UZ&UWL1(H~KP##KwKm?ietB?Z z-6lm&0#Hn~vl0)pCIoYMh1Gj0DMWSybX;y-^O5*n%QCi-ldW#ES<5NzbsV$o0YS2% z2Y#De3za0Kc#kg=XD-m@!wtbhwKGA{MdDnd<_cAWH3wNKyELlP^kDhsu=z$Z4vn z5;nIFcwP@Iqf;y4SF=+uYb=tf!ecr&#mBLlU;DUc9DJ14zZ*su{+C}#bfa&rY%1Fv zj_n)@0!gGfA^lLNaBeyys zgrfZK>U}PMNU&Y+Rsk-TfFW#{!Tho+8ZGN%T(bJ1P#WyXCc2>T?2ljrhC}FNVy{w2 z*XJqUEx|Z3`l}-Sy501RX;<0J+<{~?!TBU=uITRY0B3c2H#y!4jW zymYgfmUJv-W}_(Nr{~(TUz%&Ay=k*Z%3M-w#jItr4B=|H^T$99^je*1vq15s{gB%a zEFT3V+WmoCY2B~hqxXz+t7;`mWs!O_ zV65>q>8qUOS~%o*$*z&k`5K;`&<5thP}RCR&|Q30>`wI z^r!$4QzPe(`FR%b8PihccF%i$L_JdhOh)5!PMzix&v0h<@ni4Ewl6qvB{1bqQ8?0p zKWGt{$>UFni}~vhiLaAkVD8&jGIt$6e3@Pewz;szrmYwNa@EFm<#v}!zuWooBu^p) z8e}EFP2We>V1n%Al+Gv-BA``l;h zqm1NO7TDzeyL{RIdm?E0atJT(L0N+>LD{>idbdr5+DF?X^0m3V&tGbu^0x>NY*~wd zK~B`QZoEG~7YN2bxwst7m@}3eP<8@RuEEtE^#jTIss18hz;9d6X(`Rc;ac*O zyLV1X#KTaLjm+#M$SkcZ4#b5=H|mdf^}w7}D98_xW^C~J*=!jI$wy;aAB1$^4-LyX zJwv$-Feq0}NYK$Vuv$5Ex<7*6`^kTLVpnTXo=hNK{ky^jqKHmWuB)RMRu?`Ya5Toae#$I9|ur zW9(N|hA!3psmqqSgZ}%RzqwB?1hEBLv2bY@6ghGMQW8=bBI9b$wlEAECZl35+eJ5C z#ur7y+@#Qik61EjJwA-H{HhOVFag0bkPm85^Fg|_9$^7LdU?u4F}M+gcEQgOS-D@z zRCpE7Vy~nI8!XFGmpI3PRC(V@jh(~8!nqfEnMUs%(6oGFtkRO`dpukiqhovq$vC2C z*_CHv)Z|y9q8$$whYm0bkE=gOgpeDMZqb7{`o3-TT4ImnfHl?+Pmp+!H^=G3X>9`T zm$AnwuH0w%BsCh`Ku z*J9F!@1EtNiE3ryz?@LUYwq3o5bU>5<;XL1tkC7{A12UNJth-VKaD*Jt5OtDtEm?^ z!s6tlr(h)5=zmF7k8lW=JYZ0v{e_5xsvK^S%gI+&IIo6nP>Om&MR|;YPP$KulDpr6 zCti-{vot!c@F5s1L`4Ew6@2Oh1_o*-|b+McPUf~7~8YbkF z)o^7vI%j+v(~oRNRG;osQ>PD2Ls&zgWjlj%?bj?9h(-$ErPE8{T}F(zSFv*yrs!ws zD9q_yDuBi|C}FG&`_pSu;y5fWD{3A8`Zi; zG0={#`!BlV@fi~UHmM(iJB>FVomHyzbwDHPCF!d{*E`z}g&@wsF@>`g+Qt-^R zmmOsv$A=?mHS>xjLD+e%j9_xPmauEJ6y}OAeIckV)V{L?>kaf6qN)m!1LM$d#4m7m z0c5~(CXAR~0(t@<7Go{trm0QvhY{G*jJ*NOD@sgWbrCU{+q%lTuwGb($aB1xak4A$ z44W!st(j-k`7Xlrz)Y;S$dNe;sMT&4{!^M>Q~U|)z!sO$v0rM!%-3=0&>^^ zj;4+;rJ8LLH?yzkr{rkl;-ye#DP0}u-F`rvK?(D)n&-pDmNgUin0}hj&8juH=M^b7h669zV3Z-F#)2G=Ou+|y ziIZVtE}xp%s0q>Plp0k{8O0XVg%I>Z+58Cqz>jWCZdPMCKU?e=XQa)?H)tW=l7d`{ z={gbGdiGXGUIs%|A|73KjOve{16_=VV1-`6AXG?|s`?E0`GikY^(9D2oi?AAMn-X_ z{!7r@S;W@JEG&AiK)#OiKu{E%_XaWW+ODHRCu2A;wrl==^HtZe^Sy&-cWLeZ8^KBQ zdCL`7qh7`R1f;V8Q8}hjno`PnJ0BV@_qn2#qScPFl}X;$u*|kMI+bwVpBz8Gf)>-A z#JdQy2aPJUFf4(oXhei|y&xospS*34kZVxj{y>}EM^5juBTKGY;Fy&56xI_4>Iv)V z`nM=T%?6i6spYHSwfT+2B@>noNF6`wMw2m2jEE29eE35J6CZB$BFY>!s{{v)D1#jq zTI+ALR%P>aYI&7VQbL&we-o%e zWknVqsW7+wRFk*{-EUox!RwpC29E;^4Z2$<^6xwHhJ2 z@z`gj#Hi*X97-Sp2b`|KzBkeEBoPs2dul`m(%vJfBshwdlg+XSnuVt!s%cH_%RcH) zf-OS1`LC67UXsg-2StY*@lF(M$!Y)m8K>@&IqVgpY`gq{s-8+Fr~nvmo2Ogiq=gm- zT>!9Dppm!F+hM%Z`|@nohA&C~v&mtDyZh&K2MtEtC!V9mt~pu zr!QZ=^gE6~-EX_=(pv$8tAex#%y2tPWz`7g7g5^{kwB`@E%d}GKm9x>iuj!|MZmB? zUp>yGyI;O}{R%q^_pu1ZzPVP|hHFRs=NaNyPFS{jTZOY6PWjCOULo=1g`>2%EoJmf z^WqrNWR!dC*s{^^A$Zgk|Rb0wZHB)q^60zWben%^7A6!@Hs zlP2Jtu<3YOKXRE_9g2~hXkGR#rf29(Q7fkwMhjs{4R5M*b~@PBf)TMFCGdr6_A?DC zJcdaJYWaPG;!e91>2;99B>2+p$W_n^A$g?3G(t-f6LQ56luup8ri@y&Dr9wR3K8Q+ zvFQ=-Aa}#C+oc2)mtjuYNM2han#oftR<>wMG5fH5AwxeAwI3k8n9)UFYNrf8{tBQ*61f z`8u_xiZpPAtn?c#_G26RTBUR#jC%ag&Ay-7qSS2CXa!U$ z16>k|x)O6CWEFTB7(*8?Zxywba!uC=1qJ?hHeyXC4nfUui7_lgC{4ipF_blMZPiW1 z7QL*R|Gh}J)TFwkDywwyjKu*<7fPdhZpx8EX*u1jszxfGo$Kw2BQJm)AdLzKrU_8@ zyY=c$vk(Rjjp~Uclwr?lfGUNr(kIv4IgJRkjm+y5ia>I=I&&?8Dt963 zP)6T4&046Sed5Dni;<=;F(I;SS&27iptij4YHN3JA;|n)Ny_YML1V%+O5D;Zcy5zG zeX2(Y{Qah#S}V zO3yZR;65BG?2G}Wn@4j_^+Lm=9?%-{HmbG<#=UAHqE$y{1!dCLz3(=67iSABGkb3= zZMnds;aZ$V`**k6*TOHS@10;X7C^`Xz@e<)O&9;bOOq8)hG&$YGv1L{q)-8sR6-T& z1d^|NVv3-NrqEBi(6z5?&NsU$4}e#R9v$`*JX&~%Blohe21B0QH7xDHHEhKCw!#M( z0)9>xPy*7dSG}Qp(!d_1nwkxF{MDKx7O-Wf;oG<;lcqQ-rXrN;Aj;e;>2LoP%Ux-; z_A=w@XnysFov7&B5}EMY#XJIm7OIWFi0y)r^{hbWG=6T|udJ0!{N3(9z;^+j!oIq3 zvFJXiar9Fxq|ksv%^`Axm~Vypp5XB!$AQM!4eZ)f0ByfJQ4v1G`A@YXWL5ZsNXd-AS|?pXD)R04kDfT~7 zT?7B;82GoEKN-~yMW)z)#cv+DI&(}Zy8iD4!5{%X#|O>W*Jgw*?A5e&+08&@V0gmU z=5)f|x3{3-iFBLmef?TnpQ1YtlKC@={DYZSc~ciny0DR|#SDUZHTZ7MVmB+zuQmH7 z8|81_+^=1xI*+?cEI=ef8kP)b%}VUVjk9#2!H24q^v#cUZqpuhFp};pCb2OCsV1(( z>p8{?;l5a*WkNuM7jE~ByvOI$lju#ecZjpqQ8#;ugDK@L0iF5wZcEU)`un9zS2XxesqWaJc#l()pLossrl z5lkE&-+g?2eR*4XUJ4{bp$;52Wc%igCa2(1@>Omf*$7^rxQa(W#8gIs=4vI|o<^wr z;9O_|%n%5M3kK~s2(~%rbl5Ro>YC9Z8LC`dQAZ+oDN6UY;>uW<>-aU~@X6O%iiNwQ z`OWdNJf+-ZQGxm6V8PmX`o!FseeL)Up7_o@ZP`$4Y1qddItmWxdsbPRGo%)hwI!L8 zDXO7qben4eCA>5UGmBcYA$+_MVzd!(^b`A6q6A>h)wEC_M)uK&!hNKjwUlW@MgG;3 z_`z&yBJ$G9m@Wc`#YEAe5lol`H@&X$z|qI1FCoB$E1KzQgmT*MM5 z{uIGjGb7?_aOTs6C9kX4y;qx8BQLJ>wzALA*n$0qA2hwu7$;(w*|Pg>;`$GN=g|@A zc)tOkkOCq%SsoDHVnRE^tq$Jk#PH@O+LS445Y{W*iptWJgbF7g4bo)}Oz*s#x|$7e z+0nM}v7t*XY%mN&aT}2O2vg+W>;UZbZZH+QPTS{fzwA`t;MI}>G79%Gb+T$H(K@#vCqbzH{y>N$g`BmURw5+GU z=;Z`nG(InRHKetrwVdC3Kcck||HT)wMKE4Nfl&+hu1Hf{&D7L3PDV|QOWT&-tp}H> zw%mdxTYWyf`M2aqfcoqc&~nd?+P9Yd^kKCIV_+1gay!6-(lSOm_!@npC4;@h`O3+a zOKLQY6qU_5G1<@wvrZ+6extaTq~ zD}U`A>|!XZUJF~mq|@$zu+AiQ9h7Y-;tKd)P$#4Mc@e_j+5jtP-FHRey@OBhr6?r( z?zJSxvJ>~~dLE5l@1V*5(pLMZ4!pub18df5PR>9b@I-IfH@48o>V(dlU>7 z4DwIp*;?P$=Qs#ZqS0Fppbq^vHKz>dh5wUEP1gf({F@+4z0m_;{yjp_2XOswwxuZm z5di`c6AUEtkH}OXeE{#@(=GY{)xQZe1Hi|B6J)6t1_1Q`J=!n>!2iAcpIEq!wFUst zwjF;2$r}MaB7+hVrz3@qq_SXOq_U|oMgVTuKY4%RsisaC0kFA$fX>*Oxfn5f*xD${ z0>ClBFv0#2Ch^}ufry|etN;6Hlusoz2HPl76StV{rS(QK@R}t?BZ!-=KTLtX{wJtz-;6qo)c$`nF8-7_ zu}DQS17Tm)QyCb^e-FU)EmN~i0Ze}{`(+A}lWv)MV){=%s4@d( zTv?`ym;tE&miU`NYn2-M|77S}rH+F#{?=rLB}bzMihH{VihGOo|J0OZm5N~wpn+zz I0i6c>Kb`b5*8l(j delta 47584 zcmZ^KRal%$)9v7dgy0a|gS)#+aCaxTyYu3%JGi?uxI2O1?m>eFcORHP-?={*-F;h6 zS5?p`?s4DVs@Yo;_G6=+C$(M|(gz@i?PMq`cS``C<{izkpv>+tJ=Q!PO*3dPolLQh&G5c(WNlhI4UxJMYiO|Hu zwcotJLsLF&>;q|FtGD&3O5-;+!J8y_y8l38piljM|L;Z%ZxSbl z{A^55TJv}SPYj6^MStlVeR(GmBM*Wy-S+XOyvGOqs;j^Fp{jzsGsCGF$(%+9^sfY0 za|Dx`Lqwig;IKjP*^#GcdNT&Z#_y#X5TJ1vC=ia4fsx*YN&QKoBT`ZD>&cZCy0@(V zB`|<2g<5fyWZ)0Vm0bu+g27AQ9XqE+g{evd`87foOjwvoRR)w5^t03XHna?uUdfOd zzrrn4HuC|h_KX+QxpDf1S8}|Q3O@ZZ?DKf*mn8Y_X_=-|q*$#0$l>!qa$sP&CV-6b=%u&j|h#ESACDuZE$76ZYgf$GApj5i*QvofsO9DbW2PO>R@sVGtS1sdk?gF?CfCd;`&C|Iy>8`!+8$S9g=tYpwG zop7G}ppaqOU@lmGN-}krh|gwU6aYMgy6IZb^tWGn=@~psPfRx*h&(IH9#ZwlprN~; zPhRi)w7iW7pIU+|{&m^@H z{o>T3gJ5HMfWP=^_?rqv+{^_%e3Q>CA<{S0jm3$1)hJ}|eIlVoI8=t3-4LJzhB%!Z z&mYR@J;E$2c*y=2{a1&T*)=b3Y(MqMUMGb!JUTy3(4)N>ctrXm$Sf(L-vB4>`|6fs zMs>pjZFhaY2JwT`z(=ea$$?&_*&jho_$uUQWsbbL2ZQuq?ZL!SoamY+bLIx2CraWA zupRUh8e&%13N=wnZvi~Po^U`;4jdS+jHto49lJLG$(d+J1${|O66b3WdL&tN-%!Mk zXgzYj$0wZNh10*%@<9JkcR(Y@IzUZde+;u(j?aByJ45 zLD#%OiYRmL|AH%#_c(X#V$Op}6Imq?5IP*6IzO2XF7D-zYZ^H^U5x>GcDfty@|n)1 zgPb@S_>OU*N}(=2QR`^#Jvh4{3LW^XKj?qiu;Y=@-~m1Je$HY45;4=?6pCLeMG3l8 z9X#^p$)jLd)3A{0f8Kl|Aehjs3<>YfH^Rm=_RI)*+PQDLqGZ5vyj#9E3@qAN;CTs^oDe7y!GF>27nOfw~QEctj}x4o5Ydc#PD$-1=)T7vG)t} zBZG-hH4XgyljzHUIcu1p%4J=^N9Oo3qgBlliL{0 zAOlHF@8W8!!EN}LHs|KkfoRYnL`;Wj97R)gZ^eSLjKDP?+U-C$37q$TQedAftT z?LlY40!Fu*YlknE=AV5Ok#gqB;gD1YJqIDS#6}fwKmvmiR@O@Xja0gWAKoGRf*@vC z^QocjZgZntaT3teO{aHhW-3fn&@N&#peWz~PcrKnOkLYQvh;D8t9H1uMz9X{4iN70 z;0{oT+L47S!!PuYFk>VAs60NHY${Janh%lMb2-RJa9ks(Ueuw8Aq4^tj*jR_yZgG7 zCDJD94--Kh7m{InmOU3=hfx28lS<;M>16AN;zjMQ4nPrdAj44IAguC>RVC?SUMTUH z-axORqg@$T-~G>t4<7T+>YsTh$@{2E%)$Qqv}Nn8<((IEa|nk2Phoe)9_A1!5c6lS zOqY7H7j$|0c&(_2%vgFBM{N*IhXhAoBhKMBozaogzFH^33Z?gl!dFjEPbzmMP}uDK zXqGb(Iq(Ou=N;J;!;+Duj7^p`gaHf(Lm32_7=8xpzox7sWfdJpe*ac({nl_fW$zo5 zH9hrWq?rpJ*WP@HE;TIfO!%sERRw2snzb*h0k1APf-@0aWU|~yt@W5Oz9T3?Fw=+N z+mc-9v@TjkCTYV9?GyfIu<%3`iZELv@ON|X1%p+kNN&YvQi^<{Mn z*cSDb#MqHrOq)6<1s=hqff8Ke^(RzajmcZjHAmkzgz~4Oh0h% zRYSeA7_@6c7?v1Z`aS7>sWgf*%2Zuh7+Q2(3C?d4Lsz>Vq6j~D6s$=6OKJ-wbCp`4 zL*5NxQzMh*SZ!wD3#!*iQ*3JEa6zApLilJ*M;dGmdCk+!J{o*SPW){;S%p^aq_}FL zb==&q5mDjSDwN+GU-&qTs&ui*cGN?v`E?vl_-0n?J6pe7IP0v zyu@dF;SCFHd1fm*Emf~<{`^Xn~VgZ;Of{hF?a*0-s)@?NMO8}7gz`?=gIrm4alp%nUu+5VFax)x`C zZD%hunhT|(bft7pG?_L}f(Y2wyR77j#(5`U9=%}xPPNp&v=U7ROrF``+J=Glmt7qT zKVyy3tx+?npR;A!7%HCU-YNi?jSgMm`V6CjXxFMqu1IKQW3mEu7hkA}>j7ImXN_JIxK}onc$N=S#L87;r_JB)N zRlk<&i>-g5_1l|yQ_yT`PPQ!V0eyT{#yT-mx(3wx9kDL?Kipl9f6s~YH&??}3)T(? zZLbDv)6D7BW?iO13WwR^nx6G5AwJFV=_4^toImAK`u1|B|NZ`cltqHaDozXMRb4d>%|Xf9yZ-`cj)eUozhy;*DC?4d8ypwd^Lc3lFOu2L|V zH-KzqAR`W6ouy3}!{JsSvX;tj$ifLSA{C z>0e7(o+YmPzzlgV4F?klU0KP__qAd2XfG=}eCRRWBJ_}6)XQ^z5I3$wSn01ejN1TM z84#PDT<{NiK)}EaJrwF`yp$ZexLIk}Np&J49`%-aXAzHP z#zg;vbYY6wkCDtrQpjt8_;SU!4K-0@D;tP62}NTDkZc-iO^YXEHR^Uw`ca**Z&SHM zjGr-Km-59sc!CMC!wc?>p^R(`WM-Ob>ocfD=5*2$)uRj@Bp9tH-jMiKAyz@bS3k6xS?+gk z5%=Y~K}r4F_+_*Kil7it8IAotFcc4vHwZZP zv}hRcyLbwk5$LG;AtJIiS(V`lt|)puma0duMPFx~XO(gD@XRQi$kyU$cNPWBytQUY zCUYO^(f(e0OMC3f{1(Bf$3Rb(pcdg}@t6RPL}X)bNLCc~I7Y&HMqCJzWEk zHg1fu;38;YmybSg^LhS;eM(kJ&Cq23d9%c%+S{}n3fJc}Ma-fYN4~UZX2ZZi&YrKC zn&Oss?i_+K?2$1KUi$@B8Dne8y?5C0)O(iJP4sNB!FMIh6%+jyIk#vJ%x^ zNwkK-PT%wlnTmJcHKD{f-7dV?`r&gdQ|};Sw(w9j{;k}78%n~{)qb^Teh##CBk&%q z3iY8@eVN^upI{k@@U+u7=D$(#*zGQCgTZ)0ji1)AfP4}UkQ*<4_?i6hXCGW!xq;F! z>HV0bnak?ircGm<`4kDI50U-yCl=e7_%j;Yx=GZ0Sv%G^e-Uy`9|S1^)JtnwbtXW= zCOwN5d`obE+F?Mile6PpKms^^Ei^UpxgqkAOg;J%!)xugYbcCb5r>1L*-XHet+FVI z;dBj{94BM!8N|Z*yJ{#T+vjyre_lp<69SmI$jO^uxnzlUHfPox3^w9RHo+ z%YLPtB)^T7PgSej9f+!0ijlvK)rFOGC+h1FUwXZubB?8UT^>3zjR)4k*GM5~)_;S%5E@=l+Sz)0P7jnM(rU5e zC27I6x^rjgQ?^a~`E6xv%3?+sRVlUhK&sfu?|l`E)~xDU-Jf<>t)x~qb>J6?3aUUHtlEfePOvTr209pALeFU02Pj>GpAhuG6>n*?relcgo~T3a z6{1@1J7w6kEB$hc-3W!%#$RsTFu$}gNR>$|69ds`E|8Y=Fo1Y$$rC{cI^SgAqs5h) z>#fO%wb4x_8AiUe!d^oAWdf>SYCttN&61$MG)a%1`F32Z;4M#qR3Vw~_#+ z=>fCLHa;9a;nMY_Nl&t#f~{-(Ot88z6@$AU^4)B7yPf^(Lq-)>MnXdZ^1d6Y_wDm%q5La)+*hMomrN7i*(P5fFl~10iB66xh~!bTE^w3E#Y&DdZ0KO|Ob6#IqLa z2lxy_0Xm%bJ6Hih(DRT1}wzQfDXa5+xmNQ@a|F zIIDLmIT0cic&`rTanY6RhDt2uBy0=Ig;^Edh`GczsZ$G9HYy3$zy>zHwiF>SziHoq3- zS0Yf{w;H%v%wcI$AO9Z`7bt7I*L$6=#WG!2VfnzDcwn5uZS z7^Vinskm|9K^OUXR{Oul{$z$?Y3JWCYrr(}jI$!~^NsVS5Ea$p!gjPt{`#wZTEhJ6Llp9iLHuFLwOe4Yz6hU=)*^*| zTQl%fn5*0)7~uK6!)Vt47Y;ZmcA-=mDBwf13-f;wx9l)eUR(_GX6uMpWW20>v=?qK zee%fk`kpdVFT zM6q4I>$6`Z%Z8ZR*u?omxs{v@GPgdijA@5Qsx-?PuoMcbQhn$ zC`=W>;bQ&>Zz9$r#GPnPwPUyRKijRcJb9&@tUhRhsmO3(4*>KLCI(@MicemAsjL#G z{YE_%>1eb5yYlU<#a^HNzqtOk&EH!KWd<2-$ZEd3-VG+%omuQ4o-CHhbsCnmX_mDJ z`jjssG#Sc2`w`kUa{^a-Gr;|kiOidgwE@aJg1y#EmQj^4pRzU7u)u2BE-m}}DJ_6} ziCQsfm$CB66>u5I>m@AFFkbG)Mv(uClWfYkoZYB+DUfuLu&1GM`mI2lh9T6jVek##@Bz17(t=O2ct_Un2%XvDvn3Jv2&#aUhFEF_p77zem#u<&mrZ2vYF4q#m0 zCJlJ^ZXLQZT&IZ6VCU0(8PGkQ5t#>e#ngtWGLs9P%)^9{iYwY+F&%DDtgv+_&2 zY)n-^U7hs2!@yuY#TKZRim$#L(wx|v>z?q{yBt1~<}2$+|$JH+li zQ&;np0*RJY`*IUHyfv$te6Kryp74NH|CI#VxhKgB2Z9(>G@M&%Cz-Aj4D{Ik4n8N^ z+I(YI-Z$oN3Bex5`wGiK8|7(LTOW%{bFPr{x z?CEY;7^?f_mROaGATSZuucC;?wl;UitzEsOy%1j|R#59ImL(_1@ zfH@9Nm_wBeS&69Dz1arBS@Qun|47rsAa4oYb)Ax_I2(MFG^jJ$4rkB|Q*kl%SCD&u zC#C5_e##n=M9oQ`=kMy=vKUgiQ@kO^QUm9EP2{+2_>#K?8xHIkOFn1!H?hGg5(V+2ZK z%q8Hd3xQQ&`~2;Gqn?3YFIYYoNDR#S)N|un#2X@q{hfN~8PZ4J@*}=;>h-+tbi(AnZo69VSg|P1Fr~2q;=R?_#huO1 zjw90EpG48QPYYP*GD5#aS_Z5SG>#R%_2|F;?u9P4_i;WKb1HZ3GwZ?mYVC!cNMh3n3FE<=RqHibKGn+MOtoO?F94Rfm|JjKtx+Ts|ps+M{z!ieH4cZoV?Ur)omjeG1XJAMBb$i0VTiqiFNGAHUz ziitB?(v}Wf5m@v@*=>!VpPK$@WjmRv6ro6ncf&b$a4B@W5Qna=c-!F~9cOhm57$CZ zv#O%#u}u9=vuXgk`FJPPyx?t9;%qjcIFh0;CeY0FzIc81=dF_I+tSV7yZ?k*&1fjc z#~SAIh#j*3L(v`yPj=`s4rDHumSPt)P6%rgE^;L-Fq?aIlE1uAVPVvhiY?iN;Ana1 zu!odu+e=7fMaXG3I@$S~_Ud`+EZrCp*ImW_3DL(SL?{COV{UUUF8f7#OI>Pkf6?1= zdoEBk!jj@J`ZYaQ=nXOb_<(&*bFvnCTr+iP&1asQQ+pf&=Sri0umF$9BxSty{U2DQ zf;f{J!bAjneU{wzl#fI(ocWoH@^&hN*I~iBK^ky0u(+!hZm(3s*a0Wg9H`F6p>@u> zs~I|z`?mqeuOT93Y5NPuit?wLvUGF(_m)Q+{+}1Dd&~5VOA?Q+A1}+yTux2BVU2#Y zo#ir6@g-`Q{k8`ek}P z4G&;Her}qp1uK+uRdv-r3G2UwcHdu}l)6Z)%!O6}Ty91*5&Lh5vd-WJjq#`AHmPS< zwpMHE;v_TcuZxZd#yL$WB&(R3BDYg)iG<5Fd7LvRe783-A$5J<$erO7N4h~VrUR8D~tZL`yKoPMMG=$eV5TK`z_e{8(R#vx<_ObqN zBe8k*4cK!oy}RgZ-oNVVMoXt=cZyjoKRZN2HnC#;OEYIWJLxgk7;xs73P&_}J4|dM z=J7|;Mk-`jnAeNa>dxRMhSNr`kjNNdu%79F&V;X4jc7SotkuBXj^y>N?Y^`4 zfHa>;eXs~|I(;)5XC-CbzhKJ-aa364Q|~uEbXM3au1&bH_QJp8<8t?v;V7azoHjNQ zh5hAew$*c1H=`SBEhK+F_~RI8bxQ2L{_aufD?=RkjoJv|&G}UW#D5Hm6 zzN|#~u2V{E&ROn-7{C99yjxIost^3_pXTiJ%+JtTIa+GO)^b{)#O_TNfS^k5AcMZ! zNUj%pAiWS6_<0m!u#V>;WZXUZx%9G^qxIN>8ETXrS%xU`y%G{!wK7&BGb4ghN6Cp8Y}iE7UGlk^D!*tO3H|qR+T6 zk{FhQdYnh-r=UJ=BnNnb@KOvQK!x>8$pmL0&0w&);_&pXxO)sn3JtI{sbV*D?}?8o zK<|fUc^Kd)h{S?|RWNeHo2V$Me1N zz>ARw^`)w}Hp(*DcA>QOb46E=dLrPu;S;#>pc0kOKx%2tdqVKn# zjhb*VDL}jL2HL#o{c6FoN!T)9pa(6OP|`iL&!~D0-n%yb5uz;?tGbo3qe@xXbU<Z9A)A$Ezi(RMA%*b+9|6JEMLXl=r5V|K5NEJgV?-KCG_f zvoI|?7e6`|W2gV5@mQGEDK^6BYp$|z`zeh@ZB!BBavB6a+I%g{_Pp5ou74ptAH~E^oc>vvV+&vQVsVG zTj@hRL4c>Oh_qHBQZ0{oDw4&y|9gfv=N!pe7r+37&%7DD1i*@h*&$qB>2EzJtY1=r zg@oJ&W`A@cdI7I8P}EAp+ooc)-lP2LhYb<`J-5=V_|sIoaPCk|jKohCeHtaRh`C9? z5pPen=eE})TvDY|-wg8-ZXa4 zLBIh%0Tg(UBVKuT^Y{ygk3%o5BKo6LG1ylXiI$Mmcj~LGD;{W!z9TM*TMnbih#WSn zpot|_R+=T+z+7Re|K^5B3o8n|5jX-!$S)MDH&IG=CwgCj1ul3BR?;pUoa7JX++q^6 zQ>!CNj9O{O^rX{dG778w+Ab(q)0DDj`d%1Cvf_R*$K?yS9>Hys3KgEjq`WVCy`S`s z@_hK`S7X|C-`Z2zkr=7f%lB!K@RpAJ9&dN=i75hiytKngyHwky;DKD?GH!sMPrSq8 zObe$6uB!uF7;vuow7;NCTlH%NcA~{jlV*aOGH{iM!#zdGDBgQPTL zPOi4d`q%KowD_OXs;a{qd>n>6i7fggD>ylLVxsk8qs<(2?YNZ5ji)_j;TqHP@5(b{ zZgt0T=oqGegPi=k6V;HOJxn_|BD37Ky$KR2g9pPZ?p4f z4bZ)rUXz6!nfoyH@%9=4A&@GJ-a%=W-PZ`31mJXuV*(>>J>YaR5{-!7B0W#oQ;=jX zfF}+%_3lv8voVfq-zy_cO;(cNL#U>$*e64fMyT?`vcC_J-KtqX@OPHr04Qmc()CMc zYwwW{KPiLTT%gg`i#I{V$MI^|9XJ@HV=8Q~srh0CIQ9ybNlIBc;BPeW6Cf#77>3fF z@LVCgpu(qu?gd%mGX3tJFKtG14UJqkP~h||J;O4N4YONQ<>awdN={1Ed>k!@9uG#tZiVhl;I!9QO>djp|m>zph@bPig1^*VyX!HjRQP@$v4@K@@Z*4!P zXOarvILuM>vjlwzw*IL!z@hy1rNw$bh9b;vAL2unU38KK@a}3kV8d8yJQ}Dmt;KvM zWY^t>0R;+0eANoa=d`pHacRs`92-&92=6P-X)E|Ws$p+W%(<dS!KG|rtPOvr1*DUKN&Y(acM)9bkih@6U z2N6RTen^*rCL%3@50+;RE)wXxupm=)akbLx?^J65zP`uLSb7SxSh>786~>bu4XyPb z?5+H{t~T86hG*~elhZy4t4Qc1bCQgj)x7j8mKqE6I37*JY$KjI;)V9oMJcQPVLDVN zK2(-}V?W|gUA}~v)SfBDGdpfA9i<5Q4Vz<3v*EIt%VHZBs&eQtrFWD#oorfYYOy~z zmeDH!&5g|+8(6!;Q`U~u5*EMV`gGc)(`Zoc#vhc(_5PU}^}7MZ$Y5OXC0v`;A;mAK zzcqY&A#Rb&UD)o`aQQ=Jmsm%|hXa}PE#)zg5-w%9$kj1rR-yPO^tNzK6V-Dx=sq;K z1t&eE@V%Br2&!{*_*N$j4MmYB*=l0KJ=Fm+eV${tp<6<2y>I)&xB2f@(37?DzU$E+ ziDXiHl0WH_$VH1Rqlr*g3|<``{gblE{623qIL%BK%K(cU-4k>(eV&JBDhPf?bh$Fpa=S@-`t$-2yHmM9h z;kq;+Q4j83?Ize`a$Z395jblW07z^WOW(H9Xq+hszF!zzcECWnn&^WQ7(@ zgY(hN$F98fwH77}HavN*1_az)RI(s=>jH9%IBz{pkE1`YR$rB&P5|VHTjkTJM(xDC z99Z8uBk<-RAC_mK=XSu)NeFUK2kF97&S6(H)I%_!!|SSr6dpajg8ZqkdV#m48Go5D zw?VUExuLxlz0&IH6*t>(GbK(YHBJd^ZHTnSu*l0=8$NX!Ww)w7?+k`#cm?X8o4KJ3 zercB8tG`3T+Ue=Iwx4ZcWnxOjwO4{7#)j_v_34jwxS}e+GG&-!;>UIytfM&eD8GhE z7cI83JKKg)#hgFP)_j^SodFtR)j$4t>j?-qTZX%8Y2aAr-tJ@-Szce)uG~LtbbH)- z*IJ)8%;BXAVZ*Sd!)6#Sa&{;+x1$hXet?l||LPWOUonTPEvUv$$!n(=vsshE$SP%s zV;ak-X&Aw&s>6f16tsaP>qYpp%eR~0oXC-ZQS18(>*N;^5Ako64(NbS2~pw+wO5** z@p&l!wVbbHMAZC^IQ2}tB^V+paPR=jvszr^tq56X1nMN?$7rH zu0*%7bqRdGJs^qg&^mO;7AY2#f-y0M@rh@&4ay+4nQ|?l*BdyvUPp>-HZhH%%+0KG zsdK)AE;<@qIsEAOk`!YqzdbMg)sJ$p5o7Et{9pEK-T3P}<#lhDhu#%gX>wI-X~!BE z6}0=G6`50$-zVggyOk^J+SvLMBMiR7^vLb@eR>g1QwJQB9S}e@wP(Yqk}Ak5XvyWt z;FBK6(M{-5$#U<>?X{v3{c8zka($OxN99_42ufs96sIMjt7~|mn48q8oia<{y9BdUh z3bP4P1J>?b-_WsPpLS1pTdtF}I|bH1KIh=aYsN?^KN$^`NQaxm&bv4`d>C>0Z*2c; zgUH5ZmE#z7X-alz5hWb+*Xc`_At8h!*);3oA=Bwy`9z@1adp-oc4YH$qt@gw#rAsY zXG=$e>-34R`@*F8HpffbuyN~&toetcRJK>7E*8{S%2b-DxUtW2!%Ob*j@`W->`igvoBdT~tu5-l zl)#7nsK}zq0WYDjFZ9N|)t>jJhL@e?PVax+x`gk`t87?zaAl?kz(SGGN9BKD%gz@n zGr#UjKk+Zmx#J2ZMb?*#tNU5q-ylmeF z)NhZbP+GcI{43kfZU$zy2ewpcXBU?eK#JB)W{4&={HIS&@zx+ygGsCQqz|=Tdl*6A zU_sL`Uty_09fi+H6iOfXJ908l7~^u5nAC{#*}M3w<@SwkJIT6n~OZY#yi3PlP+D&(=FN8zv4MV=_!e*+YYGdx z12o?KRghFypYDr|beiYNh}hU&Lu3jikVso~i`j<9FR{h^D42kisoxjM-NfvFx5;p! zHA1*b_W0xS*XnaaK&k(=M#Q?pM-juM(#VvJCXq40H+YT3!Adu`AeT~PU|$+WvgMd2 zTbNWNCX5i&k#1~>BAxZQVQ{(KA~5tHoVzj}W>ui2FQ#Rp&OH^LUhdS1k7_*BV>(jX zM)=EPTE6IE=!iPuF4N7OZ@!Q}DhVzw7cHW@7w@o}HG;HLm#8CrWO3A7(cqw{f$i&_ zvtSTgXhYDF$Y&N3+hVx}?&Mm2UGm^+N}V(JtyX5>MqSc6>Y5Awej11DftsuM{W)8- zhQm6mNxP?!abdpK!gtrR0(XBo3_PV0E?a=`>X-Th%Io#D)*i3Rt#!w)S`}I%Az#ky zDegD%_srh%`|-9{`e;V!PX6vApU_YVr;f+zF|{-AUYTK!wVHh36Ml;*@RqjZ$BmPs7gB&FgLrI6H@y+il@thIY~l-W3GC>4QF_)ek_ zp|-pR_=oKN_}n4W?ZVUBxQ303xsC=wXl-z4@5c`5(OQSGM;(#^LUAa}zv@+0Ra{&= zBlMV4W;E_Fkav21etsYnNn1;c z7e@BypVg>Ss+O!$TLSFD6_VXOGpcRoce9$S32UZ=3DO+?08eP%l+{MtJ0Rjg?k1UP z^tms#XEZtauqKIT>;q$NBo{qNMPo_}EFl8LlgaWE{-qbd?^iyZE7>|B8JOs($gYzwaOK&;kK_+Q-cbhu0Jii0&$voME;KviXnJNwM#_V-Sc{I2 zgbTG;uQmq7KvJ-kaf0>Jw4dg{5lRVu)j%sJCZW|SEpqig zUk2s;!6pX)f|bwnLK2tl(9zM%J*=jWkfUg+I|FVs^!cY)uCKYtNGC6PS zu0dO=Wd0nySBavhXOxV|h{AS-vO#j^yxA9>5 z!M#>*tHDXx)A4ZkiL=w4X29jyL}T)8==WkBAu{lrc*z&`9TU%qXN7IQ)F-15T<`OiVwwC+ucuzGzP)L{|49Iz z@YtOlgDVX$ofy{F&zCue&*gA8EaoJYiyQeL@mJASF|E}SXunKzW$wCc?Gg!CA*RGT zG>iGM)r+q^`mApCK(zXLUKj8|Ekoe}ItP$$TTwm+zix90&VDpGXE1)*y}M&PLbfJA ziT4!}CBa-*w*9{CfcCz;`AFltXKp@WxuHOPH@0R|(niP@j(hli4t-9aj59j?5FB_I z^Re}AqznD*oPEp1Z@qF(O;L;aZ6G$h?`ieWY(Tn9E06vyy*$ke6~9^JW{#4;7zMCo za+0spW%k+jvpl|TqBi{gxLEs9Vx_>c`>_~U#dGpI@37MKdYsA@m{FQ)9TtE-^u1ew z8@@C=tb3tJggEiLmy}youzR&yQdRSuL)!A6YpT9xakV{VUd!6w-5tYuy)85Gpd}>k zZw1br;~!EB+6imFFNnOQH{s2=DFU(!l$So2mqY(WOHMN4%IQZ&%2>&`@om)d(a12{ zH>3Y2iYZFpmMnX>$S||*wJkKk@gwn#Re)RAaQq0->zotjix3Zc2fF|}+ZtPepXSZt z$C)n}99494-d*7KIuem;!idWu>+yiVGhsOFT*qJ@;!8K%!W&3>RujaCH-)YQb&6_J&c$M~g9<6wKX+VNv} zI@nxUr0f7?Nry2+LqUiqd#ZflTs?Lz%H!nJIQNotLr{cE%`I$ms-@W8vD8vM+^4gM z*X$aGFcR(`)icHo&#s5Kbf~?*XhX%m7F~s+@a!4+A5g1Ws2_~?#{vczMI*{s^z^u{ z9+|Q&sj)pa1P}#BxFIt(s)6`E*N%;5-{G4^-ZowykKLB^XBgr7sHTFTYD|AjP1e9v zm7km@l)Z?HryVZ2$oiktxp?{1pIOjW3}mqV0m&aSy$bn%f!ISSJ-jy^^X&k{0dWU#WbJEyHo42OR0 zH>tZTkjZe+@s06u;b|P*EgCO=L=l`VR?$k`sDzVs_ypNUpnxqJ^%vSn*qJn8g<4dN)3Da^~1x z{&qZa>T1gF%*!WS&S3~JMt5|vY?N-gEK3jP0?!P*bhXU|&*Vc(w$hYie~Zqe>a`u2 zasaEBu?q2y`ek=|O55u^LU%EDQh~ZM6*(h*R+QsYs(|lSs7*FE7 z+5T`9Ls3PF6hV^h;GR_I5a0rPZs4TVc@@x^hbR&sEA@ zg@W~nGvA(meZ zE{}`}p;h}m=xdZRv`S&#OH~%q6ru{~kk~t+_g&Zyg6~*P@-izTvC<}AyTpjZ{`mx= z*{+Zj9v&`BwfnKBF6ru7edTVew^!VfExWDV=Sa_pJymH)VZt-Ye2R&&)zZn?(YXaU zDvTuAZ4}_)X{&;cb|gvzN?H}v(p1lwtI^CH1=l#A;#TSn z7fmjLsN*wR0ts#fgNiHHNu6&$%GKm*$jh-Cc6d*YA9k!M3e6C$BDE)gPg>S63hOTg z76sK6%O>y=1;Q!%;LO$!`?*=Vf{KB2N7Lxlge@WD?yTG?3QdR2;E2XJ?qcdq1>Fq( z3EkYHY%hC4A-~Sr9{r#0J~jwuDRHIk9_oy^&)REqYhFj8NO>YJ*vpg$gK~xY&X^|Q zx0%<;?&9OAG1;8j9RDp7!cYdmr%Oo{;m@dpz550Fu8xsJCl(1d< z@1eJibZ(*YcIIv04o#%W+O~S{zeErO-A>9z5iMz5>u(P!3HcA7&FtFS z(PC{t&&%rZY&2RXAChUBRjWTND~&IvyQ=%WvPH}4=yy0~^tOA&TWecdf=D+1&D>v; zbRm&ykNaA|QhqKLkJHW6IddST2;4QLRwz4fykiXuy%prJDF>>78 zYH!_%CKUmNlgTk{nv^Hc4Egt$ugR#aS@2Fq<%g%@OY;+!>_Q!3=v$5dY0?oa5 zXYB&-_1SR`&-sb9o$)l2zDC0#OJ2@cx!HOC%K}hHcQLPKG^ZXwoD7ss%CRrqr}jZN z8gA8Xz>;qvzb5)T;gdnNe}qNpx3kI!JN23zH0QebJ5g}FzTfQzpb3jkCkox3%{-C_ z4_UbsviCIj#6o*fBA&&|a#Viv^J5(g*ELpneI^ogo-i9GI3X04l1~(V-;>TcEVb7A zobHILn%I0E@NrB*ObF1+=F1cjF8}H~X|`;X2bcU=jV_I5Mty|iH6rFn`R_4|pNm=! zUjRFxkf%GY_p#~a;-?W?b-C|(Y3%B3wffQQm19qFWEwknrIEnh=*mKnjHr{$Zng3A z%>%I?yZnNtK#z@^Na5Dxphk;jrS$${^pB>`y*95qnH(eOxaq*_3nGY7x~1^Ph|y;E zk;;&@rHXyZF)93iAn_lL*24E{f+$7M(>^GwaaC7WS9NK(eDEo6qR35qh|bq+0iVMZ za7=i&spB%L02tx9fs-g52^Qw&#|ViJ3g&vLs`|tMN{;EA#!RS;<154%8fi9luuH*W zu6a^YoGN~(x&oA<1S{fH2v-O_jsk6XUz0FO!PFG*4TGBghaKp2Yw0A90_c!e-alhc^24Bebs9xC}MP68pl4+QM&av4qJEZZIc0SyYCNO-L%7+`7=2x2Lmq? zlb}8Z+*p_2&W777QCV<@ZulZbwK^snI_`1@W-j|50WY}~)MGg@GntCv7J_(bMZ-F} z^XNjcbfEbxnaf6|>SZUWW;*Z2_{vbEfXlLF*uMQA3P2ISsjaT2K54pcmDc&2`k5in zRF5;O$K;aCUdG9|Qtdcti=*|#uL`LrIPIGGL!3h;w~61@^mjF{Gre}SdgdM5Px-BA zM7;eLD~a~EiEZ#qMv6=}IdFOgI}o~bKW5Tnci`nmKIFR+XQgy44bP1I=e0>vYl7{m ze?O#3*4N8S+20?ODLrkpDYxN%pDntKSs1#AmPnrbGz{ec-tZ60C5`m1LVEuEhbyiv zGn-AFYVUy@tZai}+V^kv3($RXS|pBCCQ(r**P9^YAqplB!iELAFVHo2Imr-$S?Sg;_65K6#a0%{CaEAcF-66O;MR2z!xCD0y?ry;? zxVyXiaB_0)ckY^*JAZ~}(Xgnls=fQ&t9nb+dKC<`k!FH@L-SeYQIlK^tsEI*+3qQjDAOXFA`k*f6grc{ErC- z{;!z`{=aPi{)co2|KCpXKb(W^|9z5;7wCiaucYz#0mlgns&BA>zmiFS{p;&B%>ruQb)f_Taslee@jK-5+hD7v(!enx)xSCHfumZXBU{LXcVcz$8*a$L4S!IQr0|plv;}*UQv0Z3r960Ri zup{XO6B?#wo!y8msIb>@Ut9ikT5ub+a$o|K@{BwzlG2}|-EI8M%J@P@Q{M|XZL8f< ztle_etNFdSXr?#um3CO3t$$CiEde4MaR~$P-c<%HqwKee1cxz3&X8r-=h$gve$XQ4 zi3^?z-$)&CAz&yV%lDa9C$Y@jfa4>3ToLkMcf23j)2Km`p2?6c*9;=t_1Lz96QCP!efv zMU|OitKEk*TP}ZCn7JbNM6-0dDPy|z{t~wmgpCqQ0@%1tO@_LFFZTJIn|I+M^5jaN zC|v}7NR=r-z5;vVge;Rni<=PM3`rj`_Ht`l>+zk5k)Sz9^?!dbP;Ln6$yVCR-Q&^xwKe}@peS9ZhO)*j8DQcS)@3xopx?~FdBO%Jov$DSIgpB&W)lRxM zgzygk>0?`7D1som$^(9v7q-$)=`SgLS%zDI4Ee z5y0<|joJ|CP$kG@;5OaM_6}U6%^Njv6L)R@SUa0aizI48T-mM3;VT1uFKH-OyAwYi zZJ+f$$};A*G@qd$er<3_8v9%5k$r80RteQoG@<%#WnG9onI~`Cv{-P!4DHTygXQep z#0<4!3=qb~cbP%VX(@Hb_zR*oZsf%f8^F8uu`;T+GC1?R!C*l_f{&D()^#LUE{S8o zxF4_Zw0_&}DCO7gV+x_EvQj1^v_=R4UA8-b7hVHe2)`c{c2%x>v!osnHbw?{gtMmP z+PTHFcFPJO1`+LrBF{Ox3?WTt)@;D8_uWyJ*nq`8HqHWv0>uK*;9u06P=OQ{UPVj%5=F}S=N+VZ3r9L)^$FMlnJ z-`t+%tKpqGNPUVohZ6Y4S!*~&B8F0WDS{_FdBw`tl$pm{lq{1|b2Z?}Gl559I-UBJ zoKi+&C@znnz>-N}ihNEHJiri!+qLB4;P_e_x+n4No71{ks^y^r^Fn;nRB~V<0){pt ziM=-vfZiW2&Ale}jV-;_4o%)jbw^YhkhK`ZU5khI*ddb0lDmPc4{O0ie9rf9`msu| z{z5D9kuA}wEM?y9Ig)CHzhlJGcoG|>nfxmK#_+h<^<=I|ZE~u5ybLu1r+#m<99;|j zWll8PAkFZEC!~{11EXP2oD$e2D5AYGk@IcJS;s-5Eayc-@_Sz0R~G4H%t2VZHnQbg(Ukp!VW*lq~e4u(hDx;Sqz`FZ$7v!i6&7o?I&bW6K&)YZgRNg`uD=fMiincb_Z~|f@#r02vCe8X#ia_8ZmjCR1*f2A^ zf(h@`9h68Y6BwoqA!o-(NsI+;DAu$f-tsqJzG{BxzGM@aF zUst~~9lw89ieT`I!+j0K> z&2#WwY=bIdSgFq&llNF{6+VnLP+wG)M;lU#o-c?51Q7gojF<_+#aFaXgS8?13j|tm z!jD^wv6uXx_Vw~a5XIECgwl|A>$=)gHPsi)#%=gZV zDM)!Mk?zR-mka7I3ulJR8jDv}*ZVe4Ho4@Ja|!wz3r{_BO{V=j>XRvF^NTGlQNzHT zZfeVe$;892OOs|S|Kye@=f%tI(Mf$*p0|Ypub-ar6QbishR$ATZ35~uh6&qRnjbIs zlrAO3Hu{&yT}HI;s|Mk25@z`LWdom!Ql5>OWlsC@5_s1W!Yqhl)yvJFPUkJZiOC}q zfg}ybFHKq$i}~eZskOk%!w9P8{3>rY?`JNwHy=*4xGw#AJv_+R$PiW35 zJWhmdoOLH#&$cjAF99M|8`Y;T=VO<3r}L==_n;2LjTxEpk!1nGs08I6yorwE;t%hX z687x2_vI}ZURZI4VkNiFpXUc3TJ!0dc1!6AEn7M9!}F?^&B&AXy7Xr2F<0l4<$wh|L|hlB=2})qidmd5 zF2tYN;n1DVU0nXdQ+p~qCCAX0jUQg?5PW+sWl!bM4c%P{Zxvi2S0Z&&T zWJf5+^L@=7gYjnqyR$EuGdY~>Em~NMzTW7|*Y}dHpv$01)tA)s$x2R%sEXwk?dEg@ zcVRbFFc;b~AJjWz?{$)&iwjLS@?z;0ZB4Eji+qS4pvAk(5#W>_=PMc5L0-n&$-N$9 zzm>N8PpWT#ys!+j%fOyK`CjA}h2zk^juK?g>qS2gJ20!;y*h|bF?rGU>)iQk(t#kehXsR!Gh0MR*U6N{ z!#aD_nU2woIpFELE_rITaeM-+S;6$x-B?plZCR0w`!-ZE&8-tF7>EJ3tjxk#vH3@c zS*wg`zi@Hp%P*qEOLl~ZDK-YheB+(nyoc(t(W}+9`$9PPc*^U$gF*gA=^d-fg9N8= zUWOCC;ogGUYvGqva^;6nEo~dZyYs&7npC{4HNzA73!rFA^MoSz^}D678vAy=ZyEB5 z zFEJTAK>+sQiL*Ds7v4{4I<5lg2+vGnPBe#z&@SJv9v>zh+3TMan@@3yCv~;28NtXs z6tLqMfycvJCD#mu<72_O#v{ah+iT~?qb|K_+O3$72jszZc|SZ>O}8AfI0I>uoF=(n z>nK_2kwJbbC8-Y^Yp&-@J-|N8#aJJdS|-XYN#4_c}iXU$s*R404XbWZt$fcN#-V9@|4%Ox89JUOU^P-n+8i(iER& zmW^r~Rn-88>rf*{B`h-Sa^Y%`*D`*966x}$GcPn(+%AClQrD()e2mogkkQ%0ULoCmBF_;&)}XfyY@Wx7VK~m@2~lH|x$3b2Gxs!7Alw=C z4dc#>TOlFElR(h1{x>QL((I_3s1|=0zx&y+%ZlutiUivOy`cHrFuG)wtFsj?j-OPc zL5jhmpT~cJBXG4Eqc^8Q6bRLFA&W@X0u89n2QPE9PKWt5ovE_v1F`Aw41B7%N)yTr zbCx;vONtW=JCB9Yj!56-z=xVW!``mMlL)9}jA{C$Ekl0Yk!Hq^6nN5Je10N@F?Q6; ztV_5Rd=yefD-SqvdgdZVS~{RZ|Mtv9S8Evz!5I@dP_dXvrQ2!h*y@!mUQuSh3W&s7 z?KN#Mtf`qFziTnRJ;@BPNGd|3pZPKfb0B?-kqm!BD`9NUlUJ%KOdn zJrhKnq4)R!f}6#i#8hZ_xou*ePez*#g>}*CHkqgTo}eSJy}~v0U^td|!B1_PI{e@~ z7TkjvOIL)1?qia3l}53PMx*kvfKeRRGpA>=04i>_0KU%=_Z!qFLfj2x!Q+jTI|9F; zWo@)bB9Etz!@>0gMPfb#3iLz$)(_T`aSRE*wy}YV4;ANV@l*jOI`3IYNu5dvDbLWv zZdEt!d~27dH^_8Gs01Y6m!5H+N%SogIrhcpj7ul01!(S(Ed+1WpVK5e1LZ9t8v(-_ zp(oYyzJ=^L4V;RVUiuzs?rIhRkv+{CF#?^({zV{ao`7!DJllNvuQ z+(f+W_^fmOy!17(%QA;(0c`BKejfTv-|c&#_r008ktfb00XwM+FZ`JuGBk}Fc<|%l zXq5tS#rsf}qW^TvJ=5`93_zb%CetN zy_&A);8!2sQX1guaaP1dfqe;1Xtjr@;=#Pf-hY#0?7SW_kHL}B1_W^}KF{>-t{Xe)p22F0>M%WxFSy* ze=^pJz_G({aGmoJ(6Azax%yspC(jn9EtYDN)LmxY*iJtQMPgouc{w zmq9-ytp`8H%KzRza0cf}2UD`a7|x&Hhsjwj%2A{0VL4(Y0nS=dOtUo31FUNQJNSYa zyAi?E;5OLw_?K~AzZb%COC-GsOFqrDf{(k=g$D$ts?N(vz_q!4oY)j`z9+8YnTuj< z;-a(DW5TC1bJ*RB(6JxeNR4S#zPr!hy>4(WDg<`N${Jyc0YBwLI_l zMQo(CdJB5)@StDdXn zNbjuFt*aYG1E#L0@pMThrd1?l?d)uXxX%^`Qw>j`>>187%5@~`7!o|(e5GG(NF_qg zk2c%aofqg}`b!pvl?pbFAPL&E98bd`=}7WwyH{_&$FCH^C5sMN?@@=CXC9_O9$nV* zC71N2L0V14^KkUU9Wlg3ZS|764bnYsX2^!b~{ zjrlnGe9F|rb^K#;Ez_pPI#pryDTFQjwP^(7DsQrKxf7No%~BkTT!-Qg6@lqmOA{%@ zIbtoNou&(GVzUt{X=6%BmYSLzYFXntggUDmfEzq4ovcMVBXWZdbi$P_@{NGZie$Ks zOk@O!arnhC`#hB(LC`zc@~*VoDS8V+<-U??zgaX_txPr4Sn1$;_u+_G6-8PWm(}{2 ze!Wd#gFpEAmW{_@^b#*UiUvShO0@0nOK8hBu3tjb1JlSm#Et=M(UX7=sK@-_jP^-ck%k zbx#IBo9O88^+JMPT-=Bep|dwv`XF}IL1&Y@c?!D2Zk2hGKzQjb`H+py?){T7phMry zn?@dG+dm2OpmRNb)1v;ue;JXOAfY@3>=<9j#d-~`X1XL8ErsPeaJd-G*HH8uPWWYA z+T6uU+T-#22Aiyz)}Oc>(BD?B`I5LG2D+jF?D#9oa_A+qC%#aimLFZUxEz&~Hqb1q zW47G4;)-tI?wyv8to;o>9vOr5dQ^k83M}$=m@#2&B0Ai4Wasxk+Si9DKjOUu*s3~Q z{kz_GZ&0j(uLQQHh!hhi3*6 za6t`;C^p$KTtqEB6HdgTRr!v)MC+^3tRGFAp}=Qe4!2b6qo1imwQL71d4kwS1#L;%5apmw4Bc9@|*zZr_sm5R}|SxZYC__sNM zu4>70(m*ci!2Y8%5d`7is(?UqP>jr9M-YzjHhA|c5!)n@U5PGOUoKSNexVAj3qM+4 zqm;n`4x=*4O;m(fT#A@A$Ra37N0ekj%=*I~-|!mu;$kl%x^hbGZ0n2`_a&?3S!`2P z>n!VLUa7}5qdwp7t_cJyV!f!**IDzbv@Bh3s#DR0j zU1K7*<$?NUUlsm&5#A+}zDzM9%>9l5(9Os&#K;=(T&$n?$oS8Wb39#R2oJBj{rqzz zH~qj&*xP@C*3I~!|1LS>1(OMZ&JDmva9EaZ9*e!vC5WI(PJfvigDJ%jV-<>rQIMFy z5V|-C5eojpIR_=#T^>W|k4n-2l(9wkER;W@38?)+)`lc+^g%I(Li!&A$63*zQ*i=a z{eLrh;T4)(8%+|2W5P5xCxQFB#_-Gt!|^Yn?*ik!d65FSP6{KdFE?hyNm&h_i!)K@ zKisOB`6FO5e{RKnt-_TqV%H~krayavSdCr_29oHo^#1M1h$h5C6TU_5Xb{ z*VDf@C$c--r%XuD48dKF>My&yHebD15;mK6{R;1&+bjgF7?I#6fKOtB7ndg24Et9a zT27aYB;b7w=>Q6QWI=ewL;x16cQv5cWp*NbRf1O~d%mVx9Q!kvr6o7VDupa{I(Ec- zi;~Qo5T~o|n`4UJ<;;8Y{Kn|Zrr1QmGCqd7T#N*Ab*|2n^+7Dg>bsM85^IUT+zM-4*h)6yF!=y{1*1Rh2L94=aT7lCEwW-Ez{I%Hpp+2s}f2px(X1?xr zE|!1c+%yF#n$D~$2Y@+N5oJ`Zy_<@hb$>4QKpE^X^9ti;*BfGMGkp|*&Ou&|d@j=8 zCqH;Oe{``O*g@p{ahyiv4&oU*-SPG_J{-W73q_^VO}-~THA*w*eBckTBxtHW<#vMo zbJU@{6_hGUzh}ks3<^J-dojY^caJ!7opXK4L?>RC6`6ZQI7K4$0q$`PwvVh`8l_pY z#Y(NURS*5Qd&m520ttae!=hCTQOYyCk@XZaA!t$Tmjh&d%)UWS+3DZbtK?IlNrZtEyvkWurqAU+lk8I7>hb=8CffL$~UEeT2;HR<-iqM5dYDpY5KT zPO5Ko2~R)@HnsRWn(y6t4E5LaXb>zbB4cRe%WyR;`y>!QKd@cta(W@FzF@I~Gkr-{ zsEY7)atCBQdlVD3W2L!;xD8gSFU>=jw~*yL(IL z)T(X!Q1I|m5os{8v&CKv1@Fp}j@ zvCLLT-=!1Q6E(j~C_8ymi#Gm(73;%q2@71hKtCCVnWdsPw}lrQNk4iMi~n@% za0t^9@HKt4W9Yw=5c0tWc9a2wji15QIyzDG)U*23^Ou@T`vR_O|Ay!0=^P0|+^T~v zSF9CoT&#|K60$s(U_3!mi~Tc`zh&!VKO`#62U!LG+WM9EZ1J`kwi%0`^C3?$L^-it z`H#&MZT#+Bf|79GIqV8 zwq8hX!#~3+yxQ*q$(vIE7%1lRFUWlQ55oP}b4U(4W>6qx@+i&8FBxm=?yVkp! zBE6MgKH*Q3e~uj%ECg4gIbQx2Emib7b~S&NCMPIiS>wQV%%!ax>yiS6CbxEF&cNJ# z(xJ*DKxsCba#6X#QY=5_GT7kx<_CNtT|Z7UuB;+y(4oi&Xk$yA!WKQm+^vGj z_|poD+w{Wbk4xn7-7|fVw{k5aXM!^e#zVL;slQxqS!Ri8ejJ!izWe&!#?=h79J^1aXaR13&vKJ%UyTK@xs`W$@>pE%kwmAXe0W*3$H#w-?JZ;72ZB`9zYeQ^ zV5x_@_;G`JtWGZ=pxgI!Nk+y4LW$~Dmy&Rz%Dkl|&*1?@2mex+`sqkP>=u7svDCjK zW5<0gBeXALH~;#Si*EA~ZuA&_y6+>Y>BW-?8w(490%w4S?JE7(r_1ikDS=UWPy7{6 zb;4C=0zJsKd($?XK2o)>dq{-v*upQ~jMDd2`I423kb8sn<*_N^t&8<$p%iS(0s<~A z^L<9oO7B;oHwBjWSRh82GE z6`*~p>*Yq1T6dbpKz3Dkr`x=HbnbCzvr07vz(3nppuD_%qhL09xH_`e#NV}*?;?z> zH3n{qn!jS(+^(pfX5{;gpS-xj>w3R%)^9RBY^PiyqxJ-SwDNnQ735C9BCCEjQE$e% zkbc>Vsv}VYGH8}L@C4ZFWwC88l8V>1@np|>Sz~HC(rV8U1;-9-!isflH?+(0lJRUd zb6Y`DuXjdW9;d9~hfSQ|MA2)(ur>coO9Q-d+tp;O?VG5EM`iUa$=7yJX3b7dFMrG* zHMq9kaPehC8f`{?c~npRxo#p%g-Q1vBV42~)HeA;s_++D`0xAhFe<7)z9a=3lqNEs z>id(bk-y^8R{K_}H}y^_a7soWuDF7Fw`dw0rKC(x8T?9QUIJCK(4WHUo8C9QE5Ki0 zd8)I>46InA+gNQFBk#w4PJh=I+A2AphA?X6y$jG}-*SQHGel~e`^CXxmER0)Gj9~+ zIJTlWj@^gcjR9(Kvsb&>kh|4~^cT73Cmo)3ihg3W%wwgbWsMU-Q80}C5Yo|-(Qatb zb1*13$6ulJ9Hn-A6oND6iu5c9E}|dFF0wiidb-n&fUg-A2I{@Vz17bL(RR^yGiY{+T69&!>}|Z~ zSl(!!@1ZTCEv7>*vV%`PJvbWYT<=Wsp?@X^82>ut-S_=v(3dyLY)g=(fTOXmlaaLHudb>D}#FL$6a4Y{iB~cRU`d<`0R4Hz~$m@c5_6 zX9zk?5;)6974bAO1|{h9YtmDbQtlv zONdpi-kb=$W`LEgX>?aF=-ZOgD>cQKE}w2lvQ#5;Rj-yZMu%?*v(z_sEAGLWh`<8B z6_a+YWv{^O=J-qBAP0aKQ!zXsMr%;ENz{)ylF8)kW*HuE42QQA%X-R&@);7xER?@jgo_z#!Y^4 zyb^0tLt{n%!zGV11!)o2ro5aCB!I$={#ynoH%BDWw9T&x$XspIT+z5CRh~#VebsTS zJONqT`aSN|rGMthl@VC#j6nP!oqQ=10jPS>EO+6bnBr zl;;D}MCbRkRd1Y8O>G@~LJDqoZLp#RTz8kqJ9gIeE*I)-dFkrgG*S5FCob&O3N*co zT8{MKm?QLPD3Yrx64ZVqqFC`fd9kSHsEB z*^Gq^qr!Uw_lXvJ6%@l+NPfR?FkIa%K>UID;O@Oe%aNkMQ$@%5Gl@Y34IV?l*3*R^ z&2~T9H?jWSsi&3Z2ew--9CFYyqw5Xpo4pzOy_eUX{{7-p@-s~6x7d=5U7$WG&L{~~ ziA416=H|43(c{uyy$Z??iQ-A$$!}(-hxJ_Mn~UENrjRk+tfF0Q>>@1@Dc$Wp(!e_!=?x;3U(b~mnvBr`aKZ7FrK zr*A*6^t*D(z<^-?sB1S`TN)zJKR=kWWb|m1{LdDbE+f>!5ym@)mYog)H(Hnw*x%|z z43exJW?<1a0?pn_PlUjN7+nZ~4aey&Y90YHmiDSQM+%ZZWL|lq{qW?4s|;Nq!PvZM zN$KFcjYdFcAnG*yZ3Df1V?z-XA@AXE^hW&&U+b-nz@tQ>%x|8wiJn|RflW~S8#9L~ zx=}W!8DJQ`Goa@uu%n0k{;VSP=q5KUg!rIk$9Neb&oSU}kqxXz*2W&f&%x7!XO~hZ zbP`OU?TAm%>Er6tsWXC;=18f;zj}voZ}ek(_6GCzy`CPi>SE+vd%*FS>m+y*=3wau zY>c;j{;W*2pI^5EOxxmJsZ9}zg6QY+q=jnOO^_~{#HlzSH zI>Y2TY{E{1Fa$lCHHhJ-5fK;9T?{k2q6@#up?~)D)}ngW)OKXqF~{4R&;D4Pe%pkU z5(Qc|ik2B|)c%#c)y-{#U1i>=<7eWAc+0n&?|6@XJpF9Wdl!*hP?foHSE!Bt#E26W zJ9F}k|4~!cZ)FG9bgA@sed^`WCZZe&Bx<~$8GL<1eodlQs@s-4Z*nlDQkvKMIsI29 zJm#)Mofg|$?SPNw8Bt*kem89?JnF?*<%@N6g3gSJ$6Es!TB)G~pN8~W+!+u?x`D-_v_KC|mv36c`>KU$(` z0kowrbGs9}qv^Iz5S^NTEYL2WM>;KJxcjYDyQDzQ=;l>zer=Ecqc#z{L$-Uq>j>|b zeQ#L#pVGembR9}U-wGZld;D+L-QQJQ z5Nq60mASm)T~@QT-H?3iAd;!YK1u(k=|6uR>34_K;9vQVWqCb#xY92XRjNLWkd**r z&N_VCC8D91+>F*Np z5u|#lCw!f)ZjeAv=s0L=B?$K>Eq}8{`!}j;l6CE_L#<5mxPOSo8K*(Fd%MYq**r_3 zNTUGKuMZyJC=b3Y)(hDOSCgAucS3_BXw84|zB)~wL##)4^?Lo}LIkYV-`ebLF?pHo z>IM!DZv0FCrpr~EwVtef{39%Gwm5&ya*Rk|Bv$g&gdIoBzvdrYB}WExT^VmPKq;$j z2x2afHIOyHUk{Kqg@f(RrPI$>T?{s`M*sNmu@>Iz`nCJ{CPxIl1;!BeH>b_U-r2Ux zbZD1)7fyZJKS~a`A=xt|J}wh;fR|DnF!wLKD|3;a+|`5ecKl8yZZ>_=i(zP%M> z6UN;Pim)P+H^A|>9g`ZN`=mNE?3v(&!weV($0`21)W1i(im7jJ@2pN>iy**Hg6a$l z@P0OOq;@joP!bUZ>2{_Vao6Jo_%QabUm6RCe#(OqhpClXEnQhAPT4phneskasX?C2b=#dv(QUe}NrylEKu2nw>!`&g`a}K|5ceY^ ze#y9%l@we@8geW^g&)vu-F*z`a-t|3rJ=H zzx;Ay_fMDeecG!hD&iJYpNsp~J965!k=nE&Zs{pSHbL8!b0|X9fky`%<>ZvJ6u{J&=DZ6OGX_9;Yc!fL@ zEDyQ%H%g>6mw>h)X<7OqLew~2A+BM_637xtxRRhzbFq)ZTdW&|_G!7S8L%aa+^iim z9s4*P`^%G=Z&YOI2{e3Kr02%?1_CMm8tf{mEdlJT?M1`J!eJ7Q(FnlPHo+F1(lYZK z);E{^;EuA(=csPh!~Bo6;*j4qJ;;BM{osZDVfi-T;ie_Iq1=g~b%xYe>^uXF6^%6= znf1#!9B#_3{P3c#)U(p-UrW$RyfJ^!Sm@V$cqvSJkZ!ulc-Gnxu(}u{DqD$BichWd zzxTh-54~rTn_GZwb|wej6lj32reNzP#H^8LK!k-);Jmesv7JNE7|EX9c-eRPYm^l2 z0U=T47#1n;vZYI3uKV(?wN+&**h}5@c1sEYPc&;G7*c1%8Uxl;-7WZT0*7b75aAH@ z5&Pr*F}Xq;BLE>892^omRTC$2ueE!ng#T4SuxgnW)*JcW&ls5g8lqpKbez(z%OW#6 zM>TEh0;UV;eV6CNFt2P9p=xf9xm1umz(A5Pnm6Q3{$qG#EQt~!%%JDnHx)SPR$fH` zi6>6ED?`4FQ+Ep`D+(1nE^7{E3nV0$TxF=Y+ml_6?6>%j;D_8u-ncTctIVz!gHur% zM3nVQgMD^3_Ei9chh@W0RlIwuxowi2lc9^ZT-+h{!LLTb?H#=&yjZphsN(HOH;ioK z?w+Vz^6;aiCe`PiPgjDL`}CRNms{qhq}=Qdx<#*y@Il5xtb(uXIFkzo6?*J6LZYlp ztdb-e6uoJ}a#p-5r(ZA!f373&NV~RM%I(FRBR5tqbGYi@5>p=1rozUv{$GQ zE?2`5jj(5qPP7Qi|4v*M4&wGT1)^w8wdggAi93?+?}$_IK8eSFv!ud2!z}f>Rwp6J zU^D3EVor;r5z4nj(5BmtN1sA{MZhp+CDMD(Hs&lMhEu3kF51gU!GZodukbpvFqicu zh1D2qubxDY-iSQ|>6FA7=QCT(^iF|ImJ>vLbhVZle7Dd~iC3=8KLNEJA*L&)6%+cn zPy|B@2OP%l;YIi)^HZ{-lrZ-kB|2_`tPXDuf(G2`0+8a6xLm&a3B*Dr0j!UX?HY~3(?$<$nfXO#!EwCe^6>|}d<`QM z<0yQnzd1?Bm-0SxSLStgoyb_X#KS72g7{|K_EK=~=WE9v}H@r$JD4 z_Qt2~kDO?;{h^;2v3hYMzj`JLj0(#e64#x=e=5Rw9$yS*i;I(Y5@Iaf%wgM`M%4I@ z;XA^!^Cq9AGXZtSjk!^4+UjA^O(}zLiLCD9;Qit509a#0&N7#hmUgrI%}(3X-W70{ zu<9J#R_pK<&}5O#=C9Io*sc#%Ggmv)2K6eK!lnZ2999%*Az`XBRYtMle#L|z=k*+C zuCyvh6>TtBn0A+A{odk#&`=Njg{No}sopXN(|2WV%hPEWH^12Qo*6gzs|9kO&FVsq zIF-yF&E#+?5-xc-Z3n|2IS8RnK+m_<7^naR`fnM$6y?{xT9=oPWZ|G>1id@p5!~CGS=>Zr@2H74G{Mc1i@U|J#}a^x6of;gmkF&HXRDw{43#Xe9rc1Vf7v{WY%j%F1sRpfJcyF%(bCiHBOrj{dC%x<8%qp0+5=y$zmkPq@V5VzuI7;gV4eF+o@#dkAaTUZE_ozu^=bxd)v^i znf?Nk8qI$S>1Qd*gC9;KI9sWKqEkhCZ+h&cnx{$?N=~m~jdP_QSB6p_<@C9Mk$c)& zXMDc!(0N$SH$_rV1kh?A`B0tmXhRxcgB=`a>zp=$G-N-87w%iPakha}HGdKI)1{|@ zx32+_-JF3TTKBsah|R+l+f(`=40QqR6(Q=v@zkizAWZQEUYxNN?oR$@wKhjL=`Y84 z_KlP?|3_)a+P{dusofZ->X@VYHr*-5vzeuhF7B2s28hs%sI$!-CNQ+>nG1m_%9)sx zSGhZAcyf3Ojf4Al@JZV?sB62Zrtrzj#&K zChk)n<{8$jX@yf2Rg|G~l%dAJRr>RiGSim1$vDQu-);{X6L+TcmTPcn)u44!4pxu! zxTTpm-=k3c#t(&Jo$LY_Uc9$#daJ+Oe4$?63 zrEhMJ^x`g0JU}=d48NT)wV!wkB`#uu?9P=CEXh98Ey+uoAaU)v83@$`D6(J)MrUrUU23#u)lB4Wjzq;vB>Gq zb-?QmNx+v&UwAt^b1kR-VRXQd?|xjuXyIe}yU zm@IU9ZlHG3qjZ(^`shco9pGf?I2}gmXaqTWC%T42)K~1dIj40dCa#l8U1GNh+cOT@ zvE2@5dsN3EUV|?_-LRt*0RwiFHS>4*dy+DoSTn%pzfvv5XBj=JGv6A}XlH3L5w_n= zKX0{M52aG6J7YdOS87ejE9rl^F8TJNIY&3%viP&9StZWhR3Z82oYt)&bxAJk1{KG^ zxTdZ^U5XX8-15iNw?*$@0!5= zRXyxiD<#Ot3mANtsP;5kRr{y=zSFB35=e^Q_}@TU^Byp^_4l6t5QbDqqh)`%=f2(A zXoK)kzzF!1_zJki_*7p=NGpOAkPwtM?502fI-Pax$iMfiboXl(P$&jXl;TKCA(BKp)!Vr z;-cOmqjhLeE=g)@xYTA(MH$3{QG-LQgb4V`XBdT8$~LM2FThtTG%&5PTd{7-=ltY2 z3p{G6wIFeC&;@z>HOx~x-_QNjHSh7AgifY*r!3(b zOQfc>It6V?aTYw^?qDa{aHTZeClxKhdd017y9`a_=jSo${*ydh+WBnwsgLa6>Qxa} z52_NX6B@+BidqD@L9QO3wo$`twFQ#|9RR1d1Bi+e#UIrxYQC353JuU!@+hs$=fc;1 zJGcM3LzhcCt(3tFdG#%)Wd724QePJf(Y8@)zz5!m7Ep@3mVuAS-XcxgBV4MOo}?&X)Am3p&6qZ~kb zZb3VLr65eTT--&zYnjYdE^y;n@0#4zMPSy6dqkT(%5Z!#J}4|K`M&=EU5ve{Q@uJl zqbfFgz|1tgJ$wKT4QBXeL}p|3VaD8}q6RF2Y8oxQRMt|}FQj>PvxkD=`pm6@<2~t0 zKVFrH*tQW4E=z!enInBvgw}4^4?7@I6RMkL4RyI0EPm0@cWMR0m|k8kS@BE>v~3*O z8Wqy}jqKm6BcUO^H&n7&XGrE+Fj;ngoPnRgNVB$zo1;azNNiM4{Cfw(*X5MB*X5KT z)>f(W|17lVe$0MdPswHlLVBCB>{i+JLwf&VOo*HN{;zGXYb`l;Cc*!;0lvG7Yy6fl zSCVy^IHEZ5zm{N{Kk{mzMVu%ki);`~xH1_CvdaBIs4n_fvC^3%cJ7+J;ScIne#okf zF*z?_I)2ig;IYxy25JIFf*y>fAB0b>8!r^O`>I-_QDV%9Q(kXU0MFXHdgy0N$_aj~ z6L_2vc+|Yn%xH3s9Y3p$jVV>pxv$_zT7pJyX;NUuyM*}E7|7v)COfoWA!QR65nN4k zMEfZYkzP30fkj|lq7+Y=$CArO=z&+7BKviEo+Ew8d70fC0;smo`9PnxP;5C~4i|Kl zwHc!|%%>4DQJPwo0WWQkFFo0#{%b|h{(gPx=A=xmq-nWiO`kC-oO12SmHD>MmGuBo zuzw#hU-J3Vf%z-F4zHU?9#T|pX#MzU9}rt8oQIUe-Dk0nufjKgPO$2L2etRG+}%cB z0Cqyr?|9ED2{ft!3Y#v>fnC`b*=G`)>}2aP!~(c6@UI4Ej>RF&hUnNJq2eFS2&5*{ zeh?k)nx(@2JMfJE1U_*J&EY>Agb2kfga0&@QoG!L1@SuD{!f!gaF7WGdP4qy-_9O? zT`-H=rmk{LAK_VsTNn(_mzF%gFOoAZtOMohFi3kDRK}$ZOjuG%*XYFW zcNGw=5oP-Z_Lurn2Y-{y?$pq!JLxC|MM-j{2QBy!kO?XQ@5FZ6q6HRza zxmm_mXV3?@4a|>@SGe+nI%8-74{12EKMR|bSPVNAs9`QNG6rW}uZ>c?rYeNm4;w{J;*2-t zrY{@y@!C~Ua!nJSgAOlTkM%j;^Zu9wz(8erU+g2UQF8v~&IOxUg8=wy*t&T>3O+(F z-|?o-mIZ65cs6BdqOQ}Q&)R;=rHir~cGb|VAeYfbWd=ThdRT{Uk_hs)K31p-EMOp= z&G063>M)6=CVIGvY?4!C%An7(ifq!7eu1Bd|7tEp+kZ6|2++TR$13rsea3&4{8#L7 z$^MC5`@dpGiHP>6RT=C3D|Vo_A+Pty{MD>(byyg&@4hXH%I&bwYF7_HV?bSF6@Df! zAFle|5sOE0ToBnti6l7yF+kyhu{$0 zT^4tDUEC$OJHg!v9$bS6z(MUDeM#?L(<2 z3Y`uEvO;TOIH#V7qO8n;*(K)!A&Ub%_X>W)3uFo(JSOjj`UyR3!a^AFax8W5?P#f< zGlEeCUmoqlAG`g^c@eBXwu^RvO0)kdF^zS zwS&|3L8R@|Q0~|WyYGpT^#t{>?DS_NXsL~#w0!K$V;vn~L3@XMs8=^%x3D!~q&Cr= z$9Pq1X!1V%KlXW+zxMf+7$uf-2j1(!Z>^SAu2hoDd9sE_!V^?`TaI9-f>ihnYOq#P z9OrrYTwJ{|e=WSyU`YyHfvlpX_@b068h5eSQR~F!#;EhQAuO0IM4PN9H~wkDgQ$n_ zt*~$cT;TTOfi$hpABX~dc&`P0wt@%JU3&$fD$qAMTPvtyTu67Icoy2wFMGkSl2>RL z4F9;d1ZV%n15Pm5*YT6I)kJNC4|3FPrICCyl$^G&N*MD;P`7Fhm~<0mC!em25*y$^A0A|!{2rvzQAdAvKyxY13 zoTqTY8&OYRUjO51rF=?Ki$sZ_?f#ET78v4_1rxD}ppB@lLa1%Tq{~y@8tqe$VT!vt z=OLh*UfYtbYYkPiFv__g6T2o)!)^O_)|(EcCTIE6*-<#`#ywTNOJ8g!poVpJsM=AW z4QjUE_l`-arZUpzgXM5tS|U>gLJ=YaNmxbpv@_(%uiBT3V1KFi??iQ>l)z9ksel=) z_nlz!+N}!oU(rR$&y_FIjgLHo)wK~27pO`rwxFfL z^XE!Ovd5uYA-CBS_YBoS)mKfm{#k+C{HjI**r}n=V?5WQSZ1l*54aY-DK0D4HOTIy zBsRyenHPu6Zpd!LijPC``e1C9OubB+SGRhMGh*2sOK*kiR2~Pdh(&!e@pR9!AMTAw z{gcuR-Q(JKv87e6YU9NAJzqtkTyrf!z80Pl73_Qu<5Hmz|Ip#hx)%>x`D74O8r5I7 zf~h)EITot6=%tc<^|)W?)hOS#uqvD7>Ue9R^O_$L)CKY~b5wc2QPray#Dw)QnHzet z5H~ORO26SszYewYj9NUMT$@ex%#{NJyz(TL1}CgfxKwzv^bFMIhSeKTbgsjy-KZGB z>C^Sj>W*qldcz7Deq#yRdq|@Y>eO2+!>pe(tuE=#8BIk=r@j381k;E$yBb`2J}e<$ zXLjrfYgT%+P2XgK!?^kq%?Pm%|HWnM4_9XtRGgBKdqW4$L@`B}{bMOY@P1sC!0y;% z8iGkMhl7KInOFdw(rMe#jOB$OkCi0nV-DGuqXcP9<+kYz-#u^PO9X9c_%rQ#`*l{} zWQMoG6mB`I$R%IR$KRU73z&}g-zUjE0f-AscTx!)Nw^YQm)ghtqO@AL{;2c1PLuSQOs(r>H_*| zvWlB3y&gnhR$#nRe|*gx~8RAcaP>P ze*HPTr=)u5Q{l6C0duBREk^iNq@-EU+X?diE z@5*xl`TirW2H#uhC}Rb?JB&H>m;HX7w|mW3V6RLjTOH{~yrni_;ZeCl{w6tc9a(Z- zrjeq={z)2!xW%}ijR4W@Siq$KVm^GVKRmLPXVQQQb0zVRJ zd+SfBrtBw?fdBZ#K;95zr}y@q8O6dPpG}6^P!9D*A9vb5#@}}FT&`mtX5w*C&(h6; z=^#IJvu~%(BI)EFX7ISujtRbnbqFSb3i*3Mg(re*$Gn$G2xew9vQOn86IzQ_ta;XX z(fN0+Y+0f8>8<{0rmxrAVN+t#%sxJAJiqRsP$?};7jn3aXUo?d?VA#I}iez z9s4q(42^Gv5=1tp?m`>y5M6>vP$WbwJ=QS)R4&$7IF$kJLq8R#6g1r*lIQ-ym z#_ak+d0h=)Ybpla<4jyJ#h4=^o4+zVy2`3MVoqvq!*+B5&@KD@{A5X<~~}py_)k8h(I^guN1-BgrdP zg+XtVbVVTZVQ|GKqL0*8pJR8mxw3rnk^H7m%QnCG4LFIVKRs|RiMtDaw^FE89O9ks z({P(v2J2w_-k7ox$t6xa#2u5!2I|?hN%*8exn<5~yI-uJFZfBJwSVg&)fSez!1Lby z3g^VYr7aYH0QtG;t@Ce=Tobk(J{05D&R&pA7?n&F0zN$6Np<`GdO=z+a7TPOa6 zcgu3Hk(X0Vk(7pr7f@s1;wbcvi@K#5U|?{cnRBx|tc6oX02eFmQaW%;}mH#E_BlzSV7NKhvFpFiwHP@1~vJ%4ZH${yi6Nj1Kr z$B47Oe`f#Hi$R3)8I+W3!f-UUHnIEu&3m)LQOT3X;Dl7~M|5EvxEjt**=g5=?OI)ZJc1*j*$X%|$Q^uMYMg0ow4t%}X(V8XSUR>~{& zTr6>Hkgt+;m^I6GwCbAGkr|lpVk0m^*Vpu*+j}{dV$76Bo&Cvt!LR9IK zvgbsD+dqN#Y4-XgHgpP{Q8Du1M+Q15_xcc@U2=wkVt?S_0>U)ob=JC8-fw*h2+6@O zQ#1`2%wsSE_v}HPCv~lI0-P|uDFHfROn8DuR>i1QlIq}tU_ALh9B+$pomz8jTd)^C z5Pe3s^c%+jT$zOjo=nK;M}NnPnt#syV27Kh3%4Fo5?T#*QS6>u>B zfU;&Pi5B@)iWC-KBvLyqzERdl)&SZxhr`h|83+t9+U>a-K%WEp@Q*#L?e|QibqZW5 zpp%lJ6;u2gXb23Eqa+2IdXd0jgf* zbn>Vo9{{=8Pk0WJAYl-^t@MP#C~2$NheEEn0bxAD5gC_#hlzHs3=a6??8N;NbQ~0Z zmfDP1N!ehT*5pXEZ-6ikG7EbLAw+Gg*4JuG7Q5{p^e|u)RAncHKeaBpI|18JQzST& zXq3PP!w zz1cVWA|L`ZY*V^CA)SNL9drbgzIg3pAx;6tAg^qofEERA3O6^YKHgjf^G~o)m_FIr zrm+aZ9MYIfgao@a>=*@G80U#J)+HY(o@yVOz6cI|lXf&Hq!<57YB6o%Bg_{BEPn(q ztOLOam?XMvS^7Z~v7|88QOrz|ZM8Q0#Q>^3t!`9?-l~UMUd~?$)^!Z3fYub?+O8as z!z!YZ9>LKfXRW-hr9={JD+6CDj9yr2LO__k7m zGCOz{PfP;{MM*DS5#u^Q+-hIeD%4)&^dV13heK&1D}lwvlDRG+!UY_G$F1f-=n^%r z4K1zv7J!>sG;RP3tyA+6&qHo$)WTp+V2H!-N55!^=!M#TfG%DZ*u;YfZn32kQ z6Ol!I>aOn5nHHM;5j9R-eD_ArqxV6QD{5)F$(j&0w%I5T8duFf$@EcSD?Jm>$C&~T zMC^Xqqp+!Yid*T*iVQBd`8lEK*-Z+)78D@t#QpkOI*gdGv_D$5L8>V2ftdrjay(4coDTV zdK9#Dwf1Ukwe#qDo&Xw7YwbWZ!9IqpL0%2d@K&=ozj#3$Jn4eKkRJ)(Lzo1Ou|5<9 zpFv~Bk@DegwB+`5%uulX19`QdYbLtGG{bhRf^;|L84ny&<#D^Q z$*5p5(oFsz;MRifA8Kub#zph?Jl?;okuv)H_oEteSkW4d=OWpKap7de-XV!=g%^tT z%KK!<_>%_yS~Er~PAN`ull`n~Fr+xnc=M6d+=j;k7?hO`@(Bra{4tsthokCL%W*1W zGwm$?BLP@Y)>2f9sAU~s+Ax1@w?l>7E!tts73qqY0FH6UMypLn7mY!HeeFb3+RHTgl()bUE%}Yz zNH06KetJcKu#qXu?pqeOI)Qa8a~W>SWDyT2Fcd5UdSk`0jpxTjfN}ckN-L72ByNg7 zY%X~q0D%2FdH_Sz>J8o?=m^3*LCMS8?oEWnfk(-Xrm_1}MDx@5_&36aZ@zS}GN?Bt zNl+m;7L6AZP6yM<%z--!Z^$qLmg|!@88@jJdj8gN?x=Twz2HwKD#-AFBqiw+{g1cx z1NC6q7M2VKTW>R^WgJiN zm}O8}h`taIHDn2b1{whxMANWu`|&XnZDZD#%czSYyr7QDiE(fgX!*Sc=CGX9C4omhizGP7#cp9?%i`WfH4xcTclQ?@kbj;G2?a zeRWVmBkmaeE-B7FqEQS|Vie)c492&Hn0DJGJL<>E`nA!ylz(Xbx};w8W~fd-n1@~= zQEZp}J5<7__}lWHWHdM)+P1J2z2li>3OjRMD;OsB++KgtIrk1S{Vxc;hCZlRkp+@q zVNZS-t=KsmHPhoLZRv`<2v-DBBu#{dpKAg`x+qL$Lr9nPnUs%OIPE1#FlA^Uf)M^^ z-;+6{pYw`wo?m#1R_d$Fbkc2@c?D-cx*_t;pe!8jAk2}bP|T66_%1vu&mMbJUg1{I z^p2xF>7`Awm7-T7D=NMC@?dTx6psd2bEi|q<9&b=X7wEWv9F+&*qu8{2y!h7{qO*K zuxL>ZdTul~<9Vq*Kh=mKj~Fx*YVcBiWO!US3Y=W9s|rD>s`&H+s6_gz#mFEVX;ld2 zCjWOXD24;IGx1~IO;tU?E;rnl3`<-z1;iGfuBF1Zsp zgRa>3h{4696HP4STe(2CY*Ou*er*P?zqU?WE@G>(^``V`eTI=%CWd%&}v2--< zn?qLYqV6O^d*ngi&SywOxUj`F|K$JaDRtn_YOR1Ru&b&ZW(R6?D zRw;VbH+r>>lFY?|9OG4z^FjqKspZ-NUHt@0}Q}j%{c|Kxc z;HByj5eV-;6VFAH@ji~fjQuJ%*3>9J0(P5t4Zr+@vFOYFc?6joA9dL8P8BfUKa+Iy zByg=wH^tsK>MSZqF@!QUuTGyYK&DY9{U&$1esu3@q|~dS^RcQO(Yp7Jr&%>*)O0Dk zeyf=I)Ubs-qr3crizw5-c8w22+3|ZaxhC&@HEdM}PT89Jd$$&ft`voLdlE2UEv}h0 zQyN0P_hnzzQ`{8ZRl}vkCmE%{E`C%`k#JymXT10x z&Moz{`Vjit`B+ZKgcD#eNI^8v8?#xiw!P_A;wU#FkIzPJ)S-_}t1W)hi&MKcB;Hqh z6jU!71%Rv7-rDkrV49UZaO<;?;iEbhiU~*^Adcx~vNv-z+HD+~&PuEW$6QA3zc?SL z(%pnlV{@v^eigP&$7%ugX=%2F(AI45*fw#%Q}kG_?3(4QRDDW}qdHszcnd9e^~QKZ zA9&yJB5lRXw)_dwvinpSKJzcy)@r%dn1u$8YB-2|z+^Ki6ONa)bfH+R*bo$=v93BZ zn)EL%AL)U`@IWnQx4td<>=?jFsr86@!{NLyo-Mwf*Jqyng~7APfFNwc*TP0lD!_T`saIQHIH}AaPCKV1V>z|hFaa_P6npD(A^L56Ff&n z(Q@S8waT^n2)3>f_8jX-s^w<8pXVhd?gWBW>6#aEo?Z^_E~HCZn@85X{e+kWqy1b#bDEaz6ddQwd!W;m%n zjHE7;;i-I=+rLWO`p%Px?r#2?cZ;gu9`X})fv?Xma41b8rN3SKtbRZpmO!(2qM z+f(X}yNleK9aIt}zrn%+(S<~=G(QM+jaa~CXGbY5lZQzzPNe|G zF7&p!&*}!g=q8Mvn#kA$nB=qI6t);I9&B&h@fNu+EEe5)fnT-TODGHmi>&UTsGmvX z4+-Q~HD^dHN8wz=;U6caj!8$7kJ7xQ6qO+eLIbWm%ePhkmT$B4wkF9&qh0RrI~AFQ z`|Kax6x3Nr4GD0)I-2e+z_Fx(yggqZDMb67V$VREY3i{Sd>F3{D&XGS7}cGdo1XHU?8mHg_lyr5?*^0Ur?dNp;tIhQC{9*`So(@H@_nb4NjNZMtJB z4HE&|NKLi&{dkQNZ*v{qgU@O%5qIcbIkX6{WI$hRwBb!L;0-mR2ZPH#upS|Bww}iLH(R@vG5Nf`d-DqcW zpwsR$5(5r={YJ0;E=M6zPclcM_80QnU81>CcFJ+Kn>*IpiX!5#lEeJzEs3^|{w0RY z@8vRwDnp_T8Ut?ytxzl*D;o&1>KDKC}%@%w;J0Nyv_U9*N|4~Acm zC7$SAou61;HfO$Ut-kObE0iud8iDboWU*_bPohuy{7OUg-y-k+B^)zWex$i zlQI!nM*70FpQ???DRc~)EyFC=jz0+v)R)nU7aSZCb9lB5q>b3^u!G2Mh z)Muoj-TG}|d^zpE&5(aFYi(#ns@2AkF4n2-pRK~zZpn5Hmfzmo`k^d+6HSxgS3Tfn z<)si0{al2qGrrQ9Wo{)NAeG-n;`wD|R97bXi+eN_O)B?vI;S{(-B2^+I=q z!<9WAef?NRi^@Rp;Q71Ppx~~wIpi>tf>gwXJ88XGz9i%nU%&`OcKH491GvkcVl8*; z0M6L^ZzL{RU^y@-=&OIIDT>%KWNMwgJTnfz{J*Ex2d4i{t&_%cX?<-+%?Mzu0r9R@ zXY+a{f49r8;L`g(?~>TUL!cockUCLAdmFPvr(T$-9QY;v!$;JNA5cumV(G&PRG0m6Zr>Y$dUXP9Yin}tQ66d^r>nD zzcqIai!`lgV;|Pl<>2kxCWK(vj7)}RYW8EIYhrt|>vjk`d6SofqpSK;02AtZV-Z8?)54>iZE9V$D=N#Q zB#&zNd5IdV8 ztb5a^(St6VwuHQyFIfNz>n^Tj9F8o}1-Ih5*a?omp+dvsx|iH~sa=cHw(2>W<6^0r z0!kXk01hh!;@*>As^Siwv~`B5;vLf9S))o{5;uEE19c`abR!gwV-TZB%38El{*7Pi7Eh=0h0)ITk2#_b&zeqHz9!90V&j`?O;_3F;{LYWAQV8bl;8_XBUCt0g{DBSFL&Z)&ssz2z$kJ*^Cyfpe2W^ zQ=BTDGwxX$CKs-t4o5o}UMdUsX4V~wz0sRzG&|vcgHRlZKYy*ou8RraMTBXI?ZmKt zxW3e7^^^_k6m$=&Kax{E1`w9r6kHoTW_K~@Hn6m5RQSAqVcP*`Yg9aT@~G&=?8S$$ z1Af)EUp0;HBQiGXq=oAgg|q3|$ksT!T|-8=55jlHYd=ztkzcmOyCjSdK?7Y=sTf^k zU{`&S`jMRb0BDV|baNuztQn`oH+60tsMs6~)h>R{Yl`h)>*Pns8+?`06%!PzB*CDU6KBH30s`CHs66er&lRkGBxZ6kQ%d`Q2!1?a zgq}U+Il=b3$4s#gBaXBQPR4z1ltZpEav0olYjSy@uH$!8|2!zG4@C|^VJV;x|5HvZ z_xi1Fm?|{In_5^Ay7ov*9#Tv~v{Cr9Av&Fr09C6a)lv@h6Qnazbz) zQ8WU=L`%)~(_&CFr|HS8Z^49z%xS|5!wBNlm7mwT1hMfs%0W*nmEK|Rz@2o+eNYZJ zcmbl?hR*#J=^Nb-#+Go$)852~-VQ=W=SK8E$uXj#a9l&~jxBK*4#G<*J1M&{X}huT z@6v0wykN#e9Jn^9yhV*cbGBh%I;-LQSzRlE$P_RIs#$OmqmQ;$!4i z(^pKIG%HjK2{I}CKd=_VFNgj(r}k}nUM=!lFsDaZQaZ=VO@PMeTr$q4?`Sl~AI!1{ z>Yg@jr{z+>bfhrYmvJf9^$(@Pf7YSjw&vL;uOiVBP}VwIYGR5tFZ2&@ zR8$;@aG9KF0Doq#1zBh18x0mQpWn$~A#0)Z?wuBV@9;;Oj8F|#HJ69K)J}{s|DlS0#=gcUUZ2Kfwr>j6rs(Ez(3f!-YqjPa_g+c_9 zLg_-U{EX(YW=8uVr{e4H`5tjFf=ZjyQO_uJ2a3q93|yRIC(XYsE@s~48?G2Yx2{e4 z&BRA1Cyf3MRU^e!vW{{Izpy!+RuWfR^Hx3RJvYEB#0ca)M`?VZifLL}_3?|b-aAiPSWjZDE;(g|8ql>GlP9j;xs!o?} z{=uq+ATa-%wm6%zxQlRPJv53pj2y~UB`RrRejhHJC$=zmy_OHhR?v!b8b9JG6Hn9? z$95~VA^ckUa^1-&r4YM=HV*6d6u$A&WYyB*m-c zR1Q4?Rj zXe^i)(7#vbXk&}$rtw%Jk?9_ovm zle2ibM?de|5d9*&$_pAhp->*R(+enwrp5mEYniI3u5@1t)&No?R4u^%asZq& zSLOl5loW4cG3f(pTe=6)5DDl6GsQ1+PG(3&H2A!>qBdH z7(2fR)zg;s=0j=Y*`^(hz70sI6r-j<GKb#P*oBYs(?gr4cj=G1$a zt%ttTH(_sJCE1jr!$R6qjPv4M;k+!te(Z7A%h4>{yO*YGWj${8=#YNfWvWJ9J#|4I zfp^gO@te`c!C6*4zBoNg{50;vPX<5neyS3DDYi_EUJncN60wb#gae9gZLo0~8L? zq2;Y^_i`Q55|?N^dn~J+KgxXF`fF+ zO_ZzwOB6B+ELJO^7n-Lokz5*HrDSYa?%sIfLg*QjtA^ambI@SU`%%w#)zxb*REN%E zTAidF>t?gXAl!GVVz{aoZ#B_Vi&TX3WeUp9EMepF$IT<&&aUZ#RkY*{hw((_OIaVB zJaOn4{E6{qZQXWT&jv~K{X#EwV8@|<1PAm}kkj}HQDFau`iT2JF@o7`0JCK&l@5cj zRg~TeQ=wD~s|ePH4X5=2q{K~Z0afEla`?9+K-F%e>0&1!Z4MCGY9b6y^27e=1Gu2y zUIRHNnkR58;I-!i3S1COC(@s8b=EPoEOjnc9;}7`NhbtY`!^3`dfojW;a&^0`k1p? z(~NWjHt+GuU~R>}OucdBYpj-fGt?EU_fKgr`k)3$@cFGtZ#Gqna~?)M#-XelOg(3<60iEwl~tCt?Kepqn%}LN3T&(pkd&Yg~7w# z<1s2owjmE*m*u#0dnFeGeZ|1ufY1rV<5wYPDbGJK}T}KAeXmnxno$|ZQMDzjtQCQYy~=clDE(I zJge_&qk*w-X^pVy5_$>|8#K6>s1!tE+;a^!JJ}>~T((JSBCm?9k`JtJRy}fwx!}6cU{qwm#6!4lJ8}!EIU?UmoKVt=bwTEkNfgG$A4`%OnBAiUN>LZ zdCv;^fV1p%nA~UyoVBNJ-I`g1ltn3vB*Y?uk$_s{P}Kp#R-#FIR%ACQ24A91?S5{L z_B0deS};F-Y030dRUjqPm?nYYBzdOKYoe)X1P4ej4yyZ=i5Lc26O zqY)`sneSUHcfjmpa-pc#TM`sI_pn;GAfPAW494f@BMzhHG`tuE!6xJnXR=4uVR6Xj z>}QT#xb=J#T0tYUBpCIcJd&#T=A5LN=&=lGnpw``u@(4#)%q-zN(qO3SY%$KHy3H#p`Cq91@kh56w+bXT;Gwr;1l2x zOg~S*zbLjHTg}MlFz)xm<{ReHYf)j%J6@}unjiH{G-jjQfB1Ge436gYar=uO1zKf= zNgaA;vuxXaPvjuvHK_Jr7SPv?oAo#e=%%%4J1RdmK3~LAZCi70OSOHjM9Z_0g z9mNgi+F2TBbSdbbefxSyKmW}T*1;dD?hs$K94mr_t7LEq^}& z2%FNKdobo4?=kMVM8zA3*^fYaOIDhx-ZGnP+g-TnX z=3n8}7RdFlK$)p+2gLf%-i!+n>2LP`ux)k}+W|9ye~$|}0J+d0*CCTM@9VuL4gjo) zX9hR``L+L~t?U1XuJaF3>wh*AiT}{Hswx1XZ~!=ff2lnGn~ClZmFIspXQ@miM<9{# zpS=IOxsd;(0+9yZ0RZOCP9By{9)H+B|IPm2rU5wuiJ@G(GXougLVv5!4M~7@Rs?4H z=>S<}|5g9@Y7QNVy%LcC09r`^0P^==+qgOq;O^n;VELD1^xyOTEk@Z1QcR*zro9u8 z{%aj542`fQ-xc^l@0HE9?6X*)0gA+A{90vRkTO_Tl From 82b8a42929e5bdcd53e6f8f7b2ee6995fbdf0707 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 1 Feb 2015 10:53:48 +0100 Subject: [PATCH 268/269] fix timestamp problem in dummy pos estimator --- .../ros/nodes/position_estimator/position_estimator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/ros/nodes/position_estimator/position_estimator.cpp b/src/platforms/ros/nodes/position_estimator/position_estimator.cpp index fae304bf49..ed3a4efa5c 100644 --- a/src/platforms/ros/nodes/position_estimator/position_estimator.cpp +++ b/src/platforms/ros/nodes/position_estimator/position_estimator.cpp @@ -52,7 +52,7 @@ PositionEstimator::PositionEstimator() : _n(), _sub_modelstates(_n.subscribe("/gazebo/model_states", 1, &PositionEstimator::ModelStatesCallback, this)), _vehicle_position_pub(_n.advertise("vehicle_local_position", 1)), - _startup_time(px4::get_time_micros()) + _startup_time(1) { } From 7c958a2cca90a6262dc491fe9ef86d85bacdf3da Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 1 Feb 2015 10:57:56 +0100 Subject: [PATCH 269/269] multiplatform pos ctrl: fix division by zero --- .../mc_pos_control.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp index aa23e0a60d..a46163e6f6 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -227,14 +227,14 @@ MulticopterPositionControl::reset_pos_sp() if (_reset_pos_sp) { _reset_pos_sp = false; /* shift position setpoint to make attitude setpoint continuous */ - _pos_sp(0) = _pos(0) + (_vel(0) - PX4_R(_att_sp_msg.data().R_body, 0, 2) * _att_sp_msg.data().thrust / _params.vel_p(0) - - _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0); - _pos_sp(1) = _pos(1) + (_vel(1) - PX4_R(_att_sp_msg.data().R_body, 1, 2) * _att_sp_msg.data().thrust / _params.vel_p(1) - - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1); + _pos_sp(0) = _pos(0); //+ (_vel(0) - PX4_R(_att_sp_msg.data().R_body, 0, 2) * _att_sp_msg.data().thrust / _params.vel_p(0) + // - _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0); + _pos_sp(1) = _pos(1); //+ (_vel(1) - PX4_R(_att_sp_msg.data().R_body, 1, 2) * _att_sp_msg.data().thrust / _params.vel_p(1) + // - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1); //XXX: port this once a mavlink like interface is available // mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1)); - PX4_INFO("[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1)); + PX4_INFO("[mpc] reset pos sp: %2.3f, %2.3f", (double)_pos_sp(0), (double)_pos_sp(1)); } } @@ -247,7 +247,7 @@ MulticopterPositionControl::reset_alt_sp() //XXX: port this once a mavlink like interface is available // mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2)); - PX4_INFO("[mpc] reset alt sp: %d", -(int)_pos_sp(2)); + PX4_INFO("[mpc] reset alt sp: %2.3f", -(double)_pos_sp(2)); } } @@ -557,7 +557,7 @@ void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti static uint64_t t_prev = 0; uint64_t t = get_time_micros(); - float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.0f; + float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.005f; t_prev = t; if (_control_mode->data().flag_armed && !was_armed) {