diff options
Diffstat (limited to 'src/modules')
181 files changed, 8280 insertions, 9550 deletions
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp deleted file mode 100644 index 668bac5d9..000000000 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ /dev/null @@ -1,815 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012, 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 KalmanNav.cpp - * - * Kalman filter navigation code - */ - -#include <poll.h> - -#include "KalmanNav.hpp" -#include <systemlib/err.h> -#include <geo/geo.h> - -// constants -// Titterton pg. 52 -static const float omega = 7.2921150e-5f; // earth rotation rate, rad/s -static const float R0 = 6378137.0f; // earth radius, m -static const float g0 = 9.806f; // standard gravitational accel. m/s^2 -static const int8_t ret_ok = 0; // no error in function -static const int8_t ret_error = -1; // error occurred - -KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : - SuperBlock(parent, name), - // subscriptions - _sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz - _gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz - _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz - // publications - _pos(&getPublications(), ORB_ID(vehicle_global_position)), - _localPos(&getPublications(), ORB_ID(vehicle_local_position)), - _att(&getPublications(), ORB_ID(vehicle_attitude)), - // timestamps - _pubTimeStamp(hrt_absolute_time()), - _predictTimeStamp(hrt_absolute_time()), - _attTimeStamp(hrt_absolute_time()), - _outTimeStamp(hrt_absolute_time()), - // frame count - _navFrames(0), - // miss counts - _miss(0), - // accelerations - fN(0), fE(0), fD(0), - // state - phi(0), theta(0), psi(0), - vN(0), vE(0), vD(0), - lat(0), lon(0), alt(0), - lat0(0), lon0(0), alt0(0), - // parameters for ground station - _vGyro(this, "V_GYRO"), - _vAccel(this, "V_ACCEL"), - _rMag(this, "R_MAG"), - _rGpsVel(this, "R_GPS_VEL"), - _rGpsPos(this, "R_GPS_POS"), - _rGpsAlt(this, "R_GPS_ALT"), - _rPressAlt(this, "R_PRESS_ALT"), - _rAccel(this, "R_ACCEL"), - _magDip(this, "ENV_MAG_DIP"), - _magDec(this, "ENV_MAG_DEC"), - _g(this, "ENV_G"), - _faultPos(this, "FAULT_POS"), - _faultAtt(this, "FAULT_ATT"), - _attitudeInitialized(false), - _positionInitialized(false), - _attitudeInitCounter(0) -{ - using namespace math; - - F.zero(); - G.zero(); - V.zero(); - HAtt.zero(); - RAtt.zero(); - HPos.zero(); - RPos.zero(); - - // initial state covariance matrix - P0.identity(); - P0 *= 0.01f; - P = P0; - - // initial state - phi = 0.0f; - theta = 0.0f; - psi = 0.0f; - vN = 0.0f; - vE = 0.0f; - vD = 0.0f; - lat = 0.0f; - lon = 0.0f; - alt = 0.0f; - - // initialize rotation quaternion with a single raw sensor measurement - _sensors.update(); - q = init( - _sensors.accelerometer_m_s2[0], - _sensors.accelerometer_m_s2[1], - _sensors.accelerometer_m_s2[2], - _sensors.magnetometer_ga[0], - _sensors.magnetometer_ga[1], - _sensors.magnetometer_ga[2]); - - // initialize dcm - C_nb = q.to_dcm(); - - // HPos is constant - HPos(0, 3) = 1.0f; - HPos(1, 4) = 1.0f; - HPos(2, 6) = 1.0e7f * M_RAD_TO_DEG_F; - HPos(3, 7) = 1.0e7f * M_RAD_TO_DEG_F; - HPos(4, 8) = 1.0f; - HPos(5, 8) = 1.0f; - - // initialize all parameters - updateParams(); -} - -math::Quaternion KalmanNav::init(float ax, float ay, float az, float mx, float my, float mz) -{ - 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); - - 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); - - float q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading; - float q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading; - float q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading; - float q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; - - return math::Quaternion(q0, q1, q2, q3); - -} - -void KalmanNav::update() -{ - using namespace math; - - struct pollfd fds[1]; - fds[0].fd = _sensors.getHandle(); - fds[0].events = POLLIN; - - // poll for new data - int ret = poll(fds, 1, 1000); - - if (ret < 0) { - // XXX this is seriously bad - should be an emergency - return; - - } else if (ret == 0) { // timeout - return; - } - - // get new timestamp - uint64_t newTimeStamp = hrt_absolute_time(); - - // check updated subscriptions - if (_param_update.updated()) updateParams(); - - bool gpsUpdate = _gps.updated(); - bool sensorsUpdate = _sensors.updated(); - - // get new information from subscriptions - // this clears update flag - updateSubscriptions(); - - // initialize attitude when sensors online - if (!_attitudeInitialized && sensorsUpdate) { - if (correctAtt() == ret_ok) _attitudeInitCounter++; - - if (_attitudeInitCounter > 100) { - warnx("initialized EKF attitude"); - warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f", - double(phi), double(theta), double(psi)); - _attitudeInitialized = true; - } - } - - // initialize position when gps received - if (!_positionInitialized && - _attitudeInitialized && // wait for attitude first - gpsUpdate && - _gps.fix_type > 2 - //&& _gps.counter_pos_valid > 10 - ) { - vN = _gps.vel_n_m_s; - vE = _gps.vel_e_m_s; - vD = _gps.vel_d_m_s; - setLatDegE7(_gps.lat); - setLonDegE7(_gps.lon); - setAltE3(_gps.alt); - // set reference position for - // local position - lat0 = lat; - lon0 = lon; - alt0 = alt; - // XXX map_projection has internal global - // states that multiple things could change, - // should make map_projection take reference - // lat/lon and not have init - map_projection_init(lat0, lon0); - _positionInitialized = true; - warnx("initialized EKF state with GPS"); - warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f", - double(vN), double(vE), double(vD), - lat, lon, double(alt)); - } - - // prediction step - // using sensors timestamp so we can account for packet lag - float dt = (_sensors.timestamp - _predictTimeStamp) / 1.0e6f; - //printf("dt: %15.10f\n", double(dt)); - _predictTimeStamp = _sensors.timestamp; - - // don't predict if time greater than a second - if (dt < 1.0f) { - predictState(dt); - predictStateCovariance(dt); - // count fast frames - _navFrames += 1; - } - - // count times 100 Hz rate isn't met - if (dt > 0.01f) _miss++; - - // gps correction step - if (_positionInitialized && gpsUpdate) { - correctPos(); - } - - // attitude correction step - if (_attitudeInitialized // initialized - && sensorsUpdate // new data - && _sensors.timestamp - _attTimeStamp > 1e6 / 50 // 50 Hz - ) { - _attTimeStamp = _sensors.timestamp; - correctAtt(); - } - - // publication - if (newTimeStamp - _pubTimeStamp > 1e6 / 50) { // 50 Hz - _pubTimeStamp = newTimeStamp; - - updatePublications(); - } - - // output - if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz - _outTimeStamp = newTimeStamp; - //printf("nav: %4d Hz, miss #: %4d\n", - // _navFrames / 10, _miss / 10); - _navFrames = 0; - _miss = 0; - } -} - -void KalmanNav::updatePublications() -{ - using namespace math; - - // global position publication - _pos.timestamp = _pubTimeStamp; - _pos.time_gps_usec = _gps.timestamp_position; - _pos.global_valid = true; - _pos.lat = lat * M_RAD_TO_DEG; - _pos.lon = lon * M_RAD_TO_DEG; - _pos.alt = float(alt); - _pos.vel_n = vN; - _pos.vel_e = vE; - _pos.vel_d = vD; - _pos.yaw = psi; - - // local position publication - float x; - float y; - bool landed = alt < (alt0 + 0.1); // XXX improve? - map_projection_project(lat, lon, &x, &y); - _localPos.timestamp = _pubTimeStamp; - _localPos.xy_valid = true; - _localPos.z_valid = true; - _localPos.v_xy_valid = true; - _localPos.v_z_valid = true; - _localPos.x = x; - _localPos.y = y; - _localPos.z = alt0 - alt; - _localPos.vx = vN; - _localPos.vy = vE; - _localPos.vz = vD; - _localPos.yaw = psi; - _localPos.xy_global = true; - _localPos.z_global = true; - _localPos.ref_timestamp = _pubTimeStamp; - _localPos.ref_lat = getLatDegE7(); - _localPos.ref_lon = getLonDegE7(); - _localPos.ref_alt = 0; - _localPos.landed = landed; - - // attitude publication - _att.timestamp = _pubTimeStamp; - _att.roll = phi; - _att.pitch = theta; - _att.yaw = psi; - _att.rollspeed = _sensors.gyro_rad_s[0]; - _att.pitchspeed = _sensors.gyro_rad_s[1]; - _att.yawspeed = _sensors.gyro_rad_s[2]; - // TODO, add gyro offsets to filter - _att.rate_offsets[0] = 0.0f; - _att.rate_offsets[1] = 0.0f; - _att.rate_offsets[2] = 0.0f; - - for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - _att.R[i][j] = C_nb(i, j); - - for (int i = 0; i < 4; i++) _att.q[i] = q(i); - - _att.R_valid = true; - _att.q_valid = true; - - // selectively update publications, - // do NOT call superblock do-all method - if (_positionInitialized) { - _pos.update(); - _localPos.update(); - } - - if (_attitudeInitialized) - _att.update(); -} - -int KalmanNav::predictState(float dt) -{ - using namespace math; - - // trig - float sinL = sinf(lat); - float cosL = cosf(lat); - float cosLSing = cosf(lat); - - // prevent singularity - if (fabsf(cosLSing) < 0.01f) { - if (cosLSing > 0) cosLSing = 0.01; - else cosLSing = -0.01; - } - - // attitude prediction - if (_attitudeInitialized) { - Vector<3> w(_sensors.gyro_rad_s); - - // attitude - q = q + q.derivative(w) * dt; - - // renormalize quaternion if needed - if (fabsf(q.length() - 1.0f) > 1e-4f) { - q.normalize(); - } - - // C_nb update - C_nb = q.to_dcm(); - - // euler update - Vector<3> euler = C_nb.to_euler(); - phi = euler.data[0]; - theta = euler.data[1]; - psi = euler.data[2]; - - // specific acceleration in nav frame - Vector<3> accelB(_sensors.accelerometer_m_s2); - Vector<3> accelN = C_nb * accelB; - fN = accelN(0); - fE = accelN(1); - fD = accelN(2); - } - - // position prediction - if (_positionInitialized) { - // neglects angular deflections in local gravity - // see Titerton pg. 70 - float R = R0 + float(alt); - float LDot = vN / R; - float lDot = vE / (cosLSing * R); - float rotRate = 2 * omega + lDot; - - // XXX position prediction using speed - float vNDot = fN - vE * rotRate * sinL + - vD * LDot; - float vDDot = fD - vE * rotRate * cosL - - vN * LDot + _g.get(); - float vEDot = fE + vN * rotRate * sinL + - vDDot * rotRate * cosL; - - // rectangular integration - vN += vNDot * dt; - vE += vEDot * dt; - vD += vDDot * dt; - lat += double(LDot * dt); - lon += double(lDot * dt); - alt += double(-vD * dt); - } - - return ret_ok; -} - -int KalmanNav::predictStateCovariance(float dt) -{ - using namespace math; - - // trig - float sinL = sinf(lat); - float cosL = cosf(lat); - float cosLSq = cosL * cosL; - float tanL = tanf(lat); - - // prepare for matrix - float R = R0 + float(alt); - float RSq = R * R; - - // F Matrix - // Titterton pg. 291 - - F(0, 1) = -(omega * sinL + vE * tanL / R); - F(0, 2) = vN / R; - F(0, 4) = 1.0f / R; - F(0, 6) = -omega * sinL; - F(0, 8) = -vE / RSq; - - F(1, 0) = omega * sinL + vE * tanL / R; - F(1, 2) = omega * cosL + vE / R; - F(1, 3) = -1.0f / R; - F(1, 8) = vN / RSq; - - F(2, 0) = -vN / R; - F(2, 1) = -omega * cosL - vE / R; - F(2, 4) = -tanL / R; - F(2, 6) = -omega * cosL - vE / (R * cosLSq); - F(2, 8) = vE * tanL / RSq; - - F(3, 1) = -fD; - F(3, 2) = fE; - F(3, 3) = vD / R; - F(3, 4) = -2 * (omega * sinL + vE * tanL / R); - F(3, 5) = vN / R; - F(3, 6) = -vE * (2 * omega * cosL + vE / (R * cosLSq)); - F(3, 8) = (vE * vE * tanL - vN * vD) / RSq; - - F(4, 0) = fD; - F(4, 2) = -fN; - F(4, 3) = 2 * omega * sinL + vE * tanL / R; - F(4, 4) = (vN * tanL + vD) / R; - F(4, 5) = 2 * omega * cosL + vE / R; - F(4, 6) = 2 * omega * (vN * cosL - vD * sinL) + - vN * vE / (R * cosLSq); - F(4, 8) = -vE * (vN * tanL + vD) / RSq; - - F(5, 0) = -fE; - F(5, 1) = fN; - F(5, 3) = -2 * vN / R; - F(5, 4) = -2 * (omega * cosL + vE / R); - F(5, 6) = 2 * omega * vE * sinL; - F(5, 8) = (vN * vN + vE * vE) / RSq; - - F(6, 3) = 1 / R; - F(6, 8) = -vN / RSq; - - F(7, 4) = 1 / (R * cosL); - F(7, 6) = vE * tanL / (R * cosL); - F(7, 8) = -vE / (cosL * RSq); - - F(8, 5) = -1; - - // G Matrix - // Titterton pg. 291 - G(0, 0) = -C_nb(0, 0); - G(0, 1) = -C_nb(0, 1); - G(0, 2) = -C_nb(0, 2); - G(1, 0) = -C_nb(1, 0); - G(1, 1) = -C_nb(1, 1); - G(1, 2) = -C_nb(1, 2); - G(2, 0) = -C_nb(2, 0); - G(2, 1) = -C_nb(2, 1); - G(2, 2) = -C_nb(2, 2); - - G(3, 3) = C_nb(0, 0); - G(3, 4) = C_nb(0, 1); - G(3, 5) = C_nb(0, 2); - G(4, 3) = C_nb(1, 0); - G(4, 4) = C_nb(1, 1); - G(4, 5) = C_nb(1, 2); - G(5, 3) = C_nb(2, 0); - G(5, 4) = C_nb(2, 1); - G(5, 5) = C_nb(2, 2); - - // continuous prediction equations - // for discrete time EKF - // http://en.wikipedia.org/wiki/Extended_Kalman_filter - P = P + (F * P + P * F.transposed() + G * V * G.transposed()) * dt; - - return ret_ok; -} - -int KalmanNav::correctAtt() -{ - using namespace math; - - // trig - float cosPhi = cosf(phi); - float cosTheta = cosf(theta); - // float cosPsi = cosf(psi); - float sinPhi = sinf(phi); - float sinTheta = sinf(theta); - // float sinPsi = sinf(psi); - - // mag predicted measurement - // choosing some typical magnetic field properties, - // TODO dip/dec depend on lat/ lon/ time - //float dip = _magDip.get() / M_RAD_TO_DEG_F; // dip, inclination with level - float dec = _magDec.get() / M_RAD_TO_DEG_F; // declination, clockwise rotation from north - - // compensate roll and pitch, but not yaw - // XXX take the vectors out of the C_nb matrix to avoid singularities - math::Matrix<3,3> C_rp; - C_rp.from_euler(phi, theta, 0.0f);//C_nb.transposed(); - - // mag measurement - Vector<3> magBody(_sensors.magnetometer_ga); - - // transform to earth frame - Vector<3> magNav = C_rp * magBody; - - // calculate error between estimate and measurement - // apply declination correction for true heading as well. - float yMag = -atan2f(magNav(1),magNav(0)) - psi - dec; - if (yMag > M_PI_F) yMag -= 2*M_PI_F; - if (yMag < -M_PI_F) yMag += 2*M_PI_F; - - // accel measurement - Vector<3> zAccel(_sensors.accelerometer_m_s2); - float accelMag = zAccel.length(); - zAccel.normalize(); - - // ignore accel correction when accel mag not close to g - Matrix<4,4> RAttAdjust = RAtt; - - bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f; - - if (ignoreAccel) { - RAttAdjust(1, 1) = 1.0e10; - RAttAdjust(2, 2) = 1.0e10; - RAttAdjust(3, 3) = 1.0e10; - - } else { - //printf("correcting attitude with accel\n"); - } - - // accel predicted measurement - Vector<3> zAccelHat = (C_nb.transposed() * Vector<3>(0, 0, -_g.get())).normalized(); - - // calculate residual - Vector<4> y(yMag, zAccel(0) - zAccelHat(0), zAccel(1) - zAccelHat(1), zAccel(2) - zAccelHat(2)); - - // HMag - HAtt(0, 2) = 1; - - // HAccel - HAtt(1, 1) = cosTheta; - HAtt(2, 0) = -cosPhi * cosTheta; - HAtt(2, 1) = sinPhi * sinTheta; - HAtt(3, 0) = sinPhi * cosTheta; - HAtt(3, 1) = cosPhi * sinTheta; - - // compute correction - // http://en.wikipedia.org/wiki/Extended_Kalman_filter - Matrix<4, 4> S = HAtt * P * HAtt.transposed() + RAttAdjust; // residual covariance - Matrix<9, 4> K = P * HAtt.transposed() * S.inversed(); - Vector<9> xCorrect = K * y; - - // check correciton is sane - for (size_t i = 0; i < xCorrect.get_size(); i++) { - float val = xCorrect(i); - - if (isnan(val) || isinf(val)) { - // abort correction and return - warnx("numerical failure in att correction"); - // reset P matrix to P0 - P = P0; - return ret_error; - } - } - - // correct state - if (!ignoreAccel) { - phi += xCorrect(PHI); - theta += xCorrect(THETA); - } - - psi += xCorrect(PSI); - - // attitude also affects nav velocities - if (_positionInitialized) { - vN += xCorrect(VN); - vE += xCorrect(VE); - vD += xCorrect(VD); - } - - // update state covariance - // http://en.wikipedia.org/wiki/Extended_Kalman_filter - P = P - K * HAtt * P; - - // fault detection - float beta = y * (S.inversed() * y); - - if (beta > _faultAtt.get()) { - warnx("fault in attitude: beta = %8.4f", (double)beta); - warnx("y:"); y.print(); - } - - // update quaternions from euler - // angle correction - q.from_euler(phi, theta, psi); - - return ret_ok; -} - -int KalmanNav::correctPos() -{ - using namespace math; - - // residual - Vector<6> y; - y(0) = _gps.vel_n_m_s - vN; - y(1) = _gps.vel_e_m_s - vE; - y(2) = double(_gps.lat) - double(lat) * 1.0e7 * M_RAD_TO_DEG; - y(3) = double(_gps.lon) - double(lon) * 1.0e7 * M_RAD_TO_DEG; - y(4) = _gps.alt / 1.0e3f - alt; - y(5) = _sensors.baro_alt_meter - alt; - - // compute correction - // http://en.wikipedia.org/wiki/Extended_Kalman_filter - Matrix<6,6> S = HPos * P * HPos.transposed() + RPos; // residual covariance - Matrix<9,6> K = P * HPos.transposed() * S.inversed(); - Vector<9> xCorrect = K * y; - - // check correction is sane - for (size_t i = 0; i < xCorrect.get_size(); i++) { - float val = xCorrect(i); - - if (!isfinite(val)) { - // abort correction and return - warnx("numerical failure in gps correction"); - // fallback to GPS - vN = _gps.vel_n_m_s; - vE = _gps.vel_e_m_s; - vD = _gps.vel_d_m_s; - setLatDegE7(_gps.lat); - setLonDegE7(_gps.lon); - setAltE3(_gps.alt); - // reset P matrix to P0 - P = P0; - return ret_error; - } - } - - // correct state - vN += xCorrect(VN); - vE += xCorrect(VE); - vD += xCorrect(VD); - lat += double(xCorrect(LAT)); - lon += double(xCorrect(LON)); - alt += xCorrect(ALT); - - // update state covariance - // http://en.wikipedia.org/wiki/Extended_Kalman_filter - P = P - K * HPos * P; - - // fault detetcion - float beta = y * (S.inversed() * y); - - static int counter = 0; - if (beta > _faultPos.get() && (counter % 10 == 0)) { - warnx("fault in gps: beta = %8.4f", (double)beta); - warnx("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f, baro: %8.4f", - double(y(0) / sqrtf(RPos(0, 0))), - double(y(1) / sqrtf(RPos(1, 1))), - double(y(2) / sqrtf(RPos(2, 2))), - double(y(3) / sqrtf(RPos(3, 3))), - double(y(4) / sqrtf(RPos(4, 4))), - double(y(5) / sqrtf(RPos(5, 5)))); - } - counter++; - - return ret_ok; -} - -void KalmanNav::updateParams() -{ - using namespace math; - using namespace control; - SuperBlock::updateParams(); - - // gyro noise - V(0, 0) = _vGyro.get(); // gyro x, rad/s - V(1, 1) = _vGyro.get(); // gyro y - V(2, 2) = _vGyro.get(); // gyro z - - // accel noise - V(3, 3) = _vAccel.get(); // accel x, m/s^2 - V(4, 4) = _vAccel.get(); // accel y - V(5, 5) = _vAccel.get(); // accel z - - // magnetometer noise - float noiseMin = 1e-6f; - float noiseMagSq = _rMag.get() * _rMag.get(); - - if (noiseMagSq < noiseMin) noiseMagSq = noiseMin; - - RAtt(0, 0) = noiseMagSq; // normalized direction - - // accelerometer noise - float noiseAccelSq = _rAccel.get() * _rAccel.get(); - - // bound noise to prevent singularities - if (noiseAccelSq < noiseMin) noiseAccelSq = noiseMin; - - RAtt(1, 1) = noiseAccelSq; // normalized direction - RAtt(2, 2) = noiseAccelSq; - RAtt(3, 3) = noiseAccelSq; - - // gps noise - float R = R0 + float(alt); - float cosLSing = cosf(lat); - - // prevent singularity - if (fabsf(cosLSing) < 0.01f) { - if (cosLSing > 0) cosLSing = 0.01; - else cosLSing = -0.01; - } - - float noiseVel = _rGpsVel.get(); - float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R; - float noiseLonDegE7 = noiseLatDegE7 / cosLSing; - float noiseGpsAlt = _rGpsAlt.get(); - float noisePressAlt = _rPressAlt.get(); - - // bound noise to prevent singularities - if (noiseVel < noiseMin) noiseVel = noiseMin; - - if (noiseLatDegE7 < noiseMin) noiseLatDegE7 = noiseMin; - - if (noiseLonDegE7 < noiseMin) noiseLonDegE7 = noiseMin; - - if (noiseGpsAlt < noiseMin) noiseGpsAlt = noiseMin; - - if (noisePressAlt < noiseMin) noisePressAlt = noiseMin; - - RPos(0, 0) = noiseVel * noiseVel; // vn - RPos(1, 1) = noiseVel * noiseVel; // ve - RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7; // lat - RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7; // lon - RPos(4, 4) = noiseGpsAlt * noiseGpsAlt; // h - RPos(5, 5) = noisePressAlt * noisePressAlt; // h - // XXX, note that RPos depends on lat, so updateParams should - // be called if lat changes significantly -} diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp deleted file mode 100644 index caf93bc78..000000000 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp +++ /dev/null @@ -1,192 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012, 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 KalmanNav.hpp - * - * kalman filter navigation code - */ - -#pragma once - -//#define MATRIX_ASSERT -//#define VECTOR_ASSERT - -#include <nuttx/config.h> - -#include <mathlib/mathlib.h> -#include <controllib/blocks.hpp> -#include <controllib/block/BlockParam.hpp> -#include <uORB/Subscription.hpp> -#include <uORB/Publication.hpp> - -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/vehicle_local_position.h> -#include <uORB/topics/sensor_combined.h> -#include <uORB/topics/vehicle_gps_position.h> -#include <uORB/topics/parameter_update.h> - -#include <drivers/drv_accel.h> -#include <drivers/drv_gyro.h> -#include <drivers/drv_mag.h> - -#include <drivers/drv_hrt.h> -#include <poll.h> -#include <unistd.h> - -/** - * Kalman filter navigation class - * http://en.wikipedia.org/wiki/Extended_Kalman_filter - * Discrete-time extended Kalman filter - */ -class KalmanNav : public control::SuperBlock -{ -public: - /** - * Constructor - */ - KalmanNav(SuperBlock *parent, const char *name); - - /** - * Deconstuctor - */ - - virtual ~KalmanNav() {}; - - math::Quaternion init(float ax, float ay, float az, float mx, float my, float mz); - - /** - * The main callback function for the class - */ - void update(); - - - /** - * Publication update - */ - virtual void updatePublications(); - - /** - * State prediction - * Continuous, non-linear - */ - int predictState(float dt); - - /** - * State covariance prediction - * Continuous, linear - */ - int predictStateCovariance(float dt); - - /** - * Attitude correction - */ - int correctAtt(); - - /** - * Position correction - */ - int correctPos(); - - /** - * Overloaded update parameters - */ - virtual void updateParams(); -protected: - // kalman filter - math::Matrix<9,9> F; /**< Jacobian(f,x), where dx/dt = f(x,u) */ - math::Matrix<9,6> G; /**< noise shaping matrix for gyro/accel */ - math::Matrix<9,9> P; /**< state covariance matrix */ - math::Matrix<9,9> P0; /**< initial state covariance matrix */ - math::Matrix<6,6> V; /**< gyro/ accel noise matrix */ - math::Matrix<4,9> HAtt; /**< attitude measurement matrix */ - math::Matrix<4,4> RAtt; /**< attitude measurement noise matrix */ - math::Matrix<6,9> HPos; /**< position measurement jacobian matrix */ - math::Matrix<6,6> RPos; /**< position measurement noise matrix */ - // attitude - math::Matrix<3,3> C_nb; /**< direction cosine matrix from body to nav frame */ - math::Quaternion q; /**< quaternion from body to nav frame */ - // subscriptions - uORB::Subscription<sensor_combined_s> _sensors; /**< sensors sub. */ - uORB::Subscription<vehicle_gps_position_s> _gps; /**< gps sub. */ - uORB::Subscription<parameter_update_s> _param_update; /**< parameter update sub. */ - // publications - uORB::Publication<vehicle_global_position_s> _pos; /**< position pub. */ - uORB::Publication<vehicle_local_position_s> _localPos; /**< local position pub. */ - uORB::Publication<vehicle_attitude_s> _att; /**< attitude pub. */ - // time stamps - uint64_t _pubTimeStamp; /**< output data publication time stamp */ - uint64_t _predictTimeStamp; /**< prediction time stamp */ - uint64_t _attTimeStamp; /**< attitude correction time stamp */ - uint64_t _outTimeStamp; /**< output time stamp */ - // frame count - uint16_t _navFrames; /**< navigation frames completed in output cycle */ - // miss counts - uint16_t _miss; /**< number of times fast prediction loop missed */ - // accelerations - float fN, fE, fD; /**< navigation frame acceleration */ - // states - enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT}; /**< state enumeration */ - float phi, theta, psi; /**< 3-2-1 euler angles */ - float vN, vE, vD; /**< navigation velocity, m/s */ - double lat, lon; /**< lat, lon radians */ - // parameters - float alt; /**< altitude, meters */ - double lat0, lon0; /**< reference latitude and longitude */ - float alt0; /**< refeerence altitude (ground height) */ - control::BlockParamFloat _vGyro; /**< gyro process noise */ - control::BlockParamFloat _vAccel; /**< accelerometer process noise */ - control::BlockParamFloat _rMag; /**< magnetometer measurement noise */ - control::BlockParamFloat _rGpsVel; /**< gps velocity measurement noise */ - control::BlockParamFloat _rGpsPos; /**< gps position measurement noise */ - control::BlockParamFloat _rGpsAlt; /**< gps altitude measurement noise */ - control::BlockParamFloat _rPressAlt; /**< press altitude measurement noise */ - control::BlockParamFloat _rAccel; /**< accelerometer measurement noise */ - control::BlockParamFloat _magDip; /**< magnetic inclination with level */ - control::BlockParamFloat _magDec; /**< magnetic declination, clockwise rotation */ - control::BlockParamFloat _g; /**< gravitational constant */ - control::BlockParamFloat _faultPos; /**< fault detection threshold for position */ - control::BlockParamFloat _faultAtt; /**< fault detection threshold for attitude */ - // status - bool _attitudeInitialized; - bool _positionInitialized; - uint16_t _attitudeInitCounter; - // accessors - int32_t getLatDegE7() { return int32_t(lat * 1.0e7 * M_RAD_TO_DEG); } - void setLatDegE7(int32_t val) { lat = val / 1.0e7 / M_RAD_TO_DEG; } - int32_t getLonDegE7() { return int32_t(lon * 1.0e7 * M_RAD_TO_DEG); } - void setLonDegE7(int32_t val) { lon = val / 1.0e7 / M_RAD_TO_DEG; } - int32_t getAltE3() { return int32_t(alt * 1.0e3); } - void setAltE3(int32_t val) { alt = double(val) / 1.0e3; } -}; diff --git a/src/modules/att_pos_estimator_ekf/kalman_main.cpp b/src/modules/att_pos_estimator_ekf/kalman_main.cpp deleted file mode 100644 index 3d20d4d2d..000000000 --- a/src/modules/att_pos_estimator_ekf/kalman_main.cpp +++ /dev/null @@ -1,157 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: James Goppert - * - * 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 kalman_main.cpp - * Combined attitude / position estimator. - * - * @author James Goppert - */ - -#include <nuttx/config.h> -#include <unistd.h> -#include <stdio.h> -#include <stdlib.h> -#include <string.h> -#include <systemlib/systemlib.h> -#include <systemlib/param/param.h> -#include <systemlib/err.h> -#include <drivers/drv_hrt.h> -#include <math.h> -#include "KalmanNav.hpp" - -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int daemon_task; /**< Handle of deamon task / thread */ - -/** - * Deamon management function. - */ -extern "C" __EXPORT int att_pos_estimator_ekf_main(int argc, char *argv[]); - -/** - * Mainloop of deamon. - */ -int kalman_demo_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - warnx("usage: att_pos_estimator_ekf {start|stop|status} [-p <additional params>]"); - exit(1); -} - -/** - * 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(). - */ -int att_pos_estimator_ekf_main(int argc, char *argv[]) -{ - - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - warnx("already running"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - - daemon_task = task_spawn_cmd("att_pos_estimator_ekf", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 30, - 8192, - kalman_demo_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("is running\n"); - exit(0); - - } else { - warnx("not started\n"); - exit(1); - } - - } - - usage("unrecognized command"); - exit(1); -} - -int kalman_demo_thread_main(int argc, char *argv[]) -{ - - warnx("starting"); - - using namespace math; - - thread_running = true; - - KalmanNav nav(NULL, "KF"); - - while (!thread_should_exit) { - nav.update(); - } - - warnx("exiting."); - - thread_running = false; - - return 0; -} diff --git a/src/modules/att_pos_estimator_ekf/module.mk b/src/modules/att_pos_estimator_ekf/module.mk deleted file mode 100644 index 8d4a40d95..000000000 --- a/src/modules/att_pos_estimator_ekf/module.mk +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 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. -# -############################################################################ - -# -# Full attitude / position Extended Kalman Filter -# - -MODULE_COMMAND = att_pos_estimator_ekf - -SRCS = kalman_main.cpp \ - KalmanNav.cpp \ - params.c diff --git a/src/modules/att_pos_estimator_ekf/params.c b/src/modules/att_pos_estimator_ekf/params.c deleted file mode 100644 index 4af5edead..000000000 --- a/src/modules/att_pos_estimator_ekf/params.c +++ /dev/null @@ -1,49 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012, 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. - * - ****************************************************************************/ - -#include <systemlib/param/param.h> - -/*PARAM_DEFINE_FLOAT(NAME,0.0f);*/ -PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.008f); -PARAM_DEFINE_FLOAT(KF_V_ACCEL, 1.0f); -PARAM_DEFINE_FLOAT(KF_R_MAG, 0.8f); -PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 0.5f); -PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 2.0f); -PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 3.0f); -PARAM_DEFINE_FLOAT(KF_R_PRESS_ALT, 0.1f); -PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f); -PARAM_DEFINE_FLOAT(KF_FAULT_POS, 10.0f); -PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f); -PARAM_DEFINE_FLOAT(KF_ENV_G, 9.765f); -PARAM_DEFINE_FLOAT(KF_ENV_MAG_DIP, 60.0f); -PARAM_DEFINE_FLOAT(KF_ENV_MAG_DEC, 0.0f); 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 10a6cd2c5..2ec889efe 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli <naegelit@student.ethz.ch> - * Lorenz Meier <lm@inf.ethz.ch> + * Copyright (c) 2012-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 @@ -34,9 +32,12 @@ ****************************************************************************/ /* - * @file attitude_estimator_ekf_main.c + * @file attitude_estimator_ekf_main.cpp * * Extended Kalman Filter for Attitude Estimation. + * + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> */ #include <nuttx/config.h> @@ -111,7 +112,7 @@ usage(const char *reason) * Makefile does only apply to this management task. * * The actual stack size should be set in the call - * to task_create(). + * to task_spawn_cmd(). */ int attitude_estimator_ekf_main(int argc, char *argv[]) { @@ -407,7 +408,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds vel(2) = gps.vel_d_m_s; } - } else if (ekf_params.acc_comp == 2 && global_pos.global_valid && hrt_absolute_time() < global_pos.timestamp + 500000) { + } else if (ekf_params.acc_comp == 2 && gps.eph_m < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) { vel_valid = true; if (global_pos_updated) { vel_t = global_pos.timestamp; diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 4154e3db4..3ff3d9922 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -40,6 +40,7 @@ */ #include "attitude_estimator_ekf_params.h" +#include <math.h> /* Extended Kalman Filter covariances */ @@ -113,6 +114,7 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru param_get(h->yaw_off, &(p->yaw_off)); param_get(h->mag_decl, &(p->mag_decl)); + p->mag_decl *= M_PI / 180.0f; param_get(h->acc_comp, &(p->acc_comp)); diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk index d98647f99..99d0c5bf2 100644 --- a/src/modules/attitude_estimator_ekf/module.mk +++ b/src/modules/attitude_estimator_ekf/module.mk @@ -50,3 +50,5 @@ SRCS = attitude_estimator_ekf_main.cpp \ codegen/rtGetNaN.c \ codegen/norm.c \ codegen/cross.c + +MODULE_STACKSIZE = 1200 diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp index 3653defa6..e55b01160 100755 --- a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Hyon Lim <limhyon@gmail.com> - * Anton Babushkin <anton.babushkin@me.com> + * 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 @@ -36,6 +34,9 @@ /* * @file attitude_estimator_so3_main.cpp * + * @author Hyon Lim <limhyon@gmail.com> + * @author Anton Babushkin <anton.babushkin@me.com> + * * Implementation of nonlinear complementary filters on the SO(3). * This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer. * Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix. @@ -131,7 +132,7 @@ usage(const char *reason) * Makefile does only apply to this management task. * * The actual stack size should be set in the call - * to task_create(). + * to task_spawn_cmd(). */ int attitude_estimator_so3_main(int argc, char *argv[]) { diff --git a/src/modules/attitude_estimator_so3/module.mk b/src/modules/attitude_estimator_so3/module.mk index e29bb16a6..f52715abb 100644 --- a/src/modules/attitude_estimator_so3/module.mk +++ b/src/modules/attitude_estimator_so3/module.mk @@ -6,3 +6,5 @@ MODULE_COMMAND = attitude_estimator_so3 SRCS = attitude_estimator_so3_main.cpp \ attitude_estimator_so3_params.c + +MODULE_STACKSIZE = 1200 diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 1cbdf9bf8..7180048ff 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -194,13 +194,13 @@ int do_accel_calibration(int mavlink_fd) int32_t board_rotation_int; param_get(board_rotation_h, &(board_rotation_int)); enum Rotation board_rotation_id = (enum Rotation)board_rotation_int; - math::Matrix<3,3> board_rotation; + math::Matrix<3, 3> board_rotation; get_rot_matrix(board_rotation_id, &board_rotation); - math::Matrix<3,3> board_rotation_t = board_rotation.transposed(); + math::Matrix<3, 3> board_rotation_t = board_rotation.transposed(); math::Vector<3> accel_offs_vec(&accel_offs[0]); - math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec; - math::Matrix<3,3> accel_T_mat(&accel_T[0][0]); - math::Matrix<3,3> accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation; + math::Vector<3> accel_offs_rotated = board_rotation_t *accel_offs_vec; + math::Matrix<3, 3> accel_T_mat(&accel_T[0][0]); + math::Matrix<3, 3> accel_T_rotated = board_rotation_t *accel_T_mat * board_rotation; accel_scale.x_offset = accel_offs_rotated(0); accel_scale.x_scale = accel_T_rotated(0, 0); @@ -277,11 +277,13 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float } } - if (old_done_count != done_count) + if (old_done_count != done_count) { mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * done_count); + } - if (done) + if (done) { break; + } mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s", (!data_collected[0]) ? "x+ " : "", @@ -380,11 +382,13 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) d = d * d; accel_disp[i] = accel_disp[i] * (1.0f - w); - if (d > still_thr2 * 8.0f) + if (d > still_thr2 * 8.0f) { d = still_thr2 * 8.0f; + } - if (d > accel_disp[i]) + if (d > accel_disp[i]) { accel_disp[i] = d; + } } /* still detector with hysteresis */ @@ -432,33 +436,39 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) if (fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr && fabsf(accel_ema[1]) < accel_err_thr && - fabsf(accel_ema[2]) < accel_err_thr) - return 0; // [ g, 0, 0 ] + fabsf(accel_ema[2]) < accel_err_thr) { + return 0; // [ g, 0, 0 ] + } if (fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr && fabsf(accel_ema[1]) < accel_err_thr && - fabsf(accel_ema[2]) < accel_err_thr) - return 1; // [ -g, 0, 0 ] + fabsf(accel_ema[2]) < accel_err_thr) { + return 1; // [ -g, 0, 0 ] + } if (fabsf(accel_ema[0]) < accel_err_thr && fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr && - fabsf(accel_ema[2]) < accel_err_thr) - return 2; // [ 0, g, 0 ] + fabsf(accel_ema[2]) < accel_err_thr) { + return 2; // [ 0, g, 0 ] + } if (fabsf(accel_ema[0]) < accel_err_thr && fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr && - fabsf(accel_ema[2]) < accel_err_thr) - return 3; // [ 0, -g, 0 ] + fabsf(accel_ema[2]) < accel_err_thr) { + return 3; // [ 0, -g, 0 ] + } if (fabsf(accel_ema[0]) < accel_err_thr && fabsf(accel_ema[1]) < accel_err_thr && - fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr) - return 4; // [ 0, 0, g ] + fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr) { + return 4; // [ 0, 0, g ] + } if (fabsf(accel_ema[0]) < accel_err_thr && fabsf(accel_ema[1]) < accel_err_thr && - fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) - return 5; // [ 0, 0, -g ] + fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) { + return 5; // [ 0, 0, -g ] + } mavlink_log_critical(mavlink_fd, "ERROR: invalid orientation"); @@ -485,8 +495,9 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp struct sensor_combined_s sensor; orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - for (int i = 0; i < 3; i++) + for (int i = 0; i < 3; i++) { accel_sum[i] += sensor.accelerometer_m_s2[i]; + } count++; @@ -495,8 +506,9 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp continue; } - if (errcount > samples_num / 10) + if (errcount > samples_num / 10) { return ERROR; + } } for (int i = 0; i < 3; i++) { @@ -512,8 +524,9 @@ int mat_invert3(float src[3][3], float dst[3][3]) src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) + src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]); - if (det == 0.0f) - return ERROR; // Singular matrix + if (det == 0.0f) { + return ERROR; // Singular matrix + } dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det; dst[1][0] = (src[1][2] * src[2][0] - src[1][0] * src[2][2]) / det; @@ -549,8 +562,9 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo /* calculate inverse matrix for A */ float mat_A_inv[3][3]; - if (mat_invert3(mat_A, mat_A_inv) != OK) + if (mat_invert3(mat_A, mat_A_inv) != OK) { return ERROR; + } /* copy results to accel_T */ for (int i = 0; i < 3; i++) { diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 6039d92a7..5d21d89d0 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * 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 @@ -64,9 +64,9 @@ int do_airspeed_calibration(int mavlink_fd) { /* give directions */ mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); - mavlink_log_info(mavlink_fd, "don't move system"); + mavlink_log_info(mavlink_fd, "ensure airspeed sensor is not registering wind"); - const int calibration_count = 2500; + const int calibration_count = 2000; int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); struct differential_pressure_s diff_pres; @@ -82,16 +82,21 @@ int do_airspeed_calibration(int mavlink_fd) bool paramreset_successful = false; int fd = open(AIRSPEED_DEVICE_PATH, 0); + if (fd > 0) { if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { paramreset_successful = true; + + } else { + mavlink_log_critical(mavlink_fd, "airspeed offset zero failed"); } + close(fd); } if (!paramreset_successful) { - warn("WARNING: failed to set scale / offsets for airspeed sensor"); - mavlink_log_critical(mavlink_fd, "could not reset dpress sensor"); + warn("FAILED to set scale / offsets for airspeed"); + mavlink_log_critical(mavlink_fd, "dpress reset failed"); mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); return ERROR; } @@ -107,11 +112,12 @@ int do_airspeed_calibration(int mavlink_fd) if (poll_ret) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - diff_pres_offset += diff_pres.differential_pressure_pa; + diff_pres_offset += diff_pres.differential_pressure_raw_pa; calibration_counter++; - if (calibration_counter % (calibration_count / 20) == 0) + if (calibration_counter % (calibration_count / 20) == 0) { mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count); + } } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b5570f424..96735d25e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -67,6 +67,7 @@ #include <uORB/topics/home_position.h> #include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/vehicle_local_position.h> +#include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_command.h> #include <uORB/topics/subsystem_info.h> @@ -112,16 +113,15 @@ extern struct system_load_s system_load; #define MAVLINK_OPEN_INTERVAL 50000 -#define STICK_ON_OFF_LIMIT 0.75f -#define STICK_THRUST_RANGE 1.0f +#define STICK_ON_OFF_LIMIT 0.9f #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */ -#define RC_TIMEOUT 100000 -#define RC_TIMEOUT_HIL 500000 -#define OFFBOARD_TIMEOUT 200000 -#define DIFFPRESS_TIMEOUT 2000000 +#define POSITION_TIMEOUT (600 * 1000) /**< consider the local or global position estimate invalid after 600ms */ +#define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */ +#define RC_TIMEOUT 500000 +#define OFFBOARD_TIMEOUT 500000 +#define DIFFPRESS_TIMEOUT 2000000 #define PRINT_INTERVAL 5000000 #define PRINT_MODE_REJECT_INTERVAL 2000000 @@ -139,7 +139,7 @@ enum MAV_MODE_FLAG { }; /* Mavlink file descriptors */ -static int mavlink_fd; +static int mavlink_fd = 0; /* flags */ static bool commander_initialized = false; @@ -155,6 +155,7 @@ static bool on_usb_power = false; static float takeoff_alt = 5.0f; static int parachute_enabled = 0; +static float eph_epv_threshold = 5.0f; static struct vehicle_status_s status; static struct actuator_armed_s armed; @@ -197,7 +198,7 @@ void usage(const char *reason); /** * React to commands that are sent e.g. from the mavlink module. */ -bool handle_command(struct vehicle_status_s *status, struct vehicle_command_s *cmd, struct actuator_armed_s *armed); +bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub); /** * Mainloop of commander. @@ -210,7 +211,7 @@ void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status); -transition_result_t set_main_state_rc(struct vehicle_status_s *status); +transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man); void set_control_mode(); @@ -220,11 +221,10 @@ void print_reject_arm(const char *msg); void print_status(); -int arm(); -int disarm(); - transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos); +transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy); + /** * Loop that runs at a lower rate and priority for calibration and parameter tasks. */ @@ -235,8 +235,9 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul int commander_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 1) { usage("missing command"); + } if (!strcmp(argv[1], "start")) { @@ -250,7 +251,7 @@ int commander_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 3000, + 2950, commander_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); @@ -263,8 +264,9 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "stop")) { - if (!thread_running) + if (!thread_running) { errx(0, "commander already stopped"); + } thread_should_exit = true; @@ -291,12 +293,12 @@ int commander_main(int argc, char *argv[]) } if (!strcmp(argv[1], "arm")) { - arm(); + arm_disarm(true, mavlink_fd, "command line"); exit(0); } - if (!strcmp(argv[1], "disarm")) { - disarm(); + if (!strcmp(argv[1], "2")) { + arm_disarm(false, mavlink_fd, "command line"); exit(0); } @@ -306,8 +308,9 @@ int commander_main(int argc, char *argv[]) void usage(const char *reason) { - if (reason) + if (reason) { fprintf(stderr, "%s\n", reason); + } fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n"); exit(1); @@ -366,38 +369,35 @@ void print_status() static orb_advert_t status_pub; -int arm() +transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy) { - int arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); - - if (arming_res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline"); - return 0; - - } else { - return 1; - } -} + transition_result_t arming_res = TRANSITION_NOT_CHANGED; -int disarm() -{ - int arming_res = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + // 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, mavlink_fd); - if (arming_res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline"); - return 0; + if (arming_res == TRANSITION_CHANGED && mavlink_fd) { + mavlink_log_info(mavlink_fd, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); - } else { - return 1; + } else if (arming_res == TRANSITION_DENIED) { + tune_negative(true); } + + return arming_res; } -bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) +bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) { /* result of the command */ enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED; bool ret = false; + /* only handle commands that are meant to be handled by this system and component */ + if (cmd->target_system != status->system_id || ((cmd->target_component != status->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components + return false; + } + /* only handle high-priority commands here */ /* request to set different system mode */ @@ -424,43 +424,16 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } } - if (hil_ret == OK) + if (hil_ret == OK) { ret = true; - - // TODO remove debug code - //mavlink_log_critical(mavlink_fd, "#audio: command setmode: %d %d", base_mode, custom_main_mode); - /* set arming state */ - arming_res = TRANSITION_NOT_CHANGED; - - if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - if (safety->safety_switch_available && !safety->safety_off && status->hil_state == HIL_STATE_OFF) { - print_reject_arm("NOT ARMING: Press safety switch first."); - arming_res = TRANSITION_DENIED; - - } else { - arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); - } - - if (arming_res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by command"); - } - - } else { - if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { - arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - arming_res = arming_state_transition(status, safety, new_arming_state, armed); - - if (arming_res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command"); - } - - } else { - arming_res = TRANSITION_NOT_CHANGED; - } } - if (arming_res == TRANSITION_CHANGED) + // Transition the arming state + arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); + + if (arming_res == TRANSITION_CHANGED) { ret = true; + } /* set main state */ transition_result_t main_res = TRANSITION_DENIED; @@ -471,13 +444,13 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* MANUAL */ main_res = main_state_transition(status, MAIN_STATE_MANUAL); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) { - /* SEATBELT */ - main_res = main_state_transition(status, MAIN_STATE_SEATBELT); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) { + /* ALTCTL */ + main_res = main_state_transition(status, MAIN_STATE_ALTCTL); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) { - /* EASY */ - main_res = main_state_transition(status, MAIN_STATE_EASY); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) { + /* POSCTL */ + main_res = main_state_transition(status, MAIN_STATE_POSCTL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ @@ -496,8 +469,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { - /* EASY */ - main_res = main_state_transition(status, MAIN_STATE_EASY); + /* POSCTL */ + main_res = main_state_transition(status, MAIN_STATE_POSCTL); } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { /* MANUAL */ @@ -506,8 +479,9 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } } - if (main_res == TRANSITION_CHANGED) + if (main_res == TRANSITION_CHANGED) { ret = true; + } if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) { result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -520,25 +494,26 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } case VEHICLE_CMD_COMPONENT_ARM_DISARM: { - transition_result_t arming_res = TRANSITION_NOT_CHANGED; + // Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm. + // We use an float epsilon delta to test float equality. + if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) { + mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", cmd->param1); - if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) { - if (safety->safety_switch_available && !safety->safety_off && status->hil_state == HIL_STATE_OFF) { - print_reject_arm("NOT ARMING: Press safety switch first."); - arming_res = TRANSITION_DENIED; + } else { - } else { - arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); + // Flick to inair restore first if this comes from an onboard system + if (cmd->source_system == status->system_id && cmd->source_component == status->component_id) { + status->arming_state = ARMING_STATE_IN_AIR_RESTORE; } - if (arming_res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: ARMED by component arm cmd"); - result = VEHICLE_CMD_RESULT_ACCEPTED; - ret = true; + transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command"); - } else { + if (arming_res == TRANSITION_DENIED) { mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + + } else { + result = VEHICLE_CMD_RESULT_ACCEPTED; } } } @@ -568,7 +543,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } break; - /* Flight termination */ + /* Flight termination */ case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command //XXX: to enable the parachute, a param needs to be set @@ -586,6 +561,53 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } break; + case VEHICLE_CMD_DO_SET_HOME: { + bool use_current = cmd->param1 > 0.5f; + + if (use_current) { + /* use current position */ + if (status->condition_global_position_valid) { + home->lat = global_pos->lat; + home->lon = global_pos->lon; + home->alt = global_pos->alt; + + home->timestamp = hrt_absolute_time(); + + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + + } else { + /* use specified position */ + home->lat = cmd->param5; + home->lon = cmd->param6; + home->alt = cmd->param7; + + home->timestamp = hrt_absolute_time(); + + result = VEHICLE_CMD_RESULT_ACCEPTED; + } + + if (result == VEHICLE_CMD_RESULT_ACCEPTED) { + 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; + } + } + break; + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: case VEHICLE_CMD_PREFLIGHT_CALIBRATION: case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: @@ -594,7 +616,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; default: - /* warn about unsupported commands */ + /* Warn about unsupported commands, this makes sense because only commands + * to this component ID (or all) are passed by mavlink. */ answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); break; } @@ -618,6 +641,7 @@ int commander_thread_main(int argc, char *argv[]) commander_initialized = false; bool arm_tune_played = false; + bool was_armed = false; /* set parameters */ param_t _param_sys_type = param_find("MAV_TYPE"); @@ -631,8 +655,8 @@ int commander_thread_main(int argc, char *argv[]) char *main_states_str[MAIN_STATE_MAX]; main_states_str[0] = "MANUAL"; - main_states_str[1] = "SEATBELT"; - main_states_str[2] = "EASY"; + main_states_str[1] = "ALTCTL"; + main_states_str[2] = "POSCTL"; main_states_str[3] = "AUTO"; main_states_str[4] = "OFFBOARD"; @@ -726,7 +750,7 @@ int commander_thread_main(int argc, char *argv[]) pthread_attr_t commander_low_prio_attr; pthread_attr_init(&commander_low_prio_attr); - pthread_attr_setstacksize(&commander_low_prio_attr, 2992); + pthread_attr_setstacksize(&commander_low_prio_attr, 2900); struct sched_param param; (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); @@ -745,8 +769,9 @@ int commander_thread_main(int argc, char *argv[]) bool low_battery_voltage_actions_done = false; bool critical_battery_voltage_actions_done = false; - uint64_t last_idle_time = 0; - uint64_t start_time = 0; + hrt_abstime last_idle_time = 0; + hrt_abstime start_time = 0; + hrt_abstime last_auto_state_valid = 0; bool status_changed = true; bool param_init_forced = true; @@ -774,6 +799,9 @@ int commander_thread_main(int argc, char *argv[]) int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); struct vehicle_global_position_s global_position; memset(&global_position, 0, sizeof(global_position)); + /* Init EPH and EPV */ + global_position.eph = 1000.0f; + global_position.epv = 1000.0f; /* Subscribe to local position data */ int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); @@ -821,6 +849,11 @@ int commander_thread_main(int argc, char *argv[]) struct subsystem_info_s info; memset(&info, 0, sizeof(info)); + /* Subscribe to position setpoint triplet */ + int pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); + struct position_setpoint_triplet_s pos_sp_triplet; + memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet)); + control_status_leds(&status, &armed, true); /* now initialized */ @@ -871,6 +904,7 @@ int commander_thread_main(int argc, char *argv[]) /* re-check RC calibration */ rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd)); } + /* navigation parameters */ param_get(_param_takeoff_alt, &takeoff_alt); param_get(_param_enable_parachute, ¶chute_enabled); @@ -911,6 +945,7 @@ int commander_thread_main(int argc, char *argv[]) /* 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 (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch"); } @@ -926,7 +961,53 @@ int commander_thread_main(int argc, char *argv[]) } /* update condition_global_position_valid */ - check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.global_valid, &(status.condition_global_position_valid), &status_changed); + /* hysteresis for EPH/EPV */ + bool eph_epv_good; + + if (status.condition_global_position_valid) { + if (global_position.eph > eph_epv_threshold * 2.0f || global_position.epv > eph_epv_threshold * 2.0f) { + eph_epv_good = false; + + } else { + eph_epv_good = true; + } + + } else { + if (global_position.eph < eph_epv_threshold && global_position.epv < eph_epv_threshold) { + eph_epv_good = true; + + } else { + eph_epv_good = false; + } + } + + check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_epv_good, &(status.condition_global_position_valid), &status_changed); + + /* check if GPS fix is ok */ + + /* update home position */ + if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed && + (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) { + + home.lat = global_position.lat; + home.lon = global_position.lon; + home.alt = global_position.alt; + + 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 local position estimate */ orb_check(local_position_sub, &updated); @@ -937,10 +1018,11 @@ int commander_thread_main(int argc, char *argv[]) } /* update condition_local_position_valid and condition_local_altitude_valid */ - check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed); + check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && eph_epv_good, &(status.condition_local_position_valid), &status_changed); check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); static bool published_condition_landed_fw = false; + if (status.is_rotary_wing && status.condition_local_altitude_valid) { if (status.condition_landed != local_position.landed) { status.condition_landed = local_position.landed; @@ -954,6 +1036,7 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "#audio: IN AIR"); } } + } else { if (!published_condition_landed_fw) { status.condition_landed = false; // Fixedwing does not have a landing detector currently @@ -969,7 +1052,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(battery_status), battery_sub, &battery); /* only consider battery voltage if system has been running 2s and battery voltage is valid */ - if (status.hil_state == HIL_STATE_OFF && hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) { + if (hrt_absolute_time() > start_time + 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; @@ -1012,12 +1095,20 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; } + /* update position setpoint triplet */ + orb_check(pos_sp_triplet_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet); + } + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { /* compute system load */ uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; - if (last_idle_time > 0) - status.load = 1.0f - ((float)interval_runtime / 1e6f); //system load is time spent in non-idle + if (last_idle_time > 0) { + status.load = 1.0f - ((float)interval_runtime / 1e6f); //system load is time spent in non-idle + } last_idle_time = system_load.tasks[0].total_runtime; @@ -1074,57 +1165,10 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); - /* check if GPS fix is ok */ - float hdop_threshold_m = 4.0f; - float vdop_threshold_m = 8.0f; - - /* - * If horizontal dilution of precision (hdop / eph) - * and vertical diluation of precision (vdop / epv) - * are below a certain threshold (e.g. 4 m), AND - * home position is not yet set AND the last GPS - * GPS measurement is not older than two seconds AND - * the system is currently not armed, set home - * position to the current position. - */ - - if (!status.condition_home_position_valid && gps_position.fix_type >= 3 && - (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && - (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed - && global_position.global_valid) { - - /* copy position data to uORB home message, store it locally as well */ - home.lat = global_position.lat; - home.lon = global_position.lon; - home.alt = global_position.alt; - - warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt); - mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", 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); - } } - - /* - * XXX workaround: - * Prevent RC loss in HIL when sensors.cpp is only publishing sp_man at a low rate (e.g. 30Hz) - * which can trigger RC loss if the computer/simulator lags. - */ - uint64_t rc_timeout = status.hil_state == HIL_STATE_ON ? RC_TIMEOUT_HIL : RC_TIMEOUT; - /* start RC input check */ - if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + rc_timeout) { + if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; @@ -1140,22 +1184,22 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_lost = false; - transition_result_t res; // store all transitions results here + transition_result_t arming_res; // store all transitions results here /* arm/disarm by RC */ - res = TRANSITION_NOT_CHANGED; + arming_res = TRANSITION_NOT_CHANGED; - /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm + /* 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.condition_landed) && - sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + 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); - res = arming_state_transition(&status, &safety, new_arming_state, &armed); + arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed); stick_off_counter = 0; } else { @@ -1168,7 +1212,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 && - sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) { print_reject_arm("NOT ARMING: Press safety switch first."); @@ -1177,7 +1221,7 @@ int commander_thread_main(int argc, char *argv[]) print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); } else { - res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); } stick_on_counter = 0; @@ -1190,7 +1234,7 @@ int commander_thread_main(int argc, char *argv[]) stick_on_counter = 0; } - if (res == TRANSITION_CHANGED) { + if (arming_res == TRANSITION_CHANGED) { if (status.arming_state == ARMING_STATE_ARMED) { mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); @@ -1198,31 +1242,56 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); } - } else if (res == TRANSITION_DENIED) { + } else if (arming_res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); } if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { /* recover from failsafe */ - transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); + (void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); } - /* fill status according to mode switches */ - check_mode_switches(&sp_man, &status); - /* evaluate the main state machine according to mode switches */ - res = set_main_state_rc(&status); + transition_result_t main_res = set_main_state_rc(&status, &sp_man); /* play tune on mode change only if armed, blink LED always */ - if (res == TRANSITION_CHANGED) { + if (main_res == TRANSITION_CHANGED) { tune_positive(armed.armed); - } else if (res == TRANSITION_DENIED) { + } else if (main_res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied"); } + /* set navigation state */ + /* RETURN switch, overrides MISSION switch */ + if (sp_man.return_switch == SWITCH_POS_ON) { + /* switch to RTL if not already landed after RTL and home position set */ + status.set_nav_state = NAV_STATE_RTL; + status.set_nav_state_timestamp = hrt_absolute_time(); + + } else { + + /* LOITER switch */ + if (sp_man.loiter_switch == SWITCH_POS_ON) { + /* stick is in LOITER position */ + status.set_nav_state = NAV_STATE_LOITER; + status.set_nav_state_timestamp = hrt_absolute_time(); + + } else if (sp_man.loiter_switch != SWITCH_POS_NONE) { + /* stick is in MISSION position */ + status.set_nav_state = NAV_STATE_MISSION; + status.set_nav_state_timestamp = hrt_absolute_time(); + + } else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) && + pos_sp_triplet.nav_state == NAV_STATE_RTL) { + /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ + status.set_nav_state = NAV_STATE_MISSION; + status.set_nav_state_timestamp = hrt_absolute_time(); + } + } + } else { if (!status.rc_signal_lost) { mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); @@ -1230,60 +1299,58 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; } - /* switch to OFFBOARD mode if offboard signal available */ - transition_result_t res = main_state_transition(&status, MAIN_STATE_OFFBOARD); + if (armed.armed) { + if (status.main_state == MAIN_STATE_AUTO) { + /* check if AUTO mode still allowed */ + transition_result_t auto_res = main_state_transition(&status, MAIN_STATE_AUTO); - if (res == TRANSITION_DENIED) { - /* can't switch to OFFBOARD, do normal failsafe if needed */ - if (armed.armed) { - if (status.main_state == MAIN_STATE_AUTO) { - /* check if AUTO mode still allowed */ - transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + if (auto_res == TRANSITION_NOT_CHANGED) { + last_auto_state_valid = hrt_absolute_time(); + } - if (res == TRANSITION_DENIED) { - /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ - res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + /* still invalid state after the timeout interval, execute failsafe */ + if ((hrt_elapsed_time(&last_auto_state_valid) > FAILSAFE_DEFAULT_TIMEOUT) && (auto_res == TRANSITION_DENIED)) { + /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ + auto_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); - if (res == TRANSITION_DENIED) { - /* LAND not allowed, set TERMINATION state */ - transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); - } + if (auto_res == TRANSITION_DENIED) { + /* LAND not allowed, set TERMINATION state */ + (void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); } + } - } else if (status.main_state == MAIN_STATE_OFFBOARD) { + } else { + transition_result_t offboard_res = TRANSITION_DENIED; + if (status.main_state == MAIN_STATE_OFFBOARD) { /* check if OFFBOARD mode still allowed */ - transition_result_t res = main_state_transition(&status, MAIN_STATE_OFFBOARD); + offboard_res = main_state_transition(&status, MAIN_STATE_OFFBOARD); + } - if (res == TRANSITION_DENIED) { - /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ - res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + if (offboard_res == TRANSITION_DENIED) { + /* not in OFFBOARD mode or OFFBOARD is not allowed anymore, switch to failsafe */ + transition_result_t failsafe_res = TRANSITION_DENIED; - if (res == TRANSITION_DENIED) { - /* LAND not allowed, set TERMINATION state */ - transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); - } + if (!status.condition_landed) { + /* vehicle is not landed, try to perform RTL */ + failsafe_res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); } - } else { - /* failsafe for manual modes */ - transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); + if (failsafe_res == TRANSITION_DENIED) { + /* RTL not allowed (no global position estimate) or not wanted, try LAND */ + failsafe_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); - if (res == TRANSITION_DENIED) { - /* RTL not allowed (no global position estimate), try LAND */ - res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); - - if (res == TRANSITION_DENIED) { + if (failsafe_res == TRANSITION_DENIED) { /* LAND not allowed, set TERMINATION state */ - res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + (void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); } } } + } - } else { - if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { - /* reset failsafe when disarmed */ - transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); - } + } else { + if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { + /* reset failsafe when disarmed */ + (void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); } } } @@ -1313,8 +1380,8 @@ int commander_thread_main(int argc, char *argv[]) } // TODO remove this hack - /* flight termination in manual mode if assisted switch is on easy position */ - if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { + /* flight termination in manual mode if assist switch is on POSCTL position */ + if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.posctl_switch == SWITCH_POS_ON) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(armed.armed); } @@ -1328,8 +1395,9 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - if (handle_command(&status, &safety, &cmd, &armed)) + if (handle_command(&status, &safety, &cmd, &armed, &home, &global_position, &home_pub)) { status_changed = true; + } } /* check which state machines for changes, clear "changed" flag */ @@ -1343,8 +1411,34 @@ int commander_thread_main(int argc, char *argv[]) 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_epv_threshold) && (global_position.epv < eph_epv_threshold)) { + + // TODO remove code duplication + home.lat = global_position.lat; + home.lon = global_position.lon; + home.alt = global_position.alt; + + 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; + } } + was_armed = armed.armed; + if (main_state_changed) { status_changed = true; mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]); @@ -1506,21 +1600,24 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a } else if (actuator_armed->ready_to_arm) { /* ready to arm, blink at 1Hz */ - if (leds_counter % 20 == 0) + if (leds_counter % 20 == 0) { led_toggle(LED_BLUE); + } } else { /* not ready to arm, blink at 10Hz */ - if (leds_counter % 2 == 0) + if (leds_counter % 2 == 0) { led_toggle(LED_BLUE); + } } #endif /* give system warnings on error LED, XXX maybe add memory usage warning too */ if (status->load > 0.95f) { - if (leds_counter % 2 == 0) + if (leds_counter % 2 == 0) { led_toggle(LED_AMBER); + } } else { led_off(LED_AMBER); @@ -1529,77 +1626,14 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a leds_counter++; } -void -check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status) -{ - /* main mode switch */ - if (!isfinite(sp_man->mode_switch)) { - /* default to manual if signal is invalid */ - status->mode_switch = MODE_SWITCH_MANUAL; - - } else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) { - status->mode_switch = MODE_SWITCH_AUTO; - - } else if (sp_man->mode_switch < -STICK_ON_OFF_LIMIT) { - status->mode_switch = MODE_SWITCH_MANUAL; - - } else { - status->mode_switch = MODE_SWITCH_ASSISTED; - } - - /* return switch */ - if (!isfinite(sp_man->return_switch)) { - status->return_switch = RETURN_SWITCH_NONE; - - } else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) { - status->return_switch = RETURN_SWITCH_RETURN; - - } else { - status->return_switch = RETURN_SWITCH_NORMAL; - } - - /* assisted switch */ - if (!isfinite(sp_man->assisted_switch)) { - status->assisted_switch = ASSISTED_SWITCH_SEATBELT; - - } else if (sp_man->assisted_switch > STICK_ON_OFF_LIMIT) { - status->assisted_switch = ASSISTED_SWITCH_EASY; - - } else { - status->assisted_switch = ASSISTED_SWITCH_SEATBELT; - } - - /* mission switch */ - if (!isfinite(sp_man->mission_switch)) { - status->mission_switch = MISSION_SWITCH_NONE; - - } else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) { - status->mission_switch = MISSION_SWITCH_LOITER; - - } else { - status->mission_switch = MISSION_SWITCH_MISSION; - } - - /* offboard switch */ - if (!isfinite(sp_man->offboard_switch)) { - status->offboard_switch = OFFBOARD_SWITCH_NONE; - - } else if (sp_man->offboard_switch > STICK_ON_OFF_LIMIT) { - status->offboard_switch = OFFBOARD_SWITCH_OFFBOARD; - - } else { - status->offboard_switch = OFFBOARD_SWITCH_ONBOARD; - } -} - transition_result_t -set_main_state_rc(struct vehicle_status_s *status) +set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man) { /* set main state according to RC switches */ transition_result_t res = TRANSITION_DENIED; /* offboard switch overrides main switch */ - if (status->offboard_switch == OFFBOARD_SWITCH_OFFBOARD) { + if (sp_man->offboard_switch == SWITCH_POS_ON) { res = main_state_transition(status, MAIN_STATE_OFFBOARD); if (res == TRANSITION_DENIED) { print_reject_mode(status, "OFFBOARD"); @@ -1609,49 +1643,58 @@ set_main_state_rc(struct vehicle_status_s *status) } } - /* offboard switched off or denied, check mode switch */ - switch (status->mode_switch) { - case MODE_SWITCH_MANUAL: + /* offboard switched off or denied, check main mode switch */ + switch (sp_man->mode_switch) { + case SWITCH_POS_NONE: + res = TRANSITION_NOT_CHANGED; + break; + + case SWITCH_POS_OFF: // MANUAL res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; - case MODE_SWITCH_ASSISTED: - if (status->assisted_switch == ASSISTED_SWITCH_EASY) { - res = main_state_transition(status, MAIN_STATE_EASY); + case SWITCH_POS_MIDDLE: // ASSIST + if (sp_man->posctl_switch == SWITCH_POS_ON) { + res = main_state_transition(status, MAIN_STATE_POSCTL); - if (res != TRANSITION_DENIED) + if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state + } - // else fallback to SEATBELT - print_reject_mode(status, "EASY"); + // else fallback to ALTCTL + print_reject_mode(status, "POSCTL"); } - res = main_state_transition(status, MAIN_STATE_SEATBELT); + res = main_state_transition(status, MAIN_STATE_ALTCTL); - if (res != TRANSITION_DENIED) + if (res != TRANSITION_DENIED) { break; // changed successfully or already in this mode + } - if (status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages - print_reject_mode(status, "SEATBELT"); + if (sp_man->posctl_switch != SWITCH_POS_ON) { + print_reject_mode(status, "ALTCTL"); + } // else fallback to MANUAL res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; - case MODE_SWITCH_AUTO: + case SWITCH_POS_ON: // AUTO res = main_state_transition(status, MAIN_STATE_AUTO); - if (res != TRANSITION_DENIED) + if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state + } - // else fallback to SEATBELT (EASY likely will not work too) + // else fallback to ALTCTL (POSCTL likely will not work too) print_reject_mode(status, "AUTO"); - res = main_state_transition(status, MAIN_STATE_SEATBELT); + res = main_state_transition(status, MAIN_STATE_ALTCTL); - if (res != TRANSITION_DENIED) + if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state + } // else fallback to MANUAL res = main_state_transition(status, MAIN_STATE_MANUAL); @@ -1673,9 +1716,8 @@ set_control_mode() control_mode.flag_armed = armed.armed; control_mode.flag_external_manual_override_ok = !status.is_rotary_wing; control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON; - - control_mode.flag_control_auto_enabled = false; control_mode.flag_control_offboard_enabled = false; + control_mode.flag_control_termination_enabled = false; /* set this flag when navigator should act */ @@ -1686,6 +1728,7 @@ set_control_mode() switch (status.main_state) { case MAIN_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; control_mode.flag_control_attitude_enabled = status.is_rotary_wing; control_mode.flag_control_altitude_enabled = false; @@ -1694,8 +1737,9 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; - case MAIN_STATE_SEATBELT: + case MAIN_STATE_ALTCTL: control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = true; control_mode.flag_control_altitude_enabled = true; @@ -1704,8 +1748,9 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; - case MAIN_STATE_EASY: + case MAIN_STATE_POSCTL: control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = true; control_mode.flag_control_altitude_enabled = true; @@ -1716,9 +1761,11 @@ set_control_mode() case MAIN_STATE_AUTO: navigator_enabled = true; + break; case MAIN_STATE_OFFBOARD: control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = false; control_mode.flag_control_offboard_enabled = true; switch (sp_offboard.mode) { @@ -1766,6 +1813,7 @@ set_control_mode() case FAILSAFE_STATE_TERMINATION: /* disable all controllers on termination */ control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = false; control_mode.flag_control_attitude_enabled = false; control_mode.flag_control_position_enabled = false; @@ -1785,8 +1833,11 @@ set_control_mode() control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_position_enabled = true; - control_mode.flag_control_velocity_enabled = true; + + /* in failsafe LAND mode position may be not available */ + control_mode.flag_control_position_enabled = status.condition_local_position_valid; + control_mode.flag_control_velocity_enabled = status.condition_local_position_valid; + control_mode.flag_control_altitude_enabled = true; control_mode.flag_control_climb_rate_enabled = true; } @@ -1827,7 +1878,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: @@ -1877,8 +1928,9 @@ void *commander_low_prio_loop(void *arg) int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200); /* timed out - periodic check for thread_should_exit, etc. */ - if (pret == 0) + if (pret == 0) { continue; + } /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { @@ -1893,8 +1945,9 @@ void *commander_low_prio_loop(void *arg) if (cmd.command == VEHICLE_CMD_DO_SET_MODE || cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM || cmd.command == VEHICLE_CMD_NAV_TAKEOFF || - cmd.command == VEHICLE_CMD_DO_SET_SERVO) + cmd.command == VEHICLE_CMD_DO_SET_SERVO) { continue; + } /* only handle low-priority commands here */ switch (cmd.command) { @@ -1972,6 +2025,7 @@ void *commander_low_prio_loop(void *arg) /* airspeed calibration */ answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); calib_ret = do_airspeed_calibration(mavlink_fd); + } else if ((int)(cmd.param4) == 0) { /* RC calibration ended - have we been in one worth confirming? */ if (status.rc_input_blocked) { @@ -1986,10 +2040,12 @@ void *commander_low_prio_loop(void *arg) } - if (calib_ret == OK) + if (calib_ret == OK) { tune_positive(true); - else + + } else { tune_negative(true); + } arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); @@ -2009,11 +2065,13 @@ void *commander_low_prio_loop(void *arg) mavlink_log_critical(mavlink_fd, "#audio: parameters load ERROR"); /* convenience as many parts of NuttX use negative errno */ - if (ret < 0) + if (ret < 0) { ret = -ret; + } - if (ret < 1000) + if (ret < 1000) { mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret)); + } answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); } @@ -2029,11 +2087,13 @@ void *commander_low_prio_loop(void *arg) mavlink_log_critical(mavlink_fd, "#audio: parameters save error"); /* convenience as many parts of NuttX use negative errno */ - if (ret < 0) + if (ret < 0) { ret = -ret; + } - if (ret < 1000) + if (ret < 1000) { mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret)); + } answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); } @@ -2043,8 +2103,8 @@ void *commander_low_prio_loop(void *arg) } case VEHICLE_CMD_START_RX_PAIR: - /* handled in the IO driver */ - break; + /* handled in the IO driver */ + break; default: /* don't answer on unsupported commands, it will be done in main loop */ diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index fe6c9bfaa..940a04aa1 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -113,17 +113,22 @@ void buzzer_deinit() close(buzzer); } -void set_tune(int tune) { +void set_tune(int tune) +{ unsigned int new_tune_duration = tune_durations[tune]; + /* don't interrupt currently playing non-repeating tune by repeating */ if (tune_end == 0 || new_tune_duration != 0 || hrt_absolute_time() > tune_end) { /* allow interrupting current non-repeating tune by the same tune */ if (tune != tune_current || new_tune_duration != 0) { ioctl(buzzer, TONE_SET_ALARM, tune); } + tune_current = tune; + if (new_tune_duration != 0) { tune_end = hrt_absolute_time() + new_tune_duration; + } else { tune_end = 0; } @@ -138,6 +143,7 @@ void tune_positive(bool use_buzzer) blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; rgbled_set_color(RGBLED_COLOR_GREEN); rgbled_set_mode(RGBLED_MODE_BLINK_FAST); + if (use_buzzer) { set_tune(TONE_NOTIFY_POSITIVE_TUNE); } @@ -151,6 +157,7 @@ void tune_neutral(bool use_buzzer) blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; rgbled_set_color(RGBLED_COLOR_WHITE); rgbled_set_mode(RGBLED_MODE_BLINK_FAST); + if (use_buzzer) { set_tune(TONE_NOTIFY_NEUTRAL_TUNE); } @@ -164,6 +171,7 @@ void tune_negative(bool use_buzzer) blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; rgbled_set_color(RGBLED_COLOR_RED); rgbled_set_mode(RGBLED_MODE_BLINK_FAST); + if (use_buzzer) { set_tune(TONE_NOTIFY_NEGATIVE_TUNE); } @@ -198,16 +206,10 @@ int led_init() return ERROR; } - /* the blue LED is only available on FMUv1 but not FMUv2 */ -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 - - if (ioctl(leds, LED_ON, LED_BLUE)) { - warnx("Blue LED: ioctl fail\n"); - return ERROR; - } - -#endif + /* the blue LED is only available on FMUv1 & AeroCore but not FMUv2 */ + (void)ioctl(leds, LED_ON, LED_BLUE); + /* we consider the amber led mandatory */ if (ioctl(leds, LED_ON, LED_AMBER)) { warnx("Amber LED: ioctl fail\n"); return ERROR; @@ -217,11 +219,7 @@ int led_init() rgbleds = open(RGBLED_DEVICE_PATH, 0); if (rgbleds == -1) { -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 - errx(1, "Unable to open " RGBLED_DEVICE_PATH); -#else - warnx("No RGB LED found"); -#endif + warnx("No RGB LED found at " RGBLED_DEVICE_PATH); } return 0; @@ -254,22 +252,25 @@ int led_off(int led) void rgbled_set_color(rgbled_color_t color) { - if (rgbleds != -1) + if (rgbleds != -1) { ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color); + } } void rgbled_set_mode(rgbled_mode_t mode) { - if (rgbleds != -1) + if (rgbleds != -1) { ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode); + } } void rgbled_set_pattern(rgbled_pattern_t *pattern) { - if (rgbleds != -1) + if (rgbleds != -1) { ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); + } } float battery_remaining_estimate_voltage(float voltage, float discharged) @@ -309,6 +310,7 @@ float battery_remaining_estimate_voltage(float voltage, float discharged) if (bat_capacity > 0.0f) { /* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */ ret = fminf(remaining_voltage, 1.0f - discharged / bat_capacity); + } else { /* else use voltage */ ret = remaining_voltage; diff --git a/src/modules/commander/commander_tests/commander_tests.cpp b/src/modules/commander/commander_tests/commander_tests.cpp index 6e72cf0d9..0abb84a82 100644 --- a/src/modules/commander/commander_tests/commander_tests.cpp +++ b/src/modules/commander/commander_tests/commander_tests.cpp @@ -48,8 +48,7 @@ extern "C" __EXPORT int commander_tests_main(int argc, char *argv[]); int commander_tests_main(int argc, char *argv[]) { - state_machine_helper_test(); - //state_machine_test(); + stateMachineHelperTest(); return 0; } 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 40bedd9f3..ee0d08391 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -49,13 +49,12 @@ public: StateMachineHelperTest(); virtual ~StateMachineHelperTest(); - virtual const char* run_tests(); + virtual void runTests(void); private: - const char* arming_state_transition_test(); - const char* arming_state_transition_arm_disarm_test(); - const char* main_state_transition_test(); - const char* is_safe_test(); + bool armingStateTransitionTest(); + bool mainStateTransitionTest(); + bool isSafeTest(); }; StateMachineHelperTest::StateMachineHelperTest() { @@ -64,61 +63,242 @@ StateMachineHelperTest::StateMachineHelperTest() { StateMachineHelperTest::~StateMachineHelperTest() { } -const char* -StateMachineHelperTest::arming_state_transition_test() +bool StateMachineHelperTest::armingStateTransitionTest(void) { + // These are the critical values from vehicle_status_s and actuator_armed_s which must be primed + // to simulate machine state prior to testing an arming state transition. This structure is also + // use to represent the expected machine state after the transition has been requested. + typedef struct { + arming_state_t arming_state; // vehicle_status_s.arming_state + 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. + typedef struct { + const char* assertMsg; // Text to show when test case fails + ArmingTransitionVolatileState_t current_state; // Machine state prior to transtion + hil_state_t hil_state; // Current vehicle_status_s.hil_state + bool condition_system_sensors_initialized; // Current vehicle_status_s.condition_system_sensors_initialized + bool safety_switch_available; // Current safety_s.safety_switch_available + bool safety_off; // Current safety_s.safety_off + arming_state_t requested_state; // Requested arming state to transition to + 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 + #define ATT_READY_TO_ARM true + #define ATT_NOT_READY_TO_ARM false + #define ATT_SENSORS_INITIALIZED true + #define ATT_SENSORS_NOT_INITIALIZED false + #define ATT_SAFETY_AVAILABLE true + #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 }, + + // 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 }, + + { "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 }, + + { "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 }, + + { "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 }, + + { "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 }, + + { "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 }, + + { "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 }, + + { "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 }, + + { "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 }, + + { "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 }, + + { "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 }, + + { "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 }, + + // 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 }, + + // Safety switch arming tests + + { "transition: init to standby, 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 }, + + { "transition: init to standby, 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 }, + + // 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 }, + + // 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 }, + + { "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 }, + + { "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 }, + + { "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 }, + + { "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 }, + + { "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 }, + + { "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 }, + + { "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 }, + + { "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 }, + + { "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 }, + + // 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 }, + + // 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 }, + }; + struct vehicle_status_s status; - struct safety_s safety; - arming_state_t new_arming_state; - struct actuator_armed_s armed; - - // Identical states. - status.arming_state = ARMING_STATE_INIT; - new_arming_state = ARMING_STATE_INIT; - mu_assert("no transition: identical states", - TRANSITION_NOT_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)); - - // INIT to STANDBY. - armed.armed = false; - armed.ready_to_arm = false; - status.arming_state = ARMING_STATE_INIT; - status.condition_system_sensors_initialized = true; - new_arming_state = ARMING_STATE_STANDBY; - mu_assert("transition: init to standby", - TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)); - mu_assert("current state: standby", ARMING_STATE_STANDBY == status.arming_state); - mu_assert("not armed", !armed.armed); - mu_assert("ready to arm", armed.ready_to_arm); - - // INIT to STANDBY, sensors not initialized. - armed.armed = false; - armed.ready_to_arm = false; - status.arming_state = ARMING_STATE_INIT; - status.condition_system_sensors_initialized = false; - new_arming_state = ARMING_STATE_STANDBY; - mu_assert("no transition: sensors not initialized", - TRANSITION_DENIED == arming_state_transition(&status, &safety, new_arming_state, &armed)); - mu_assert("current state: init", ARMING_STATE_INIT == status.arming_state); - mu_assert("not armed", !armed.armed); - mu_assert("not ready to arm", !armed.ready_to_arm); - - return 0; -} - -const char* -StateMachineHelperTest::arming_state_transition_arm_disarm_test() -{ - struct vehicle_status_s status; - struct safety_s safety; - arming_state_t new_arming_state; + struct safety_s safety; struct actuator_armed_s armed; - - // TODO(sjwilks): ARM then DISARM. - return 0; + + size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]); + for (size_t i=0; i<cArmingTransitionTests; i++) { + const ArmingTransitionTest_t* test = &rgArmingTransitionTests[i]; + + // Setup initial machine state + status.arming_state = test->current_state.arming_state; + status.condition_system_sensors_initialized = test->condition_system_sensors_initialized; + status.hil_state = test->hil_state; + safety.safety_switch_available = test->safety_switch_available; + 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); + + // 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); + ut_assert(test->assertMsg, armed.armed == test->expected_state.armed); + ut_assert(test->assertMsg, armed.ready_to_arm == test->expected_state.ready_to_arm); + } + + return true; } -const char* -StateMachineHelperTest::main_state_transition_test() +bool StateMachineHelperTest::mainStateTransitionTest(void) { struct vehicle_status_s current_state; main_state_t new_main_state; @@ -126,70 +306,69 @@ StateMachineHelperTest::main_state_transition_test() // Identical states. current_state.main_state = MAIN_STATE_MANUAL; new_main_state = MAIN_STATE_MANUAL; - mu_assert("no transition: identical states", + ut_assert("no transition: identical states", TRANSITION_NOT_CHANGED == main_state_transition(¤t_state, new_main_state)); - mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); // AUTO to MANUAL. current_state.main_state = MAIN_STATE_AUTO; new_main_state = MAIN_STATE_MANUAL; - mu_assert("transition changed: auto to manual", + ut_assert("transition changed: auto to manual", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - mu_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state); + ut_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state); - // MANUAL to SEATBELT. + // MANUAL to ALTCTRL. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = true; - new_main_state = MAIN_STATE_SEATBELT; - mu_assert("tranisition: manual to seatbelt", + new_main_state = MAIN_STATE_ALTCTL; + ut_assert("tranisition: manual to altctrl", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - mu_assert("new state: seatbelt", MAIN_STATE_SEATBELT == current_state.main_state); + ut_assert("new state: altctrl", MAIN_STATE_ALTCTL == current_state.main_state); - // MANUAL to SEATBELT, invalid local altitude. + // MANUAL to ALTCTRL, invalid local altitude. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = false; - new_main_state = MAIN_STATE_SEATBELT; - mu_assert("no transition: invalid local altitude", + new_main_state = MAIN_STATE_ALTCTL; + ut_assert("no transition: invalid local altitude", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); - mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); - // MANUAL to EASY. + // MANUAL to POSCTRL. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = true; - new_main_state = MAIN_STATE_EASY; - mu_assert("transition: manual to easy", + new_main_state = MAIN_STATE_POSCTL; + ut_assert("transition: manual to posctrl", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - mu_assert("current state: easy", MAIN_STATE_EASY == current_state.main_state); + ut_assert("current state: posctrl", MAIN_STATE_POSCTL == current_state.main_state); - // MANUAL to EASY, invalid local position. + // MANUAL to POSCTRL, invalid local position. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = false; - new_main_state = MAIN_STATE_EASY; - mu_assert("no transition: invalid position", + new_main_state = MAIN_STATE_POSCTL; + ut_assert("no transition: invalid position", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); - mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); // MANUAL to AUTO. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_global_position_valid = true; new_main_state = MAIN_STATE_AUTO; - mu_assert("transition: manual to auto", + ut_assert("transition: manual to auto", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - mu_assert("current state: auto", MAIN_STATE_AUTO == current_state.main_state); + ut_assert("current state: auto", MAIN_STATE_AUTO == current_state.main_state); // MANUAL to AUTO, invalid global position. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_global_position_valid = false; new_main_state = MAIN_STATE_AUTO; - mu_assert("no transition: invalid global position", + ut_assert("no transition: invalid global position", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); - mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); - return 0; + return true; } -const char* -StateMachineHelperTest::is_safe_test() +bool StateMachineHelperTest::isSafeTest(void) { struct vehicle_status_s current_state; struct safety_s safety; @@ -199,49 +378,45 @@ StateMachineHelperTest::is_safe_test() armed.lockdown = false; safety.safety_switch_available = true; safety.safety_off = false; - mu_assert("is safe: not armed", is_safe(¤t_state, &safety, &armed)); + ut_assert("is safe: not armed", is_safe(¤t_state, &safety, &armed)); armed.armed = false; armed.lockdown = true; safety.safety_switch_available = true; safety.safety_off = true; - mu_assert("is safe: software lockdown", is_safe(¤t_state, &safety, &armed)); + ut_assert("is safe: software lockdown", is_safe(¤t_state, &safety, &armed)); armed.armed = true; armed.lockdown = false; safety.safety_switch_available = true; safety.safety_off = true; - mu_assert("not safe: safety off", !is_safe(¤t_state, &safety, &armed)); + ut_assert("not safe: safety off", !is_safe(¤t_state, &safety, &armed)); armed.armed = true; armed.lockdown = false; safety.safety_switch_available = true; safety.safety_off = false; - mu_assert("is safe: safety off", is_safe(¤t_state, &safety, &armed)); + ut_assert("is safe: safety off", is_safe(¤t_state, &safety, &armed)); armed.armed = true; armed.lockdown = false; safety.safety_switch_available = false; safety.safety_off = false; - mu_assert("not safe: no safety switch", !is_safe(¤t_state, &safety, &armed)); + ut_assert("not safe: no safety switch", !is_safe(¤t_state, &safety, &armed)); - return 0; + return true; } -const char* -StateMachineHelperTest::run_tests() +void StateMachineHelperTest::runTests(void) { - mu_run_test(arming_state_transition_test); - mu_run_test(arming_state_transition_arm_disarm_test); - mu_run_test(main_state_transition_test); - mu_run_test(is_safe_test); - - return 0; + ut_run_test(armingStateTransitionTest); + ut_run_test(mainStateTransitionTest); + ut_run_test(isSafeTest); } -void -state_machine_helper_test() +void stateMachineHelperTest(void) { StateMachineHelperTest* test = new StateMachineHelperTest(); - test->UnitTest::print_results(test->run_tests()); + test->runTests(); + test->printResults(); } diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.h b/src/modules/commander/commander_tests/state_machine_helper_test.h index 10a68e602..bbf66255e 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.h +++ b/src/modules/commander/commander_tests/state_machine_helper_test.h @@ -39,6 +39,6 @@ #ifndef STATE_MACHINE_HELPER_TEST_H_ #define STATE_MACHINE_HELPER_TEST_ -void state_machine_helper_test(); +void stateMachineHelperTest(void); #endif /* STATE_MACHINE_HELPER_TEST_H_ */ diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 30cd0d48d..cbc2844c1 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -110,8 +110,9 @@ int do_gyro_calibration(int mavlink_fd) gyro_scale.z_offset += gyro_report.z; calibration_counter++; - if (calibration_counter % (calibration_count / 20) == 0) + if (calibration_counter % (calibration_count / 20) == 0) { mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count); + } } else { poll_errcount++; @@ -163,8 +164,9 @@ int do_gyro_calibration(int mavlink_fd) /* apply new offsets */ fd = open(GYRO_DEVICE_PATH, 0); - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) { warn("WARNING: failed to apply new offsets for gyro"); + } close(fd); @@ -178,9 +180,9 @@ int do_gyro_calibration(int mavlink_fd) float mag_last = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]); - if (mag_last > M_PI_F) mag_last -= 2 * M_PI_F; + if (mag_last > M_PI_F) { mag_last -= 2 * M_PI_F; } - if (mag_last < -M_PI_F) mag_last += 2 * M_PI_F; + if (mag_last < -M_PI_F) { mag_last += 2 * M_PI_F; } uint64_t last_time = hrt_absolute_time(); @@ -220,15 +222,15 @@ int do_gyro_calibration(int mavlink_fd) //float mag = -atan2f(magNav(1),magNav(0)); float mag = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]); - if (mag > M_PI_F) mag -= 2 * M_PI_F; + if (mag > M_PI_F) { mag -= 2 * M_PI_F; } - if (mag < -M_PI_F) mag += 2 * M_PI_F; + if (mag < -M_PI_F) { mag += 2 * M_PI_F; } float diff = mag - mag_last; - if (diff > M_PI_F) diff -= 2 * M_PI_F; + if (diff > M_PI_F) { diff -= 2 * M_PI_F; } - if (diff < -M_PI_F) diff += 2 * M_PI_F; + if (diff < -M_PI_F) { diff += 2 * M_PI_F; } baseline_integral += diff; mag_last = mag; diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 4ebf266f4..0ead22f77 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -72,7 +72,7 @@ int do_mag_calibration(int mavlink_fd) uint64_t calibration_interval = 45 * 1000 * 1000; /* maximum 500 values */ - const unsigned int calibration_maxcount = 500; + const unsigned int calibration_maxcount = 240; unsigned int calibration_counter; struct mag_scale mscale_null = { @@ -121,9 +121,24 @@ int do_mag_calibration(int mavlink_fd) if (x == NULL || y == NULL || z == NULL) { mavlink_log_critical(mavlink_fd, "ERROR: out of memory"); + + /* clean up */ + if (x != NULL) { + free(x); + } + + if (y != NULL) { + free(y); + } + + if (z != NULL) { + free(z); + } + res = ERROR; return res; } + } else { /* exit */ return ERROR; @@ -163,8 +178,9 @@ int do_mag_calibration(int mavlink_fd) calibration_counter++; - if (calibration_counter % (calibration_maxcount / 20) == 0) + 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++; @@ -198,14 +214,17 @@ int do_mag_calibration(int mavlink_fd) } } - if (x != NULL) + if (x != NULL) { free(x); + } - if (y != NULL) + if (y != NULL) { free(y); + } - if (z != NULL) + if (z != NULL) { free(z); + } if (res == OK) { /* apply calibration and set parameters */ @@ -234,23 +253,29 @@ int do_mag_calibration(int mavlink_fd) if (res == OK) { /* set parameters */ - if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) + if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) { res = ERROR; + } - if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) + if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) { res = ERROR; + } - if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) + if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) { res = ERROR; + } - if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) + if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) { res = ERROR; + } - if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) + if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) { res = ERROR; + } - if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) + if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) { res = ERROR; + } if (res != OK) { mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index 554dfcb08..27ca5c182 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -47,3 +47,7 @@ SRCS = commander.cpp \ baro_calibration.cpp \ rc_calibration.cpp \ airspeed_calibration.cpp + +MODULE_STACKSIZE = 1200 + +MAXOPTIMIZATION = -Os diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index 65c8e8947..9589c9acb 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -12,8 +12,8 @@ enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_MANUAL = 1, - PX4_CUSTOM_MAIN_MODE_SEATBELT, - PX4_CUSTOM_MAIN_MODE_EASY, + PX4_CUSTOM_MAIN_MODE_ALTCTL, + PX4_CUSTOM_MAIN_MODE_POSCTL, PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_MAIN_MODE_OFFBOARD, }; diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp index 41f3ca0aa..0776894fb 100644 --- a/src/modules/commander/rc_calibration.cpp +++ b/src/modules/commander/rc_calibration.cpp @@ -69,11 +69,11 @@ int do_trim_calibration(int mavlink_fd) orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); /* set parameters */ - float p = sp.roll; + float p = sp.y; param_set(param_find("TRIM_ROLL"), &p); - p = sp.pitch; + p = sp.x; param_set(param_find("TRIM_PITCH"), &p); - p = sp.yaw; + p = sp.r; param_set(param_find("TRIM_YAW"), &p); /* store to permanent storage */ diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 9cecf5371..aaabde66b 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -69,10 +69,44 @@ static bool arming_state_changed = true; static bool main_state_changed = true; static bool failsafe_state_changed = true; +// This array defines the arming state transitions. The rows are the new state, and the columns +// are the current state. Using new state and current state you can index into the array which +// 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] = { + // 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 +}; + +// You can index into the array with an arming_state_t in order to get it's textual representation +static const char *state_names[ARMING_STATE_MAX] = { + "ARMING_STATE_INIT", + "ARMING_STATE_STANDBY", + "ARMING_STATE_ARMED", + "ARMING_STATE_ARMED_ERROR", + "ARMING_STATE_STANDBY_ERROR", + "ARMING_STATE_REBOOT", + "ARMING_STATE_IN_AIR_RESTORE", +}; + transition_result_t -arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed) +arming_state_transition(struct vehicle_status_s *status, /// current vehicle status + const struct safety_s *safety, /// current safety settings + arming_state_t new_arming_state, /// arming state requested + struct actuator_armed_s *armed, /// current armed status + 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); + /* * Perform an atomic state update */ @@ -85,7 +119,6 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * ret = TRANSITION_NOT_CHANGED; } else { - /* enforce lockdown in HIL */ if (status->hil_state == HIL_STATE_ON) { armed->lockdown = true; @@ -94,95 +127,43 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * armed->lockdown = false; } - switch (new_arming_state) { - case ARMING_STATE_INIT: - - /* allow going back from INIT for calibration */ - if (status->arming_state == ARMING_STATE_STANDBY) { - ret = TRANSITION_CHANGED; - armed->armed = false; - armed->ready_to_arm = false; - } - - break; - - case ARMING_STATE_STANDBY: - - /* allow coming from INIT and disarming from ARMED */ - if (status->arming_state == ARMING_STATE_INIT - || status->arming_state == ARMING_STATE_ARMED - || status->hil_state == HIL_STATE_ON) { + // Check that we have a valid state transition + bool valid_transition = arming_transitions[new_arming_state][status->arming_state]; + + if (valid_transition) { + // We have a good transition. Now perform any secondary validation. + if (new_arming_state == ARMING_STATE_ARMED) { + // Fail transition if we need safety switch press + // Allow 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 && safety->safety_switch_available && !safety->safety_off) { + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first."); + } - /* sensors need to be initialized for STANDBY state */ - if (status->condition_system_sensors_initialized) { - ret = TRANSITION_CHANGED; - armed->armed = false; - armed->ready_to_arm = true; + valid_transition = false; } - } - - break; - - case ARMING_STATE_ARMED: - /* allow arming from STANDBY and IN-AIR-RESTORE */ - if ((status->arming_state == ARMING_STATE_STANDBY - || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) - && (!safety->safety_switch_available || safety->safety_off || status->hil_state == HIL_STATE_ON)) { /* only allow arming if safety is off */ - ret = TRANSITION_CHANGED; - armed->armed = true; - armed->ready_to_arm = true; - } - - break; - - case ARMING_STATE_ARMED_ERROR: - - /* an armed error happens when ARMED obviously */ - if (status->arming_state == ARMING_STATE_ARMED) { - ret = TRANSITION_CHANGED; - armed->armed = true; - armed->ready_to_arm = false; + } else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) { + new_arming_state = ARMING_STATE_STANDBY_ERROR; } + } - break; - - case ARMING_STATE_STANDBY_ERROR: - - /* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */ - if (status->arming_state == ARMING_STATE_STANDBY - || status->arming_state == ARMING_STATE_INIT - || status->arming_state == ARMING_STATE_ARMED_ERROR) { - ret = TRANSITION_CHANGED; - armed->armed = false; - armed->ready_to_arm = false; - } - - break; - - case ARMING_STATE_REBOOT: - - /* an armed error happens when ARMED obviously */ - if (status->arming_state == ARMING_STATE_INIT - || status->arming_state == ARMING_STATE_STANDBY - || status->arming_state == ARMING_STATE_STANDBY_ERROR) { - ret = TRANSITION_CHANGED; - armed->armed = false; - armed->ready_to_arm = false; - } - - break; - - case ARMING_STATE_IN_AIR_RESTORE: - - /* XXX implement */ - break; + // HIL can always go to standby + if (status->hil_state == HIL_STATE_ON && new_arming_state == ARMING_STATE_STANDBY) { + valid_transition = true; + } - default: - break; + /* Sensors need to be initialized for STANDBY state */ + if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { + valid_transition = false; } - if (ret == TRANSITION_CHANGED) { + // 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; + ret = TRANSITION_CHANGED; status->arming_state = new_arming_state; arming_state_changed = true; } @@ -191,8 +172,15 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * /* end of atomic state update */ irqrestore(flags); - if (ret == TRANSITION_DENIED) - warnx("arming transition rejected"); + if (ret == TRANSITION_DENIED) { + static const char *errMsg = "Invalid arming transition from %s to %s"; + + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]); + } + + warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]); + } return ret; } @@ -234,7 +222,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta ret = TRANSITION_CHANGED; break; - case MAIN_STATE_SEATBELT: + case MAIN_STATE_ALTCTL: /* need at minimum altitude estimate */ if (!status->is_rotary_wing || @@ -245,7 +233,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta break; - case MAIN_STATE_EASY: + case MAIN_STATE_POSCTL: /* need at minimum local position estimate */ if (status->condition_local_position_valid || @@ -266,7 +254,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta case MAIN_STATE_OFFBOARD: - /* need global position estimate */ + /* need offboard signal */ if (!status->offboard_control_signal_lost) { ret = TRANSITION_CHANGED; } @@ -351,6 +339,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s /* list directory */ DIR *d; d = opendir("/dev"); + if (d) { struct dirent *direntry; @@ -362,26 +351,32 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s if (!strncmp("tty", direntry->d_name, 3)) { continue; } + /* skip mtd devices */ if (!strncmp("mtd", direntry->d_name, 3)) { continue; } + /* skip ram devices */ if (!strncmp("ram", direntry->d_name, 3)) { continue; } + /* skip MMC devices */ if (!strncmp("mmc", direntry->d_name, 3)) { continue; } + /* skip mavlink */ if (!strcmp("mavlink", direntry->d_name)) { continue; } + /* skip console */ if (!strcmp("console", direntry->d_name)) { continue; } + /* skip null */ if (!strcmp("null", direntry->d_name)) { continue; diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index f04879ff9..0ddd4f05a 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -57,7 +57,7 @@ typedef enum { } transition_result_t; transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed); + arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd = 0); bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 34d20e485..1a65313e8 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -1,10 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Jean Cyr - * Lorenz Meier - * Julian Oes - * Thomas Gubler + * 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 @@ -37,6 +33,11 @@ /** * @file dataman.c * DATAMANAGER driver. + * + * @author Jean Cyr + * @author Lorenz Meier + * @author Julian Oes + * @author Thomas Gubler */ #include <nuttx/config.h> @@ -44,7 +45,9 @@ #include <stdlib.h> #include <fcntl.h> #include <systemlib/systemlib.h> +#include <systemlib/err.h> #include <queue.h> +#include <string.h> #include "dataman.h" @@ -60,7 +63,7 @@ __EXPORT ssize_t dm_write(dm_item_t item, unsigned char index, dm_persitence_t __EXPORT int dm_clear(dm_item_t item); __EXPORT int dm_restart(dm_reset_reason restart_type); -/* Types of function calls supported by the worker task */ +/** Types of function calls supported by the worker task */ typedef enum { dm_write_func = 0, dm_read_func, @@ -69,11 +72,12 @@ typedef enum { dm_number_of_funcs } dm_function_t; -/* Work task work item */ +/** Work task work item */ typedef struct { sq_entry_t link; /**< list linkage */ sem_t wait_sem; - dm_function_t func; + unsigned char first; + unsigned char func; ssize_t result; union { struct { @@ -98,6 +102,8 @@ typedef struct { }; } work_q_item_t; +const size_t k_work_item_allocation_chunk_size = 8; + /* Usage statistics */ static unsigned g_func_counts[dm_number_of_funcs]; @@ -175,9 +181,23 @@ create_work_item(void) unlock_queue(&g_free_q); - /* If we there weren't any free items then obtain memory for a new one */ - if (item == NULL) - item = (work_q_item_t *)malloc(sizeof(work_q_item_t)); + /* If we there weren't any free items then obtain memory for a new ones */ + if (item == NULL) { + item = (work_q_item_t *)malloc(k_work_item_allocation_chunk_size * sizeof(work_q_item_t)); + if (item) { + item->first = 1; + lock_queue(&g_free_q); + for (int i = 1; i < k_work_item_allocation_chunk_size; i++) { + (item + i)->first = 0; + sq_addfirst(&(item + i)->link, &(g_free_q.q)); + } + /* Update the queue size and potentially the maximum queue size */ + g_free_q.size += k_work_item_allocation_chunk_size - 1; + if (g_free_q.size > g_free_q.max_size) + g_free_q.max_size = g_free_q.size; + unlock_queue(&g_free_q); + } + } /* If we got one then lock the item*/ if (item) @@ -409,31 +429,31 @@ _clear(dm_item_t item) return result; } -/* Tell the data manager about the type of the last reset */ +/** Tell the data manager about the type of the last reset */ static int _restart(dm_reset_reason reason) { unsigned char buffer[2]; - int offset, result = 0; + int offset = 0, result = 0; /* We need to scan the entire file and invalidate and data that should not persist after the last reset */ /* Loop through all of the data segments and delete those that are not persistent */ - offset = 0; - while (1) { size_t len; /* Get data segment at current offset */ if (lseek(g_task_fd, offset, SEEK_SET) != offset) { - result = -1; + /* must be at eof */ break; } len = read(g_task_fd, buffer, sizeof(buffer)); - if (len == 0) + if (len != sizeof(buffer)) { + /* must be at eof */ break; + } /* check if segment contains data */ if (buffer[0]) { @@ -441,12 +461,12 @@ _restart(dm_reset_reason reason) /* Whether data gets deleted depends on reset type and data segment's persistence setting */ if (reason == DM_INIT_REASON_POWER_ON) { - if (buffer[1] != DM_PERSIST_POWER_ON_RESET) { + if (buffer[1] > DM_PERSIST_POWER_ON_RESET) { clear_entry = 1; } } else { - if ((buffer[1] != DM_PERSIST_POWER_ON_RESET) && (buffer[1] != DM_PERSIST_IN_FLIGHT_RESET)) { + if (buffer[1] > DM_PERSIST_IN_FLIGHT_RESET) { clear_entry = 1; } } @@ -478,7 +498,7 @@ _restart(dm_reset_reason reason) return result; } -/* write to the data manager file */ +/** Write to the data manager file */ __EXPORT ssize_t dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const void *buf, size_t count) { @@ -503,7 +523,7 @@ dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const return (ssize_t)enqueue_work_item_and_wait_for_result(work); } -/* Retrieve from the data manager file */ +/** Retrieve from the data manager file */ __EXPORT ssize_t dm_read(dm_item_t item, unsigned char index, void *buf, size_t count) { @@ -594,6 +614,20 @@ task_main(int argc, char *argv[]) sem_init(&g_work_queued_sema, 1, 0); + /* See if the data manage file exists and is a multiple of the sector size */ + g_task_fd = open(k_data_manager_device_path, O_RDONLY | O_BINARY); + if (g_task_fd >= 0) { + /* File exists, check its size */ + int file_size = lseek(g_task_fd, 0, SEEK_END); + if ((file_size % k_sector_size) != 0) { + warnx("Incompatible data manager file %s, resetting it", k_data_manager_device_path); + close(g_task_fd); + unlink(k_data_manager_device_path); + } + else + close(g_task_fd); + } + /* Open or create the data manager file */ g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY); @@ -603,7 +637,7 @@ task_main(int argc, char *argv[]) return -1; } - if (lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) { + if ((unsigned)lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) { close(g_task_fd); warnx("Could not seek data manager file %s", k_data_manager_device_path); sem_post(&g_init_sema); /* Don't want to hang startup */ @@ -612,6 +646,23 @@ task_main(int argc, char *argv[]) fsync(g_task_fd); + /* see if we need to erase any items based on restart type */ + int sys_restart_val; + if (param_get(param_find("SYS_RESTART_TYPE"), &sys_restart_val) == OK) { + if (sys_restart_val == DM_INIT_REASON_POWER_ON) { + warnx("Power on restart"); + _restart(DM_INIT_REASON_POWER_ON); + } + else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) { + warnx("In flight restart"); + _restart(DM_INIT_REASON_IN_FLIGHT); + } + else + warnx("Unknown restart"); + } + else + warnx("Unknown restart"); + /* We use two file descriptors, one for the caller context and one for the worker thread */ /* They are actually the same but we need to some way to reject caller request while the */ /* worker thread is shutting down but still processing requests */ @@ -684,8 +735,8 @@ task_main(int argc, char *argv[]) for (;;) { if ((work = (work_q_item_t *)sq_remfirst(&(g_free_q.q))) == NULL) break; - - free(work); + if (work->first) + free(work); } destroy_q(&g_work_q); @@ -703,12 +754,12 @@ start(void) sem_init(&g_init_sema, 1, 0); /* start the worker thread */ - if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, NULL)) <= 0) { + if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, task_main, NULL)) <= 0) { warn("task start failed"); return -1; } - /* wait for the thread to actuall initialize */ + /* wait for the thread to actually initialize */ sem_wait(&g_init_sema); sem_destroy(&g_init_sema); @@ -776,4 +827,3 @@ dataman_main(int argc, char *argv[]) exit(1); } - diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index a70638ccc..1dfb26f73 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * 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 @@ -46,7 +46,7 @@ extern "C" { #endif - /* Types of items that the data manager can store */ + /** Types of items that the data manager can store */ typedef enum { DM_KEY_SAFE_POINTS = 0, /* Safe points coordinates, safe point 0 is home point */ DM_KEY_FENCE_POINTS, /* Fence vertex coordinates */ @@ -56,7 +56,7 @@ extern "C" { DM_KEY_NUM_KEYS /* Total number of item types defined */ } dm_item_t; - /* The maximum number of instances for each item type */ + /** The maximum number of instances for each item type */ enum { DM_KEY_SAFE_POINTS_MAX = 8, DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES, @@ -65,23 +65,24 @@ extern "C" { DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED }; - /* Data persistence levels */ + /** Data persistence levels */ typedef enum { DM_PERSIST_POWER_ON_RESET = 0, /* Data survives all resets */ DM_PERSIST_IN_FLIGHT_RESET, /* Data survives in-flight resets only */ DM_PERSIST_VOLATILE /* Data does not survive resets */ } dm_persitence_t; - /* The reason for the last reset */ + /** The reason for the last reset */ typedef enum { DM_INIT_REASON_POWER_ON = 0, /* Data survives resets */ - DM_INIT_REASON_IN_FLIGHT /* Data survives in-flight resets only */ + DM_INIT_REASON_IN_FLIGHT, /* Data survives in-flight resets only */ + DM_INIT_REASON_VOLATILE /* Data does not survive reset */ } dm_reset_reason; - /* Maximum size in bytes of a single item instance */ - #define DM_MAX_DATA_SIZE 126 + /** Maximum size in bytes of a single item instance */ + #define DM_MAX_DATA_SIZE 124 - /* Retrieve from the data manager store */ + /** Retrieve from the data manager store */ __EXPORT ssize_t dm_read( dm_item_t item, /* The item type to retrieve */ @@ -90,7 +91,7 @@ extern "C" { size_t buflen /* Length in bytes of data to retrieve */ ); - /* write to the data manager store */ + /** write to the data manager store */ __EXPORT ssize_t dm_write( dm_item_t item, /* The item type to store */ @@ -100,13 +101,13 @@ extern "C" { size_t buflen /* Length in bytes of data to retrieve */ ); - /* Retrieve from the data manager store */ + /** Erase all items of this type */ __EXPORT int dm_clear( dm_item_t item /* The item type to clear */ ); - /* Tell the data manager about the type of the last reset */ + /** Tell the data manager about the type of the last reset */ __EXPORT int dm_restart( dm_reset_reason restart_type /* The last reset type */ diff --git a/src/modules/dataman/module.mk b/src/modules/dataman/module.mk index 27670dd3f..234607b3d 100644 --- a/src/modules/dataman/module.mk +++ b/src/modules/dataman/module.mk @@ -38,3 +38,5 @@ MODULE_COMMAND = dataman SRCS = dataman.c + +MODULE_STACKSIZE = 1200 diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp new file mode 100644 index 000000000..23ecd89ac --- /dev/null +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -0,0 +1,2609 @@ +#include "estimator.h" +#include <string.h> +#include <stdarg.h> + +// Define EKF_DEBUG here to enable the debug print calls +// if the macro is not set, these will be completely +// optimized out by the compiler. +#define EKF_DEBUG + +#ifdef EKF_DEBUG +#include <stdio.h> + +static void +ekf_debug_print(const char *fmt, va_list args) +{ + fprintf(stderr, "%s: ", "[ekf]"); + vfprintf(stderr, fmt, args); + + fprintf(stderr, "\n"); +} + +static void +ekf_debug(const char *fmt, ...) +{ + va_list args; + + va_start(args, fmt); + ekf_debug_print(fmt, args); +} + +#else + +static void ekf_debug(const char *fmt, ...) { while(0){} } +#endif + +float Vector3f::length(void) const +{ + return sqrt(x*x + y*y + z*z); +} + +void Vector3f::zero(void) +{ + x = 0.0f; + y = 0.0f; + z = 0.0f; +} + +Mat3f::Mat3f() { + identity(); +} + +void Mat3f::identity() { + x.x = 1.0f; + x.y = 0.0f; + x.z = 0.0f; + + y.x = 0.0f; + y.y = 1.0f; + y.z = 0.0f; + + z.x = 0.0f; + z.y = 0.0f; + z.z = 1.0f; +} + +Mat3f Mat3f::transpose(void) const +{ + Mat3f ret = *this; + swap_var(ret.x.y, ret.y.x); + swap_var(ret.x.z, ret.z.x); + swap_var(ret.y.z, ret.z.y); + return ret; +} + +// overload + operator to provide a vector addition +Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2) +{ + Vector3f vecOut; + vecOut.x = vecIn1.x + vecIn2.x; + vecOut.y = vecIn1.y + vecIn2.y; + vecOut.z = vecIn1.z + vecIn2.z; + return vecOut; +} + +// overload - operator to provide a vector subtraction +Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2) +{ + Vector3f vecOut; + vecOut.x = vecIn1.x - vecIn2.x; + vecOut.y = vecIn1.y - vecIn2.y; + vecOut.z = vecIn1.z - vecIn2.z; + return vecOut; +} + +// overload * operator to provide a matrix vector product +Vector3f operator*( Mat3f matIn, Vector3f vecIn) +{ + Vector3f vecOut; + vecOut.x = matIn.x.x*vecIn.x + matIn.x.y*vecIn.y + matIn.x.z*vecIn.z; + vecOut.y = matIn.y.x*vecIn.x + matIn.y.y*vecIn.y + matIn.y.z*vecIn.z; + vecOut.z = matIn.x.x*vecIn.x + matIn.z.y*vecIn.y + matIn.z.z*vecIn.z; + return vecOut; +} + +// overload % operator to provide a vector cross product +Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2) +{ + Vector3f vecOut; + vecOut.x = vecIn1.y*vecIn2.z - vecIn1.z*vecIn2.y; + vecOut.y = vecIn1.z*vecIn2.x - vecIn1.x*vecIn2.z; + vecOut.z = vecIn1.x*vecIn2.y - vecIn1.y*vecIn2.x; + return vecOut; +} + +// overload * operator to provide a vector scaler product +Vector3f operator*(Vector3f vecIn1, float sclIn1) +{ + Vector3f vecOut; + vecOut.x = vecIn1.x * sclIn1; + vecOut.y = vecIn1.y * sclIn1; + vecOut.z = vecIn1.z * sclIn1; + return vecOut; +} + +// overload * operator to provide a vector scaler product +Vector3f operator*(float sclIn1, Vector3f vecIn1) +{ + Vector3f vecOut; + vecOut.x = vecIn1.x * sclIn1; + vecOut.y = vecIn1.y * sclIn1; + vecOut.z = vecIn1.z * sclIn1; + return vecOut; +} + +void swap_var(float &d1, float &d2) +{ + float tmp = d1; + d1 = d2; + d2 = tmp; +} + +AttPosEKF::AttPosEKF() + + /* NOTE: DO NOT initialize class members here. Use ZeroVariables() + * instead to allow clean in-air re-initialization. + */ +{ + + ZeroVariables(); + 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; + float rotationMag; + float qUpdated[4]; + float quatMag; + float 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 - states[13]; + +// 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] = cosf(0.5f*rotationMag); + float rotScaler = (sinf(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; + float dvz_b; + + // arrays + float processNoise[n_states]; + float SF[15]; + float SG[8]; + float SQ[11]; + float SPP[8] = {0}; + float nextP[n_states][n_states]; + + // calculate covariance prediction process noise + for (uint8_t i= 0; i<=9; i++) processNoise[i] = 1.0e-9f; + for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma; + // scale gyro bias noise when on ground to allow for faster bias estimation + for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma; + processNoise[13] = dVelBiasSigma; + for (uint8_t i=14; i<=15; i++) processNoise[i] = dt * windVelSigma; + for (uint8_t i=16; i<=18; i++) processNoise[i] = dt * magEarthSigma; + for (uint8_t i=19; i<=21; i++) processNoise[i] = dt * magBodySigma; + processNoise[22] = dt * sqrtf(sq(states[4]) + sq(states[5])) * gndHgtSigma; + + // square all sigmas + for (unsigned i = 0; i < n_states; 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]; + dvz_b = states[13]; + gyroProcessNoise = ConstrainFloat(gyroProcessNoise, 1e-3f, 5e-2f); + daxCov = sq(dt*gyroProcessNoise); + dayCov = sq(dt*gyroProcessNoise); + dazCov = sq(dt*gyroProcessNoise); + if (onGround) dazCov = dazCov * sq(yawVarScale); + accelProcessNoise = ConstrainFloat(accelProcessNoise, 5e-2, 1.0f); + dvxCov = sq(dt*accelProcessNoise); + dvyCov = sq(dt*accelProcessNoise); + dvzCov = sq(dt*accelProcessNoise); + + // Predicted covariance calculation + SF[0] = dvz - dvz_b; + SF[1] = 2*q3*SF[0] + 2*dvx*q1 + 2*dvy*q2; + SF[2] = 2*dvx*q3 - 2*q1*SF[0] + 2*dvy*q0; + SF[3] = 2*q2*SF[0] + 2*dvx*q0 - 2*dvy*q3; + SF[4] = day/2 - day_b/2; + SF[5] = daz/2 - daz_b/2; + SF[6] = dax/2 - dax_b/2; + SF[7] = dax_b/2 - dax/2; + SF[8] = daz_b/2 - daz/2; + SF[9] = day_b/2 - day/2; + SF[10] = 2*q0*SF[0]; + SF[11] = q1/2; + SF[12] = q2/2; + SF[13] = q3/2; + SF[14] = 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[10] + SF[14] - 2*dvx*q2; + SPP[1] = 2*q2*SF[0] + 2*dvx*q0 - 2*dvy*q3; + SPP[2] = 2*dvx*q3 - 2*q1*SF[0] + 2*dvy*q0; + SPP[3] = 2*q0*q1 - 2*q2*q3; + SPP[4] = 2*q0*q2 + 2*q1*q3; + SPP[5] = sq(q0) - sq(q1) - sq(q2) + sq(q3); + SPP[6] = SF[13]; + SPP[7] = SF[12]; + + nextP[0][0] = P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6] + (daxCov*SQ[10])/4 + SF[7]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[9]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) + SF[8]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SF[11]*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]) + SPP[7]*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]) + SPP[6]*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]) + (dayCov*sq(q2))/4 + (dazCov*sq(q3))/4; + nextP[0][1] = P[0][1] + SQ[8] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6] + SF[6]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[5]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) + SF[9]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SPP[6]*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]) - SPP[7]*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]) - (q0*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]))/2; + nextP[0][2] = P[0][2] + SQ[7] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6] + SF[4]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[8]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[6]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SF[11]*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]) - SPP[6]*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]) - (q0*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]))/2; + nextP[0][3] = P[0][3] + SQ[6] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6] + SF[5]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[4]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[7]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) - SF[11]*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]) + SPP[7]*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]) - (q0*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]))/2; + nextP[0][4] = P[0][4] + P[1][4]*SF[7] + P[2][4]*SF[9] + P[3][4]*SF[8] + P[10][4]*SF[11] + P[11][4]*SPP[7] + P[12][4]*SPP[6] + SF[3]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[1]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SPP[0]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) - SPP[2]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) - SPP[4]*(P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]); + nextP[0][5] = P[0][5] + P[1][5]*SF[7] + P[2][5]*SF[9] + P[3][5]*SF[8] + P[10][5]*SF[11] + P[11][5]*SPP[7] + P[12][5]*SPP[6] + SF[2]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[1]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) + SF[3]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) - SPP[0]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SPP[3]*(P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]); + nextP[0][6] = P[0][6] + P[1][6]*SF[7] + P[2][6]*SF[9] + P[3][6]*SF[8] + P[10][6]*SF[11] + P[11][6]*SPP[7] + P[12][6]*SPP[6] + SF[2]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[1]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SPP[0]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) - SPP[1]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]); + nextP[0][7] = P[0][7] + P[1][7]*SF[7] + P[2][7]*SF[9] + P[3][7]*SF[8] + P[10][7]*SF[11] + P[11][7]*SPP[7] + P[12][7]*SPP[6] + dt*(P[0][4] + P[1][4]*SF[7] + P[2][4]*SF[9] + P[3][4]*SF[8] + P[10][4]*SF[11] + P[11][4]*SPP[7] + P[12][4]*SPP[6]); + nextP[0][8] = P[0][8] + P[1][8]*SF[7] + P[2][8]*SF[9] + P[3][8]*SF[8] + P[10][8]*SF[11] + P[11][8]*SPP[7] + P[12][8]*SPP[6] + dt*(P[0][5] + P[1][5]*SF[7] + P[2][5]*SF[9] + P[3][5]*SF[8] + P[10][5]*SF[11] + P[11][5]*SPP[7] + P[12][5]*SPP[6]); + nextP[0][9] = P[0][9] + P[1][9]*SF[7] + P[2][9]*SF[9] + P[3][9]*SF[8] + P[10][9]*SF[11] + P[11][9]*SPP[7] + P[12][9]*SPP[6] + dt*(P[0][6] + P[1][6]*SF[7] + P[2][6]*SF[9] + P[3][6]*SF[8] + P[10][6]*SF[11] + P[11][6]*SPP[7] + P[12][6]*SPP[6]); + nextP[0][10] = P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]; + nextP[0][11] = P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]; + nextP[0][12] = P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]; + nextP[0][13] = P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]; + nextP[0][14] = P[0][14] + P[1][14]*SF[7] + P[2][14]*SF[9] + P[3][14]*SF[8] + P[10][14]*SF[11] + P[11][14]*SPP[7] + P[12][14]*SPP[6]; + nextP[0][15] = P[0][15] + P[1][15]*SF[7] + P[2][15]*SF[9] + P[3][15]*SF[8] + P[10][15]*SF[11] + P[11][15]*SPP[7] + P[12][15]*SPP[6]; + nextP[0][16] = P[0][16] + P[1][16]*SF[7] + P[2][16]*SF[9] + P[3][16]*SF[8] + P[10][16]*SF[11] + P[11][16]*SPP[7] + P[12][16]*SPP[6]; + nextP[0][17] = P[0][17] + P[1][17]*SF[7] + P[2][17]*SF[9] + P[3][17]*SF[8] + P[10][17]*SF[11] + P[11][17]*SPP[7] + P[12][17]*SPP[6]; + nextP[0][18] = P[0][18] + P[1][18]*SF[7] + P[2][18]*SF[9] + P[3][18]*SF[8] + P[10][18]*SF[11] + P[11][18]*SPP[7] + P[12][18]*SPP[6]; + 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; + nextP[1][3] = P[1][3] + SQ[4] + 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[5]*(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[4]*(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[7]*(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[11]*(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][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][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))/2; + nextP[1][4] = P[1][4] + P[0][4]*SF[6] + P[2][4]*SF[5] + P[3][4]*SF[9] + P[11][4]*SPP[6] - P[12][4]*SPP[7] - (P[10][4]*q0)/2 + SF[3]*(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[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] - (P[10][1]*q0)/2) + SPP[0]*(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) - SPP[2]*(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[4]*(P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2); + nextP[1][5] = P[1][5] + P[0][5]*SF[6] + P[2][5]*SF[5] + P[3][5]*SF[9] + P[11][5]*SPP[6] - P[12][5]*SPP[7] - (P[10][5]*q0)/2 + SF[2]*(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[1]*(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[3]*(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[0]*(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) + SPP[3]*(P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2); + nextP[1][6] = P[1][6] + P[0][6]*SF[6] + P[2][6]*SF[5] + P[3][6]*SF[9] + P[11][6]*SPP[6] - P[12][6]*SPP[7] - (P[10][6]*q0)/2 + SF[2]*(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[1]*(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[0]*(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) - SPP[1]*(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) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2); + nextP[1][7] = P[1][7] + P[0][7]*SF[6] + P[2][7]*SF[5] + P[3][7]*SF[9] + P[11][7]*SPP[6] - P[12][7]*SPP[7] - (P[10][7]*q0)/2 + dt*(P[1][4] + P[0][4]*SF[6] + P[2][4]*SF[5] + P[3][4]*SF[9] + P[11][4]*SPP[6] - P[12][4]*SPP[7] - (P[10][4]*q0)/2); + nextP[1][8] = P[1][8] + P[0][8]*SF[6] + P[2][8]*SF[5] + P[3][8]*SF[9] + P[11][8]*SPP[6] - P[12][8]*SPP[7] - (P[10][8]*q0)/2 + dt*(P[1][5] + P[0][5]*SF[6] + P[2][5]*SF[5] + P[3][5]*SF[9] + P[11][5]*SPP[6] - P[12][5]*SPP[7] - (P[10][5]*q0)/2); + nextP[1][9] = P[1][9] + P[0][9]*SF[6] + P[2][9]*SF[5] + P[3][9]*SF[9] + P[11][9]*SPP[6] - P[12][9]*SPP[7] - (P[10][9]*q0)/2 + dt*(P[1][6] + P[0][6]*SF[6] + P[2][6]*SF[5] + P[3][6]*SF[9] + P[11][6]*SPP[6] - P[12][6]*SPP[7] - (P[10][6]*q0)/2); + nextP[1][10] = 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; + nextP[1][11] = 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; + nextP[1][12] = 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][13] = P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2; + nextP[1][14] = P[1][14] + P[0][14]*SF[6] + P[2][14]*SF[5] + P[3][14]*SF[9] + P[11][14]*SPP[6] - P[12][14]*SPP[7] - (P[10][14]*q0)/2; + nextP[1][15] = P[1][15] + P[0][15]*SF[6] + P[2][15]*SF[5] + P[3][15]*SF[9] + P[11][15]*SPP[6] - P[12][15]*SPP[7] - (P[10][15]*q0)/2; + nextP[1][16] = P[1][16] + P[0][16]*SF[6] + P[2][16]*SF[5] + P[3][16]*SF[9] + P[11][16]*SPP[6] - P[12][16]*SPP[7] - (P[10][16]*q0)/2; + nextP[1][17] = P[1][17] + P[0][17]*SF[6] + P[2][17]*SF[5] + P[3][17]*SF[9] + P[11][17]*SPP[6] - P[12][17]*SPP[7] - (P[10][17]*q0)/2; + nextP[1][18] = P[1][18] + P[0][18]*SF[6] + P[2][18]*SF[5] + P[3][18]*SF[9] + P[11][18]*SPP[6] - P[12][18]*SPP[7] - (P[10][18]*q0)/2; + 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; + nextP[2][3] = P[2][3] + SQ[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[5]*(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[4]*(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[7]*(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[11]*(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][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) - (q0*(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))/2; + nextP[2][4] = P[2][4] + P[0][4]*SF[4] + P[1][4]*SF[8] + P[3][4]*SF[6] + P[12][4]*SF[11] - P[10][4]*SPP[6] - (P[11][4]*q0)/2 + SF[3]*(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[1]*(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) + SPP[0]*(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) - SPP[2]*(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[4]*(P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2); + nextP[2][5] = P[2][5] + P[0][5]*SF[4] + P[1][5]*SF[8] + P[3][5]*SF[6] + P[12][5]*SF[11] - P[10][5]*SPP[6] - (P[11][5]*q0)/2 + SF[2]*(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[1]*(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[3]*(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[0]*(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) + SPP[3]*(P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2); + nextP[2][6] = P[2][6] + P[0][6]*SF[4] + P[1][6]*SF[8] + P[3][6]*SF[6] + P[12][6]*SF[11] - P[10][6]*SPP[6] - (P[11][6]*q0)/2 + SF[2]*(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[1]*(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[0]*(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) - SPP[1]*(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) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2); + nextP[2][7] = P[2][7] + P[0][7]*SF[4] + P[1][7]*SF[8] + P[3][7]*SF[6] + P[12][7]*SF[11] - P[10][7]*SPP[6] - (P[11][7]*q0)/2 + dt*(P[2][4] + P[0][4]*SF[4] + P[1][4]*SF[8] + P[3][4]*SF[6] + P[12][4]*SF[11] - P[10][4]*SPP[6] - (P[11][4]*q0)/2); + nextP[2][8] = P[2][8] + P[0][8]*SF[4] + P[1][8]*SF[8] + P[3][8]*SF[6] + P[12][8]*SF[11] - P[10][8]*SPP[6] - (P[11][8]*q0)/2 + dt*(P[2][5] + P[0][5]*SF[4] + P[1][5]*SF[8] + P[3][5]*SF[6] + P[12][5]*SF[11] - P[10][5]*SPP[6] - (P[11][5]*q0)/2); + nextP[2][9] = P[2][9] + P[0][9]*SF[4] + P[1][9]*SF[8] + P[3][9]*SF[6] + P[12][9]*SF[11] - P[10][9]*SPP[6] - (P[11][9]*q0)/2 + dt*(P[2][6] + P[0][6]*SF[4] + P[1][6]*SF[8] + P[3][6]*SF[6] + P[12][6]*SF[11] - P[10][6]*SPP[6] - (P[11][6]*q0)/2); + nextP[2][10] = 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; + nextP[2][11] = 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; + nextP[2][12] = 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][13] = P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2; + nextP[2][14] = P[2][14] + P[0][14]*SF[4] + P[1][14]*SF[8] + P[3][14]*SF[6] + P[12][14]*SF[11] - P[10][14]*SPP[6] - (P[11][14]*q0)/2; + nextP[2][15] = P[2][15] + P[0][15]*SF[4] + P[1][15]*SF[8] + P[3][15]*SF[6] + P[12][15]*SF[11] - P[10][15]*SPP[6] - (P[11][15]*q0)/2; + nextP[2][16] = P[2][16] + P[0][16]*SF[4] + P[1][16]*SF[8] + P[3][16]*SF[6] + P[12][16]*SF[11] - P[10][16]*SPP[6] - (P[11][16]*q0)/2; + nextP[2][17] = P[2][17] + P[0][17]*SF[4] + P[1][17]*SF[8] + P[3][17]*SF[6] + P[12][17]*SF[11] - P[10][17]*SPP[6] - (P[11][17]*q0)/2; + nextP[2][18] = P[2][18] + P[0][18]*SF[4] + P[1][18]*SF[8] + P[3][18]*SF[6] + P[12][18]*SF[11] - P[10][18]*SPP[6] - (P[11][18]*q0)/2; + 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; + nextP[3][3] = 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] + (dayCov*SQ[10])/4 + dazCov*SQ[9] - (P[12][3]*q0)/2 + SF[5]*(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[4]*(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[7]*(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[11]*(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][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) + (daxCov*sq(q2))/4 - (q0*(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))/2; + nextP[3][4] = P[3][4] + P[0][4]*SF[5] + P[1][4]*SF[4] + P[2][4]*SF[7] - P[11][4]*SF[11] + P[10][4]*SPP[7] - (P[12][4]*q0)/2 + SF[3]*(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[1]*(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) + SPP[0]*(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) - SPP[2]*(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[4]*(P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2); + nextP[3][5] = P[3][5] + P[0][5]*SF[5] + P[1][5]*SF[4] + P[2][5]*SF[7] - P[11][5]*SF[11] + P[10][5]*SPP[7] - (P[12][5]*q0)/2 + SF[2]*(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[1]*(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[3]*(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[0]*(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) + SPP[3]*(P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2); + nextP[3][6] = P[3][6] + P[0][6]*SF[5] + P[1][6]*SF[4] + P[2][6]*SF[7] - P[11][6]*SF[11] + P[10][6]*SPP[7] - (P[12][6]*q0)/2 + SF[2]*(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[1]*(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[0]*(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) - SPP[1]*(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) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2); + nextP[3][7] = P[3][7] + P[0][7]*SF[5] + P[1][7]*SF[4] + P[2][7]*SF[7] - P[11][7]*SF[11] + P[10][7]*SPP[7] - (P[12][7]*q0)/2 + dt*(P[3][4] + P[0][4]*SF[5] + P[1][4]*SF[4] + P[2][4]*SF[7] - P[11][4]*SF[11] + P[10][4]*SPP[7] - (P[12][4]*q0)/2); + nextP[3][8] = P[3][8] + P[0][8]*SF[5] + P[1][8]*SF[4] + P[2][8]*SF[7] - P[11][8]*SF[11] + P[10][8]*SPP[7] - (P[12][8]*q0)/2 + dt*(P[3][5] + P[0][5]*SF[5] + P[1][5]*SF[4] + P[2][5]*SF[7] - P[11][5]*SF[11] + P[10][5]*SPP[7] - (P[12][5]*q0)/2); + nextP[3][9] = P[3][9] + P[0][9]*SF[5] + P[1][9]*SF[4] + P[2][9]*SF[7] - P[11][9]*SF[11] + P[10][9]*SPP[7] - (P[12][9]*q0)/2 + dt*(P[3][6] + P[0][6]*SF[5] + P[1][6]*SF[4] + P[2][6]*SF[7] - P[11][6]*SF[11] + P[10][6]*SPP[7] - (P[12][6]*q0)/2); + nextP[3][10] = 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; + nextP[3][11] = 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; + nextP[3][12] = 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][13] = P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2; + nextP[3][14] = P[3][14] + P[0][14]*SF[5] + P[1][14]*SF[4] + P[2][14]*SF[7] - P[11][14]*SF[11] + P[10][14]*SPP[7] - (P[12][14]*q0)/2; + nextP[3][15] = P[3][15] + P[0][15]*SF[5] + P[1][15]*SF[4] + P[2][15]*SF[7] - P[11][15]*SF[11] + P[10][15]*SPP[7] - (P[12][15]*q0)/2; + nextP[3][16] = P[3][16] + P[0][16]*SF[5] + P[1][16]*SF[4] + P[2][16]*SF[7] - P[11][16]*SF[11] + P[10][16]*SPP[7] - (P[12][16]*q0)/2; + nextP[3][17] = P[3][17] + P[0][17]*SF[5] + P[1][17]*SF[4] + P[2][17]*SF[7] - P[11][17]*SF[11] + P[10][17]*SPP[7] - (P[12][17]*q0)/2; + nextP[3][18] = P[3][18] + P[0][18]*SF[5] + P[1][18]*SF[4] + P[2][18]*SF[7] - P[11][18]*SF[11] + P[10][18]*SPP[7] - (P[12][18]*q0)/2; + 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; + nextP[4][3] = 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[5]*(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[4]*(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[7]*(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[11]*(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][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][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]))/2; + nextP[4][4] = P[4][4] + P[0][4]*SF[3] + P[1][4]*SF[1] + P[2][4]*SPP[0] - P[3][4]*SPP[2] - P[13][4]*SPP[4] + dvyCov*sq(SG[7] - 2*q0*q3) + dvzCov*sq(SG[6] + 2*q0*q2) + SF[3]*(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[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]) + SPP[0]*(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]) - SPP[2]*(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[4]*(P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]) + dvxCov*sq(SG[1] + SG[2] - SG[3] - SG[4]); + nextP[4][5] = P[4][5] + SQ[2] + P[0][5]*SF[3] + P[1][5]*SF[1] + P[2][5]*SPP[0] - P[3][5]*SPP[2] - P[13][5]*SPP[4] + SF[2]*(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[1]*(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[3]*(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[0]*(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]) + SPP[3]*(P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]); + nextP[4][6] = P[4][6] + SQ[1] + P[0][6]*SF[3] + P[1][6]*SF[1] + P[2][6]*SPP[0] - P[3][6]*SPP[2] - P[13][6]*SPP[4] + SF[2]*(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[1]*(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[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]) - SPP[1]*(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]) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]); + nextP[4][7] = P[4][7] + P[0][7]*SF[3] + P[1][7]*SF[1] + P[2][7]*SPP[0] - P[3][7]*SPP[2] - P[13][7]*SPP[4] + dt*(P[4][4] + P[0][4]*SF[3] + P[1][4]*SF[1] + P[2][4]*SPP[0] - P[3][4]*SPP[2] - P[13][4]*SPP[4]); + nextP[4][8] = P[4][8] + P[0][8]*SF[3] + P[1][8]*SF[1] + P[2][8]*SPP[0] - P[3][8]*SPP[2] - P[13][8]*SPP[4] + dt*(P[4][5] + P[0][5]*SF[3] + P[1][5]*SF[1] + P[2][5]*SPP[0] - P[3][5]*SPP[2] - P[13][5]*SPP[4]); + nextP[4][9] = P[4][9] + P[0][9]*SF[3] + P[1][9]*SF[1] + P[2][9]*SPP[0] - P[3][9]*SPP[2] - P[13][9]*SPP[4] + dt*(P[4][6] + P[0][6]*SF[3] + P[1][6]*SF[1] + P[2][6]*SPP[0] - P[3][6]*SPP[2] - P[13][6]*SPP[4]); + nextP[4][10] = 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]; + nextP[4][11] = 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]; + nextP[4][12] = 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][13] = P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]; + nextP[4][14] = P[4][14] + P[0][14]*SF[3] + P[1][14]*SF[1] + P[2][14]*SPP[0] - P[3][14]*SPP[2] - P[13][14]*SPP[4]; + nextP[4][15] = P[4][15] + P[0][15]*SF[3] + P[1][15]*SF[1] + P[2][15]*SPP[0] - P[3][15]*SPP[2] - P[13][15]*SPP[4]; + nextP[4][16] = P[4][16] + P[0][16]*SF[3] + P[1][16]*SF[1] + P[2][16]*SPP[0] - P[3][16]*SPP[2] - P[13][16]*SPP[4]; + nextP[4][17] = P[4][17] + P[0][17]*SF[3] + P[1][17]*SF[1] + P[2][17]*SPP[0] - P[3][17]*SPP[2] - P[13][17]*SPP[4]; + nextP[4][18] = P[4][18] + P[0][18]*SF[3] + P[1][18]*SF[1] + P[2][18]*SPP[0] - P[3][18]*SPP[2] - P[13][18]*SPP[4]; + 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; + nextP[5][3] = 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[5]*(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[4]*(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[7]*(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[11]*(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][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][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]))/2; + nextP[5][4] = P[5][4] + SQ[2] + P[0][4]*SF[2] + P[2][4]*SF[1] + P[3][4]*SF[3] - P[1][4]*SPP[0] + P[13][4]*SPP[3] + SF[3]*(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[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]) + SPP[0]*(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]) - SPP[2]*(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[4]*(P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]); + nextP[5][5] = P[5][5] + P[0][5]*SF[2] + P[2][5]*SF[1] + P[3][5]*SF[3] - P[1][5]*SPP[0] + P[13][5]*SPP[3] + dvxCov*sq(SG[7] + 2*q0*q3) + dvzCov*sq(SG[5] - 2*q0*q1) + SF[2]*(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[1]*(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[3]*(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[0]*(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]) + SPP[3]*(P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]) + dvyCov*sq(SG[1] - SG[2] + SG[3] - SG[4]); + nextP[5][6] = P[5][6] + SQ[0] + P[0][6]*SF[2] + P[2][6]*SF[1] + P[3][6]*SF[3] - P[1][6]*SPP[0] + P[13][6]*SPP[3] + SF[2]*(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[1]*(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[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]) - SPP[1]*(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]) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]); + nextP[5][7] = P[5][7] + P[0][7]*SF[2] + P[2][7]*SF[1] + P[3][7]*SF[3] - P[1][7]*SPP[0] + P[13][7]*SPP[3] + dt*(P[5][4] + P[0][4]*SF[2] + P[2][4]*SF[1] + P[3][4]*SF[3] - P[1][4]*SPP[0] + P[13][4]*SPP[3]); + nextP[5][8] = P[5][8] + P[0][8]*SF[2] + P[2][8]*SF[1] + P[3][8]*SF[3] - P[1][8]*SPP[0] + P[13][8]*SPP[3] + dt*(P[5][5] + P[0][5]*SF[2] + P[2][5]*SF[1] + P[3][5]*SF[3] - P[1][5]*SPP[0] + P[13][5]*SPP[3]); + nextP[5][9] = P[5][9] + P[0][9]*SF[2] + P[2][9]*SF[1] + P[3][9]*SF[3] - P[1][9]*SPP[0] + P[13][9]*SPP[3] + dt*(P[5][6] + P[0][6]*SF[2] + P[2][6]*SF[1] + P[3][6]*SF[3] - P[1][6]*SPP[0] + P[13][6]*SPP[3]); + nextP[5][10] = 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]; + nextP[5][11] = 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]; + nextP[5][12] = 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][13] = P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]; + nextP[5][14] = P[5][14] + P[0][14]*SF[2] + P[2][14]*SF[1] + P[3][14]*SF[3] - P[1][14]*SPP[0] + P[13][14]*SPP[3]; + nextP[5][15] = P[5][15] + P[0][15]*SF[2] + P[2][15]*SF[1] + P[3][15]*SF[3] - P[1][15]*SPP[0] + P[13][15]*SPP[3]; + nextP[5][16] = P[5][16] + P[0][16]*SF[2] + P[2][16]*SF[1] + P[3][16]*SF[3] - P[1][16]*SPP[0] + P[13][16]*SPP[3]; + nextP[5][17] = P[5][17] + P[0][17]*SF[2] + P[2][17]*SF[1] + P[3][17]*SF[3] - P[1][17]*SPP[0] + P[13][17]*SPP[3]; + nextP[5][18] = P[5][18] + P[0][18]*SF[2] + P[2][18]*SF[1] + P[3][18]*SF[3] - P[1][18]*SPP[0] + P[13][18]*SPP[3]; + 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; + nextP[6][3] = 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[5]*(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[4]*(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[7]*(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[11]*(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][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][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))))/2; + nextP[6][4] = P[6][4] + SQ[1] + P[1][4]*SF[2] + P[3][4]*SF[1] + P[0][4]*SPP[0] - P[2][4]*SPP[1] - P[13][4]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[3]*(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[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))) + SPP[0]*(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))) - SPP[2]*(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[4]*(P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))); + nextP[6][5] = P[6][5] + SQ[0] + P[1][5]*SF[2] + P[3][5]*SF[1] + P[0][5]*SPP[0] - P[2][5]*SPP[1] - P[13][5]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[2]*(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[1]*(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[3]*(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[0]*(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))) + SPP[3]*(P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))); + nextP[6][6] = P[6][6] + P[1][6]*SF[2] + P[3][6]*SF[1] + P[0][6]*SPP[0] - P[2][6]*SPP[1] - P[13][6]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + dvxCov*sq(SG[6] - 2*q0*q2) + dvyCov*sq(SG[5] + 2*q0*q1) - SPP[5]*(P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*SPP[5]) + SF[2]*(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[1]*(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[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))) - SPP[1]*(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))) + dvzCov*sq(SG[1] - SG[2] - SG[3] + SG[4]); + nextP[6][7] = P[6][7] + P[1][7]*SF[2] + P[3][7]*SF[1] + P[0][7]*SPP[0] - P[2][7]*SPP[1] - P[13][7]*SPP[5] + dt*(P[6][4] + P[1][4]*SF[2] + P[3][4]*SF[1] + P[0][4]*SPP[0] - P[2][4]*SPP[1] - P[13][4]*SPP[5]); + nextP[6][8] = P[6][8] + P[1][8]*SF[2] + P[3][8]*SF[1] + P[0][8]*SPP[0] - P[2][8]*SPP[1] - P[13][8]*SPP[5] + dt*(P[6][5] + P[1][5]*SF[2] + P[3][5]*SF[1] + P[0][5]*SPP[0] - P[2][5]*SPP[1] - P[13][5]*SPP[5]); + nextP[6][9] = P[6][9] + P[1][9]*SF[2] + P[3][9]*SF[1] + P[0][9]*SPP[0] - P[2][9]*SPP[1] - P[13][9]*SPP[5] + dt*(P[6][6] + P[1][6]*SF[2] + P[3][6]*SF[1] + P[0][6]*SPP[0] - P[2][6]*SPP[1] - P[13][6]*SPP[5]); + nextP[6][10] = 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]*SPP[5]; + nextP[6][11] = 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]*SPP[5]; + nextP[6][12] = 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]*SPP[5]; + nextP[6][13] = P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*SPP[5]; + nextP[6][14] = P[6][14] + P[1][14]*SF[2] + P[3][14]*SF[1] + P[0][14]*SPP[0] - P[2][14]*SPP[1] - P[13][14]*SPP[5]; + nextP[6][15] = P[6][15] + P[1][15]*SF[2] + P[3][15]*SF[1] + P[0][15]*SPP[0] - P[2][15]*SPP[1] - P[13][15]*SPP[5]; + nextP[6][16] = P[6][16] + P[1][16]*SF[2] + P[3][16]*SF[1] + P[0][16]*SPP[0] - P[2][16]*SPP[1] - P[13][16]*SPP[5]; + nextP[6][17] = P[6][17] + P[1][17]*SF[2] + P[3][17]*SF[1] + P[0][17]*SPP[0] - P[2][17]*SPP[1] - P[13][17]*SPP[5]; + nextP[6][18] = P[6][18] + P[1][18]*SF[2] + P[3][18]*SF[1] + P[0][18]*SPP[0] - P[2][18]*SPP[1] - P[13][18]*SPP[5]; + 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; + nextP[7][3] = P[7][3] + P[4][3]*dt + SF[5]*(P[7][0] + P[4][0]*dt) + SF[4]*(P[7][1] + P[4][1]*dt) + SF[7]*(P[7][2] + P[4][2]*dt) - SF[11]*(P[7][11] + P[4][11]*dt) + SPP[7]*(P[7][10] + P[4][10]*dt) - (q0*(P[7][12] + P[4][12]*dt))/2; + nextP[7][4] = P[7][4] + P[4][4]*dt + SF[1]*(P[7][1] + P[4][1]*dt) + SF[3]*(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) - SPP[4]*(P[7][13] + P[4][13]*dt); + nextP[7][5] = P[7][5] + P[4][5]*dt + SF[2]*(P[7][0] + P[4][0]*dt) + SF[1]*(P[7][2] + P[4][2]*dt) + SF[3]*(P[7][3] + P[4][3]*dt) - SPP[0]*(P[7][1] + P[4][1]*dt) + SPP[3]*(P[7][13] + P[4][13]*dt); + nextP[7][6] = P[7][6] + P[4][6]*dt + SF[2]*(P[7][1] + P[4][1]*dt) + SF[1]*(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) - SPP[5]*(P[7][13] + P[4][13]*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[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; + nextP[8][3] = P[8][3] + P[5][3]*dt + SF[5]*(P[8][0] + P[5][0]*dt) + SF[4]*(P[8][1] + P[5][1]*dt) + SF[7]*(P[8][2] + P[5][2]*dt) - SF[11]*(P[8][11] + P[5][11]*dt) + SPP[7]*(P[8][10] + P[5][10]*dt) - (q0*(P[8][12] + P[5][12]*dt))/2; + nextP[8][4] = P[8][4] + P[5][4]*dt + SF[1]*(P[8][1] + P[5][1]*dt) + SF[3]*(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) - SPP[4]*(P[8][13] + P[5][13]*dt); + nextP[8][5] = P[8][5] + P[5][5]*dt + SF[2]*(P[8][0] + P[5][0]*dt) + SF[1]*(P[8][2] + P[5][2]*dt) + SF[3]*(P[8][3] + P[5][3]*dt) - SPP[0]*(P[8][1] + P[5][1]*dt) + SPP[3]*(P[8][13] + P[5][13]*dt); + nextP[8][6] = P[8][6] + P[5][6]*dt + SF[2]*(P[8][1] + P[5][1]*dt) + SF[1]*(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) - SPP[5]*(P[8][13] + P[5][13]*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[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; + nextP[9][3] = P[9][3] + P[6][3]*dt + SF[5]*(P[9][0] + P[6][0]*dt) + SF[4]*(P[9][1] + P[6][1]*dt) + SF[7]*(P[9][2] + P[6][2]*dt) - SF[11]*(P[9][11] + P[6][11]*dt) + SPP[7]*(P[9][10] + P[6][10]*dt) - (q0*(P[9][12] + P[6][12]*dt))/2; + nextP[9][4] = P[9][4] + P[6][4]*dt + SF[1]*(P[9][1] + P[6][1]*dt) + SF[3]*(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) - SPP[4]*(P[9][13] + P[6][13]*dt); + nextP[9][5] = P[9][5] + P[6][5]*dt + SF[2]*(P[9][0] + P[6][0]*dt) + SF[1]*(P[9][2] + P[6][2]*dt) + SF[3]*(P[9][3] + P[6][3]*dt) - SPP[0]*(P[9][1] + P[6][1]*dt) + SPP[3]*(P[9][13] + P[6][13]*dt); + nextP[9][6] = P[9][6] + P[6][6]*dt + SF[2]*(P[9][1] + P[6][1]*dt) + SF[1]*(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) - SPP[5]*(P[9][13] + P[6][13]*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[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; + nextP[10][3] = P[10][3] + P[10][0]*SF[5] + P[10][1]*SF[4] + P[10][2]*SF[7] - P[10][11]*SF[11] + P[10][10]*SPP[7] - (P[10][12]*q0)/2; + nextP[10][4] = P[10][4] + P[10][1]*SF[1] + P[10][0]*SF[3] + P[10][2]*SPP[0] - P[10][3]*SPP[2] - P[10][13]*SPP[4]; + nextP[10][5] = P[10][5] + P[10][0]*SF[2] + P[10][2]*SF[1] + P[10][3]*SF[3] - P[10][1]*SPP[0] + P[10][13]*SPP[3]; + nextP[10][6] = P[10][6] + P[10][1]*SF[2] + P[10][3]*SF[1] + P[10][0]*SPP[0] - P[10][2]*SPP[1] - P[10][13]*SPP[5]; + 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[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; + nextP[11][3] = P[11][3] + P[11][0]*SF[5] + P[11][1]*SF[4] + P[11][2]*SF[7] - P[11][11]*SF[11] + P[11][10]*SPP[7] - (P[11][12]*q0)/2; + nextP[11][4] = P[11][4] + P[11][1]*SF[1] + P[11][0]*SF[3] + P[11][2]*SPP[0] - P[11][3]*SPP[2] - P[11][13]*SPP[4]; + nextP[11][5] = P[11][5] + P[11][0]*SF[2] + P[11][2]*SF[1] + P[11][3]*SF[3] - P[11][1]*SPP[0] + P[11][13]*SPP[3]; + nextP[11][6] = P[11][6] + P[11][1]*SF[2] + P[11][3]*SF[1] + P[11][0]*SPP[0] - P[11][2]*SPP[1] - P[11][13]*SPP[5]; + 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[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; + nextP[12][3] = P[12][3] + P[12][0]*SF[5] + P[12][1]*SF[4] + P[12][2]*SF[7] - P[12][11]*SF[11] + P[12][10]*SPP[7] - (P[12][12]*q0)/2; + nextP[12][4] = P[12][4] + P[12][1]*SF[1] + P[12][0]*SF[3] + P[12][2]*SPP[0] - P[12][3]*SPP[2] - P[12][13]*SPP[4]; + nextP[12][5] = P[12][5] + P[12][0]*SF[2] + P[12][2]*SF[1] + P[12][3]*SF[3] - P[12][1]*SPP[0] + P[12][13]*SPP[3]; + nextP[12][6] = P[12][6] + P[12][1]*SF[2] + P[12][3]*SF[1] + P[12][0]*SPP[0] - P[12][2]*SPP[1] - P[12][13]*SPP[5]; + 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[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; + nextP[13][3] = P[13][3] + P[13][0]*SF[5] + P[13][1]*SF[4] + P[13][2]*SF[7] - P[13][11]*SF[11] + P[13][10]*SPP[7] - (P[13][12]*q0)/2; + nextP[13][4] = P[13][4] + P[13][1]*SF[1] + P[13][0]*SF[3] + P[13][2]*SPP[0] - P[13][3]*SPP[2] - P[13][13]*SPP[4]; + nextP[13][5] = P[13][5] + P[13][0]*SF[2] + P[13][2]*SF[1] + P[13][3]*SF[3] - P[13][1]*SPP[0] + P[13][13]*SPP[3]; + nextP[13][6] = P[13][6] + P[13][1]*SF[2] + P[13][3]*SF[1] + P[13][0]*SPP[0] - P[13][2]*SPP[1] - P[13][13]*SPP[5]; + 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[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; + nextP[14][3] = P[14][3] + P[14][0]*SF[5] + P[14][1]*SF[4] + P[14][2]*SF[7] - P[14][11]*SF[11] + P[14][10]*SPP[7] - (P[14][12]*q0)/2; + nextP[14][4] = P[14][4] + P[14][1]*SF[1] + P[14][0]*SF[3] + P[14][2]*SPP[0] - P[14][3]*SPP[2] - P[14][13]*SPP[4]; + nextP[14][5] = P[14][5] + P[14][0]*SF[2] + P[14][2]*SF[1] + P[14][3]*SF[3] - P[14][1]*SPP[0] + P[14][13]*SPP[3]; + nextP[14][6] = P[14][6] + P[14][1]*SF[2] + P[14][3]*SF[1] + P[14][0]*SPP[0] - P[14][2]*SPP[1] - P[14][13]*SPP[5]; + 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[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; + nextP[15][3] = P[15][3] + P[15][0]*SF[5] + P[15][1]*SF[4] + P[15][2]*SF[7] - P[15][11]*SF[11] + P[15][10]*SPP[7] - (P[15][12]*q0)/2; + nextP[15][4] = P[15][4] + P[15][1]*SF[1] + P[15][0]*SF[3] + P[15][2]*SPP[0] - P[15][3]*SPP[2] - P[15][13]*SPP[4]; + nextP[15][5] = P[15][5] + P[15][0]*SF[2] + P[15][2]*SF[1] + P[15][3]*SF[3] - P[15][1]*SPP[0] + P[15][13]*SPP[3]; + nextP[15][6] = P[15][6] + P[15][1]*SF[2] + P[15][3]*SF[1] + P[15][0]*SPP[0] - P[15][2]*SPP[1] - P[15][13]*SPP[5]; + 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[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; + nextP[16][3] = P[16][3] + P[16][0]*SF[5] + P[16][1]*SF[4] + P[16][2]*SF[7] - P[16][11]*SF[11] + P[16][10]*SPP[7] - (P[16][12]*q0)/2; + nextP[16][4] = P[16][4] + P[16][1]*SF[1] + P[16][0]*SF[3] + P[16][2]*SPP[0] - P[16][3]*SPP[2] - P[16][13]*SPP[4]; + nextP[16][5] = P[16][5] + P[16][0]*SF[2] + P[16][2]*SF[1] + P[16][3]*SF[3] - P[16][1]*SPP[0] + P[16][13]*SPP[3]; + nextP[16][6] = P[16][6] + P[16][1]*SF[2] + P[16][3]*SF[1] + P[16][0]*SPP[0] - P[16][2]*SPP[1] - P[16][13]*SPP[5]; + 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[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; + nextP[17][3] = P[17][3] + P[17][0]*SF[5] + P[17][1]*SF[4] + P[17][2]*SF[7] - P[17][11]*SF[11] + P[17][10]*SPP[7] - (P[17][12]*q0)/2; + nextP[17][4] = P[17][4] + P[17][1]*SF[1] + P[17][0]*SF[3] + P[17][2]*SPP[0] - P[17][3]*SPP[2] - P[17][13]*SPP[4]; + nextP[17][5] = P[17][5] + P[17][0]*SF[2] + P[17][2]*SF[1] + P[17][3]*SF[3] - P[17][1]*SPP[0] + P[17][13]*SPP[3]; + nextP[17][6] = P[17][6] + P[17][1]*SF[2] + P[17][3]*SF[1] + P[17][0]*SPP[0] - P[17][2]*SPP[1] - P[17][13]*SPP[5]; + 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[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; + nextP[18][3] = P[18][3] + P[18][0]*SF[5] + P[18][1]*SF[4] + P[18][2]*SF[7] - P[18][11]*SF[11] + P[18][10]*SPP[7] - (P[18][12]*q0)/2; + nextP[18][4] = P[18][4] + P[18][1]*SF[1] + P[18][0]*SF[3] + P[18][2]*SPP[0] - P[18][3]*SPP[2] - P[18][13]*SPP[4]; + nextP[18][5] = P[18][5] + P[18][0]*SF[2] + P[18][2]*SF[1] + P[18][3]*SF[3] - P[18][1]*SPP[0] + P[18][13]*SPP[3]; + nextP[18][6] = P[18][6] + P[18][1]*SF[2] + P[18][3]*SF[1] + P[18][0]*SPP[0] - P[18][2]*SPP[1] - P[18][13]*SPP[5]; + 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[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; + nextP[19][3] = P[19][3] + P[19][0]*SF[5] + P[19][1]*SF[4] + P[19][2]*SF[7] - P[19][11]*SF[11] + P[19][10]*SPP[7] - (P[19][12]*q0)/2; + nextP[19][4] = P[19][4] + P[19][1]*SF[1] + P[19][0]*SF[3] + P[19][2]*SPP[0] - P[19][3]*SPP[2] - P[19][13]*SPP[4]; + nextP[19][5] = P[19][5] + P[19][0]*SF[2] + P[19][2]*SF[1] + P[19][3]*SF[3] - P[19][1]*SPP[0] + P[19][13]*SPP[3]; + nextP[19][6] = P[19][6] + P[19][1]*SF[2] + P[19][3]*SF[1] + P[19][0]*SPP[0] - P[19][2]*SPP[1] - P[19][13]*SPP[5]; + 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[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; + nextP[20][3] = P[20][3] + P[20][0]*SF[5] + P[20][1]*SF[4] + P[20][2]*SF[7] - P[20][11]*SF[11] + P[20][10]*SPP[7] - (P[20][12]*q0)/2; + nextP[20][4] = P[20][4] + P[20][1]*SF[1] + P[20][0]*SF[3] + P[20][2]*SPP[0] - P[20][3]*SPP[2] - P[20][13]*SPP[4]; + nextP[20][5] = P[20][5] + P[20][0]*SF[2] + P[20][2]*SF[1] + P[20][3]*SF[3] - P[20][1]*SPP[0] + P[20][13]*SPP[3]; + nextP[20][6] = P[20][6] + P[20][1]*SF[2] + P[20][3]*SF[1] + P[20][0]*SPP[0] - P[20][2]*SPP[1] - P[20][13]*SPP[5]; + 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]; + 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; + nextP[21][3] = P[21][3] + P[21][0]*SF[5] + P[21][1]*SF[4] + P[21][2]*SF[7] - P[21][11]*SF[11] + P[21][10]*SPP[7] - (P[21][12]*q0)/2; + nextP[21][4] = P[21][4] + P[21][1]*SF[1] + P[21][0]*SF[3] + P[21][2]*SPP[0] - P[21][3]*SPP[2] - P[21][13]*SPP[4]; + nextP[21][5] = P[21][5] + P[21][0]*SF[2] + P[21][2]*SF[1] + P[21][3]*SF[3] - P[21][1]*SPP[0] + P[21][13]*SPP[3]; + nextP[21][6] = P[21][6] + P[21][1]*SF[2] + P[21][3]*SF[1] + P[21][0]*SPP[0] - P[21][2]*SPP[1] - P[21][13]*SPP[5]; + nextP[21][7] = P[21][7] + P[21][4]*dt; + nextP[21][8] = P[21][8] + P[21][5]*dt; + nextP[21][9] = P[21][9] + P[21][6]*dt; + nextP[21][10] = P[21][10]; + nextP[21][11] = P[21][11]; + nextP[21][12] = P[21][12]; + nextP[21][13] = P[21][13]; + nextP[21][14] = P[21][14]; + nextP[21][15] = P[21][15]; + nextP[21][16] = P[21][16]; + nextP[21][17] = P[21][17]; + nextP[21][18] = P[21][18]; + 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++) + { + 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,16,21); + zeroCols(nextP,16,21); + } + + // If on ground or not using airspeed sensing, inhibit wind velocity + // covariance growth. + if (onGround || !useAirspeed) + { + zeroRows(nextP,14,15); + zeroCols(nextP,14,15); + } + + // If on ground, inhibit terrain offset updates by + // setting the coresponding covariance terms to zero. + if (onGround) + { + zeroRows(nextP,22,22); + zeroCols(nextP,22,22); + } + + // 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 <= 13; 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 > 13) || (j > 13)) { + 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] = sq(vneSigma) + sq(velErr); + R_OBS[1] = R_OBS[0]; + R_OBS[2] = sq(vdSigma) + sq(velErr); + R_OBS[3] = sq(posNeSigma) + sq(posErr); + R_OBS[4] = R_OBS[3]; + R_OBS[5] = sq(posDSigma) + sq(posErr); + + // 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 = 22; + } + else + { + indexLimit = 13; + } + // 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; + } + + // Don't update Z accel bias state unless using a height observation (GPS velocities can be biased) + if (obsIndex != 5) { + Kfusion[13] = 0; + } + + // 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(); + +} + +void AttPosEKF::FuseMagnetometer() +{ + + float &q0 = magstate.q0; + float &q1 = magstate.q1; + float &q2 = magstate.q2; + float &q3 = magstate.q3; + float &magN = magstate.magN; + float &magE = magstate.magE; + float &magD = magstate.magD; + float &magXbias = magstate.magXbias; + float &magYbias = magstate.magYbias; + float &magZbias = magstate.magZbias; + unsigned &obsIndex = magstate.obsIndex; + Mat3f &DCM = magstate.DCM; + float *MagPred = &magstate.MagPred[0]; + float &R_MAG = magstate.R_MAG; + float *SH_MAG = &magstate.SH_MAG[0]; + + float SK_MX[6]; + float SK_MY[5]; + float SK_MZ[6]; + float H_MAG[n_states]; + for (uint8_t i = 0; i < n_states; i++) { + H_MAG[i] = 0.0f; + } + unsigned indexLimit; + +// 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 = n_states; + } + else + { + indexLimit = 13 + 1; + } + + // Sequential fusion of XYZ components to spread processing load across + // three prediction time steps. + + // Calculate observation jacobians and Kalman gains + if (fuseMagData) + { + // Copy required states to local variable names + q0 = statesAtMagMeasTime[0]; + q1 = statesAtMagMeasTime[1]; + q2 = statesAtMagMeasTime[2]; + q3 = statesAtMagMeasTime[3]; + magN = statesAtMagMeasTime[16]; + magE = statesAtMagMeasTime[17]; + magD = statesAtMagMeasTime[18]; + magXbias = statesAtMagMeasTime[19]; + magYbias = statesAtMagMeasTime[20]; + magZbias = statesAtMagMeasTime[21]; + + // rotate predicted earth components into body axes and calculate + // predicted measurments + DCM.x.x = q0*q0 + q1*q1 - q2*q2 - q3*q3; + DCM.x.y = 2*(q1*q2 + q0*q3); + DCM.x.z = 2*(q1*q3-q0*q2); + DCM.y.x = 2*(q1*q2 - q0*q3); + DCM.y.y = q0*q0 - q1*q1 + q2*q2 - q3*q3; + DCM.y.z = 2*(q2*q3 + q0*q1); + DCM.z.x = 2*(q1*q3 + q0*q2); + DCM.z.y = 2*(q2*q3 - q0*q1); + DCM.z.z = q0*q0 - q1*q1 - q2*q2 + q3*q3; + MagPred[0] = DCM.x.x*magN + DCM.x.y*magE + DCM.x.z*magD + magXbias; + MagPred[1] = DCM.y.x*magN + DCM.y.y*magE + DCM.y.z*magD + magYbias; + MagPred[2] = DCM.z.x*magN + DCM.z.y*magE + DCM.z.z*magD + magZbias; + + // scale magnetometer observation error with total angular rate + R_MAG = sq(magMeasurementSigma) + 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 < n_states; 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[16] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6]; + H_MAG[17] = 2*q0*q3 + 2*q1*q2; + H_MAG[18] = 2*q1*q3 - 2*q0*q2; + H_MAG[19] = 1.0f; + + // Calculate Kalman gain + float temp = (P[19][19] + R_MAG + P[1][19]*SH_MAG[0] + P[3][19]*SH_MAG[2] - P[16][19]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[19][2] + P[1][2]*SH_MAG[0] + P[3][2]*SH_MAG[2] - P[16][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][2]*(2*q0*q3 + 2*q1*q2) - P[18][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[19][0] + P[1][0]*SH_MAG[0] + P[3][0]*SH_MAG[2] - P[16][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][0]*(2*q0*q3 + 2*q1*q2) - P[18][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[19][1] + P[1][1]*SH_MAG[0] + P[3][1]*SH_MAG[2] - P[16][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][1]*(2*q0*q3 + 2*q1*q2) - P[18][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[19][3] + P[1][3]*SH_MAG[0] + P[3][3]*SH_MAG[2] - P[16][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][3]*(2*q0*q3 + 2*q1*q2) - P[18][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[19][16] + P[1][16]*SH_MAG[0] + P[3][16]*SH_MAG[2] - P[16][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][16]*(2*q0*q3 + 2*q1*q2) - P[18][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)) + P[17][19]*(2*q0*q3 + 2*q1*q2) - P[18][19]*(2*q0*q2 - 2*q1*q3) - P[2][19]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[19][17] + P[1][17]*SH_MAG[0] + P[3][17]*SH_MAG[2] - P[16][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][17]*(2*q0*q3 + 2*q1*q2) - P[18][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)) - (2*q0*q2 - 2*q1*q3)*(P[19][18] + P[1][18]*SH_MAG[0] + P[3][18]*SH_MAG[2] - P[16][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][18]*(2*q0*q3 + 2*q1*q2) - P[18][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][19]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); + if (temp >= R_MAG) { + SK_MX[0] = 1.0f / temp; + } else { + // the calculation is badly conditioned, so we cannot perform fusion on this step + // we increase the state variances and try again next time + P[19][19] += 0.1f*R_MAG; + obsIndex = 1; + return; + } + 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][19] + 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][16]*SK_MX[1] + P[0][17]*SK_MX[5] - P[0][18]*SK_MX[4]); + Kfusion[1] = SK_MX[0]*(P[1][19] + 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][16]*SK_MX[1] + P[1][17]*SK_MX[5] - P[1][18]*SK_MX[4]); + Kfusion[2] = SK_MX[0]*(P[2][19] + 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][16]*SK_MX[1] + P[2][17]*SK_MX[5] - P[2][18]*SK_MX[4]); + Kfusion[3] = SK_MX[0]*(P[3][19] + 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][16]*SK_MX[1] + P[3][17]*SK_MX[5] - P[3][18]*SK_MX[4]); + Kfusion[4] = SK_MX[0]*(P[4][19] + 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][16]*SK_MX[1] + P[4][17]*SK_MX[5] - P[4][18]*SK_MX[4]); + Kfusion[5] = SK_MX[0]*(P[5][19] + 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][16]*SK_MX[1] + P[5][17]*SK_MX[5] - P[5][18]*SK_MX[4]); + Kfusion[6] = SK_MX[0]*(P[6][19] + 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][16]*SK_MX[1] + P[6][17]*SK_MX[5] - P[6][18]*SK_MX[4]); + Kfusion[7] = SK_MX[0]*(P[7][19] + 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][16]*SK_MX[1] + P[7][17]*SK_MX[5] - P[7][18]*SK_MX[4]); + Kfusion[8] = SK_MX[0]*(P[8][19] + 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][16]*SK_MX[1] + P[8][17]*SK_MX[5] - P[8][18]*SK_MX[4]); + Kfusion[9] = SK_MX[0]*(P[9][19] + 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][16]*SK_MX[1] + P[9][17]*SK_MX[5] - P[9][18]*SK_MX[4]); + Kfusion[10] = SK_MX[0]*(P[10][19] + 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][16]*SK_MX[1] + P[10][17]*SK_MX[5] - P[10][18]*SK_MX[4]); + Kfusion[11] = SK_MX[0]*(P[11][19] + 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][16]*SK_MX[1] + P[11][17]*SK_MX[5] - P[11][18]*SK_MX[4]); + Kfusion[12] = SK_MX[0]*(P[12][19] + 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][16]*SK_MX[1] + P[12][17]*SK_MX[5] - P[12][18]*SK_MX[4]); + // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate + Kfusion[13] = 0.0f;//SK_MX[0]*(P[13][19] + 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][16]*SK_MX[1] + P[13][17]*SK_MX[5] - P[13][18]*SK_MX[4]); + Kfusion[14] = SK_MX[0]*(P[14][19] + 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][16]*SK_MX[1] + P[14][17]*SK_MX[5] - P[14][18]*SK_MX[4]); + Kfusion[15] = SK_MX[0]*(P[15][19] + 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][16]*SK_MX[1] + P[15][17]*SK_MX[5] - P[15][18]*SK_MX[4]); + Kfusion[16] = SK_MX[0]*(P[16][19] + 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][16]*SK_MX[1] + P[16][17]*SK_MX[5] - P[16][18]*SK_MX[4]); + Kfusion[17] = SK_MX[0]*(P[17][19] + 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][16]*SK_MX[1] + P[17][17]*SK_MX[5] - P[17][18]*SK_MX[4]); + Kfusion[18] = SK_MX[0]*(P[18][19] + 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][16]*SK_MX[1] + P[18][17]*SK_MX[5] - P[18][18]*SK_MX[4]); + Kfusion[19] = SK_MX[0]*(P[19][19] + 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][16]*SK_MX[1] + P[19][17]*SK_MX[5] - P[19][18]*SK_MX[4]); + 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]); + 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]); + 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; + fuseMagData = false; + } + else if (obsIndex == 1) // we are now fusing the Y measurement + { + // Calculate observation jacobians + for (unsigned int i = 0; i < n_states; i++) H_MAG[i] = 0; + H_MAG[0] = SH_MAG[2]; + H_MAG[1] = SH_MAG[1]; + H_MAG[2] = SH_MAG[0]; + H_MAG[3] = 2*magD*q2 - SH_MAG[8] - SH_MAG[7]; + H_MAG[16] = 2*q1*q2 - 2*q0*q3; + H_MAG[17] = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6]; + H_MAG[18] = 2*q0*q1 + 2*q2*q3; + H_MAG[20] = 1; + + // Calculate Kalman gain + float temp = (P[20][20] + R_MAG + P[0][20]*SH_MAG[2] + P[1][20]*SH_MAG[1] + P[2][20]*SH_MAG[0] - P[17][20]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2*q0*q3 - 2*q1*q2)*(P[20][16] + P[0][16]*SH_MAG[2] + P[1][16]*SH_MAG[1] + P[2][16]*SH_MAG[0] - P[17][16]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][16]*(2*q0*q3 - 2*q1*q2) + P[18][16]*(2*q0*q1 + 2*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(P[20][18] + P[0][18]*SH_MAG[2] + P[1][18]*SH_MAG[1] + P[2][18]*SH_MAG[0] - P[17][18]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][18]*(2*q0*q3 - 2*q1*q2) + P[18][18]*(2*q0*q1 + 2*q2*q3) - P[3][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[20][3] + P[0][3]*SH_MAG[2] + P[1][3]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[17][3]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][3]*(2*q0*q3 - 2*q1*q2) + P[18][3]*(2*q0*q1 + 2*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[16][20]*(2*q0*q3 - 2*q1*q2) + P[18][20]*(2*q0*q1 + 2*q2*q3) + SH_MAG[2]*(P[20][0] + P[0][0]*SH_MAG[2] + P[1][0]*SH_MAG[1] + P[2][0]*SH_MAG[0] - P[17][0]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][0]*(2*q0*q3 - 2*q1*q2) + P[18][0]*(2*q0*q1 + 2*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[20][1] + P[0][1]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[2][1]*SH_MAG[0] - P[17][1]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][1]*(2*q0*q3 - 2*q1*q2) + P[18][1]*(2*q0*q1 + 2*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[20][2] + P[0][2]*SH_MAG[2] + P[1][2]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[17][2]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][2]*(2*q0*q3 - 2*q1*q2) + P[18][2]*(2*q0*q1 + 2*q2*q3) - P[3][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P[20][17] + P[0][17]*SH_MAG[2] + P[1][17]*SH_MAG[1] + P[2][17]*SH_MAG[0] - P[17][17]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][17]*(2*q0*q3 - 2*q1*q2) + P[18][17]*(2*q0*q1 + 2*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[3][20]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); + if (temp >= R_MAG) { + SK_MY[0] = 1.0f / temp; + } else { + // the calculation is badly conditioned, so we cannot perform fusion on this step + // we increase the state variances and try again next time + P[20][20] += 0.1f*R_MAG; + obsIndex = 2; + return; + } + SK_MY[1] = SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]; + SK_MY[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; + SK_MY[3] = 2*q0*q3 - 2*q1*q2; + SK_MY[4] = 2*q0*q1 + 2*q2*q3; + Kfusion[0] = SK_MY[0]*(P[0][20] + P[0][0]*SH_MAG[2] + P[0][1]*SH_MAG[1] + P[0][2]*SH_MAG[0] - P[0][3]*SK_MY[2] - P[0][17]*SK_MY[1] - P[0][16]*SK_MY[3] + P[0][18]*SK_MY[4]); + Kfusion[1] = SK_MY[0]*(P[1][20] + P[1][0]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[1][2]*SH_MAG[0] - P[1][3]*SK_MY[2] - P[1][17]*SK_MY[1] - P[1][16]*SK_MY[3] + P[1][18]*SK_MY[4]); + Kfusion[2] = SK_MY[0]*(P[2][20] + P[2][0]*SH_MAG[2] + P[2][1]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[2][3]*SK_MY[2] - P[2][17]*SK_MY[1] - P[2][16]*SK_MY[3] + P[2][18]*SK_MY[4]); + Kfusion[3] = SK_MY[0]*(P[3][20] + P[3][0]*SH_MAG[2] + P[3][1]*SH_MAG[1] + P[3][2]*SH_MAG[0] - P[3][3]*SK_MY[2] - P[3][17]*SK_MY[1] - P[3][16]*SK_MY[3] + P[3][18]*SK_MY[4]); + Kfusion[4] = SK_MY[0]*(P[4][20] + P[4][0]*SH_MAG[2] + P[4][1]*SH_MAG[1] + P[4][2]*SH_MAG[0] - P[4][3]*SK_MY[2] - P[4][17]*SK_MY[1] - P[4][16]*SK_MY[3] + P[4][18]*SK_MY[4]); + Kfusion[5] = SK_MY[0]*(P[5][20] + P[5][0]*SH_MAG[2] + P[5][1]*SH_MAG[1] + P[5][2]*SH_MAG[0] - P[5][3]*SK_MY[2] - P[5][17]*SK_MY[1] - P[5][16]*SK_MY[3] + P[5][18]*SK_MY[4]); + Kfusion[6] = SK_MY[0]*(P[6][20] + P[6][0]*SH_MAG[2] + P[6][1]*SH_MAG[1] + P[6][2]*SH_MAG[0] - P[6][3]*SK_MY[2] - P[6][17]*SK_MY[1] - P[6][16]*SK_MY[3] + P[6][18]*SK_MY[4]); + Kfusion[7] = SK_MY[0]*(P[7][20] + P[7][0]*SH_MAG[2] + P[7][1]*SH_MAG[1] + P[7][2]*SH_MAG[0] - P[7][3]*SK_MY[2] - P[7][17]*SK_MY[1] - P[7][16]*SK_MY[3] + P[7][18]*SK_MY[4]); + Kfusion[8] = SK_MY[0]*(P[8][20] + P[8][0]*SH_MAG[2] + P[8][1]*SH_MAG[1] + P[8][2]*SH_MAG[0] - P[8][3]*SK_MY[2] - P[8][17]*SK_MY[1] - P[8][16]*SK_MY[3] + P[8][18]*SK_MY[4]); + Kfusion[9] = SK_MY[0]*(P[9][20] + P[9][0]*SH_MAG[2] + P[9][1]*SH_MAG[1] + P[9][2]*SH_MAG[0] - P[9][3]*SK_MY[2] - P[9][17]*SK_MY[1] - P[9][16]*SK_MY[3] + P[9][18]*SK_MY[4]); + Kfusion[10] = SK_MY[0]*(P[10][20] + P[10][0]*SH_MAG[2] + P[10][1]*SH_MAG[1] + P[10][2]*SH_MAG[0] - P[10][3]*SK_MY[2] - P[10][17]*SK_MY[1] - P[10][16]*SK_MY[3] + P[10][18]*SK_MY[4]); + Kfusion[11] = SK_MY[0]*(P[11][20] + P[11][0]*SH_MAG[2] + P[11][1]*SH_MAG[1] + P[11][2]*SH_MAG[0] - P[11][3]*SK_MY[2] - P[11][17]*SK_MY[1] - P[11][16]*SK_MY[3] + P[11][18]*SK_MY[4]); + Kfusion[12] = SK_MY[0]*(P[12][20] + P[12][0]*SH_MAG[2] + P[12][1]*SH_MAG[1] + P[12][2]*SH_MAG[0] - P[12][3]*SK_MY[2] - P[12][17]*SK_MY[1] - P[12][16]*SK_MY[3] + P[12][18]*SK_MY[4]); + // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate + Kfusion[13] = 0.0f;//SK_MY[0]*(P[13][20] + P[13][0]*SH_MAG[2] + P[13][1]*SH_MAG[1] + P[13][2]*SH_MAG[0] - P[13][3]*SK_MY[2] - P[13][17]*SK_MY[1] - P[13][16]*SK_MY[3] + P[13][18]*SK_MY[4]); + Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][0]*SH_MAG[2] + P[14][1]*SH_MAG[1] + P[14][2]*SH_MAG[0] - P[14][3]*SK_MY[2] - P[14][17]*SK_MY[1] - P[14][16]*SK_MY[3] + P[14][18]*SK_MY[4]); + Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][0]*SH_MAG[2] + P[15][1]*SH_MAG[1] + P[15][2]*SH_MAG[0] - P[15][3]*SK_MY[2] - P[15][17]*SK_MY[1] - P[15][16]*SK_MY[3] + P[15][18]*SK_MY[4]); + Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][0]*SH_MAG[2] + P[16][1]*SH_MAG[1] + P[16][2]*SH_MAG[0] - P[16][3]*SK_MY[2] - P[16][17]*SK_MY[1] - P[16][16]*SK_MY[3] + P[16][18]*SK_MY[4]); + Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][0]*SH_MAG[2] + P[17][1]*SH_MAG[1] + P[17][2]*SH_MAG[0] - P[17][3]*SK_MY[2] - P[17][17]*SK_MY[1] - P[17][16]*SK_MY[3] + P[17][18]*SK_MY[4]); + Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][0]*SH_MAG[2] + P[18][1]*SH_MAG[1] + P[18][2]*SH_MAG[0] - P[18][3]*SK_MY[2] - P[18][17]*SK_MY[1] - P[18][16]*SK_MY[3] + P[18][18]*SK_MY[4]); + Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][0]*SH_MAG[2] + P[19][1]*SH_MAG[1] + P[19][2]*SH_MAG[0] - P[19][3]*SK_MY[2] - P[19][17]*SK_MY[1] - P[19][16]*SK_MY[3] + P[19][18]*SK_MY[4]); + Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][17]*SK_MY[1] - P[20][16]*SK_MY[3] + P[20][18]*SK_MY[4]); + Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][0]*SH_MAG[2] + P[21][1]*SH_MAG[1] + P[21][2]*SH_MAG[0] - P[21][3]*SK_MY[2] - P[21][17]*SK_MY[1] - P[21][16]*SK_MY[3] + P[21][18]*SK_MY[4]); + Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]); + varInnovMag[1] = 1.0f/SK_MY[0]; + innovMag[1] = MagPred[1] - magData.y; + } + else if (obsIndex == 2) // we are now fusing the Z measurement + { + // Calculate observation jacobians + for (uint8_t i = 0; i < n_states; i++) H_MAG[i] = 0; + H_MAG[0] = SH_MAG[1]; + H_MAG[1] = 2*magN*q3 - 2*magE*q0 - 2*magD*q1; + H_MAG[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; + H_MAG[3] = SH_MAG[0]; + H_MAG[16] = 2*q0*q2 + 2*q1*q3; + H_MAG[17] = 2*q2*q3 - 2*q0*q1; + H_MAG[18] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; + H_MAG[21] = 1; + + // Calculate Kalman gain + float temp = (P[21][21] + R_MAG + P[0][21]*SH_MAG[1] + P[3][21]*SH_MAG[0] + P[18][21]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) - (2*magD*q1 + 2*magE*q0 - 2*magN*q3)*(P[21][1] + P[0][1]*SH_MAG[1] + P[3][1]*SH_MAG[0] + P[18][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][1]*(2*q0*q2 + 2*q1*q3) - P[17][1]*(2*q0*q1 - 2*q2*q3) - P[1][1]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[21][2] + P[0][2]*SH_MAG[1] + P[3][2]*SH_MAG[0] + P[18][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][2]*(2*q0*q2 + 2*q1*q3) - P[17][2]*(2*q0*q1 - 2*q2*q3) - P[1][2]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[21][0] + P[0][0]*SH_MAG[1] + P[3][0]*SH_MAG[0] + P[18][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][0]*(2*q0*q2 + 2*q1*q3) - P[17][0]*(2*q0*q1 - 2*q2*q3) - P[1][0]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[21][3] + P[0][3]*SH_MAG[1] + P[3][3]*SH_MAG[0] + P[18][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][3]*(2*q0*q2 + 2*q1*q3) - P[17][3]*(2*q0*q1 - 2*q2*q3) - P[1][3]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P[21][18] + P[0][18]*SH_MAG[1] + P[3][18]*SH_MAG[0] + P[18][18]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][18]*(2*q0*q2 + 2*q1*q3) - P[17][18]*(2*q0*q1 - 2*q2*q3) - P[1][18]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][21]*(2*q0*q2 + 2*q1*q3) - P[17][21]*(2*q0*q1 - 2*q2*q3) - P[1][21]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + (2*q0*q2 + 2*q1*q3)*(P[21][16] + P[0][16]*SH_MAG[1] + P[3][16]*SH_MAG[0] + P[18][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][16]*(2*q0*q2 + 2*q1*q3) - P[17][16]*(2*q0*q1 - 2*q2*q3) - P[1][16]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q1 - 2*q2*q3)*(P[21][17] + P[0][17]*SH_MAG[1] + P[3][17]*SH_MAG[0] + P[18][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][17]*(2*q0*q2 + 2*q1*q3) - P[17][17]*(2*q0*q1 - 2*q2*q3) - P[1][17]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[2][21]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); + if (temp >= R_MAG) { + SK_MZ[0] = 1.0f / temp; + } else { + // the calculation is badly conditioned, so we cannot perform fusion on this step + // we increase the state variances and try again next time + P[21][21] += 0.1f*R_MAG; + obsIndex = 3; + return; + } + SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; + SK_MZ[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3; + SK_MZ[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; + SK_MZ[4] = 2*q0*q1 - 2*q2*q3; + SK_MZ[5] = 2*q0*q2 + 2*q1*q3; + Kfusion[0] = SK_MZ[0]*(P[0][21] + P[0][0]*SH_MAG[1] + P[0][3]*SH_MAG[0] - P[0][1]*SK_MZ[2] + P[0][2]*SK_MZ[3] + P[0][18]*SK_MZ[1] + P[0][16]*SK_MZ[5] - P[0][17]*SK_MZ[4]); + Kfusion[1] = SK_MZ[0]*(P[1][21] + P[1][0]*SH_MAG[1] + P[1][3]*SH_MAG[0] - P[1][1]*SK_MZ[2] + P[1][2]*SK_MZ[3] + P[1][18]*SK_MZ[1] + P[1][16]*SK_MZ[5] - P[1][17]*SK_MZ[4]); + Kfusion[2] = SK_MZ[0]*(P[2][21] + P[2][0]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[2][1]*SK_MZ[2] + P[2][2]*SK_MZ[3] + P[2][18]*SK_MZ[1] + P[2][16]*SK_MZ[5] - P[2][17]*SK_MZ[4]); + Kfusion[3] = SK_MZ[0]*(P[3][21] + P[3][0]*SH_MAG[1] + P[3][3]*SH_MAG[0] - P[3][1]*SK_MZ[2] + P[3][2]*SK_MZ[3] + P[3][18]*SK_MZ[1] + P[3][16]*SK_MZ[5] - P[3][17]*SK_MZ[4]); + Kfusion[4] = SK_MZ[0]*(P[4][21] + P[4][0]*SH_MAG[1] + P[4][3]*SH_MAG[0] - P[4][1]*SK_MZ[2] + P[4][2]*SK_MZ[3] + P[4][18]*SK_MZ[1] + P[4][16]*SK_MZ[5] - P[4][17]*SK_MZ[4]); + Kfusion[5] = SK_MZ[0]*(P[5][21] + P[5][0]*SH_MAG[1] + P[5][3]*SH_MAG[0] - P[5][1]*SK_MZ[2] + P[5][2]*SK_MZ[3] + P[5][18]*SK_MZ[1] + P[5][16]*SK_MZ[5] - P[5][17]*SK_MZ[4]); + Kfusion[6] = SK_MZ[0]*(P[6][21] + P[6][0]*SH_MAG[1] + P[6][3]*SH_MAG[0] - P[6][1]*SK_MZ[2] + P[6][2]*SK_MZ[3] + P[6][18]*SK_MZ[1] + P[6][16]*SK_MZ[5] - P[6][17]*SK_MZ[4]); + Kfusion[7] = SK_MZ[0]*(P[7][21] + P[7][0]*SH_MAG[1] + P[7][3]*SH_MAG[0] - P[7][1]*SK_MZ[2] + P[7][2]*SK_MZ[3] + P[7][18]*SK_MZ[1] + P[7][16]*SK_MZ[5] - P[7][17]*SK_MZ[4]); + Kfusion[8] = SK_MZ[0]*(P[8][21] + P[8][0]*SH_MAG[1] + P[8][3]*SH_MAG[0] - P[8][1]*SK_MZ[2] + P[8][2]*SK_MZ[3] + P[8][18]*SK_MZ[1] + P[8][16]*SK_MZ[5] - P[8][17]*SK_MZ[4]); + Kfusion[9] = SK_MZ[0]*(P[9][21] + P[9][0]*SH_MAG[1] + P[9][3]*SH_MAG[0] - P[9][1]*SK_MZ[2] + P[9][2]*SK_MZ[3] + P[9][18]*SK_MZ[1] + P[9][16]*SK_MZ[5] - P[9][17]*SK_MZ[4]); + Kfusion[10] = SK_MZ[0]*(P[10][21] + P[10][0]*SH_MAG[1] + P[10][3]*SH_MAG[0] - P[10][1]*SK_MZ[2] + P[10][2]*SK_MZ[3] + P[10][18]*SK_MZ[1] + P[10][16]*SK_MZ[5] - P[10][17]*SK_MZ[4]); + Kfusion[11] = SK_MZ[0]*(P[11][21] + P[11][0]*SH_MAG[1] + P[11][3]*SH_MAG[0] - P[11][1]*SK_MZ[2] + P[11][2]*SK_MZ[3] + P[11][18]*SK_MZ[1] + P[11][16]*SK_MZ[5] - P[11][17]*SK_MZ[4]); + Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][0]*SH_MAG[1] + P[12][3]*SH_MAG[0] - P[12][1]*SK_MZ[2] + P[12][2]*SK_MZ[3] + P[12][18]*SK_MZ[1] + P[12][16]*SK_MZ[5] - P[12][17]*SK_MZ[4]); + // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate + Kfusion[13] = 0.0f;//SK_MZ[0]*(P[13][21] + P[13][0]*SH_MAG[1] + P[13][3]*SH_MAG[0] - P[13][1]*SK_MZ[2] + P[13][2]*SK_MZ[3] + P[13][18]*SK_MZ[1] + P[13][16]*SK_MZ[5] - P[13][17]*SK_MZ[4]); + Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][0]*SH_MAG[1] + P[14][3]*SH_MAG[0] - P[14][1]*SK_MZ[2] + P[14][2]*SK_MZ[3] + P[14][18]*SK_MZ[1] + P[14][16]*SK_MZ[5] - P[14][17]*SK_MZ[4]); + Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][0]*SH_MAG[1] + P[15][3]*SH_MAG[0] - P[15][1]*SK_MZ[2] + P[15][2]*SK_MZ[3] + P[15][18]*SK_MZ[1] + P[15][16]*SK_MZ[5] - P[15][17]*SK_MZ[4]); + Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][0]*SH_MAG[1] + P[16][3]*SH_MAG[0] - P[16][1]*SK_MZ[2] + P[16][2]*SK_MZ[3] + P[16][18]*SK_MZ[1] + P[16][16]*SK_MZ[5] - P[16][17]*SK_MZ[4]); + Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][0]*SH_MAG[1] + P[17][3]*SH_MAG[0] - P[17][1]*SK_MZ[2] + P[17][2]*SK_MZ[3] + P[17][18]*SK_MZ[1] + P[17][16]*SK_MZ[5] - P[17][17]*SK_MZ[4]); + Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][0]*SH_MAG[1] + P[18][3]*SH_MAG[0] - P[18][1]*SK_MZ[2] + P[18][2]*SK_MZ[3] + P[18][18]*SK_MZ[1] + P[18][16]*SK_MZ[5] - P[18][17]*SK_MZ[4]); + Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][0]*SH_MAG[1] + P[19][3]*SH_MAG[0] - P[19][1]*SK_MZ[2] + P[19][2]*SK_MZ[3] + P[19][18]*SK_MZ[1] + P[19][16]*SK_MZ[5] - P[19][17]*SK_MZ[4]); + Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][0]*SH_MAG[1] + P[20][3]*SH_MAG[0] - P[20][1]*SK_MZ[2] + P[20][2]*SK_MZ[3] + P[20][18]*SK_MZ[1] + P[20][16]*SK_MZ[5] - P[20][17]*SK_MZ[4]); + Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][0]*SH_MAG[1] + P[21][3]*SH_MAG[0] - P[21][1]*SK_MZ[2] + P[21][2]*SK_MZ[3] + P[21][18]*SK_MZ[1] + P[21][16]*SK_MZ[5] - P[21][17]*SK_MZ[4]); + Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] + P[22][3]*SH_MAG[0] - P[22][1]*SK_MZ[2] + P[22][2]*SK_MZ[3] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[5] - P[22][17]*SK_MZ[4]); + varInnovMag[2] = 1.0f/SK_MZ[0]; + innovMag[2] = MagPred[2] - magData.z; + + } + + // Check the innovation for consistency and don't fuse if > 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 <= 15; j++) KH[i][j] = 0.0f; + if (!onGround) + { + for (uint8_t j = 16; j <= 21; j++) + { + KH[i][j] = Kfusion[i] * H_MAG[j]; + } + } + else + { + for (uint8_t j = 16; j <= 21; 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 = 16; k<=21; 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; + float R_TAS = sq(airspeedMeasurementSigma); + float SH_TAS[3]; + float SK_TAS; + float VtasPred; + + // Copy required states to local variable names + vn = statesAtVtasMeasTime[4]; + ve = statesAtVtasMeasTime[5]; + vd = statesAtVtasMeasTime[6]; + vwn = statesAtVtasMeasTime[14]; + vwe = statesAtVtasMeasTime[15]; + + // 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[n_states]; + for (uint8_t i = 0; i < n_states; 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[14] = -SH_TAS[2]; + H_TAS[15] = -SH_TAS[1]; + + // Calculate Kalman gains + float temp = (R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[14][4]*SH_TAS[2] - P[15][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[14][5]*SH_TAS[2] - P[15][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][14]*SH_TAS[2] + P[5][14]*SH_TAS[1] - P[14][14]*SH_TAS[2] - P[15][14]*SH_TAS[1] + P[6][14]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][15]*SH_TAS[2] + P[5][15]*SH_TAS[1] - P[14][15]*SH_TAS[2] - P[15][15]*SH_TAS[1] + P[6][15]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[14][6]*SH_TAS[2] - P[15][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0])); + if (temp >= R_TAS) { + SK_TAS = 1.0f / temp; + } else { + // the calculation is badly conditioned, so we cannot perform fusion on this step + // we increase the wind state variances and try again next time + P[14][14] += 0.05f*R_TAS; + P[15][15] += 0.05f*R_TAS; + return; + } + Kfusion[0] = SK_TAS*(P[0][4]*SH_TAS[2] - P[0][14]*SH_TAS[2] + P[0][5]*SH_TAS[1] - P[0][15]*SH_TAS[1] + P[0][6]*vd*SH_TAS[0]); + Kfusion[1] = SK_TAS*(P[1][4]*SH_TAS[2] - P[1][14]*SH_TAS[2] + P[1][5]*SH_TAS[1] - P[1][15]*SH_TAS[1] + P[1][6]*vd*SH_TAS[0]); + Kfusion[2] = SK_TAS*(P[2][4]*SH_TAS[2] - P[2][14]*SH_TAS[2] + P[2][5]*SH_TAS[1] - P[2][15]*SH_TAS[1] + P[2][6]*vd*SH_TAS[0]); + Kfusion[3] = SK_TAS*(P[3][4]*SH_TAS[2] - P[3][14]*SH_TAS[2] + P[3][5]*SH_TAS[1] - P[3][15]*SH_TAS[1] + P[3][6]*vd*SH_TAS[0]); + Kfusion[4] = SK_TAS*(P[4][4]*SH_TAS[2] - P[4][14]*SH_TAS[2] + P[4][5]*SH_TAS[1] - P[4][15]*SH_TAS[1] + P[4][6]*vd*SH_TAS[0]); + Kfusion[5] = SK_TAS*(P[5][4]*SH_TAS[2] - P[5][14]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[5][15]*SH_TAS[1] + P[5][6]*vd*SH_TAS[0]); + Kfusion[6] = SK_TAS*(P[6][4]*SH_TAS[2] - P[6][14]*SH_TAS[2] + P[6][5]*SH_TAS[1] - P[6][15]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]); + Kfusion[7] = SK_TAS*(P[7][4]*SH_TAS[2] - P[7][14]*SH_TAS[2] + P[7][5]*SH_TAS[1] - P[7][15]*SH_TAS[1] + P[7][6]*vd*SH_TAS[0]); + Kfusion[8] = SK_TAS*(P[8][4]*SH_TAS[2] - P[8][14]*SH_TAS[2] + P[8][5]*SH_TAS[1] - P[8][15]*SH_TAS[1] + P[8][6]*vd*SH_TAS[0]); + Kfusion[9] = SK_TAS*(P[9][4]*SH_TAS[2] - P[9][14]*SH_TAS[2] + P[9][5]*SH_TAS[1] - P[9][15]*SH_TAS[1] + P[9][6]*vd*SH_TAS[0]); + Kfusion[10] = SK_TAS*(P[10][4]*SH_TAS[2] - P[10][14]*SH_TAS[2] + P[10][5]*SH_TAS[1] - P[10][15]*SH_TAS[1] + P[10][6]*vd*SH_TAS[0]); + Kfusion[11] = SK_TAS*(P[11][4]*SH_TAS[2] - P[11][14]*SH_TAS[2] + P[11][5]*SH_TAS[1] - P[11][15]*SH_TAS[1] + P[11][6]*vd*SH_TAS[0]); + Kfusion[12] = SK_TAS*(P[12][4]*SH_TAS[2] - P[12][14]*SH_TAS[2] + P[12][5]*SH_TAS[1] - P[12][15]*SH_TAS[1] + P[12][6]*vd*SH_TAS[0]); + // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate + Kfusion[13] = 0.0f;//SK_TAS*(P[13][4]*SH_TAS[2] - P[13][14]*SH_TAS[2] + P[13][5]*SH_TAS[1] - P[13][15]*SH_TAS[1] + P[13][6]*vd*SH_TAS[0]); + Kfusion[14] = SK_TAS*(P[14][4]*SH_TAS[2] - P[14][14]*SH_TAS[2] + P[14][5]*SH_TAS[1] - P[14][15]*SH_TAS[1] + P[14][6]*vd*SH_TAS[0]); + Kfusion[15] = SK_TAS*(P[15][4]*SH_TAS[2] - P[15][14]*SH_TAS[2] + P[15][5]*SH_TAS[1] - P[15][15]*SH_TAS[1] + P[15][6]*vd*SH_TAS[0]); + Kfusion[16] = SK_TAS*(P[16][4]*SH_TAS[2] - P[16][14]*SH_TAS[2] + P[16][5]*SH_TAS[1] - P[16][15]*SH_TAS[1] + P[16][6]*vd*SH_TAS[0]); + Kfusion[17] = SK_TAS*(P[17][4]*SH_TAS[2] - P[17][14]*SH_TAS[2] + P[17][5]*SH_TAS[1] - P[17][15]*SH_TAS[1] + P[17][6]*vd*SH_TAS[0]); + Kfusion[18] = SK_TAS*(P[18][4]*SH_TAS[2] - P[18][14]*SH_TAS[2] + P[18][5]*SH_TAS[1] - P[18][15]*SH_TAS[1] + P[18][6]*vd*SH_TAS[0]); + Kfusion[19] = SK_TAS*(P[19][4]*SH_TAS[2] - P[19][14]*SH_TAS[2] + P[19][5]*SH_TAS[1] - P[19][15]*SH_TAS[1] + P[19][6]*vd*SH_TAS[0]); + 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]); + Kfusion[22] = SK_TAS*(P[22][4]*SH_TAS[2] - P[22][14]*SH_TAS[2] + P[22][5]*SH_TAS[1] - P[22][15]*SH_TAS[1] + P[22][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 <= 22; 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 <= 22; 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 <= 13; j++) KH[i][j] = 0.0; + for (uint8_t j = 14; j <= 15; j++) + { + KH[i][j] = Kfusion[i] * H_TAS[j]; + } + for (uint8_t j = 16; j <= 22; j++) KH[i][j] = 0.0; + } + for (uint8_t i = 0; i <= 22; i++) + { + for (uint8_t j = 0; j <= 22; 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 = 14; k <= 15; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + } + } + for (uint8_t i = 0; i <= 22; i++) + { + for (uint8_t j = 0; j <= 22; 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<n_states; col++) + { + covMat[row][col] = 0.0; + } + } +} + +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] = sin(rngFinderPitch); + cosRngTilt = - Tbn.z.x * SH_RNG[4] + Tbn.z.z * cos(rngFinderPitch); + if (useRangeFinder && 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 measurement innovation + rngPred = (ptd - pd)/cosRngTilt; + innovRng = rngPred - rngMea; + //printf("mea=%5.1f, pred=%5.1f, pd=%5.1f, ptd=%5.2f\n", rngMea, rngPred, pd, ptd); + + // Check the innovation for consistency and don't fuse if > 5Sigma + if ((innovRng*innovRng*SK_RNG[0]) < 25) + { + // 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::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) +{ + uint8_t row; + uint8_t col; + for (col=first; col<=last; col++) + { + for (row=0; row < n_states; row++) + { + covMat[row][col] = 0.0; + } + } +} + +float AttPosEKF::sq(float valIn) +{ + return valIn*valIn; +} + +// Store states in a history array along with time stamp +void AttPosEKF::StoreStates(uint64_t timestamp_ms) +{ + for (unsigned i=0; i<n_states; i++) + storedStates[i][storeIndex] = states[i]; + statetimeStamp[storeIndex] = timestamp_ms; + storeIndex++; + if (storeIndex == data_buffer_size) + storeIndex = 0; +} + +void AttPosEKF::ResetStoredStates() +{ + // reset all stored states + memset(&storedStates[0][0], 0, sizeof(storedStates)); + memset(&statetimeStamp[0], 0, sizeof(statetimeStamp)); + + // reset store index to first + storeIndex = 0; + + // overwrite all existing states + for (unsigned i = 0; i < n_states; i++) { + storedStates[i][storeIndex] = states[i]; + } + + statetimeStamp[storeIndex] = millis(); + + // increment to next storage index + storeIndex++; +} + +// Output the state vector stored at the time that best matches that specified by msec +int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec) +{ + int ret = 0; + + int64_t bestTimeDelta = 200; + unsigned bestStoreIndex = 0; + for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) + { + // Work around a GCC compiler bug - we know 64bit support on ARM is + // sketchy in GCC. + uint64_t timeDelta; + + if (msec > statetimeStamp[storeIndex]) { + timeDelta = msec - statetimeStamp[storeIndex]; + } else { + timeDelta = statetimeStamp[storeIndex] - msec; + } + + if (timeDelta < bestTimeDelta) + { + bestStoreIndex = storeIndex; + bestTimeDelta = timeDelta; + } + } + if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error + { + for (unsigned i=0; i < n_states; i++) { + if (isfinite(storedStates[i][bestStoreIndex])) { + statesForFusion[i] = storedStates[i][bestStoreIndex]; + } else if (isfinite(states[i])) { + statesForFusion[i] = states[i]; + } else { + // There is not much we can do here, except reporting the error we just + // found. + ret++; + } + } + } + else // otherwise output current state + { + for (unsigned i = 0; i < n_states; i++) { + if (isfinite(states[i])) { + statesForFusion[i] = states[i]; + } else { + ret++; + } + } + } + + return ret; +} + +void AttPosEKF::quat2Tnb(Mat3f &Tnb, const float (&quat)[4]) +{ + // Calculate the nav to body cosine matrix + float q00 = sq(quat[0]); + float q11 = sq(quat[1]); + float q22 = sq(quat[2]); + float q33 = sq(quat[3]); + float q01 = quat[0]*quat[1]; + float q02 = quat[0]*quat[2]; + float q03 = quat[0]*quat[3]; + float q12 = quat[1]*quat[2]; + float q13 = quat[1]*quat[3]; + float q23 = quat[2]*quat[3]; + + Tnb.x.x = q00 + q11 - q22 - q33; + Tnb.y.y = q00 - q11 + q22 - q33; + Tnb.z.z = q00 - q11 - q22 + q33; + Tnb.y.x = 2*(q12 - q03); + Tnb.z.x = 2*(q13 + q02); + Tnb.x.y = 2*(q12 + q03); + Tnb.z.y = 2*(q23 - q01); + Tnb.x.z = 2*(q13 - q02); + Tnb.y.z = 2*(q23 + q01); +} + +void AttPosEKF::quat2Tbn(Mat3f &Tbn, const float (&quat)[4]) +{ + // Calculate the body to nav cosine matrix + float q00 = sq(quat[0]); + float q11 = sq(quat[1]); + float q22 = sq(quat[2]); + float q33 = sq(quat[3]); + float q01 = quat[0]*quat[1]; + float q02 = quat[0]*quat[2]; + float q03 = quat[0]*quat[3]; + float q12 = quat[1]*quat[2]; + float q13 = quat[1]*quat[3]; + float q23 = quat[2]*quat[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); +} + +void AttPosEKF::eul2quat(float (&quat)[4], const float (&eul)[3]) +{ + float u1 = cos(0.5f*eul[0]); + float u2 = cos(0.5f*eul[1]); + float u3 = cos(0.5f*eul[2]); + float u4 = sin(0.5f*eul[0]); + float u5 = sin(0.5f*eul[1]); + float u6 = sin(0.5f*eul[2]); + quat[0] = u1*u2*u3+u4*u5*u6; + quat[1] = u4*u2*u3-u1*u5*u6; + quat[2] = u1*u5*u3+u4*u2*u6; + quat[3] = u1*u2*u6-u4*u5*u3; +} + +void AttPosEKF::quat2eul(float (&y)[3], const float (&u)[4]) +{ + y[0] = atan2f((2.0f*(u[2]*u[3]+u[0]*u[1])) , (u[0]*u[0]-u[1]*u[1]-u[2]*u[2]+u[3]*u[3])); + y[1] = -asinf(2.0f*(u[1]*u[3]-u[0]*u[2])); + y[2] = atan2f((2.0f*(u[1]*u[2]+u[0]*u[3])) , (u[0]*u[0]+u[1]*u[1]-u[2]*u[2]-u[3]*u[3])); +} + +void AttPosEKF::calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD) +{ + velNED[0] = gpsGndSpd*cosf(gpsCourse); + velNED[1] = gpsGndSpd*sinf(gpsCourse); + velNED[2] = gpsVelD; +} + +void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef) +{ + posNED[0] = earthRadius * (lat - latRef); + posNED[1] = earthRadius * cos(latRef) * (lon - lonRef); + posNED[2] = -(hgt - hgtRef); +} + +void AttPosEKF::calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef) +{ + lat = latRef + posNED[0] * earthRadiusInv; + lon = lonRef + posNED[1] * earthRadiusInv / cos(latRef); + hgt = hgtRef - posNED[2]; +} + +void AttPosEKF::OnGroundCheck() +{ + onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 6.0f)); + if (staticMode) { + staticMode = (!refSet || (GPSstatus < GPS_FIX_3D)); + } +} + +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.7f); + P[5][5] = P[4][4]; + P[6][6] = sq(0.7f); + P[7][7] = sq(15.0f); + P[8][8] = P[7][7]; + P[9][9] = sq(5.0f); + P[10][10] = sq(0.1f*deg2rad*dtIMU); + P[11][11] = P[10][10]; + P[12][12] = P[10][10]; + P[13][13] = sq(0.2f*dtIMU); + P[14][14] = sq(8.0f); + P[15][14] = P[14][14]; + P[16][16] = sq(0.02f); + P[17][17] = P[16][16]; + P[18][18] = P[16][16]; + P[19][19] = sq(0.02f); + P[20][20] = P[19][19]; + P[21][21] = P[19][19]; + P[22][22] = sq(0.5f); +} + +float AttPosEKF::ConstrainFloat(float val, float min, float max) +{ + float ret; + if (val > max) { + ret = max; + ekf_debug("> max: %8.4f, val: %8.4f", max, val); + } else if (val < min) { + ret = min; + ekf_debug("< min: %8.4f, val: %8.4f", min, val); + } else { + ret = val; + } + + if (!isfinite(val)) { + ekf_debug("constrain: non-finite!"); + } + + return ret; +} + +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: Delta Velocity bias - m/s (Z) + // 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++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); + } + + // Constrain velocity variances + for (unsigned i = 4; i <= 6; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f); + } + + // Constrain position variances + for (unsigned i = 7; i <= 9; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e6f); + } + + // Constrain delta angle bias variances + for (unsigned i = 10; i <= 12; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, sq(0.12f * dtIMU)); + } + + // Constrain delta velocity bias variance + P[13][13] = ConstrainFloat(P[13][13], 0.0f, sq(1.0f * dtIMU)); + + // Wind velocity variances + for (unsigned i = 14; i <= 15; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f); + } + + // Earth magnetic field variances + for (unsigned i = 16; i <= 18; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); + } + + // Body magnetic field variances + for (unsigned i = 19; i <= 21; i++) { + 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() +{ + 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: Delta Velocity bias - m/s (Z) + // 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++) { + states[i] = ConstrainFloat(states[i], -1.0f, 1.0f); + } + + // Constrain velocities to what GPS can do for us + for (unsigned i = 4; i <= 6; i++) { + states[i] = ConstrainFloat(states[i], -5.0e2f, 5.0e2f); + } + + // Constrain position to a reasonable vehicle range (in meters) + for (unsigned i = 7; i <= 8; 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 <= 12; i++) { + states[i] = ConstrainFloat(states[i], -0.12f * dtIMU, 0.12f * dtIMU); + } + + // Constrain delta velocity bias + states[13] = ConstrainFloat(states[13], -1.0f * dtIMU, 1.0f * dtIMU); + + // Wind velocity limits - assume 120 m/s max velocity + for (unsigned i = 14; i <= 15; i++) { + states[i] = ConstrainFloat(states[i], -120.0f, 120.0f); + } + + // Earth magnetic field limits (in Gauss) + for (unsigned i = 16; 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 = 19; i <= 21; i++) { + states[i] = ConstrainFloat(states[i], -0.5f, 0.5f); + } + + // Constrain terrain offset + states[22] = ConstrainFloat(states[22], -1000.0f, 1000.0f); + +} + +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 (unsigned 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 integrators + if (!isfinite(summedDelAng.x) || !isfinite(summedDelAng.y) || !isfinite(summedDelAng.z)) { + err_report->statesNaN = true; + ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", (double)summedDelAng.x, (double)summedDelAng.y, (double)summedDelAng.z); + err = true; + goto out; + } // delta angles + + if (!isfinite(correctedDelAng.x) || !isfinite(correctedDelAng.y) || !isfinite(correctedDelAng.z)) { + err_report->statesNaN = true; + ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", (double)correctedDelAng.x, (double)correctedDelAng.y, (double)correctedDelAng.z); + err = true; + goto out; + } // delta angles + + if (!isfinite(summedDelVel.x) || !isfinite(summedDelVel.y) || !isfinite(summedDelVel.z)) { + err_report->statesNaN = true; + ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", (double)summedDelVel.x, (double)summedDelVel.y, (double)summedDelVel.z); + err = true; + goto out; + } // delta velocities + + // 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; + ekf_debug("KH NaN"); + goto out; + } // intermediate result used for covariance updates + + if (!isfinite(KHP[i][j])) { + + err_report->covarianceNaN = true; + err = true; + ekf_debug("KHP NaN"); + goto out; + } // intermediate result used for covariance updates + + if (!isfinite(P[i][j])) { + + err_report->covarianceNaN = true; + err = true; + ekf_debug("P NaN"); + } // covariance matrix + } + + if (!isfinite(Kfusion[i])) { + + err_report->kalmanGainsNaN = true; + ekf_debug("Kfusion NaN"); + err = true; + goto out; + } // Kalman gains + + if (!isfinite(states[i])) { + + err_report->statesNaN = true; + ekf_debug("states NaN: i: %u val: %f", i, (double)states[i]); + err = true; + goto out; + } // state matrix + } + +out: + 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)) { + ekf_debug("re-initializing dynamic"); + + InitializeDynamic(velNED, magDeclination); + + return 1; + } + + // Reset the filter if the IMU data is too old + if (dtIMU > 0.3f) { + + 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 = atan2f(-ay, -az); + initialPitch = atan2f(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; + + /* normalize */ + float norm = sqrtf(initQuat[0]*initQuat[0] + initQuat[1]*initQuat[1] + initQuat[2]*initQuat[2] + initQuat[3]*initQuat[3]); + + initQuat[0] /= norm; + initQuat[1] /= norm; + initQuat[2] /= norm; + initQuat[3] /= norm; +} + +void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) +{ + + // Fill variables with valid data + velNED[0] = initvelNED[0]; + velNED[1] = initvelNED[1]; + velNED[2] = initvelNED[2]; + magDeclination = declination; + + // 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, declination, 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; + 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; + + magstate.q0 = initQuat[0]; + magstate.q1 = initQuat[1]; + magstate.q2 = initQuat[2]; + magstate.q3 = initQuat[3]; + magstate.magN = initMagNED.x; + magstate.magE = initMagNED.y; + magstate.magD = initMagNED.z; + magstate.magXbias = magBias.x; + magstate.magYbias = magBias.y; + magstate.magZbias = magBias.z; + magstate.R_MAG = sq(magMeasurementSigma); + magstate.DCM = DCM; + + // write to state vector + for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions + for (uint8_t j=4; j<=6; j++) states[j] = initvelNED[j-4]; // velocities + for (uint8_t j=7; j<=15; j++) states[j] = 0.0f; // positions, dAngBias, dVelBias, windVel + states[16] = initMagNED.x; // Magnetic Field North + states[17] = initMagNED.y; // Magnetic Field East + states[18] = initMagNED.z; // Magnetic Field Down + 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(); + + statesInitialised = true; + + // initialise the covariance matrix + CovarianceInit(); + + //Define Earth rotation vector in the NED navigation frame + calcEarthRateNED(earthRateNED, latRef); + +} + +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; + refSet = true; + + // we are at reference altitude, so measurement must be zero + hgtMea = 0.0f; + + // the baro offset must be this difference now + baroHgtOffset = baroHgt - referenceHgt; + + memset(&last_ekf_error, 0, sizeof(last_ekf_error)); + + InitializeDynamic(initvelNED, declination); +} + +void AttPosEKF::ZeroVariables() +{ + + // Initialize on-init initialized variables + fusionModeGPS = 0; + covSkipCount = 0; + statesInitialised = false; + fuseVelData = false; + fusePosData = false; + fuseHgtData = false; + fuseMagData = false; + fuseVtasData = false; + onGround = true; + staticMode = true; + useAirspeed = true; + useCompass = true; + useRangeFinder = true; + numericalProtection = true; + refSet = false; + storeIndex = 0; + gpsHgt = 0.0f; + baroHgt = 0.0f; + GPSstatus = 0; + VtasMeas = 0.0f; + magDeclination = 0.0f; + + // 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 + } + + correctedDelAng.zero(); + summedDelAng.zero(); + summedDelVel.zero(); + + dAngIMU.zero(); + dVelIMU.zero(); + + 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(&magstate, 0, sizeof(magstate)); + magstate.q0 = 1.0f; + magstate.DCM.identity(); + + 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.h b/src/modules/ekf_att_pos_estimator/estimator.h new file mode 100644 index 000000000..e821089f2 --- /dev/null +++ b/src/modules/ekf_att_pos_estimator/estimator.h @@ -0,0 +1,352 @@ +#include <math.h> +#include <stdint.h> + +#pragma once + +#define GRAVITY_MSS 9.80665f +#define deg2rad 0.017453292f +#define rad2deg 57.295780f +#define pi 3.141592657f +#define earthRate 0.000072921f +#define earthRadius 6378145.0f +#define earthRadiusInv 1.5678540e-7f + +class Vector3f +{ +private: +public: + float x; + float y; + float z; + + float length(void) const; + void zero(void); +}; + +class Mat3f +{ +private: +public: + Vector3f x; + Vector3f y; + Vector3f z; + + Mat3f(); + + void identity(); + Mat3f transpose(void) const; +}; + +Vector3f operator*(float sclIn1, Vector3f vecIn1); +Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2); +Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2); +Vector3f operator*( Mat3f matIn, Vector3f vecIn); +Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2); +Vector3f operator*(Vector3f vecIn1, float sclIn1); + +void swap_var(float &d1, float &d2); + +const unsigned int n_states = 23; +const unsigned int data_buffer_size = 50; + +enum GPS_FIX { + GPS_FIX_NOFIX = 0, + GPS_FIX_2D = 2, + GPS_FIX_3D = 3 +}; + +struct ekf_status_report { + bool velHealth; + bool posHealth; + bool hgtHealth; + bool velTimeout; + bool posTimeout; + bool hgtTimeout; + uint32_t velFailTime; + uint32_t posFailTime; + uint32_t hgtFailTime; + float states[n_states]; + bool statesNaN; + bool covarianceNaN; + bool kalmanGainsNaN; +}; + +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; + } + + struct { + unsigned obsIndex; + float MagPred[3]; + float SH_MAG[9]; + float q0; + float q1; + float q2; + float q3; + float magN; + float magE; + float magD; + float magXbias; + float magYbias; + float magZbias; + float R_MAG; + Mat3f DCM; + } magstate; + + + // 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 + float statesAtRngTime[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) + + Mat3f Tbn; // transformation matrix from body to NED coordinates + Mat3f Tnb; // transformation amtrix from NED to body coordinates + + 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 baroHgtOffset; ///< the baro (weather) offset from normalized altitude + float rngMea; // Ground distance + 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 innovRng; ///< Range finder innovation + float varInnovVtas; // innovation variance output + float VtasMeas; // true airspeed measurement (m/s) + float magDeclination; ///< magnetic declination + double latRef; // WGS-84 latitude of reference point (rad) + double lonRef; // WGS-84 longitude of reference point (rad) + float hgtRef; // WGS-84 height of reference point (m) + 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 + + // GPS input data variables + float gpsCourse; + float gpsVelD; + double gpsLat; + double 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 fuseRngData; ///< true when range data is 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 + bool useRangeFinder; ///< true when rangefinder 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 FuseRangeFinder(); + +void FuseOpticalFlow(); + +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, 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], double lat, double lon, float hgt, double latRef, double 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/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp new file mode 100644 index 000000000..907f4c2e1 --- /dev/null +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -0,0 +1,1523 @@ +/**************************************************************************** + * + * 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 fw_att_pos_estimator_main.cpp + * Implementation of the attitude and position estimator. + * + * @author Paul Riseborough <p_riseborough@live.com.au> + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +#include <nuttx/config.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <math.h> +#include <poll.h> +#include <time.h> +#include <drivers/drv_hrt.h> + +#define SENSOR_COMBINED_SUB + + +#include <drivers/drv_gyro.h> +#include <drivers/drv_accel.h> +#include <drivers/drv_mag.h> +#include <drivers/drv_baro.h> +#ifdef SENSOR_COMBINED_SUB +#include <uORB/topics/sensor_combined.h> +#endif +#include <arch/board/board.h> +#include <uORB/uORB.h> +#include <uORB/topics/airspeed.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_local_position.h> +#include <uORB/topics/vehicle_gps_position.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/parameter_update.h> +#include <uORB/topics/estimator_status.h> +#include <uORB/topics/actuator_armed.h> +#include <uORB/topics/home_position.h> +#include <systemlib/param/param.h> +#include <systemlib/err.h> +#include <geo/geo.h> +#include <systemlib/perf_counter.h> +#include <systemlib/systemlib.h> +#include <mathlib/mathlib.h> +#include <mavlink/mavlink_log.h> + +#include "estimator.h" + + + +/** + * estimator app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int ekf_att_pos_estimator_main(int argc, char *argv[]); + +__EXPORT uint32_t millis(); + +static uint64_t last_run = 0; +static uint64_t IMUmsec = 0; +static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; + +uint32_t millis() +{ + return IMUmsec; +} + +class FixedwingEstimator +{ +public: + /** + * Constructor + */ + FixedwingEstimator(); + + /** + * Destructor, also kills the sensors task. + */ + ~FixedwingEstimator(); + + /** + * Start the sensors task. + * + * @return OK on success. + */ + int start(); + + /** + * Print the current status. + */ + void print_status(); + + /** + * Trip the filter by feeding it NaN values. + */ + int trip_nan(); + +private: + + bool _task_should_exit; /**< if true, sensor task should exit */ + int _estimator_task; /**< task handle for sensor task */ +#ifndef SENSOR_COMBINED_SUB + int _gyro_sub; /**< gyro sensor subscription */ + int _accel_sub; /**< accel sensor subscription */ + int _mag_sub; /**< mag sensor subscription */ +#else + int _sensor_combined_sub; +#endif + int _airspeed_sub; /**< airspeed subscription */ + int _baro_sub; /**< barometer subscription */ + int _gps_sub; /**< GPS subscription */ + int _vstatus_sub; /**< vehicle status subscription */ + int _params_sub; /**< notification of parameter updates */ + int _manual_control_sub; /**< notification of manual control updates */ + int _mission_sub; + int _home_sub; /**< home position as defined by commander / user */ + + orb_advert_t _att_pub; /**< vehicle attitude */ + orb_advert_t _global_pos_pub; /**< global position */ + orb_advert_t _local_pos_pub; /**< position in local frame */ + orb_advert_t _estimator_status_pub; /**< status of the estimator */ + + struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct gyro_report _gyro; + struct accel_report _accel; + struct mag_report _mag; + struct airspeed_s _airspeed; /**< airspeed */ + struct baro_report _baro; /**< baro readings */ + struct vehicle_status_s _vstatus; /**< vehicle status */ + struct vehicle_global_position_s _global_pos; /**< global vehicle position */ + struct vehicle_local_position_s _local_pos; /**< local vehicle position */ + struct vehicle_gps_position_s _gps; /**< GPS position */ + + struct gyro_scale _gyro_offsets; + struct accel_scale _accel_offsets; + struct mag_scale _mag_offsets; + +#ifdef SENSOR_COMBINED_SUB + struct sensor_combined_s _sensor_combined; +#endif + + struct map_projection_reference_s _pos_ref; + + float _baro_ref; /**< barometer reference altitude */ + float _baro_gps_offset; /**< offset between GPS and baro */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _perf_gyro; ///<local performance counter for gyro updates + perf_counter_t _perf_accel; ///<local performance counter for accel updates + perf_counter_t _perf_mag; ///<local performance counter for mag updates + perf_counter_t _perf_gps; ///<local performance counter for gps updates + perf_counter_t _perf_baro; ///<local performance counter for baro updates + perf_counter_t _perf_airspeed; ///<local performance counter for airspeed updates + + bool _initialized; + bool _baro_init; + bool _gps_initialized; + uint64_t _gps_start_time; + uint64_t _filter_start_time; + bool _gyro_valid; + bool _accel_valid; + bool _mag_valid; + + int _mavlink_fd; + + struct { + int32_t vel_delay_ms; + int32_t pos_delay_ms; + int32_t height_delay_ms; + int32_t mag_delay_ms; + int32_t tas_delay_ms; + float velne_noise; + float veld_noise; + float posne_noise; + float posd_noise; + float mag_noise; + float gyro_pnoise; + float acc_pnoise; + float gbias_pnoise; + float abias_pnoise; + float mage_pnoise; + float magb_pnoise; + float eas_noise; + float pos_stddev_threshold; + } _parameters; /**< local copies of interesting parameters */ + + struct { + param_t vel_delay_ms; + param_t pos_delay_ms; + param_t height_delay_ms; + param_t mag_delay_ms; + param_t tas_delay_ms; + param_t velne_noise; + param_t veld_noise; + param_t posne_noise; + param_t posd_noise; + param_t mag_noise; + param_t gyro_pnoise; + param_t acc_pnoise; + param_t gbias_pnoise; + param_t abias_pnoise; + param_t mage_pnoise; + param_t magb_pnoise; + param_t eas_noise; + param_t pos_stddev_threshold; + } _parameter_handles; /**< handles for interesting parameters */ + + AttPosEKF *_ekf; + + /** + * Update our local parameter cache. + */ + int parameters_update(); + + /** + * Update control outputs + * + */ + void control_update(); + + /** + * Check for changes in vehicle status. + */ + void vehicle_status_poll(); + + /** + * Shim for calling task_main from task_create. + */ + static void task_main_trampoline(int argc, char *argv[]); + + /** + * Main sensor collection task. + */ + void task_main(); +}; + +namespace estimator +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +FixedwingEstimator *g_estimator; +} + +FixedwingEstimator::FixedwingEstimator() : + + _task_should_exit(false), + _estimator_task(-1), + +/* subscriptions */ +#ifndef SENSOR_COMBINED_SUB + _gyro_sub(-1), + _accel_sub(-1), + _mag_sub(-1), +#else + _sensor_combined_sub(-1), +#endif + _airspeed_sub(-1), + _baro_sub(-1), + _gps_sub(-1), + _vstatus_sub(-1), + _params_sub(-1), + _manual_control_sub(-1), + _mission_sub(-1), + _home_sub(-1), + +/* publications */ + _att_pub(-1), + _global_pos_pub(-1), + _local_pos_pub(-1), + _estimator_status_pub(-1), + + _att({}), + _gyro({}), + _accel({}), + _mag({}), + _airspeed({}), + _baro({}), + _vstatus({}), + _global_pos({}), + _local_pos({}), + _gps({}), + + _gyro_offsets({}), + _accel_offsets({}), + _mag_offsets({}), + + #ifdef SENSOR_COMBINED_SUB + _sensor_combined({}), + #endif + + _baro_ref(0.0f), + _baro_gps_offset(0.0f), + +/* performance counters */ + _loop_perf(perf_alloc(PC_COUNT, "fw_att_pos_estimator")), + _perf_gyro(perf_alloc(PC_COUNT, "fw_ekf_gyro_upd")), + _perf_accel(perf_alloc(PC_COUNT, "fw_ekf_accel_upd")), + _perf_mag(perf_alloc(PC_COUNT, "fw_ekf_mag_upd")), + _perf_gps(perf_alloc(PC_COUNT, "fw_ekf_gps_upd")), + _perf_baro(perf_alloc(PC_COUNT, "fw_ekf_baro_upd")), + _perf_airspeed(perf_alloc(PC_COUNT, "fw_ekf_aspd_upd")), + +/* states */ + _initialized(false), + _baro_init(false), + _gps_initialized(false), + _gyro_valid(false), + _accel_valid(false), + _mag_valid(false), + _mavlink_fd(-1), + _ekf(nullptr) +{ + + last_run = hrt_absolute_time(); + + _parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS"); + _parameter_handles.pos_delay_ms = param_find("PE_POS_DELAY_MS"); + _parameter_handles.height_delay_ms = param_find("PE_HGT_DELAY_MS"); + _parameter_handles.mag_delay_ms = param_find("PE_MAG_DELAY_MS"); + _parameter_handles.tas_delay_ms = param_find("PE_TAS_DELAY_MS"); + _parameter_handles.velne_noise = param_find("PE_VELNE_NOISE"); + _parameter_handles.veld_noise = param_find("PE_VELD_NOISE"); + _parameter_handles.posne_noise = param_find("PE_POSNE_NOISE"); + _parameter_handles.posd_noise = param_find("PE_POSD_NOISE"); + _parameter_handles.mag_noise = param_find("PE_MAG_NOISE"); + _parameter_handles.gyro_pnoise = param_find("PE_GYRO_PNOISE"); + _parameter_handles.acc_pnoise = param_find("PE_ACC_PNOISE"); + _parameter_handles.gbias_pnoise = param_find("PE_GBIAS_PNOISE"); + _parameter_handles.abias_pnoise = param_find("PE_ABIAS_PNOISE"); + _parameter_handles.mage_pnoise = param_find("PE_MAGE_PNOISE"); + _parameter_handles.magb_pnoise = param_find("PE_MAGB_PNOISE"); + _parameter_handles.eas_noise = param_find("PE_EAS_NOISE"); + _parameter_handles.pos_stddev_threshold = param_find("PE_POSDEV_INIT"); + + /* fetch initial parameter values */ + parameters_update(); + + /* get offsets */ + + int fd, res; + + fd = open(GYRO_DEVICE_PATH, O_RDONLY); + + if (fd > 0) { + res = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&_gyro_offsets); + close(fd); + + if (res) { + warnx("G SCALE FAIL"); + } + } + + fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + + if (fd > 0) { + res = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&_accel_offsets); + close(fd); + + if (res) { + warnx("A SCALE FAIL"); + } + } + + fd = open(MAG_DEVICE_PATH, O_RDONLY); + + if (fd > 0) { + res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&_mag_offsets); + close(fd); + + if (res) { + warnx("M SCALE FAIL"); + } + } +} + +FixedwingEstimator::~FixedwingEstimator() +{ + if (_estimator_task != -1) { + + /* task wakes up every 100ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_estimator_task); + break; + } + } while (_estimator_task != -1); + } + + estimator::g_estimator = nullptr; +} + +int +FixedwingEstimator::parameters_update() +{ + + param_get(_parameter_handles.vel_delay_ms, &(_parameters.vel_delay_ms)); + param_get(_parameter_handles.pos_delay_ms, &(_parameters.pos_delay_ms)); + param_get(_parameter_handles.height_delay_ms, &(_parameters.height_delay_ms)); + param_get(_parameter_handles.mag_delay_ms, &(_parameters.mag_delay_ms)); + param_get(_parameter_handles.tas_delay_ms, &(_parameters.tas_delay_ms)); + param_get(_parameter_handles.velne_noise, &(_parameters.velne_noise)); + param_get(_parameter_handles.veld_noise, &(_parameters.veld_noise)); + param_get(_parameter_handles.posne_noise, &(_parameters.posne_noise)); + param_get(_parameter_handles.posd_noise, &(_parameters.posd_noise)); + param_get(_parameter_handles.mag_noise, &(_parameters.mag_noise)); + param_get(_parameter_handles.gyro_pnoise, &(_parameters.gyro_pnoise)); + param_get(_parameter_handles.acc_pnoise, &(_parameters.acc_pnoise)); + param_get(_parameter_handles.gbias_pnoise, &(_parameters.gbias_pnoise)); + param_get(_parameter_handles.abias_pnoise, &(_parameters.abias_pnoise)); + param_get(_parameter_handles.mage_pnoise, &(_parameters.mage_pnoise)); + param_get(_parameter_handles.magb_pnoise, &(_parameters.magb_pnoise)); + param_get(_parameter_handles.eas_noise, &(_parameters.eas_noise)); + param_get(_parameter_handles.pos_stddev_threshold, &(_parameters.pos_stddev_threshold)); + + if (_ekf) { + // _ekf->yawVarScale = 1.0f; + // _ekf->windVelSigma = 0.1f; + _ekf->dAngBiasSigma = _parameters.gbias_pnoise; + _ekf->dVelBiasSigma = _parameters.abias_pnoise; + _ekf->magEarthSigma = _parameters.mage_pnoise; + _ekf->magBodySigma = _parameters.magb_pnoise; + // _ekf->gndHgtSigma = 0.02f; + _ekf->vneSigma = _parameters.velne_noise; + _ekf->vdSigma = _parameters.veld_noise; + _ekf->posNeSigma = _parameters.posne_noise; + _ekf->posDSigma = _parameters.posd_noise; + _ekf->magMeasurementSigma = _parameters.mag_noise; + _ekf->gyroProcessNoise = _parameters.gyro_pnoise; + _ekf->accelProcessNoise = _parameters.acc_pnoise; + _ekf->airspeedMeasurementSigma = _parameters.eas_noise; + } + + return OK; +} + +void +FixedwingEstimator::vehicle_status_poll() +{ + bool vstatus_updated; + + /* Check HIL state if vehicle status has changed */ + orb_check(_vstatus_sub, &vstatus_updated); + + if (vstatus_updated) { + + orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus); + } +} + +void +FixedwingEstimator::task_main_trampoline(int argc, char *argv[]) +{ + estimator::g_estimator->task_main(); +} + +void +FixedwingEstimator::task_main() +{ + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + _ekf = new AttPosEKF(); + float dt = 0.0f; // time lapsed since last covariance prediction + _filter_start_time = hrt_absolute_time(); + + if (!_ekf) { + errx(1, "failed allocating EKF filter - out of RAM!"); + } + + /* + * do subscriptions + */ + _baro_sub = orb_subscribe(ORB_ID(sensor_baro)); + _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _home_sub = orb_subscribe(ORB_ID(home_position)); + + /* rate limit vehicle status updates to 5Hz */ + orb_set_interval(_vstatus_sub, 200); + +#ifndef SENSOR_COMBINED_SUB + + _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); + _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); + _mag_sub = orb_subscribe(ORB_ID(sensor_mag)); + + /* rate limit gyro updates to 50 Hz */ + /* XXX remove this!, BUT increase the data buffer size! */ + orb_set_interval(_gyro_sub, 4); +#else + _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); + /* XXX remove this!, BUT increase the data buffer size! */ + orb_set_interval(_sensor_combined_sub, 4); +#endif + + /* sets also parameters in the EKF object */ + parameters_update(); + + Vector3f lastAngRate = {0.0f, 0.0f, 0.0f}; + Vector3f lastAccel = {0.0f, 0.0f, 0.0f}; + + /* wakeup source(s) */ + struct pollfd fds[2]; + + /* Setup of loop */ + fds[0].fd = _params_sub; + fds[0].events = POLLIN; +#ifndef SENSOR_COMBINED_SUB + fds[1].fd = _gyro_sub; + fds[1].events = POLLIN; +#else + fds[1].fd = _sensor_combined_sub; + fds[1].events = POLLIN; +#endif + + bool newDataGps = false; + bool newHgtData = false; + bool newAdsData = false; + bool newDataMag = false; + + while (!_task_should_exit) { + + /* wait for up to 500ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + + /* timed out - periodic check for _task_should_exit, etc. */ + if (pret == 0) + continue; + + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + perf_begin(_loop_perf); + + /* only update parameters if they changed */ + if (fds[0].revents & POLLIN) { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), _params_sub, &update); + + /* update parameters from storage */ + parameters_update(); + } + + /* only run estimator if gyro updated */ + if (fds[1].revents & POLLIN) { + + /* check vehicle status for changes to publication state */ + bool prev_hil = (_vstatus.hil_state == HIL_STATE_ON); + vehicle_status_poll(); + + bool accel_updated; + bool mag_updated; + + hrt_abstime last_sensor_timestamp; + + perf_count(_perf_gyro); + + /* Reset baro reference if switching to HIL, reset sensor states */ + if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) { + /* system is in HIL now, wait for measurements to come in one last round */ + usleep(60000); + +#ifndef SENSOR_COMBINED_SUB + orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); + orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); + orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); +#else + /* now read all sensor publications to ensure all real sensor data is purged */ + orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); +#endif + + /* set sensors to de-initialized state */ + _gyro_valid = false; + _accel_valid = false; + _mag_valid = false; + + _baro_init = false; + _gps_initialized = false; + last_sensor_timestamp = hrt_absolute_time(); + last_run = last_sensor_timestamp; + + _ekf->ZeroVariables(); + _ekf->dtIMU = 0.01f; + _filter_start_time = last_sensor_timestamp; + + /* now skip this loop and get data on the next one, which will also re-init the filter */ + continue; + } + + /** + * PART ONE: COLLECT ALL DATA + **/ + + /* load local copies */ +#ifndef SENSOR_COMBINED_SUB + orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); + + + orb_check(_accel_sub, &accel_updated); + + if (accel_updated) { + perf_count(_perf_accel); + orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); + } + + last_sensor_timestamp = _gyro.timestamp; + IMUmsec = _gyro.timestamp / 1e3f; + + float deltaT = (_gyro.timestamp - last_run) / 1e6f; + last_run = _gyro.timestamp; + + /* guard against too large deltaT's */ + if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { + deltaT = 0.01f; + } + + + // Always store data, independent of init status + /* fill in last data set */ + _ekf->dtIMU = deltaT; + + if (isfinite(_gyro.x) && + isfinite(_gyro.y) && + isfinite(_gyro.z)) { + _ekf->angRate.x = _gyro.x; + _ekf->angRate.y = _gyro.y; + _ekf->angRate.z = _gyro.z; + + if (!_gyro_valid) { + lastAngRate = _ekf->angRate; + } + + _gyro_valid = true; + } + + if (accel_updated) { + _ekf->accel.x = _accel.x; + _ekf->accel.y = _accel.y; + _ekf->accel.z = _accel.z; + + if (!_accel_valid) { + lastAccel = _ekf->accel; + } + + _accel_valid = true; + } + + _ekf->dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU; + _ekf->lastAngRate = angRate; + _ekf->dVelIMU = 0.5f * (accel + lastAccel) * dtIMU; + _ekf->lastAccel = accel; + + +#else + orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); + + static hrt_abstime last_accel = 0; + static hrt_abstime last_mag = 0; + + if (last_accel != _sensor_combined.accelerometer_timestamp) { + accel_updated = true; + } else { + accel_updated = false; + } + + last_accel = _sensor_combined.accelerometer_timestamp; + + + // Copy gyro and accel + last_sensor_timestamp = _sensor_combined.timestamp; + IMUmsec = _sensor_combined.timestamp / 1e3f; + + float deltaT = (_sensor_combined.timestamp - last_run) / 1e6f; + + /* guard against too large deltaT's */ + if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { + deltaT = 0.01f; + } + + last_run = _sensor_combined.timestamp; + + // Always store data, independent of init status + /* fill in last data set */ + _ekf->dtIMU = deltaT; + + if (isfinite(_sensor_combined.gyro_rad_s[0]) && + isfinite(_sensor_combined.gyro_rad_s[1]) && + isfinite(_sensor_combined.gyro_rad_s[2])) { + _ekf->angRate.x = _sensor_combined.gyro_rad_s[0]; + _ekf->angRate.y = _sensor_combined.gyro_rad_s[1]; + _ekf->angRate.z = _sensor_combined.gyro_rad_s[2]; + + if (!_gyro_valid) { + lastAngRate = _ekf->angRate; + } + + _gyro_valid = true; + } + + if (accel_updated) { + _ekf->accel.x = _sensor_combined.accelerometer_m_s2[0]; + _ekf->accel.y = _sensor_combined.accelerometer_m_s2[1]; + _ekf->accel.z = _sensor_combined.accelerometer_m_s2[2]; + + if (!_accel_valid) { + lastAccel = _ekf->accel; + } + + _accel_valid = true; + } + + _ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU; + lastAngRate = _ekf->angRate; + _ekf->dVelIMU = 0.5f * (_ekf->accel + lastAccel) * _ekf->dtIMU; + lastAccel = _ekf->accel; + + if (last_mag != _sensor_combined.magnetometer_timestamp) { + mag_updated = true; + newDataMag = true; + + } else { + newDataMag = false; + } + + last_mag = _sensor_combined.magnetometer_timestamp; + +#endif + + bool airspeed_updated; + orb_check(_airspeed_sub, &airspeed_updated); + + if (airspeed_updated) { + orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); + perf_count(_perf_airspeed); + + _ekf->VtasMeas = _airspeed.true_airspeed_m_s; + newAdsData = true; + + } else { + newAdsData = false; + } + + bool gps_updated; + orb_check(_gps_sub, &gps_updated); + + if (gps_updated) { + + uint64_t last_gps = _gps.timestamp_position; + + orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); + perf_count(_perf_gps); + + if (_gps.fix_type < 3) { + newDataGps = false; + + } else { + + /* store time of valid GPS measurement */ + _gps_start_time = hrt_absolute_time(); + + /* check if we had a GPS outage for a long time */ + if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) { + _ekf->ResetPosition(); + _ekf->ResetVelocity(); + _ekf->ResetStoredStates(); + } + + /* fuse GPS updates */ + + //_gps.timestamp / 1e3; + _ekf->GPSstatus = _gps.fix_type; + _ekf->velNED[0] = _gps.vel_n_m_s; + _ekf->velNED[1] = _gps.vel_e_m_s; + _ekf->velNED[2] = _gps.vel_d_m_s; + + // warnx("GPS updated: status: %d, vel: %8.4f %8.4f %8.4f", (int)GPSstatus, velNED[0], velNED[1], velNED[2]); + + _ekf->gpsLat = math::radians(_gps.lat / (double)1e7); + _ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; + _ekf->gpsHgt = _gps.alt / 1e3f; + + // 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 { + // _ekf->vneSigma = _parameters.velne_noise; + // } + + // if (_gps.p_variance_m > 0.25f && _gps.p_variance_m < 100.0f * 100.0f) { + // _ekf->posNeSigma = sqrtf(_gps.p_variance_m); + // } else { + // _ekf->posNeSigma = _parameters.posne_noise; + // } + + // warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m); + + newDataGps = true; + + } + + } + + bool baro_updated; + orb_check(_baro_sub, &baro_updated); + + if (baro_updated) { + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); + + _ekf->baroHgt = _baro.altitude; + + if (!_baro_init) { + _baro_ref = _baro.altitude; + _baro_init = true; + warnx("ALT REF INIT"); + } + + newHgtData = true; + } else { + newHgtData = false; + } + +#ifndef SENSOR_COMBINED_SUB + orb_check(_mag_sub, &mag_updated); +#endif + + if (mag_updated) { + + _mag_valid = true; + + perf_count(_perf_mag); + +#ifndef SENSOR_COMBINED_SUB + orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); + + // XXX we compensate the offsets upfront - should be close to zero. + // 0.001f + _ekf->magData.x = _mag.x; + _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset + + _ekf->magData.y = _mag.y; + _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset + + _ekf->magData.z = _mag.z; + _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset + +#else + + // XXX we compensate the offsets upfront - should be close to zero. + // 0.001f + _ekf->magData.x = _sensor_combined.magnetometer_ga[0]; + _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset + + _ekf->magData.y = _sensor_combined.magnetometer_ga[1]; + _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset + + _ekf->magData.z = _sensor_combined.magnetometer_ga[2]; + _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset + +#endif + + newDataMag = true; + + } else { + newDataMag = false; + } + + + /** + * CHECK IF THE INPUT DATA IS SANE + */ + int check = _ekf->CheckAndBound(); + + const char* ekfname = "[ekf] "; + + switch (check) { + case 0: + /* all ok */ + break; + case 1: + { + const char* str = "NaN in states, resetting"; + warnx("%s", str); + mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); + break; + } + case 2: + { + const char* str = "stale IMU data, resetting"; + warnx("%s", str); + mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); + break; + } + case 3: + { + const char* str = "switching to dynamic state"; + warnx("%s", str); + mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); + break; + } + + default: + { + const char* str = "unknown reset condition"; + warnx("%s", str); + mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); + } + } + + // warn on fatal resets + if (check == 1) { + warnx("NUMERIC ERROR IN FILTER"); + } + + // If non-zero, we got a filter reset + if (check) { + + struct ekf_status_report ekf_report; + + _ekf->GetLastErrorState(&ekf_report); + + struct estimator_status_report rep; + memset(&rep, 0, sizeof(rep)); + rep.timestamp = hrt_absolute_time(); + + rep.states_nan = ekf_report.statesNaN; + rep.covariance_nan = ekf_report.covarianceNaN; + rep.kalman_gain_nan = ekf_report.kalmanGainsNaN; + + // Copy all states or at least all that we can fit + unsigned i = 0; + unsigned ekf_n_states = (sizeof(ekf_report.states) / sizeof(ekf_report.states[0])); + unsigned max_states = (sizeof(rep.states) / sizeof(rep.states[0])); + rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states; + + while ((i < ekf_n_states) && (i < max_states)) { + + rep.states[i] = ekf_report.states[i]; + i++; + } + + if (_estimator_status_pub > 0) { + orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &rep); + } else { + _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep); + } + + /* set sensors to de-initialized state */ + _gyro_valid = false; + _accel_valid = false; + _mag_valid = false; + + _baro_init = false; + _gps_initialized = false; + last_sensor_timestamp = hrt_absolute_time(); + last_run = last_sensor_timestamp; + + _ekf->ZeroVariables(); + _ekf->dtIMU = 0.01f; + + // Let the system re-initialize itself + continue; + + } + + + /** + * PART TWO: EXECUTE THE FILTER + **/ + + if ((hrt_elapsed_time(&_filter_start_time) > FILTER_INIT_DELAY) && _baro_init && _gyro_valid && _accel_valid && _mag_valid) { + + float initVelNED[3]; + + if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph_m < _parameters.pos_stddev_threshold && _gps.epv_m < _parameters.pos_stddev_threshold) { + + initVelNED[0] = _gps.vel_n_m_s; + initVelNED[1] = _gps.vel_e_m_s; + initVelNED[2] = _gps.vel_d_m_s; + + // GPS is in scaled integers, convert + double lat = _gps.lat / 1.0e7; + double lon = _gps.lon / 1.0e7; + float gps_alt = _gps.alt / 1e3f; + + // Set up height correctly + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); + _baro_gps_offset = _baro_ref - _baro.altitude; + _ekf->baroHgt = _baro.altitude; + _ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref)); + + // Set up position variables correctly + _ekf->GPSstatus = _gps.fix_type; + + _ekf->gpsLat = math::radians(lat); + _ekf->gpsLon = math::radians(lon) - M_PI; + _ekf->gpsHgt = gps_alt; + + // Look up mag declination based on current position + float declination = math::radians(get_mag_declination(lat, lon)); + + _ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); + + // Initialize projection + _local_pos.ref_lat = lat; + _local_pos.ref_lon = lon; + _local_pos.ref_alt = gps_alt; + _local_pos.ref_timestamp = _gps.timestamp_position; + + map_projection_init(&_pos_ref, lat, lon); + mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); + warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, + (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); + warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", _ekf->baroHgt, _baro_ref, _baro_gps_offset); + warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph_m, (double)_gps.epv_m, (double)math::degrees(declination)); + + _gps_initialized = true; + + } else if (!_ekf->statesInitialised) { + + initVelNED[0] = 0.0f; + initVelNED[1] = 0.0f; + initVelNED[2] = 0.0f; + _ekf->posNED[0] = 0.0f; + _ekf->posNED[1] = 0.0f; + _ekf->posNED[2] = 0.0f; + + _ekf->posNE[0] = _ekf->posNED[0]; + _ekf->posNE[1] = _ekf->posNED[1]; + + _local_pos.ref_alt = _baro_ref; + _baro_gps_offset = 0.0f; + + _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); + } + } + + // If valid IMU data and states initialised, predict states and covariances + if (_ekf->statesInitialised) { + // Run the strapdown INS equations every IMU update + _ekf->UpdateStrapdownEquationsNED(); +#if 0 + // debug code - could be tunred into a filter mnitoring/watchdog function + float tempQuat[4]; + + for (uint8_t j = 0; j <= 3; j++) tempQuat[j] = states[j]; + + quat2eul(eulerEst, tempQuat); + + for (uint8_t j = 0; j <= 2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j]; + + if (eulerDif[2] > pi) eulerDif[2] -= 2 * pi; + + if (eulerDif[2] < -pi) eulerDif[2] += 2 * pi; + +#endif + // store the predicted states for subsequent use by measurement fusion + _ekf->StoreStates(IMUmsec); + // Check if on ground - status is used by covariance prediction + _ekf->OnGroundCheck(); + // sum delta angles and time used by covariance prediction + _ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng; + _ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU; + dt += _ekf->dtIMU; + + // perform a covariance prediction if the total delta angle has exceeded the limit + // or the time limit will be exceeded at the next IMU update + if ((dt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) { + _ekf->CovariancePrediction(dt); + _ekf->summedDelAng.zero(); + _ekf->summedDelVel.zero(); + dt = 0.0f; + } + + _initialized = true; + } + + // Fuse GPS Measurements + if (newDataGps && _gps_initialized) { + // Convert GPS measurements to Pos NE, hgt and Vel NED + _ekf->velNED[0] = _gps.vel_n_m_s; + _ekf->velNED[1] = _gps.vel_e_m_s; + _ekf->velNED[2] = _gps.vel_d_m_s; + _ekf->calcposNED(_ekf->posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef); + + _ekf->posNE[0] = _ekf->posNED[0]; + _ekf->posNE[1] = _ekf->posNED[1]; + // set fusion flags + _ekf->fuseVelData = true; + _ekf->fusePosData = true; + // recall states stored at time of measurement after adjusting for delays + _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); + _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); + // run the fusion step + _ekf->FuseVelposNED(); + + } else if (_ekf->statesInitialised) { + // Convert GPS measurements to Pos NE, hgt and Vel NED + _ekf->velNED[0] = 0.0f; + _ekf->velNED[1] = 0.0f; + _ekf->velNED[2] = 0.0f; + _ekf->posNED[0] = 0.0f; + _ekf->posNED[1] = 0.0f; + _ekf->posNED[2] = 0.0f; + + _ekf->posNE[0] = _ekf->posNED[0]; + _ekf->posNE[1] = _ekf->posNED[1]; + // set fusion flags + _ekf->fuseVelData = true; + _ekf->fusePosData = true; + // recall states stored at time of measurement after adjusting for delays + _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); + _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); + // run the fusion step + _ekf->FuseVelposNED(); + + } else { + _ekf->fuseVelData = false; + _ekf->fusePosData = false; + } + + if (newHgtData && _ekf->statesInitialised) { + // Could use a blend of GPS and baro alt data if desired + _ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref); + _ekf->fuseHgtData = true; + // recall states stored at time of measurement after adjusting for delays + _ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); + // run the fusion step + _ekf->FuseVelposNED(); + + } else { + _ekf->fuseHgtData = false; + } + + // Fuse Magnetometer Measurements + if (newDataMag && _ekf->statesInitialised) { + _ekf->fuseMagData = true; + _ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data + + } else { + _ekf->fuseMagData = false; + } + + if (_ekf->statesInitialised) _ekf->FuseMagnetometer(); + + // Fuse Airspeed Measurements + if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) { + _ekf->fuseVtasData = true; + _ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data + _ekf->FuseAirspeed(); + + } else { + _ekf->fuseVtasData = false; + } + + // Publish results + if (_initialized && (check == OK)) { + + + + // 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 - milligauss (North, East, Down) + // 18-20: Body Magnetic Field Vector - milligauss (X,Y,Z) + + math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]); + math::Matrix<3, 3> R = q.to_dcm(); + math::Vector<3> euler = R.to_euler(); + + for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) + _att.R[i][j] = R(i, j); + + _att.timestamp = last_sensor_timestamp; + _att.q[0] = _ekf->states[0]; + _att.q[1] = _ekf->states[1]; + _att.q[2] = _ekf->states[2]; + _att.q[3] = _ekf->states[3]; + _att.q_valid = true; + _att.R_valid = true; + + _att.timestamp = last_sensor_timestamp; + _att.roll = euler(0); + _att.pitch = euler(1); + _att.yaw = euler(2); + + _att.rollspeed = _ekf->angRate.x - _ekf->states[10]; + _att.pitchspeed = _ekf->angRate.y - _ekf->states[11]; + _att.yawspeed = _ekf->angRate.z - _ekf->states[12]; + // gyro offsets + _att.rate_offsets[0] = _ekf->states[10]; + _att.rate_offsets[1] = _ekf->states[11]; + _att.rate_offsets[2] = _ekf->states[12]; + + /* lazily publish the attitude only once available */ + if (_att_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att); + + } else { + /* advertise and publish */ + _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); + } + } + + if (_gps_initialized) { + _local_pos.timestamp = last_sensor_timestamp; + _local_pos.x = _ekf->states[7]; + _local_pos.y = _ekf->states[8]; + // XXX need to announce change of Z reference somehow elegantly + _local_pos.z = _ekf->states[9] - _baro_gps_offset; + + _local_pos.vx = _ekf->states[4]; + _local_pos.vy = _ekf->states[5]; + _local_pos.vz = _ekf->states[6]; + + _local_pos.xy_valid = _gps_initialized; + _local_pos.z_valid = true; + _local_pos.v_xy_valid = _gps_initialized; + _local_pos.v_z_valid = true; + _local_pos.xy_global = true; + + _local_pos.z_global = false; + _local_pos.yaw = _att.yaw; + + /* lazily publish the local position only once available */ + if (_local_pos_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &_local_pos); + + } else { + /* advertise and publish */ + _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos); + } + + _global_pos.timestamp = _local_pos.timestamp; + + if (_local_pos.xy_global) { + double est_lat, est_lon; + map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon); + _global_pos.lat = est_lat; + _global_pos.lon = est_lon; + _global_pos.time_gps_usec = _gps.time_gps_usec; + _global_pos.eph = _gps.eph_m; + _global_pos.epv = _gps.epv_m; + } + + if (_local_pos.v_xy_valid) { + _global_pos.vel_n = _local_pos.vx; + _global_pos.vel_e = _local_pos.vy; + } else { + _global_pos.vel_n = 0.0f; + _global_pos.vel_e = 0.0f; + } + + /* local pos alt is negative, change sign and add alt offsets */ + _global_pos.alt = _local_pos.ref_alt + _baro_gps_offset + (-_local_pos.z); + + if (_local_pos.v_z_valid) { + _global_pos.vel_d = _local_pos.vz; + } + + _global_pos.yaw = _local_pos.yaw; + + _global_pos.eph = _gps.eph_m; + _global_pos.epv = _gps.epv_m; + + _global_pos.timestamp = _local_pos.timestamp; + + /* lazily publish the global position only once available */ + if (_global_pos_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); + + } else { + /* advertise and publish */ + _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); + } + } + + } + + perf_end(_loop_perf); + } + + warnx("exiting.\n"); + + _estimator_task = -1; + _exit(0); +} + +int +FixedwingEstimator::start() +{ + ASSERT(_estimator_task == -1); + + /* start the task */ + _estimator_task = task_spawn_cmd("ekf_att_pos_estimator", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 40, + 6000, + (main_t)&FixedwingEstimator::task_main_trampoline, + nullptr); + + if (_estimator_task < 0) { + warn("task start failed"); + return -errno; + } + + return OK; +} + +void +FixedwingEstimator::print_status() +{ + math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]); + math::Matrix<3, 3> R = q.to_dcm(); + math::Vector<3> euler = R.to_euler(); + + printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n", + (double)math::degrees(euler(0)), (double)math::degrees(euler(1)), (double)math::degrees(euler(2))); + + // 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) + + printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec); + printf("ref alt: %8.6f\n", (double)_local_pos.ref_alt); + printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); + printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z); + printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]); + printf("states (vel m/s) [5-7]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]); + printf("states (pos m) [8-10]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]); + printf("states (delta ang) [11-13]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]); + printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[13], (double)_ekf->states[14]); + printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16], (double)_ekf->states[17]); + printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19], (double)_ekf->states[20]); + printf("states: %s %s %s %s %s %s %s %s %s %s\n", + (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT", + (_ekf->onGround) ? "ON_GROUND" : "AIRBORNE", + (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL", + (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS", + (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT", + (_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG", + (_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", + (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", + (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS", + (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); +} + +int FixedwingEstimator::trip_nan() { + + int ret = 0; + + // If system is not armed, inject a NaN value into the filter + int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + + struct actuator_armed_s armed; + orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); + + if (armed.armed) { + warnx("ACTUATORS ARMED! NOT TRIPPING SYSTEM"); + ret = 1; + } else { + + float nan_val = 0.0f / 0.0f; + + warnx("system not armed, tripping state vector with NaN values"); + _ekf->states[5] = nan_val; + usleep(100000); + + warnx("tripping covariance #1 with NaN values"); + _ekf->KH[2][2] = nan_val; // intermediate result used for covariance updates + usleep(100000); + + warnx("tripping covariance #2 with NaN values"); + _ekf->KHP[5][5] = nan_val; // intermediate result used for covariance updates + usleep(100000); + + warnx("tripping covariance #3 with NaN values"); + _ekf->P[3][3] = nan_val; // covariance matrix + usleep(100000); + + warnx("tripping Kalman gains with NaN values"); + _ekf->Kfusion[0] = nan_val; // Kalman gains + usleep(100000); + + warnx("tripping stored states[0] with NaN values"); + _ekf->storedStates[0][0] = nan_val; + usleep(100000); + + warnx("\nDONE - FILTER STATE:"); + print_status(); + } + + close(armed_sub); + return ret; +} + +int ekf_att_pos_estimator_main(int argc, char *argv[]) +{ + if (argc < 1) + errx(1, "usage: ekf_att_pos_estimator {start|stop|status}"); + + if (!strcmp(argv[1], "start")) { + + if (estimator::g_estimator != nullptr) + errx(1, "already running"); + + estimator::g_estimator = new FixedwingEstimator; + + if (estimator::g_estimator == nullptr) + errx(1, "alloc failed"); + + if (OK != estimator::g_estimator->start()) { + delete estimator::g_estimator; + estimator::g_estimator = nullptr; + err(1, "start failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + if (estimator::g_estimator == nullptr) + errx(1, "not running"); + + delete estimator::g_estimator; + estimator::g_estimator = nullptr; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (estimator::g_estimator) { + warnx("running"); + + estimator::g_estimator->print_status(); + + exit(0); + + } else { + errx(1, "not running"); + } + } + + if (!strcmp(argv[1], "trip")) { + if (estimator::g_estimator) { + int ret = estimator::g_estimator->trip_nan(); + + exit(ret); + + } else { + errx(1, "not running"); + } + } + + warnx("unrecognized command"); + return 1; +} diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c new file mode 100644 index 000000000..d2c6e1f15 --- /dev/null +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c @@ -0,0 +1,271 @@ +/**************************************************************************** + * + * 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 fw_att_pos_estimator_params.c + * + * Parameters defined by the attitude and position estimator task + * + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +#include <nuttx/config.h> + +#include <systemlib/param/param.h> + +/* + * Estimator parameters, accessible via MAVLink + * + */ + +/** + * Velocity estimate delay + * + * The delay in milliseconds of the velocity estimate from GPS. + * + * @min 0 + * @max 1000 + * @group Position Estimator + */ +PARAM_DEFINE_INT32(PE_VEL_DELAY_MS, 230); + +/** + * Position estimate delay + * + * The delay in milliseconds of the position estimate from GPS. + * + * @min 0 + * @max 1000 + * @group Position Estimator + */ +PARAM_DEFINE_INT32(PE_POS_DELAY_MS, 210); + +/** + * Height estimate delay + * + * The delay in milliseconds of the height estimate from the barometer. + * + * @min 0 + * @max 1000 + * @group Position Estimator + */ +PARAM_DEFINE_INT32(PE_HGT_DELAY_MS, 350); + +/** + * Mag estimate delay + * + * The delay in milliseconds of the magnetic field estimate from + * the magnetometer. + * + * @min 0 + * @max 1000 + * @group Position Estimator + */ +PARAM_DEFINE_INT32(PE_MAG_DELAY_MS, 30); + +/** + * True airspeeed estimate delay + * + * The delay in milliseconds of the airspeed estimate. + * + * @min 0 + * @max 1000 + * @group Position Estimator + */ +PARAM_DEFINE_INT32(PE_TAS_DELAY_MS, 210); + +/** + * GPS vs. barometric altitude update weight + * + * RE-CHECK this. + * + * @min 0.0 + * @max 1.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_GPS_ALT_WGT, 0.9f); + +/** + * Airspeed measurement noise. + * + * Increasing this value will make the filter trust this sensor + * less and trust other sensors more. + * + * @min 0.5 + * @max 5.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_EAS_NOISE, 1.4f); + +/** + * Velocity measurement noise in north-east (horizontal) direction. + * + * Generic default: 0.3, multicopters: 0.5, ground vehicles: 0.5 + * + * @min 0.05 + * @max 5.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_VELNE_NOISE, 0.3f); + +/** + * Velocity noise in down (vertical) direction + * + * Generic default: 0.5, multicopters: 0.7, ground vehicles: 0.7 + * + * @min 0.05 + * @max 5.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_VELD_NOISE, 0.5f); + +/** + * Position noise in north-east (horizontal) direction + * + * Generic defaults: 0.5, multicopters: 0.5, ground vehicles: 0.5 + * + * @min 0.1 + * @max 10.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_POSNE_NOISE, 0.5f); + +/** + * Position noise in down (vertical) direction + * + * Generic defaults: 0.5, multicopters: 1.0, ground vehicles: 1.0 + * + * @min 0.1 + * @max 10.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_POSD_NOISE, 0.5f); + +/** + * Magnetometer measurement noise + * + * Generic defaults: 0.05, multicopters: 0.05, ground vehicles: 0.05 + * + * @min 0.1 + * @max 10.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_MAG_NOISE, 0.05f); + +/** + * Gyro process noise + * + * Generic defaults: 0.015, multicopters: 0.015, ground vehicles: 0.015. + * This noise controls how much the filter trusts the gyro measurements. + * Increasing it makes the filter trust the gyro less and other sensors more. + * + * @min 0.001 + * @max 0.05 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_GYRO_PNOISE, 0.015f); + +/** + * Accelerometer process noise + * + * Generic defaults: 0.25, multicopters: 0.25, ground vehicles: 0.25. + * Increasing this value makes the filter trust the accelerometer less + * and other sensors more. + * + * @min 0.05 + * @max 1.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_ACC_PNOISE, 0.25f); + +/** + * Gyro bias estimate process noise + * + * Generic defaults: 1e-07f, multicopters: 1e-07f, ground vehicles: 1e-07f. + * Increasing this value will make the gyro bias converge faster but noisier. + * + * @min 0.0000001 + * @max 0.00001 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-07f); + +/** + * Accelerometer bias estimate process noise + * + * Generic defaults: 0.0001f, multicopters: 0.0001f, ground vehicles: 0.0001f. + * Increasing this value makes the bias estimation faster and noisier. + * + * @min 0.0001 + * @max 0.001 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_ABIAS_PNOISE, 0.0001f); + +/** + * Magnetometer earth frame offsets process noise + * + * Generic defaults: 0.0001, multicopters: 0.0001, ground vehicles: 0.0001. + * Increasing this value makes the magnetometer earth bias estimate converge + * faster but also noisier. + * + * @min 0.0001 + * @max 0.01 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_MAGE_PNOISE, 0.0003f); + +/** + * Magnetometer body frame offsets process noise + * + * Generic defaults: 0.0003, multicopters: 0.0003, ground vehicles: 0.0003. + * Increasing this value makes the magnetometer body bias estimate converge faster + * but also noisier. + * + * @min 0.0001 + * @max 0.01 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_MAGB_PNOISE, 0.0003f); + +/** + * Threshold for filter initialization. + * + * If the standard deviation of the GPS position estimate is below this threshold + * in meters, the filter will initialize. + * + * @min 0.3 + * @max 10.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_POSDEV_INIT, 5.0f); diff --git a/src/modules/fixedwing_att_control/module.mk b/src/modules/ekf_att_pos_estimator/module.mk index fd1a8724a..30955d0dd 100644 --- a/src/modules/fixedwing_att_control/module.mk +++ b/src/modules/ekf_att_pos_estimator/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# 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 @@ -32,11 +32,11 @@ ############################################################################ # -# Fixedwing Attitude Control application +# Main Attitude and Position Estimator for Fixed Wing Aircraft # -MODULE_COMMAND = fixedwing_att_control +MODULE_COMMAND = ekf_att_pos_estimator -SRCS = fixedwing_att_control_main.c \ - fixedwing_att_control_att.c \ - fixedwing_att_control_rate.c +SRCS = fw_att_pos_estimator_main.cpp \ + fw_att_pos_estimator_params.c \ + estimator.cpp diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c deleted file mode 100644 index 2aeca3a98..000000000 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c +++ /dev/null @@ -1,169 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> - * - * 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 fixedwing_att_control_rate.c - * Implementation of a fixed wing attitude controller. - */ - -#include <nuttx/config.h> -#include <stdio.h> -#include <stdlib.h> -#include <string.h> -#include <unistd.h> -#include <fcntl.h> -#include <errno.h> -#include <math.h> -#include <poll.h> -#include <time.h> -#include <drivers/drv_hrt.h> -#include <arch/board/board.h> -#include <uORB/uORB.h> - -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_attitude_setpoint.h> -#include <uORB/topics/manual_control_setpoint.h> -#include <systemlib/param/param.h> -#include <systemlib/pid/pid.h> -#include <systemlib/geo/geo.h> -#include <systemlib/systemlib.h> - -#include "fixedwing_att_control_att.h" - - -struct fw_att_control_params { - float roll_p; - float rollrate_lim; - float pitch_p; - float pitchrate_lim; - float yawrate_lim; - float pitch_roll_compensation_p; -}; - -struct fw_pos_control_param_handles { - param_t roll_p; - param_t rollrate_lim; - param_t pitch_p; - param_t pitchrate_lim; - param_t yawrate_lim; - param_t pitch_roll_compensation_p; -}; - - - -/* Internal Prototypes */ -static int parameters_init(struct fw_pos_control_param_handles *h); -static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_att_control_params *p); - -static int parameters_init(struct fw_pos_control_param_handles *h) -{ - /* PID parameters */ - h->roll_p = param_find("FW_ROLL_P"); - h->rollrate_lim = param_find("FW_ROLLR_LIM"); - h->pitch_p = param_find("FW_PITCH_P"); - h->pitchrate_lim = param_find("FW_PITCHR_LIM"); - h->yawrate_lim = param_find("FW_YAWR_LIM"); - h->pitch_roll_compensation_p = param_find("FW_PITCH_RCOMP"); - - return OK; -} - -static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_att_control_params *p) -{ - param_get(h->roll_p, &(p->roll_p)); - param_get(h->rollrate_lim, &(p->rollrate_lim)); - param_get(h->pitch_p, &(p->pitch_p)); - param_get(h->pitchrate_lim, &(p->pitchrate_lim)); - param_get(h->yawrate_lim, &(p->yawrate_lim)); - param_get(h->pitch_roll_compensation_p, &(p->pitch_roll_compensation_p)); - - return OK; -} - -int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, - const float speed_body[], - struct vehicle_rates_setpoint_s *rates_sp) -{ - static int counter = 0; - static bool initialized = false; - - static struct fw_att_control_params p; - static struct fw_pos_control_param_handles h; - - static PID_t roll_controller; - static PID_t pitch_controller; - - - if (!initialized) { - parameters_init(&h); - parameters_update(&h, &p); - pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, PID_MODE_DERIVATIV_NONE, 0.0f); //P Controller - pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, PID_MODE_DERIVATIV_NONE, 0.0f); //P Controller - initialized = true; - } - - /* load new parameters with lower rate */ - if (counter % 100 == 0) { - /* update parameters from storage */ - parameters_update(&h, &p); - pid_set_parameters(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim); - pid_set_parameters(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim); - } - - /* Roll (P) */ - rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0); - - - /* Pitch (P) */ - - /* compensate feedforward for loss of lift due to non-horizontal angle of wing */ - float pitch_sp_rollcompensation = p.pitch_roll_compensation_p * fabsf(sinf(att_sp->roll_body)); - /* set pitch plus feedforward roll compensation */ - rates_sp->pitch = pid_calculate(&pitch_controller, - att_sp->pitch_body + pitch_sp_rollcompensation, - att->pitch, 0, 0); - - /* Yaw (from coordinated turn constraint or lateral force) */ - rates_sp->yaw = (att->rollspeed * rates_sp->roll + 9.81f * sinf(att->roll) * cosf(att->pitch) + speed_body[0] * rates_sp->pitch * sinf(att->roll)) - / (speed_body[0] * cosf(att->roll) * cosf(att->pitch) + speed_body[2] * sinf(att->pitch)); - -// printf("rates_sp->yaw %.4f \n", (double)rates_sp->yaw); - - counter++; - - return 0; -} - - - diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c b/src/modules/fixedwing_att_control/fixedwing_att_control_main.c deleted file mode 100644 index b6b4546c2..000000000 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c +++ /dev/null @@ -1,367 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Doug Weibel <douglas.weibel@colorado.edu> - * - * 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 fixedwing_att_control.c - * Implementation of a fixed wing attitude controller. - */ - -#include <nuttx/config.h> -#include <stdio.h> -#include <stdlib.h> -#include <string.h> -#include <unistd.h> -#include <fcntl.h> -#include <errno.h> -#include <math.h> -#include <poll.h> -#include <time.h> -#include <drivers/drv_hrt.h> -#include <arch/board/board.h> -#include <uORB/uORB.h> -#include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/vehicle_global_position_setpoint.h> -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_control_mode.h> -#include <uORB/topics/vehicle_status.h> -#include <uORB/topics/vehicle_attitude_setpoint.h> -#include <uORB/topics/manual_control_setpoint.h> -#include <uORB/topics/actuator_controls.h> -#include <uORB/topics/vehicle_rates_setpoint.h> -#include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/debug_key_value.h> -#include <systemlib/param/param.h> -#include <systemlib/pid/pid.h> -#include <systemlib/geo/geo.h> -#include <systemlib/perf_counter.h> -#include <systemlib/systemlib.h> - -#include "fixedwing_att_control_rate.h" -#include "fixedwing_att_control_att.h" - -/* Prototypes */ -/** - * Deamon management function. - */ -__EXPORT int fixedwing_att_control_main(int argc, char *argv[]); - -/** - * Mainloop of deamon. - */ -int fixedwing_att_control_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -/* Variables */ -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int deamon_task; /**< Handle of deamon task / thread */ - -/* Main Thread */ -int fixedwing_att_control_thread_main(int argc, char *argv[]) -{ - /* read arguments */ - bool verbose = false; - - for (int i = 1; i < argc; i++) { - if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) { - verbose = true; - } - } - - /* welcome user */ - printf("[fixedwing att control] started\n"); - - /* declare and safely initialize all structs */ - struct vehicle_attitude_s att; - memset(&att, 0, sizeof(att)); - struct vehicle_attitude_setpoint_s att_sp; - memset(&att_sp, 0, sizeof(att_sp)); - struct vehicle_rates_setpoint_s rates_sp; - memset(&rates_sp, 0, sizeof(rates_sp)); - struct vehicle_global_position_s global_pos; - memset(&global_pos, 0, sizeof(global_pos)); - struct manual_control_setpoint_s manual_sp; - memset(&manual_sp, 0, sizeof(manual_sp)); - struct vehicle_control_mode_s control_mode; - memset(&control_mode, 0, sizeof(control_mode)); - struct vehicle_status_s vstatus; - memset(&vstatus, 0, sizeof(vstatus)); - - /* output structs */ - struct actuator_controls_s actuators; - memset(&actuators, 0, sizeof(actuators)); - - - /* publish actuator controls */ - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { - actuators.control[i] = 0.0f; - } - - orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); - orb_advert_t rates_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); - - /* subscribe */ - int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); - - /* Setup of loop */ - float gyro[3] = {0.0f, 0.0f, 0.0f}; - float speed_body[3] = {0.0f, 0.0f, 0.0f}; - struct pollfd fds = { .fd = att_sub, .events = POLLIN }; - - while (!thread_should_exit) { - /* wait for a sensor update, check for exit condition every 500 ms */ - poll(&fds, 1, 500); - - /* Check if there is a new position measurement or attitude setpoint */ - bool pos_updated; - orb_check(global_pos_sub, &pos_updated); - bool att_sp_updated; - orb_check(att_sp_sub, &att_sp_updated); - - /* get a local copy of attitude */ - orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - - if (att_sp_updated) - orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); - - if (pos_updated) { - orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); - - if (att.R_valid) { - speed_body[0] = att.R[0][0] * global_pos.vx + att.R[0][1] * global_pos.vy + att.R[0][2] * global_pos.vz; - speed_body[1] = att.R[1][0] * global_pos.vx + att.R[1][1] * global_pos.vy + att.R[1][2] * global_pos.vz; - speed_body[2] = att.R[2][0] * global_pos.vx + att.R[2][1] * global_pos.vy + att.R[2][2] * global_pos.vz; - - } else { - speed_body[0] = 0; - speed_body[1] = 0; - speed_body[2] = 0; - - printf("FW ATT CONTROL: Did not get a valid R\n"); - } - } - - orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp); - orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus); - - gyro[0] = att.rollspeed; - gyro[1] = att.pitchspeed; - gyro[2] = att.yawspeed; - - /* set manual setpoints if required */ - if (control_mode.flag_control_manual_enabled) { - if (control_mode.flag_control_attitude_enabled) { - - /* if the RC signal is lost, try to stay level and go slowly back down to ground */ - if (vstatus.rc_signal_lost) { - - /* put plane into loiter */ - att_sp.roll_body = 0.3f; - att_sp.pitch_body = 0.0f; - - /* limit throttle to 60 % of last value if sane */ - if (isfinite(manual_sp.throttle) && - (manual_sp.throttle >= 0.0f) && - (manual_sp.throttle <= 1.0f)) { - att_sp.thrust = 0.6f * manual_sp.throttle; - - } else { - att_sp.thrust = 0.0f; - } - - att_sp.yaw_body = 0; - - // XXX disable yaw control, loiter - - } else { - - att_sp.roll_body = manual_sp.roll; - att_sp.pitch_body = manual_sp.pitch; - att_sp.yaw_body = 0; - att_sp.thrust = manual_sp.throttle; - } - - att_sp.timestamp = hrt_absolute_time(); - - /* pass through flaps */ - if (isfinite(manual_sp.flaps)) { - actuators.control[4] = manual_sp.flaps; - - } else { - actuators.control[4] = 0.0f; - } - - } else { - /* directly pass through values */ - actuators.control[0] = manual_sp.roll; - /* positive pitch means negative actuator -> pull up */ - actuators.control[1] = manual_sp.pitch; - actuators.control[2] = manual_sp.yaw; - actuators.control[3] = manual_sp.throttle; - - if (isfinite(manual_sp.flaps)) { - actuators.control[4] = manual_sp.flaps; - - } else { - actuators.control[4] = 0.0f; - } - } - } - - /* execute attitude control if requested */ - if (control_mode.flag_control_attitude_enabled) { - /* attitude control */ - fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); - - /* angular rate control */ - fixedwing_att_control_rates(&rates_sp, gyro, &actuators); - - /* pass through throttle */ - actuators.control[3] = att_sp.thrust; - - /* set flaps to zero */ - actuators.control[4] = 0.0f; - - } - - /* publish rates */ - orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp); - - /* sanity check and publish actuator outputs */ - if (isfinite(actuators.control[0]) && - isfinite(actuators.control[1]) && - isfinite(actuators.control[2]) && - isfinite(actuators.control[3])) { - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - } - } - - printf("[fixedwing_att_control] exiting, stopping all motors.\n"); - thread_running = false; - - /* kill all outputs */ - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) - actuators.control[i] = 0.0f; - - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - - - - close(att_sub); - close(actuator_pub); - close(rates_pub); - - fflush(stdout); - exit(0); - - return 0; - -} - -/* Startup Functions */ - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - fprintf(stderr, "usage: fixedwing_att_control {start|stop|status}\n\n"); - exit(1); -} - -/** - * 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(). - */ -int fixedwing_att_control_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - printf("fixedwing_att_control already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - deamon_task = task_spawn_cmd("fixedwing_att_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 20, - 2048, - fixedwing_att_control_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - thread_running = true; - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - printf("\tfixedwing_att_control is running\n"); - - } else { - printf("\tfixedwing_att_control not started\n"); - } - - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - - - diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c deleted file mode 100644 index cdab39edc..000000000 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c +++ /dev/null @@ -1,211 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler <thomasgubler@student.ethz.ch> - * - * 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 fixedwing_att_control_rate.c - * @author Thomas Gubler <thomasgubler@student.ethz.ch> - * - * Implementation of a fixed wing attitude controller. - * - */ - -#include <nuttx/config.h> -#include <stdio.h> -#include <stdlib.h> -#include <string.h> -#include <unistd.h> -#include <fcntl.h> -#include <errno.h> -#include <math.h> -#include <poll.h> -#include <time.h> -#include <drivers/drv_hrt.h> -#include <arch/board/board.h> -#include <uORB/uORB.h> - -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_attitude_setpoint.h> -#include <uORB/topics/manual_control_setpoint.h> -#include <systemlib/param/param.h> -#include <systemlib/pid/pid.h> -#include <systemlib/geo/geo.h> -#include <systemlib/systemlib.h> - -#include "fixedwing_att_control_rate.h" - -/* - * Controller parameters, accessible via MAVLink - * - */ -// Roll control parameters -PARAM_DEFINE_FLOAT(FW_ROLLR_P, 0.9f); -PARAM_DEFINE_FLOAT(FW_ROLLR_I, 0.2f); -PARAM_DEFINE_FLOAT(FW_ROLLR_AWU, 0.9f); -PARAM_DEFINE_FLOAT(FW_ROLLR_LIM, 0.7f); // Roll rate limit in radians/sec, applies to the roll controller -PARAM_DEFINE_FLOAT(FW_ROLL_P, 4.0f); -PARAM_DEFINE_FLOAT(FW_PITCH_RCOMP, 0.1f); - -//Pitch control parameters -PARAM_DEFINE_FLOAT(FW_PITCHR_P, 0.8f); -PARAM_DEFINE_FLOAT(FW_PITCHR_I, 0.2f); -PARAM_DEFINE_FLOAT(FW_PITCHR_AWU, 0.8f); -PARAM_DEFINE_FLOAT(FW_PITCHR_LIM, 0.35f); // Pitch rate limit in radians/sec, applies to the pitch controller -PARAM_DEFINE_FLOAT(FW_PITCH_P, 8.0f); - -//Yaw control parameters //XXX TODO this is copy paste, asign correct values -PARAM_DEFINE_FLOAT(FW_YAWR_P, 0.3f); -PARAM_DEFINE_FLOAT(FW_YAWR_I, 0.0f); -PARAM_DEFINE_FLOAT(FW_YAWR_AWU, 0.0f); -PARAM_DEFINE_FLOAT(FW_YAWR_LIM, 0.35f); // Yaw rate limit in radians/sec - -/* feedforward compensation */ -PARAM_DEFINE_FLOAT(FW_PITCH_THR_P, 0.1f); /**< throttle to pitch coupling feedforward */ - -struct fw_rate_control_params { - float rollrate_p; - float rollrate_i; - float rollrate_awu; - float pitchrate_p; - float pitchrate_i; - float pitchrate_awu; - float yawrate_p; - float yawrate_i; - float yawrate_awu; - float pitch_thr_ff; -}; - -struct fw_rate_control_param_handles { - param_t rollrate_p; - param_t rollrate_i; - param_t rollrate_awu; - param_t pitchrate_p; - param_t pitchrate_i; - param_t pitchrate_awu; - param_t yawrate_p; - param_t yawrate_i; - param_t yawrate_awu; - param_t pitch_thr_ff; -}; - - - -/* Internal Prototypes */ -static int parameters_init(struct fw_rate_control_param_handles *h); -static int parameters_update(const struct fw_rate_control_param_handles *h, struct fw_rate_control_params *p); - -static int parameters_init(struct fw_rate_control_param_handles *h) -{ - /* PID parameters */ - h->rollrate_p = param_find("FW_ROLLR_P"); //TODO define rate params for fixed wing - h->rollrate_i = param_find("FW_ROLLR_I"); - h->rollrate_awu = param_find("FW_ROLLR_AWU"); - - h->pitchrate_p = param_find("FW_PITCHR_P"); - h->pitchrate_i = param_find("FW_PITCHR_I"); - h->pitchrate_awu = param_find("FW_PITCHR_AWU"); - - h->yawrate_p = param_find("FW_YAWR_P"); - h->yawrate_i = param_find("FW_YAWR_I"); - h->yawrate_awu = param_find("FW_YAWR_AWU"); - h->pitch_thr_ff = param_find("FW_PITCH_THR_P"); - - return OK; -} - -static int parameters_update(const struct fw_rate_control_param_handles *h, struct fw_rate_control_params *p) -{ - param_get(h->rollrate_p, &(p->rollrate_p)); - param_get(h->rollrate_i, &(p->rollrate_i)); - param_get(h->rollrate_awu, &(p->rollrate_awu)); - param_get(h->pitchrate_p, &(p->pitchrate_p)); - param_get(h->pitchrate_i, &(p->pitchrate_i)); - param_get(h->pitchrate_awu, &(p->pitchrate_awu)); - param_get(h->yawrate_p, &(p->yawrate_p)); - param_get(h->yawrate_i, &(p->yawrate_i)); - param_get(h->yawrate_awu, &(p->yawrate_awu)); - param_get(h->pitch_thr_ff, &(p->pitch_thr_ff)); - - return OK; -} - -int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], - struct actuator_controls_s *actuators) -{ - static int counter = 0; - static bool initialized = false; - - static struct fw_rate_control_params p; - static struct fw_rate_control_param_handles h; - - static PID_t roll_rate_controller; - static PID_t pitch_rate_controller; - static PID_t yaw_rate_controller; - - static uint64_t last_run = 0; - const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; - last_run = hrt_absolute_time(); - - if (!initialized) { - parameters_init(&h); - parameters_update(&h, &p); - pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, PID_MODE_DERIVATIV_NONE, 0.0f); // set D part to 0 because the controller layout is with a PI rate controller - pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, PID_MODE_DERIVATIV_NONE, 0.0f); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher - pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, PID_MODE_DERIVATIV_NONE, 0.0f); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher - initialized = true; - } - - /* load new parameters with lower rate */ - if (counter % 100 == 0) { - /* update parameters from storage */ - parameters_update(&h, &p); - pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1); - pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1); - pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1); - } - - - /* roll rate (PI) */ - actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT); - /* pitch rate (PI) */ - actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT); - /* yaw rate (PI) */ - actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT); - - counter++; - - return 0; -} - - - diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index cfae07275..bbb39f20f 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -229,8 +229,8 @@ void BlockMultiModeBacksideAutopilot::update() _actuators.control[CH_RDR] = _manual.yaw; _actuators.control[CH_THR] = _manual.throttle; - } else if (_status.main_state == MAIN_STATE_SEATBELT || - _status.main_state == MAIN_STATE_EASY /* TODO, implement easy */) { + } else if (_status.main_state == MAIN_STATE_ALTCTL || + _status.main_state == MAIN_STATE_POSCTL /* TODO, implement pos control */) { // calculate velocity, XXX should be airspeed, but using ground speed for now // for the purpose of control we will limit the velocity feedback between diff --git a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c deleted file mode 100644 index 888dd0942..000000000 --- a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c +++ /dev/null @@ -1,479 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Doug Weibel <douglas.weibel@colorado.edu> - * - * 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 fixedwing_pos_control.c - * Implementation of a fixed wing attitude controller. - */ - -#include <nuttx/config.h> -#include <stdio.h> -#include <stdlib.h> -#include <string.h> -#include <unistd.h> -#include <fcntl.h> -#include <errno.h> -#include <math.h> -#include <poll.h> -#include <time.h> -#include <drivers/drv_hrt.h> -#include <arch/board/board.h> -#include <uORB/uORB.h> -#include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/vehicle_global_position_setpoint.h> -#include <uORB/topics/vehicle_attitude_setpoint.h> -#include <uORB/topics/manual_control_setpoint.h> -#include <uORB/topics/actuator_controls.h> -#include <uORB/topics/vehicle_rates_setpoint.h> -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/parameter_update.h> -#include <systemlib/param/param.h> -#include <systemlib/pid/pid.h> -#include <systemlib/geo/geo.h> -#include <systemlib/perf_counter.h> -#include <systemlib/systemlib.h> - -/* - * Controller parameters, accessible via MAVLink - * - */ -PARAM_DEFINE_FLOAT(FW_HEAD_P, 0.1f); -PARAM_DEFINE_FLOAT(FW_HEADR_I, 0.1f); -PARAM_DEFINE_FLOAT(FW_HEADR_LIM, 1.5f); //TODO: think about reasonable value -PARAM_DEFINE_FLOAT(FW_XTRACK_P, 0.01745f); // Radians per meter off track -PARAM_DEFINE_FLOAT(FW_ALT_P, 0.1f); -PARAM_DEFINE_FLOAT(FW_ROLL_LIM, 0.7f); // Roll angle limit in radians -PARAM_DEFINE_FLOAT(FW_HEADR_P, 0.1f); -PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f); /**< Pitch angle limit in radians per second */ - -struct fw_pos_control_params { - float heading_p; - float headingr_p; - float headingr_i; - float headingr_lim; - float xtrack_p; - float altitude_p; - float roll_lim; - float pitch_lim; -}; - -struct fw_pos_control_param_handles { - param_t heading_p; - param_t headingr_p; - param_t headingr_i; - param_t headingr_lim; - param_t xtrack_p; - param_t altitude_p; - param_t roll_lim; - param_t pitch_lim; -}; - - -struct planned_path_segments_s { - bool segment_type; - double start_lat; // Start of line or center of arc - double start_lon; - double end_lat; - double end_lon; - float radius; // Radius of arc - float arc_start_bearing; // Bearing from center to start of arc - float arc_sweep; // Angle (radians) swept out by arc around center. - // Positive for clockwise, negative for counter-clockwise -}; - - -/* Prototypes */ -/* Internal Prototypes */ -static int parameters_init(struct fw_pos_control_param_handles *h); -static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p); - -/** - * Deamon management function. - */ -__EXPORT int fixedwing_pos_control_main(int argc, char *argv[]); - -/** - * Mainloop of deamon. - */ -int fixedwing_pos_control_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -/* Variables */ -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int deamon_task; /**< Handle of deamon task / thread */ - - -/** - * Parameter management - */ -static int parameters_init(struct fw_pos_control_param_handles *h) -{ - /* PID parameters */ - h->heading_p = param_find("FW_HEAD_P"); - h->headingr_p = param_find("FW_HEADR_P"); - h->headingr_i = param_find("FW_HEADR_I"); - h->headingr_lim = param_find("FW_HEADR_LIM"); - h->xtrack_p = param_find("FW_XTRACK_P"); - h->altitude_p = param_find("FW_ALT_P"); - h->roll_lim = param_find("FW_ROLL_LIM"); - h->pitch_lim = param_find("FW_PITCH_LIM"); - - return OK; -} - -static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p) -{ - param_get(h->heading_p, &(p->heading_p)); - param_get(h->headingr_p, &(p->headingr_p)); - param_get(h->headingr_i, &(p->headingr_i)); - param_get(h->headingr_lim, &(p->headingr_lim)); - param_get(h->xtrack_p, &(p->xtrack_p)); - param_get(h->altitude_p, &(p->altitude_p)); - param_get(h->roll_lim, &(p->roll_lim)); - param_get(h->pitch_lim, &(p->pitch_lim)); - - return OK; -} - - -/* Main Thread */ -int fixedwing_pos_control_thread_main(int argc, char *argv[]) -{ - /* read arguments */ - bool verbose = false; - - for (int i = 1; i < argc; i++) { - if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) { - verbose = true; - } - } - - /* welcome user */ - printf("[fixedwing pos control] started\n"); - - /* declare and safely initialize all structs */ - struct vehicle_global_position_s global_pos; - memset(&global_pos, 0, sizeof(global_pos)); - struct vehicle_global_position_s start_pos; // Temporary variable, replace with - memset(&start_pos, 0, sizeof(start_pos)); // previous waypoint when available - struct vehicle_global_position_setpoint_s global_setpoint; - memset(&global_setpoint, 0, sizeof(global_setpoint)); - struct vehicle_attitude_s att; - memset(&att, 0, sizeof(att)); - struct crosstrack_error_s xtrack_err; - memset(&xtrack_err, 0, sizeof(xtrack_err)); - struct parameter_update_s param_update; - memset(¶m_update, 0, sizeof(param_update)); - - /* output structs */ - struct vehicle_attitude_setpoint_s attitude_setpoint; - memset(&attitude_setpoint, 0, sizeof(attitude_setpoint)); - - /* publish attitude setpoint */ - attitude_setpoint.roll_body = 0.0f; - attitude_setpoint.pitch_body = 0.0f; - attitude_setpoint.yaw_body = 0.0f; - attitude_setpoint.thrust = 0.0f; - orb_advert_t attitude_setpoint_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &attitude_setpoint); - - /* subscribe */ - int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); - int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int param_sub = orb_subscribe(ORB_ID(parameter_update)); - - /* Setup of loop */ - struct pollfd fds[2] = { - { .fd = param_sub, .events = POLLIN }, - { .fd = att_sub, .events = POLLIN } - }; - bool global_sp_updated_set_once = false; - - float psi_track = 0.0f; - - int counter = 0; - - struct fw_pos_control_params p; - struct fw_pos_control_param_handles h; - - PID_t heading_controller; - PID_t heading_rate_controller; - PID_t offtrack_controller; - PID_t altitude_controller; - - parameters_init(&h); - parameters_update(&h, &p); - pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f, 10000.0f, PID_MODE_DERIVATIV_NONE, 0.0f); //arbitrary high limit - pid_init(&heading_rate_controller, p.headingr_p, p.headingr_i, 0.0f, 0.0f, p.roll_lim, PID_MODE_DERIVATIV_NONE, 0.0f); - pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, PID_MODE_DERIVATIV_NONE, 0.0f); - pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f * M_DEG_TO_RAD, PID_MODE_DERIVATIV_NONE, 0.0f); //TODO: remove hardcoded value - - /* error and performance monitoring */ - perf_counter_t fw_interval_perf = perf_alloc(PC_INTERVAL, "fixedwing_pos_control_interval"); - perf_counter_t fw_err_perf = perf_alloc(PC_COUNT, "fixedwing_pos_control_err"); - - while (!thread_should_exit) { - /* wait for a sensor update, check for exit condition every 500 ms */ - int ret = poll(fds, 2, 500); - - if (ret < 0) { - /* poll error, count it in perf */ - perf_count(fw_err_perf); - - } else if (ret == 0) { - /* no return value, ignore */ - } else { - - /* only update parameters if they changed */ - if (fds[0].revents & POLLIN) { - /* read from param to clear updated flag */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), param_sub, &update); - - /* update parameters from storage */ - parameters_update(&h, &p); - pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, 10000.0f); //arbitrary high limit - pid_set_parameters(&heading_rate_controller, p.headingr_p, p.headingr_i, 0, 0, p.roll_lim); - pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim); - pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f * M_DEG_TO_RAD); //TODO: remove hardcoded value - } - - /* only run controller if attitude changed */ - if (fds[1].revents & POLLIN) { - - - static uint64_t last_run = 0; - const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; - last_run = hrt_absolute_time(); - - /* check if there is a new position or setpoint */ - bool pos_updated; - orb_check(global_pos_sub, &pos_updated); - bool global_sp_updated; - orb_check(global_setpoint_sub, &global_sp_updated); - - /* load local copies */ - orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - - if (pos_updated) { - orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); - } - - if (global_sp_updated) { - orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint); - start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint) - global_sp_updated_set_once = true; - psi_track = get_bearing_to_next_waypoint(global_pos.lat, global_pos.lon, - (double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d); - - printf("next wp direction: %0.4f\n", (double)psi_track); - } - - /* Simple Horizontal Control */ - if (global_sp_updated_set_once) { - // if (counter % 100 == 0) - // printf("lat_sp %d, ln_sp %d, lat: %d, lon: %d\n", global_setpoint.lat, global_setpoint.lon, global_pos.lat, global_pos.lon); - - /* calculate crosstrack error */ - // Only the case of a straight line track following handled so far - int distance_res = get_distance_to_line(&xtrack_err, (double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d, - (double)start_pos.lat / (double)1e7d, (double)start_pos.lon / (double)1e7d, - (double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d); - - // XXX what is xtrack_err.past_end? - if (distance_res == OK /*&& !xtrack_err.past_end*/) { - - float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f); //p.xtrack_p * xtrack_err.distance - - float psi_c = psi_track + delta_psi_c; - float psi_e = psi_c - att.yaw; - - /* wrap difference back onto -pi..pi range */ - psi_e = _wrap_pi(psi_e); - - if (verbose) { - printf("xtrack_err.distance %.4f ", (double)xtrack_err.distance); - printf("delta_psi_c %.4f ", (double)delta_psi_c); - printf("psi_c %.4f ", (double)psi_c); - printf("att.yaw %.4f ", (double)att.yaw); - printf("psi_e %.4f ", (double)psi_e); - } - - /* calculate roll setpoint, do this artificially around zero */ - float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f); - float psi_rate_track = 0; //=V_gr/r_track , this will be needed for implementation of arc following - float psi_rate_c = delta_psi_rate_c + psi_rate_track; - - /* limit turn rate */ - if (psi_rate_c > p.headingr_lim) { - psi_rate_c = p.headingr_lim; - - } else if (psi_rate_c < -p.headingr_lim) { - psi_rate_c = -p.headingr_lim; - } - - float psi_rate_e = psi_rate_c - att.yawspeed; - - // XXX sanity check: Assume 10 m/s stall speed and no stall condition - float ground_speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); - - if (ground_speed < 10.0f) { - ground_speed = 10.0f; - } - - float psi_rate_e_scaled = psi_rate_e * ground_speed / 9.81f; //* V_gr / g - - attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT); - - if (verbose) { - printf("psi_rate_c %.4f ", (double)psi_rate_c); - printf("psi_rate_e_scaled %.4f ", (double)psi_rate_e_scaled); - printf("rollbody %.4f\n", (double)attitude_setpoint.roll_body); - } - - if (verbose && counter % 100 == 0) - printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n", xtrack_err.distance, delta_psi_c); - - } else { - if (verbose && counter % 100 == 0) - printf("distance_res: %d, past_end %d\n", distance_res, xtrack_err.past_end); - } - - /* Very simple Altitude Control */ - if (pos_updated) { - - //TODO: take care of relative vs. ab. altitude - attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f); - - } - - // XXX need speed control - attitude_setpoint.thrust = 0.7f; - - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), attitude_setpoint_pub, &attitude_setpoint); - - /* measure in what intervals the controller runs */ - perf_count(fw_interval_perf); - - counter++; - - } else { - // XXX no setpoint, decent default needed (loiter?) - } - } - } - } - - printf("[fixedwing_pos_control] exiting.\n"); - thread_running = false; - - - close(attitude_setpoint_pub); - - fflush(stdout); - exit(0); - - return 0; - -} - -/* Startup Functions */ - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - fprintf(stderr, "usage: fixedwing_pos_control {start|stop|status}\n\n"); - exit(1); -} - -/** - * 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(). - */ -int fixedwing_pos_control_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - printf("fixedwing_pos_control already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - deamon_task = task_spawn_cmd("fixedwing_pos_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 20, - 2048, - fixedwing_pos_control_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - thread_running = true; - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - printf("\tfixedwing_pos_control is running\n"); - - } else { - printf("\tfixedwing_pos_control not started\n"); - } - - exit(0); - } - - usage("unrecognized command"); - exit(1); -} diff --git a/src/modules/fixedwing_pos_control/module.mk b/src/modules/fixedwing_pos_control/module.mk deleted file mode 100644 index b976377e9..000000000 --- a/src/modules/fixedwing_pos_control/module.mk +++ /dev/null @@ -1,40 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 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. -# -############################################################################ - -# -# Fixedwing PositionControl application -# - -MODULE_COMMAND = fixedwing_pos_control - -SRCS = fixedwing_pos_control_main.c 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 f139c25f4..1f3e9f098 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -173,6 +173,8 @@ private: float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */ float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */ float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */ + float man_roll_max; /**< Max Roll in rad */ + float man_pitch_max; /**< Max Pitch in rad */ } _parameters; /**< local copies of interesting parameters */ @@ -211,6 +213,8 @@ private: param_t trim_yaw; param_t rollsp_offset_deg; param_t pitchsp_offset_deg; + param_t man_roll_max; + param_t man_pitch_max; } _parameter_handles; /**< handles for interesting parameters */ @@ -269,7 +273,7 @@ private: /** * Main sensor collection task. */ - void task_main() __attribute__((noreturn)); + void task_main(); }; namespace att_control @@ -354,6 +358,9 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.rollsp_offset_deg = param_find("FW_RSP_OFF"); _parameter_handles.pitchsp_offset_deg = param_find("FW_PSP_OFF"); + _parameter_handles.man_roll_max = param_find("FW_MAN_R_MAX"); + _parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX"); + /* fetch initial parameter values */ parameters_update(); } @@ -421,6 +428,10 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg)); _parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg); _parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_deg); + param_get(_parameter_handles.man_roll_max, &(_parameters.man_roll_max)); + param_get(_parameter_handles.man_pitch_max, &(_parameters.man_pitch_max)); + _parameters.man_roll_max = math::radians(_parameters.man_roll_max); + _parameters.man_pitch_max = math::radians(_parameters.man_pitch_max); /* pitch control parameters */ @@ -660,18 +671,24 @@ FixedwingAttitudeControl::task_main() float airspeed; - /* if airspeed is smaller than min, the sensor is not giving good readings */ - if ((_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) || - !isfinite(_airspeed.indicated_airspeed_m_s) || + /* if airspeed is not updating, we assume the normal average speed */ + if (!isfinite(_airspeed.true_airspeed_m_s) || hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { airspeed = _parameters.airspeed_trim; } else { - airspeed = _airspeed.indicated_airspeed_m_s; + airspeed = _airspeed.true_airspeed_m_s; } - float airspeed_scaling = _parameters.airspeed_trim / airspeed; - //warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling); + /* + * 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 + * want to do in flight and its the baseline a human pilot would choose. + * + * Forcing the scaling to this value allows reasonable handheld tests. + */ + + float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed); float roll_sp = _parameters.rollsp_offset_rad; float pitch_sp = _parameters.pitchsp_offset_rad; @@ -689,20 +706,21 @@ FixedwingAttitudeControl::task_main() } else { /* * Scale down roll and pitch as the setpoints are radians - * and a typical remote can only do 45 degrees, the mapping is - * -1..+1 to -45..+45 degrees or -0.75..+0.75 radians. + * and a typical remote can only do around 45 degrees, the mapping is + * -1..+1 to -man_roll_max rad..+man_roll_max rad (equivalent for pitch) * * With this mapping the stick angle is a 1:1 representation of - * the commanded attitude. If more than 45 degrees are desired, - * a scaling parameter can be applied to the remote. + * the commanded attitude. * * The trim gets subtracted here from the manual setpoint to get * the intended attitude setpoint. Later, after the rate control step the * trim is added again to the control signal. */ - roll_sp = (_manual.roll - _parameters.trim_roll) * 0.75f + _parameters.rollsp_offset_rad; - pitch_sp = (_manual.pitch - _parameters.trim_pitch) * 0.75f + _parameters.pitchsp_offset_rad; - throttle_sp = _manual.throttle; + roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll) + + _parameters.rollsp_offset_rad; + pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch) + + _parameters.pitchsp_offset_rad; + throttle_sp = _manual.z; _actuators.control[4] = _manual.flaps; /* @@ -765,7 +783,7 @@ FixedwingAttitudeControl::task_main() _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch; if (!isfinite(pitch_u)) { warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f, airspeed %.4f, airspeed_scaling %.4f, roll_sp %.4f, pitch_sp %.4f, _roll_ctrl.get_desired_rate() %.4f, _pitch_ctrl.get_desired_rate() %.4f att_sp.roll_body %.4f", - pitch_u, _yaw_ctrl.get_desired_rate(), airspeed, airspeed_scaling, roll_sp, pitch_sp, _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate(), _att_sp.roll_body); + (double)pitch_u, (double)_yaw_ctrl.get_desired_rate(), (double)airspeed, (double)airspeed_scaling, (double)roll_sp, (double)pitch_sp, (double)_roll_ctrl.get_desired_rate(), (double)_pitch_ctrl.get_desired_rate(), (double)_att_sp.roll_body); } float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch, @@ -774,16 +792,16 @@ FixedwingAttitudeControl::task_main() _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; if (!isfinite(yaw_u)) { - warnx("yaw_u %.4f", yaw_u); + warnx("yaw_u %.4f", (double)yaw_u); } /* throttle passed through */ _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; if (!isfinite(throttle_sp)) { - warnx("throttle_sp %.4f", throttle_sp); + warnx("throttle_sp %.4f", (double)throttle_sp); } } else { - warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", roll_sp, pitch_sp); + warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp); } /* @@ -808,10 +826,10 @@ FixedwingAttitudeControl::task_main() } else { /* manual/direct control */ - _actuators.control[0] = _manual.roll; - _actuators.control[1] = _manual.pitch; - _actuators.control[2] = _manual.yaw; - _actuators.control[3] = _manual.throttle; + _actuators.control[0] = _manual.y; + _actuators.control[1] = -_manual.x; + _actuators.control[2] = _manual.r; + _actuators.control[3] = _manual.z; _actuators.control[4] = _manual.flaps; } diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index c80a44f2a..aa637e207 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -186,3 +186,13 @@ PARAM_DEFINE_FLOAT(FW_RSP_OFF, 0.0f); // @Description An airframe specific offset of the pitch setpoint in degrees, the value is added to the pitch setpoint and should correspond to the typical cruise speed of the airframe // @Range -90.0 to 90.0 PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f); + +// @DisplayName Max Manual Roll +// @Description Max roll for manual control in attitude stabilized mode +// @Range 0.0 to 90.0 +PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f); + +// @DisplayName Max Manual Pitch +// @Description Max pitch for manual control in attitude stabilized mode +// @Range 0.0 to 90.0 +PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 45.0f); 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 7f13df785..9cbc26efe 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 @@ -89,6 +89,7 @@ #include <launchdetection/LaunchDetector.h> #include <ecl/l1/ecl_l1_pos_controller.h> #include <external_lgpl/tecs/tecs.h> +#include <drivers/drv_range_finder.h> #include "landingslope.h" @@ -134,6 +135,7 @@ private: int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ int _sensor_combined_sub; /**< for body frame accelerations */ + int _range_finder_sub; /**< range finder subscription */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint */ orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */ @@ -147,13 +149,14 @@ private: struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of mission items */ struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */ + struct range_finder_report _range_finder; /**< range finder report */ perf_counter_t _loop_perf; /**< loop performance counter */ bool _setpoint_valid; /**< flag if the position control setpoint is valid */ /** manual control states */ - float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */ + float _altctrl_hold_heading; /**< heading the system should hold in altctrl mode */ double _loiter_hold_lat; double _loiter_hold_lon; float _loiter_hold_alt; @@ -181,7 +184,7 @@ private: /* Landingslope object */ Landingslope landingslope; - float flare_curve_alt_last; + float flare_curve_alt_rel_last; /* heading hold */ float target_bearing; @@ -239,6 +242,7 @@ private: float land_flare_alt_relative; float land_thrust_lim_alt_relative; float land_heading_hold_horizontal_distance; + float range_finder_rel_alt; } _parameters; /**< local copies of interesting parameters */ @@ -283,6 +287,7 @@ private: param_t land_flare_alt_relative; param_t land_thrust_lim_alt_relative; param_t land_heading_hold_horizontal_distance; + param_t range_finder_rel_alt; } _parameter_handles; /**< handles for interesting parameters */ @@ -309,6 +314,12 @@ private: bool vehicle_airspeed_poll(); /** + * Check for range finder updates. + */ + bool range_finder_poll(); + + + /** * Check for position updates. */ void vehicle_attitude_poll(); @@ -329,6 +340,11 @@ private: void navigation_capabilities_publish(); /** + * Get the relative alt either from the difference between estimate and waypoint or from the laser range finder + */ + float get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt); + + /** * Control position. */ bool control_position(const math::Vector<2> &global_pos, const math::Vector<2> &ground_speed, @@ -345,7 +361,7 @@ private: /** * Main sensor collection task. */ - void task_main() __attribute__((noreturn)); + void task_main(); /* * Reset takeoff state @@ -384,6 +400,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _control_mode_sub(-1), _params_sub(-1), _manual_control_sub(-1), + _range_finder_sub(-1), /* publications */ _attitude_sp_pub(-1), @@ -403,7 +420,7 @@ FixedwingPositionControl::FixedwingPositionControl() : launch_detected(false), last_manual(false), usePreTakeoffThrust(false), - flare_curve_alt_last(0.0f), + flare_curve_alt_rel_last(0.0f), launchDetector(), _airspeed_error(0.0f), _airspeed_valid(false), @@ -417,7 +434,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _control_mode(), _global_pos(), _pos_sp_triplet(), - _sensor_combined() + _sensor_combined(), + _range_finder() { _nav_capabilities.turn_distance = 0.0f; @@ -442,6 +460,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT"); _parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT"); _parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST"); + _parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); @@ -531,6 +550,8 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative)); param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance)); + param_get(_parameter_handles.range_finder_rel_alt, &(_parameters.range_finder_rel_alt)); + _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); _l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit)); @@ -626,6 +647,20 @@ FixedwingPositionControl::vehicle_airspeed_poll() return airspeed_updated; } +bool +FixedwingPositionControl::range_finder_poll() +{ + /* check if there is a range finder measurement */ + bool range_finder_updated; + orb_check(_range_finder_sub, &range_finder_updated); + + if (range_finder_updated) { + orb_copy(ORB_ID(sensor_range_finder), _range_finder_sub, &_range_finder); + } + + return range_finder_updated; +} + void FixedwingPositionControl::vehicle_attitude_poll() { @@ -754,6 +789,23 @@ void FixedwingPositionControl::navigation_capabilities_publish() } } +float FixedwingPositionControl::get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt) +{ + float rel_alt_estimated = current_alt - land_setpoint_alt; + + /* only use range finder if: + * parameter (range_finder_use_relative_alt) > 0 + * the measurement is valid + * the estimated relative altitude (from global altitude estimate and landing waypoint) <= range_finder_use_relative_alt + */ + if (range_finder_use_relative_alt < 0 || !range_finder.valid || rel_alt_estimated > range_finder_use_relative_alt ) { + return rel_alt_estimated; + } + + return range_finder.distance; + +} + bool FixedwingPositionControl::control_position(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) @@ -896,12 +948,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */ float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); - float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt); + float L_altitude_rel = landingslope.getLandingSlopeRelativeAltitude(L_wp_distance); float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); - float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt); + float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp); + + float relative_alt = get_relative_landingalt(_pos_sp_triplet.current.alt, _global_pos.alt, _range_finder, _parameters.range_finder_rel_alt); - if ( (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out + if ( (relative_alt < landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ @@ -911,7 +965,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* kill the throttle if param requests it */ throttle_max = _parameters.throttle_max; - if (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.motor_lim_relative_alt() || land_motor_lim) { + if (relative_alt < landingslope.motor_lim_relative_alt() || land_motor_lim) { throttle_max = math::min(throttle_max, _parameters.throttle_land_max); if (!land_motor_lim) { land_motor_lim = true; @@ -920,16 +974,16 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } - float flare_curve_alt = landingslope.getFlareCurveAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt); + float flare_curve_alt_rel = landingslope.getFlareCurveRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp); /* avoid climbout */ - if ((flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical) || land_stayonground) + if ((flare_curve_alt_rel_last < flare_curve_alt_rel && land_noreturn_vertical) || land_stayonground) { - flare_curve_alt = pos_sp_triplet.current.alt; + flare_curve_alt_rel = 0.0f; // stay on ground land_stayonground = true; } - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, flare_curve_alt, calculate_target_airspeed(airspeed_land), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _pos_sp_triplet.current.alt + relative_alt, _pos_sp_triplet.current.alt + flare_curve_alt_rel, calculate_target_airspeed(airspeed_land), _airspeed.indicated_airspeed_m_s, eas2tas, false, flare_pitch_angle_rad, 0.0f, throttle_max, throttle_land, @@ -941,7 +995,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } //warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); - flare_curve_alt_last = flare_curve_alt; + flare_curve_alt_rel_last = flare_curve_alt_rel; } else { /* intersect glide slope: @@ -949,20 +1003,20 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi * if current position is higher or within 10m of slope follow the glide slope * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope * */ - float altitude_desired = _global_pos.alt; - if (_global_pos.alt > landing_slope_alt_desired - 10.0f) { + float altitude_desired_rel = relative_alt; + if (relative_alt > landing_slope_alt_rel_desired - 10.0f) { /* stay on slope */ - altitude_desired = landing_slope_alt_desired; + altitude_desired_rel = landing_slope_alt_rel_desired; if (!land_onslope) { mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope"); land_onslope = true; } } else { /* continue horizontally */ - altitude_desired = math::max(_global_pos.alt, L_altitude); + altitude_desired_rel = math::max(relative_alt, L_altitude_rel); } - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _pos_sp_triplet.current.alt + relative_alt, _pos_sp_triplet.current.alt + altitude_desired_rel, calculate_target_airspeed(airspeed_approach), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, @@ -1051,16 +1105,16 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_reset_integral = true; } - } else if (0/* easy mode enabled */) { + } else if (0/* posctrl mode enabled */) { - /** EASY FLIGHT **/ + /** POSCTRL FLIGHT **/ - if (0/* switched from another mode to easy */) { - _seatbelt_hold_heading = _att.yaw; + if (0/* switched from another mode to posctrl */) { + _altctrl_hold_heading = _att.yaw; } - if (0/* easy on and manual control yaw non-zero */) { - _seatbelt_hold_heading = _att.yaw + _manual.yaw; + if (0/* posctrl on and manual control yaw non-zero */) { + _altctrl_hold_heading = _att.yaw + _manual.r; } //XXX not used @@ -1073,44 +1127,44 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // climb_out = true; // } - /* if in seatbelt mode, set airspeed based on manual control */ + /* if in altctrl mode, set airspeed based on manual control */ // XXX check if ground speed undershoot should be applied here - float seatbelt_airspeed = _parameters.airspeed_min + + float altctrl_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * - _manual.throttle; + _manual.z; - _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); + _l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, - seatbelt_airspeed, + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.x * 2.0f, + altctrl_airspeed, _airspeed.indicated_airspeed_m_s, eas2tas, false, _parameters.pitch_limit_min, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, _parameters.pitch_limit_min, _parameters.pitch_limit_max); - } else if (0/* seatbelt mode enabled */) { + } else if (0/* altctrl mode enabled */) { - /** SEATBELT FLIGHT **/ + /** ALTCTRL FLIGHT **/ - if (0/* switched from another mode to seatbelt */) { - _seatbelt_hold_heading = _att.yaw; + if (0/* switched from another mode to altctrl */) { + _altctrl_hold_heading = _att.yaw; } - if (0/* seatbelt on and manual control yaw non-zero */) { - _seatbelt_hold_heading = _att.yaw + _manual.yaw; + if (0/* altctrl on and manual control yaw non-zero */) { + _altctrl_hold_heading = _att.yaw + _manual.r; } - /* if in seatbelt mode, set airspeed based on manual control */ + /* if in altctrl mode, set airspeed based on manual control */ // XXX check if ground speed undershoot should be applied here - float seatbelt_airspeed = _parameters.airspeed_min + + float altctrl_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * - _manual.throttle; + _manual.z; /* user switched off throttle */ - if (_manual.throttle < 0.1f) { + if (_manual.z < 0.1f) { throttle_max = 0.0f; /* switch to pure pitch based altitude control, give up speed */ _tecs.set_speed_weight(0.0f); @@ -1120,15 +1174,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi bool climb_out = false; /* user wants to climb out */ - if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) { + if (_manual.x > 0.3f && _manual.z > 0.8f) { climb_out = true; } - _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); - _att_sp.roll_body = _manual.roll; - _att_sp.yaw_body = _manual.yaw; - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, - seatbelt_airspeed, + _l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed); + _att_sp.roll_body = _manual.y; + _att_sp.yaw_body = _manual.r; + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.x * 2.0f, + altctrl_airspeed, _airspeed.indicated_airspeed_m_s, eas2tas, climb_out, _parameters.pitch_limit_min, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, @@ -1185,6 +1239,7 @@ FixedwingPositionControl::task_main() _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + _range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_control_mode_sub, 200); @@ -1264,6 +1319,7 @@ FixedwingPositionControl::task_main() vehicle_setpoint_poll(); vehicle_sensor_combined_poll(); vehicle_airspeed_poll(); + range_finder_poll(); // vehicle_baro_poll(); math::Vector<2> ground_speed(_global_pos.vel_n, _global_pos.vel_e); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 37f06dbe5..f258f77da 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -375,3 +375,14 @@ PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f); * @group L1 Control */ PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); + +/** + * Relative altitude threshold for range finder measurements for use during landing + * + * range finder measurements will only be used if the estimated relative altitude (gobal_pos.alt - landing_waypoint.alt) is < FW_LND_RFRALT + * set to < 0 to disable + * the correct value of this parameter depends on your range measuring device as well as on the terrain at the landing location + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_LND_RFRALT, -1.0f); diff --git a/src/modules/fw_pos_control_l1/landingslope.cpp b/src/modules/fw_pos_control_l1/landingslope.cpp index e5f7023ae..8ce465fe8 100644 --- a/src/modules/fw_pos_control_l1/landingslope.cpp +++ b/src/modules/fw_pos_control_l1/landingslope.cpp @@ -69,26 +69,46 @@ void Landingslope::calculateSlopeValues() _horizontal_slope_displacement = (_flare_length - _d1); } -float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude) +float Landingslope::getLandingSlopeRelativeAltitude(float wp_landing_distance) { - return Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude, _horizontal_slope_displacement, _landing_slope_angle_rad); + return Landingslope::getLandingSlopeRelativeAltitude(wp_landing_distance, _horizontal_slope_displacement, _landing_slope_angle_rad); } -float Landingslope::getLandingSlopeAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude) +float Landingslope::getLandingSlopeRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp) { /* If airplane is in front of waypoint return slope altitude, else return waypoint altitude */ if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f)) - return getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude); + return getLandingSlopeRelativeAltitude(wp_landing_distance); + else + return 0.0f; +} + +float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_altitude) +{ + return Landingslope::getLandingSlopeAbsoluteAltitude(wp_landing_distance, wp_altitude, _horizontal_slope_displacement, _landing_slope_angle_rad); +} + +float Landingslope::getLandingSlopeAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude) +{ + /* If airplane is in front of waypoint return slope altitude, else return waypoint altitude */ + if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f)) + return getLandingSlopeAbsoluteAltitude(wp_landing_distance, wp_altitude); else return wp_altitude; } -float Landingslope::getFlareCurveAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude) +float Landingslope::getFlareCurveRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp) { /* If airplane is in front of waypoint return flare curve altitude, else return waypoint altitude */ if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f)) - return wp_landing_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt; + return _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt; else - return wp_landing_altitude; + return 0.0f; +} + +float Landingslope::getFlareCurveAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude) +{ + + return wp_landing_altitude + getFlareCurveRelativeAltitudeSave(wp_landing_distance, bearing_lastwp_currwp, bearing_airplane_currwp); } diff --git a/src/modules/fw_pos_control_l1/landingslope.h b/src/modules/fw_pos_control_l1/landingslope.h index 76d65a55f..b54fd501c 100644 --- a/src/modules/fw_pos_control_l1/landingslope.h +++ b/src/modules/fw_pos_control_l1/landingslope.h @@ -63,11 +63,26 @@ public: Landingslope() {} ~Landingslope() {} + + /** + * + * @return relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance + */ + float getLandingSlopeRelativeAltitude(float wp_landing_distance); + + /** + * + * @return relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance + * Performs check if aircraft is in front of waypoint to avoid climbout + */ + float getLandingSlopeRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp); + + /** * * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance */ - float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude); + float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_altitude); /** * @@ -78,11 +93,20 @@ public: /** * + * @return Relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance + */ + __EXPORT static float getLandingSlopeRelativeAltitude(float wp_landing_distance, float horizontal_slope_displacement, float landing_slope_angle_rad) + { + return (wp_landing_distance - horizontal_slope_displacement) * tanf(landing_slope_angle_rad); //flare_relative_alt is negative + } + + /** + * * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance */ __EXPORT static float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude, float horizontal_slope_displacement, float landing_slope_angle_rad) { - return (wp_landing_distance - horizontal_slope_displacement) * tanf(landing_slope_angle_rad) + wp_landing_altitude; //flare_relative_alt is negative + return getLandingSlopeRelativeAltitude(wp_landing_distance, horizontal_slope_displacement, landing_slope_angle_rad) + wp_landing_altitude; } /** @@ -95,8 +119,9 @@ public: } + float getFlareCurveRelativeAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp); - float getFlareCurveAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude); + float getFlareCurveAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude); void update(float landing_slope_angle_rad, float flare_relative_alt, diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index ad435b251..e49288a74 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -62,6 +62,12 @@ PARAM_DEFINE_INT32(MAV_COMP_ID, 50); * @group MAVLink */ PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING); +/** + * Use/Accept HIL GPS message (even if not in HIL mode) + * If set to 1 incomming HIL GPS messages are parsed. + * @group MAVLink + */ +PARAM_DEFINE_INT32(MAV_USEHILGPS, 0); mavlink_system_t mavlink_system = { 100, diff --git a/src/modules/position_estimator_mc/position_estimator_mc_params.c b/src/modules/mavlink/mavlink_commands.cpp index 72e5bc73b..1c1e097a4 100755..100644 --- a/src/modules/position_estimator_mc/position_estimator_mc_params.c +++ b/src/modules/mavlink/mavlink_commands.cpp @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Damian Aregger <daregger@student.ethz.ch> - * Tobias Naegeli <naegelit@student.ethz.ch> -* Lorenz Meier <lm@inf.ethz.ch> + * 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 @@ -34,35 +31,43 @@ * ****************************************************************************/ -/* - * @file position_estimator_mc_params.c - * - * Parameters for position_estimator_mc +/** + * @file mavlink_commands.cpp + * Mavlink commands stream implementation. + * + * @author Anton Babushkin <anton.babushkin@me.com> */ -#include "position_estimator_mc_params.h" +#include "mavlink_commands.h" -/* Kalman Filter covariances */ -/* gps measurement noise standard deviation */ -PARAM_DEFINE_FLOAT(POS_EST_ADDN, 1.0f); -PARAM_DEFINE_FLOAT(POS_EST_SIGMA, 0.0f); -PARAM_DEFINE_FLOAT(POS_EST_R, 1.0f); -PARAM_DEFINE_INT32(POS_EST_BARO, 0.0f); +MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel) +{ + _cmd_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command)); + _cmd = (struct vehicle_command_s *)_cmd_sub->get_data(); +} -int parameters_init(struct position_estimator_mc_param_handles *h) +MavlinkCommandsStream::~MavlinkCommandsStream() { - h->addNoise = param_find("POS_EST_ADDN"); - h->sigma = param_find("POS_EST_SIGMA"); - h->r = param_find("POS_EST_R"); - h->baro_param_handle = param_find("POS_EST_BARO"); - return OK; } -int parameters_update(const struct position_estimator_mc_param_handles *h, struct position_estimator_mc_params *p) +void +MavlinkCommandsStream::update(const hrt_abstime t) { - param_get(h->addNoise, &(p->addNoise)); - param_get(h->sigma, &(p->sigma)); - param_get(h->r, &(p->R)); - param_get(h->baro_param_handle, &(p->baro)); - return OK; + if (_cmd_sub->update(t)) { + /* only send commands for other systems/components */ + if (_cmd->target_system != mavlink_system.sysid || _cmd->target_component != mavlink_system.compid) { + mavlink_msg_command_long_send(_channel, + _cmd->target_system, + _cmd->target_component, + _cmd->command, + _cmd->confirmation, + _cmd->param1, + _cmd->param2, + _cmd->param3, + _cmd->param4, + _cmd->param5, + _cmd->param6, + _cmd->param7); + } + } } diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_att.h b/src/modules/mavlink/mavlink_commands.h index 600e35b89..6255d65df 100644 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_att.h +++ b/src/modules/mavlink/mavlink_commands.h @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> - * + * 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 @@ -33,19 +31,35 @@ * ****************************************************************************/ -/* @file Fixed Wing Attitude Control */ +/** + * @file mavlink_commands.h + * Mavlink commands stream definition. + * + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#ifndef MAVLINK_COMMANDS_H_ +#define MAVLINK_COMMANDS_H_ + +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_command.h> + +class Mavlink; +class MavlinkCommansStream; -#ifndef FIXEDWING_ATT_CONTROL_ATT_H_ -#define FIXEDWING_ATT_CONTROL_ATT_H_ +#include "mavlink_main.h" -#include <uORB/topics/vehicle_rates_setpoint.h> -#include <uORB/topics/vehicle_attitude_setpoint.h> -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_global_position.h> +class MavlinkCommandsStream +{ +private: + MavlinkOrbSubscription *_cmd_sub; + struct vehicle_command_s *_cmd; + mavlink_channel_t _channel; -int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, - const float speed_body[], - struct vehicle_rates_setpoint_s *rates_sp); +public: + MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel); + ~MavlinkCommandsStream(); + void update(const hrt_abstime t); +}; -#endif /* FIXEDWING_ATT_CONTROL_ATT_H_ */ +#endif /* MAVLINK_COMMANDS_H_ */ diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 18df577fe..199e85305 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -81,6 +81,7 @@ #include "mavlink_messages.h" #include "mavlink_receiver.h" #include "mavlink_rate_limiter.h" +#include "mavlink_commands.h" /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -166,12 +167,12 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length int buf_free = 0; if (instance->get_flow_control_enabled() - && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { + && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { if (buf_free == 0) { if (last_write_times[(unsigned)channel] != 0 && - hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) { + hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) { warnx("DISABLING HARDWARE FLOW CONTROL"); instance->enable_flow_control(false); @@ -185,12 +186,26 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length } } - ssize_t ret = write(uart, ch, desired); + /* If the wait until transmit flag is on, only transmit after we've received messages. + Otherwise, transmit all the time. */ + if (instance->should_transmit()) { - if (ret != desired) { - // XXX do something here, but change to using FIONWRITE and OS buf size for detection + /* check if there is space in the buffer, let it overflow else */ + if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) { + + if (desired > buf_free) { + desired = buf_free; + } + } + + ssize_t ret = write(uart, ch, desired); + if (ret != desired) { + warnx("TX FAIL"); + } } + + } static void usage(void); @@ -202,15 +217,23 @@ Mavlink::Mavlink() : _mavlink_fd(-1), _task_running(false), _hil_enabled(false), + _use_hil_gps(false), _is_usb_uart(false), + _wait_to_transmit(false), + _received_messages(false), _main_loop_delay(1000), _subscriptions(nullptr), _streams(nullptr), _mission_pub(-1), + _verbose(false), + _forwarding_on(false), + _passing_on(false), + _uart_fd(-1), _mavlink_param_queue_index(0), _subscribe_to_stream(nullptr), _subscribe_to_stream_rate(0.0f), _flow_control_enabled(true), + _message_buffer({}), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) @@ -261,7 +284,6 @@ Mavlink::Mavlink() : errx(1, "instance ID is out of range"); break; } - } Mavlink::~Mavlink() @@ -394,6 +416,18 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self) return false; } +void +Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self) +{ + + Mavlink *inst; + LL_FOREACH(_mavlink_instances, inst) { + if (inst != self) { + inst->pass_message(msg); + } + } +} + int Mavlink::get_uart_fd(unsigned index) { @@ -463,11 +497,13 @@ void Mavlink::mavlink_update_system(void) static param_t param_system_id; static param_t param_component_id; static param_t param_system_type; + static param_t param_use_hil_gps; if (!initialized) { param_system_id = param_find("MAV_SYS_ID"); param_component_id = param_find("MAV_COMP_ID"); param_system_type = param_find("MAV_TYPE"); + param_use_hil_gps = param_find("MAV_USEHILGPS"); initialized = true; } @@ -492,6 +528,11 @@ void Mavlink::mavlink_update_system(void) if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) { mavlink_system.type = system_type; } + + int32_t use_hil_gps; + param_get(param_use_hil_gps, &use_hil_gps); + + _use_hil_gps = (bool)use_hil_gps; } int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb) @@ -550,6 +591,11 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * /* open uart */ _uart_fd = open(uart_name, O_RDWR | O_NOCTTY); + if (_uart_fd < 0) { + return _uart_fd; + } + + /* Try to set baud rate */ struct termios uart_config; int termios_state; @@ -690,9 +736,9 @@ int Mavlink::mavlink_pm_send_param(param_t param) if (param == PARAM_INVALID) { return 1; } /* buffers for param transmission */ - static char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN]; + char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN]; float val_buf; - static mavlink_message_t tx_msg; + mavlink_message_t tx_msg; /* query parameter type */ param_type_t type = param_type(param); @@ -808,10 +854,10 @@ void Mavlink::publish_mission() { /* Initialize mission publication if necessary */ if (_mission_pub < 0) { - _mission_pub = orb_advertise(ORB_ID(mission), &mission); + _mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission); } else { - orb_publish(ORB_ID(mission), _mission_pub, &mission); + orb_publish(ORB_ID(offboard_mission), _mission_pub, &mission); } } @@ -1486,6 +1532,8 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) void Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg) { + uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN]; + uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg); mavlink_send_uart_bytes(_channel, missionlib_msg_buf, len); @@ -1498,6 +1546,8 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string) { const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN; mavlink_statustext_t statustext; + statustext.severity = MAV_SEVERITY_INFO; + int i = 0; while (i < len - 1) { @@ -1617,6 +1667,125 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate) } int +Mavlink::message_buffer_init(int size) +{ + _message_buffer.size = size; + _message_buffer.write_ptr = 0; + _message_buffer.read_ptr = 0; + _message_buffer.data = (char*)malloc(_message_buffer.size); + return (_message_buffer.data == 0) ? ERROR : OK; +} + +void +Mavlink::message_buffer_destroy() +{ + _message_buffer.size = 0; + _message_buffer.write_ptr = 0; + _message_buffer.read_ptr = 0; + free(_message_buffer.data); +} + +int +Mavlink::message_buffer_count() +{ + int n = _message_buffer.write_ptr - _message_buffer.read_ptr; + + if (n < 0) { + n += _message_buffer.size; + } + + return n; +} + +int +Mavlink::message_buffer_is_empty() +{ + return _message_buffer.read_ptr == _message_buffer.write_ptr; +} + + +bool +Mavlink::message_buffer_write(void *ptr, int size) +{ + // bytes available to write + int available = _message_buffer.read_ptr - _message_buffer.write_ptr - 1; + + if (available < 0) { + available += _message_buffer.size; + } + + if (size > available) { + // buffer overflow + return false; + } + + char *c = (char *) ptr; + int n = _message_buffer.size - _message_buffer.write_ptr; // bytes to end of the buffer + + if (n < size) { + // message goes over end of the buffer + memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), c, n); + _message_buffer.write_ptr = 0; + + } else { + n = 0; + } + + // now: n = bytes already written + int p = size - n; // number of bytes to write + memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), &(c[n]), p); + _message_buffer.write_ptr = (_message_buffer.write_ptr + p) % _message_buffer.size; + return true; +} + +int +Mavlink::message_buffer_get_ptr(void **ptr, bool *is_part) +{ + // bytes available to read + int available = _message_buffer.write_ptr - _message_buffer.read_ptr; + + if (available == 0) { + return 0; // buffer is empty + } + + int n = 0; + + if (available > 0) { + // read pointer is before write pointer, all available bytes can be read + n = available; + *is_part = false; + + } else { + // read pointer is after write pointer, read bytes from read_ptr to end of the buffer + n = _message_buffer.size - _message_buffer.read_ptr; + *is_part = _message_buffer.write_ptr > 0; + } + + *ptr = &(_message_buffer.data[_message_buffer.read_ptr]); + return n; +} + +void +Mavlink::message_buffer_mark_read(int n) +{ + _message_buffer.read_ptr = (_message_buffer.read_ptr + n) % _message_buffer.size; +} + +void +Mavlink::pass_message(mavlink_message_t *msg) +{ + if (_passing_on) { + /* size is 8 bytes plus variable payload */ + int size = MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len; + pthread_mutex_lock(&_message_buffer_mutex); + message_buffer_write(msg, size); + pthread_mutex_unlock(&_message_buffer_mutex); + } +} + + + +int Mavlink::task_main(int argc, char *argv[]) { int ch; @@ -1632,7 +1801,7 @@ Mavlink::task_main(int argc, char *argv[]) * set error flag instead */ bool err_flag = false; - while ((ch = getopt(argc, argv, "b:r:d:m:v")) != EOF) { + while ((ch = getopt(argc, argv, "b:r:d:m:fpvw")) != EOF) { switch (ch) { case 'b': _baudrate = strtoul(optarg, NULL, 10); @@ -1672,10 +1841,22 @@ Mavlink::task_main(int argc, char *argv[]) break; + case 'f': + _forwarding_on = true; + break; + + case 'p': + _passing_on = true; + break; + case 'v': _verbose = true; break; + case 'w': + _wait_to_transmit = true; + break; + default: err_flag = true; break; @@ -1740,6 +1921,17 @@ Mavlink::task_main(int argc, char *argv[]) /* initialize mavlink text message buffering */ mavlink_logbuffer_init(&_logbuffer, 5); + /* if we are passing on mavlink messages, we need to prepare a buffer for this instance */ + if (_passing_on) { + /* initialize message buffer if multiplexing is on */ + if (OK != message_buffer_init(500)) { + errx(1, "can't allocate message buffer, exiting"); + } + + /* initialize message buffer mutex */ + pthread_mutex_init(&_message_buffer_mutex, NULL); + } + /* create the device node that's used for sending text log messages, etc. */ register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL); @@ -1766,6 +1958,8 @@ Mavlink::task_main(int argc, char *argv[]) struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->get_data(); + MavlinkCommandsStream commands_stream(this, _channel); + /* add default streams depending on mode and intervals depending on datarate */ float rate_mult = _datarate / 1000.0f; @@ -1783,6 +1977,9 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult); configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult); configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult); + configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f * rate_mult); + configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f * rate_mult); + configure_stream("DISTANCE_SENSOR", 0.5f); break; case MAVLINK_MODE_CAMERA: @@ -1826,6 +2023,9 @@ Mavlink::task_main(int argc, char *argv[]) set_hil_enabled(status->hil_state == HIL_STATE_ON); } + /* update commands stream */ + commands_stream.update(t); + /* check for requested subscriptions */ if (_subscribe_to_stream != nullptr) { if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) { @@ -1884,6 +2084,37 @@ Mavlink::task_main(int argc, char *argv[]) } } + /* pass messages from other UARTs */ + if (_passing_on) { + + bool is_part; + void *read_ptr; + + /* guard get ptr by mutex */ + pthread_mutex_lock(&_message_buffer_mutex); + int available = message_buffer_get_ptr(&read_ptr, &is_part); + pthread_mutex_unlock(&_message_buffer_mutex); + + if (available > 0) { + /* write first part of buffer */ + _mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr); + message_buffer_mark_read(available); + + /* write second part of buffer if there is some */ + if (is_part) { + /* guard get ptr by mutex */ + pthread_mutex_lock(&_message_buffer_mutex); + available = message_buffer_get_ptr(&read_ptr, &is_part); + pthread_mutex_unlock(&_message_buffer_mutex); + + _mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr); + message_buffer_mark_read(available); + } + } + } + + + perf_end(_loop_perf); } @@ -1928,6 +2159,10 @@ Mavlink::task_main(int argc, char *argv[]) /* close mavlink logging device */ close(_mavlink_fd); + if (_passing_on) { + message_buffer_destroy(); + pthread_mutex_destroy(&_message_buffer_mutex); + } /* destroy log buffer */ mavlink_logbuffer_destroy(&_logbuffer); @@ -1969,7 +2204,7 @@ Mavlink::start(int argc, char *argv[]) task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2048, + 2000, (main_t)&Mavlink::start_helper, (const char **)argv); @@ -2067,7 +2302,7 @@ Mavlink::stream(int argc, char *argv[]) static void usage() { - warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-v]"); + warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-f] [-p] [-v] [-w]"); } int mavlink_main(int argc, char *argv[]) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 5a118a0ad..c7a7d32f8 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -138,6 +138,8 @@ public: static bool instance_exists(const char *device_name, Mavlink *self); + static void forward_message(mavlink_message_t *msg, Mavlink *self); + static int get_uart_fd(unsigned index); int get_uart_fd(); @@ -153,10 +155,14 @@ public: void set_mode(enum MAVLINK_MODE); enum MAVLINK_MODE get_mode() { return _mode; } - bool get_hil_enabled() { return _hil_enabled; }; + bool get_hil_enabled() { return _hil_enabled; } + + bool get_use_hil_gps() { return _use_hil_gps; } bool get_flow_control_enabled() { return _flow_control_enabled; } + bool get_forwarding_on() { return _forwarding_on; } + /** * Handle waypoint related messages. */ @@ -196,6 +202,16 @@ public: bool _task_should_exit; /**< if true, mavlink task should exit */ + int get_mavlink_fd() { return _mavlink_fd; } + + + /* Functions for waiting to start transmission until message received. */ + void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } + bool get_has_received_messages() { return _received_messages; } + void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; } + bool get_wait_to_transmit() { return _wait_to_transmit; } + bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } + protected: Mavlink *next; @@ -209,7 +225,10 @@ private: /* states */ bool _hil_enabled; /**< Hardware In the Loop mode */ + bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */ bool _is_usb_uart; /**< Port is USB */ + bool _wait_to_transmit; /**< Wait to transmit until received messages. */ + bool _received_messages; /**< Whether we've received valid mavlink messages. */ unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ @@ -218,7 +237,6 @@ private: orb_advert_t _mission_pub; struct mission_s mission; - uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN]; MAVLINK_MODE _mode; uint8_t _mavlink_wpm_comp_id; @@ -234,6 +252,8 @@ private: mavlink_wpm_storage *_wpm; bool _verbose; + bool _forwarding_on; + bool _passing_on; int _uart_fd; int _baudrate; int _datarate; @@ -252,6 +272,18 @@ private: bool _flow_control_enabled; + struct mavlink_message_buffer { + int write_ptr; + int read_ptr; + int size; + char *data; + }; + mavlink_message_buffer _message_buffer; + + pthread_mutex_t _message_buffer_mutex; + + + /** * Send one parameter. * @@ -315,6 +347,22 @@ private: int configure_stream(const char *stream_name, const float rate); void configure_stream_threadsafe(const char *stream_name, const float rate); + int message_buffer_init(int size); + + void message_buffer_destroy(); + + int message_buffer_count(); + + int message_buffer_is_empty(); + + bool message_buffer_write(void *ptr, int size); + + int message_buffer_get_ptr(void **ptr, bool *is_part); + + void message_buffer_mark_read(int n); + + void pass_message(mavlink_message_t *msg); + static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); /** diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 4ca3840d4..79dd88657 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -72,6 +72,7 @@ #include <uORB/topics/navigation_capabilities.h> #include <drivers/drv_rc_input.h> #include <drivers/drv_pwm_output.h> +#include <drivers/drv_range_finder.h> #include "mavlink_messages.h" @@ -123,13 +124,13 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set *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; - } else if (status->main_state == MAIN_STATE_SEATBELT) { + } else if (status->main_state == MAIN_STATE_ALTCTL) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL; - } else if (status->main_state == MAIN_STATE_EASY) { + } else if (status->main_state == MAIN_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_EASY; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL; } else if (status->main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; @@ -262,22 +263,21 @@ protected: void send(const hrt_abstime t) { - if (status_sub->update(t)) { - mavlink_msg_sys_status_send(_channel, - status->onboard_control_sensors_present, - status->onboard_control_sensors_enabled, - status->onboard_control_sensors_health, - status->load * 1000.0f, - status->battery_voltage * 1000.0f, - status->battery_current * 1000.0f, - status->battery_remaining, - status->drop_rate_comm, - status->errors_comm, - status->errors_count1, - status->errors_count2, - status->errors_count3, - status->errors_count4); - } + status_sub->update(t); + mavlink_msg_sys_status_send(_channel, + status->onboard_control_sensors_present, + status->onboard_control_sensors_enabled, + status->onboard_control_sensors_health, + status->load * 1000.0f, + status->battery_voltage * 1000.0f, + status->battery_current * 1000.0f, + status->battery_remaining * 100.0f, + status->drop_rate_comm, + status->errors_comm, + status->errors_count1, + status->errors_count2, + status->errors_count3, + status->errors_count4); } }; @@ -641,6 +641,47 @@ protected: }; + +class MavlinkStreamViconPositionEstimate : public MavlinkStream +{ +public: + const char *get_name() + { + return "VICON_POSITION_ESTIMATE"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamViconPositionEstimate(); + } + +private: + MavlinkOrbSubscription *pos_sub; + struct vehicle_vicon_position_s *pos; + +protected: + void subscribe(Mavlink *mavlink) + { + pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); + pos = (struct vehicle_vicon_position_s *)pos_sub->get_data(); + } + + void send(const hrt_abstime t) + { + if (pos_sub->update(t)) { + mavlink_msg_vicon_position_estimate_send(_channel, + pos->timestamp / 1000, + pos->x, + pos->y, + pos->z, + pos->roll, + pos->pitch, + pos->yaw); + } + } +}; + + class MavlinkStreamGPSGlobalOrigin : public MavlinkStream { public: @@ -778,11 +819,11 @@ protected: void send(const hrt_abstime t) { - bool updated = status_sub->update(t); - updated |= pos_sp_triplet_sub->update(t); - updated |= act_sub->update(t); + bool updated = act_sub->update(t); + (void)pos_sp_triplet_sub->update(t); + (void)status_sub->update(t); - if (updated) { + if (updated && (status->arming_state == ARMING_STATE_ARMED)) { /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state; uint8_t mavlink_base_mode; @@ -1097,10 +1138,10 @@ protected: if (manual_sub->update(t)) { mavlink_msg_manual_control_send(_channel, mavlink_system.sysid, - manual->roll * 1000, - manual->pitch * 1000, - manual->yaw * 1000, - manual->throttle * 1000, + manual->x * 1000, + manual->y * 1000, + manual->z * 1000, + manual->r * 1000, 0); } } @@ -1253,8 +1294,6 @@ protected: { status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); status = (struct vehicle_status_s *)status_sub->get_data(); - - } void send(const hrt_abstime t) @@ -1265,11 +1304,57 @@ protected: || status->arming_state == ARMING_STATE_ARMED_ERROR) { /* send camera capture on */ - mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); + mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); } else { /* send camera capture off */ - mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); + mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); + } + } +}; + +class MavlinkStreamDistanceSensor : public MavlinkStream +{ +public: + const char *get_name() + { + return "DISTANCE_SENSOR"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamDistanceSensor(); + } + +private: + MavlinkOrbSubscription *range_sub; + struct range_finder_report *range; + +protected: + void subscribe(Mavlink *mavlink) + { + range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder)); + range = (struct range_finder_report *)range_sub->get_data(); + } + + void send(const hrt_abstime t) + { + if (range_sub->update(t)) { + + uint8_t type; + + switch (range->type) { + case RANGE_FINDER_TYPE_LASER: + type = MAV_DISTANCE_SENSOR_LASER; + break; + } + + uint8_t id = 0; + uint8_t orientation = 0; + uint8_t covariance = 20; + + mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation, + range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance); } } }; @@ -1300,5 +1385,7 @@ MavlinkStream *streams_list[] = { new MavlinkStreamAttitudeControls(), new MavlinkStreamNamedValueFloat(), new MavlinkStreamCameraCapture(), + new MavlinkStreamDistanceSensor(), + new MavlinkStreamViconPositionEstimate(), nullptr }; diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 4de722832..d432edd2b 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -88,6 +88,7 @@ MavlinkOrbSubscription::update(const hrt_abstime t) if (_updated) { orb_copy(_topic, _fd, _data); + _published = true; return true; } } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d7e300670..b03a68c07 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -88,8 +88,6 @@ static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f; MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _mavlink(parent), - _manual_sub(-1), - _global_pos_pub(-1), _local_pos_pub(-1), _attitude_pub(-1), @@ -162,6 +160,9 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) * The HIL mode is enabled by the HIL bit flag * in the system mode. Either send a set mode * COMMAND_LONG message or a SET_MODE message + * + * Accept HIL GPS messages if use_hil_gps flag is true. + * This allows to provide fake gps measurements to the system. */ if (_mavlink->get_hil_enabled()) { switch (msg->msgid) { @@ -169,10 +170,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_hil_sensor(msg); break; - case MAVLINK_MSG_ID_HIL_GPS: - handle_message_hil_gps(msg); - break; - case MAVLINK_MSG_ID_HIL_STATE_QUATERNION: handle_message_hil_state_quaternion(msg); break; @@ -181,6 +178,23 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) break; } } + + + if (_mavlink->get_hil_enabled() || (_mavlink->get_use_hil_gps() && msg->sysid == mavlink_system.sysid)) { + switch (msg->msgid) { + case MAVLINK_MSG_ID_HIL_GPS: + handle_message_hil_gps(msg); + break; + + default: + break; + } + + } + + /* If we've received a valid message, mark the flag indicating so. + This is used in the '-w' command-line flag. */ + _mavlink->set_has_received_messages(true); } void @@ -203,6 +217,12 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) _mavlink->_task_should_exit = true; } else { + + if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) { + warnx("ignoring CMD spoofed with same SYS/COMP ID"); + return; + } + struct vehicle_command_s vcmd; memset(&vcmd, 0, sizeof(vcmd)); @@ -222,12 +242,10 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) vcmd.source_component = msg->compid; vcmd.confirmation = cmd_mavlink.confirmation; - /* check if topic is advertised */ - if (_cmd_pub <= 0) { + if (_cmd_pub < 0) { _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); } else { - /* publish */ orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); } } @@ -245,6 +263,7 @@ MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg) memset(&f, 0, sizeof(f)); f.timestamp = hrt_absolute_time(); + f.flow_timestamp = flow.time_usec; f.flow_raw_x = flow.flow_x; f.flow_raw_y = flow.flow_y; f.flow_comp_x_m = flow.flow_comp_m_x; @@ -253,7 +272,7 @@ MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg) f.quality = flow.quality; f.sensor_id = flow.sensor_id; - if (_flow_pub <= 0) { + if (_flow_pub < 0) { _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); } else { @@ -287,7 +306,7 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg) vcmd.source_component = msg->compid; vcmd.confirmation = 1; - if (_cmd_pub <= 0) { + if (_cmd_pub < 0) { _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); } else { @@ -312,7 +331,7 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg) vicon_position.pitch = pos.pitch; vicon_position.yaw = pos.yaw; - if (_vicon_position_pub <= 0) { + if (_vicon_position_pub < 0) { _vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position); } else { @@ -373,7 +392,7 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message offboard_control_sp.timestamp = hrt_absolute_time(); - if (_offboard_control_sp_pub <= 0) { + if (_offboard_control_sp_pub < 0) { _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); } else { @@ -401,7 +420,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) tstatus.rxerrors = rstatus.rxerrors; tstatus.fixed = rstatus.fixed; - if (_telemetry_status_pub <= 0) { + if (_telemetry_status_pub < 0) { _telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); } else { @@ -415,47 +434,20 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) mavlink_manual_control_t man; mavlink_msg_manual_control_decode(msg, &man); - /* rc channels */ - { - struct rc_channels_s rc; - memset(&rc, 0, sizeof(rc)); - - rc.timestamp = hrt_absolute_time(); - rc.chan_count = 4; + struct manual_control_setpoint_s manual; + memset(&manual, 0, sizeof(manual)); - rc.chan[0].scaled = man.x / 1000.0f; - rc.chan[1].scaled = man.y / 1000.0f; - rc.chan[2].scaled = man.r / 1000.0f; - rc.chan[3].scaled = man.z / 1000.0f; + manual.timestamp = hrt_absolute_time(); + manual.x = man.x / 1000.0f; + manual.y = man.y / 1000.0f; + manual.r = man.r / 1000.0f; + manual.z = man.z / 1000.0f; - if (_rc_pub == 0) { - _rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); + if (_manual_pub < 0) { + _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); - } else { - orb_publish(ORB_ID(rc_channels), _rc_pub, &rc); - } - } - - /* manual control */ - { - struct manual_control_setpoint_s manual; - memset(&manual, 0, sizeof(manual)); - - /* get a copy first, to prevent altering values that are not sent by the mavlink command */ - orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &manual); - - manual.timestamp = hrt_absolute_time(); - manual.roll = man.x / 1000.0f; - manual.pitch = man.y / 1000.0f; - manual.yaw = man.r / 1000.0f; - manual.throttle = man.z / 1000.0f; - - if (_manual_pub == 0) { - _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); - - } else { - orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual); - } + } else { + orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual); } } @@ -619,11 +611,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_sensors.differential_pressure_timestamp = timestamp; /* publish combined sensor topic */ - if (_sensors_pub > 0) { - orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors); + if (_sensors_pub < 0) { + _sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); } else { - _sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); + orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors); } } @@ -638,11 +630,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_battery_status.current_a = 10.0f; hil_battery_status.discharged_mah = -1.0f; - if (_battery_pub > 0) { - orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); + if (_battery_pub < 0) { + _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); } else { - _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); + orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); } } @@ -694,11 +686,11 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) hil_gps.fix_type = gps.fix_type; hil_gps.satellites_visible = gps.satellites_visible; - if (_gps_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps); + if (_gps_pub < 0) { + _gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); } else { - _gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); + orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps); } } @@ -752,11 +744,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_attitude.pitchspeed = hil_state.pitchspeed; hil_attitude.yawspeed = hil_state.yawspeed; - if (_attitude_pub > 0) { - orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude); + if (_attitude_pub < 0) { + _attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); } else { - _attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); + orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude); } } @@ -766,7 +758,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) memset(&hil_global_pos, 0, sizeof(hil_global_pos)); hil_global_pos.timestamp = timestamp; - hil_global_pos.global_valid = true; hil_global_pos.lat = hil_state.lat; hil_global_pos.lon = hil_state.lon; hil_global_pos.alt = hil_state.alt / 1000.0f; @@ -774,30 +765,35 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_global_pos.vel_e = hil_state.vy / 100.0f; hil_global_pos.vel_d = hil_state.vz / 100.0f; hil_global_pos.yaw = hil_attitude.yaw; + hil_global_pos.eph = 2.0f; + hil_global_pos.epv = 4.0f; - if (_global_pos_pub > 0) { - orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos); + if (_global_pos_pub < 0) { + _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); } else { - _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); + orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos); } } /* local position */ { + double lat = hil_state.lat * 1e-7; + double lon = hil_state.lon * 1e-7; + if (!_hil_local_proj_inited) { _hil_local_proj_inited = true; _hil_local_alt0 = hil_state.alt / 1000.0f; - map_projection_init(hil_state.lat, hil_state.lon); + map_projection_init(&_hil_local_proj_ref, hil_state.lat, hil_state.lon); hil_local_pos.ref_timestamp = timestamp; - hil_local_pos.ref_lat = hil_state.lat; - hil_local_pos.ref_lon = hil_state.lon; + hil_local_pos.ref_lat = lat; + hil_local_pos.ref_lon = lon; hil_local_pos.ref_alt = _hil_local_alt0; } float x; float y; - map_projection_project(hil_state.lat * 1e-7, hil_state.lon * 1e-7, &x, &y); + map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y); hil_local_pos.timestamp = timestamp; hil_local_pos.xy_valid = true; hil_local_pos.z_valid = true; @@ -816,11 +812,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) 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) { - orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos); + if (_local_pos_pub < 0) { + _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos); } else { - _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos); + orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos); } } @@ -857,11 +853,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_battery_status.current_a = 10.0f; hil_battery_status.discharged_mah = -1.0f; - if (_battery_pub > 0) { - orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); + if (_battery_pub < 0) { + _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); } else { - _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); + orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); } } } @@ -885,8 +881,6 @@ MavlinkReceiver::receive_thread(void *arg) sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id()); prctl(PR_SET_NAME, thread_name, getpid()); - _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - struct pollfd fds[1]; fds[0].fd = uart_fd; fds[0].events = POLLIN; @@ -913,6 +907,11 @@ MavlinkReceiver::receive_thread(void *arg) /* handle packet with parameter component */ _mavlink->mavlink_pm_message_handler(_mavlink->get_channel(), &msg); + + if (_mavlink->get_forwarding_on()) { + /* forward any messages to other mavlink instances */ + Mavlink::forward_message(&msg, _mavlink); + } } } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 0a5a1b5c7..9ab84b58a 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -120,7 +120,6 @@ private: mavlink_status_t status; struct vehicle_local_position_s hil_local_pos; - int _manual_sub; orb_advert_t _global_pos_pub; orb_advert_t _local_pos_pub; orb_advert_t _attitude_pub; @@ -143,4 +142,5 @@ private: uint64_t _old_timestamp; bool _hil_local_proj_inited; float _hil_local_alt0; + struct map_projection_reference_s _hil_local_proj_ref; }; diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 135e1bce0..def40d9ad 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file mavlink_stream.cpp + * @file mavlink_stream.h * Mavlink messages stream definition. * * @author Anton Babushkin <anton.babushkin@me.com> diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index f09efa2e6..f532e26fe 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -42,6 +42,11 @@ SRCS += mavlink_main.cpp \ mavlink_orb_subscription.cpp \ mavlink_messages.cpp \ mavlink_stream.cpp \ - mavlink_rate_limiter.cpp + mavlink_rate_limiter.cpp \ + mavlink_commands.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink + +MAXOPTIMIZATION = -Os + +MODULE_STACKSIZE = 1024 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 9cb8e8344..b23166a5e 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -1,9 +1,6 @@ /**************************************************************************** * * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. - * Author: @author Tobias Naegeli <naegelit@student.ethz.ch> - * @author Lorenz Meier <lm@inf.ethz.ch> - * @author Anton Babushkin <anton.babushkin@me.com> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,9 +32,13 @@ ****************************************************************************/ /** - * @file mc_att_control_main.c + * @file mc_att_control_main.cpp * Multicopter attitude controller. * + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + * * The controller has two loops: P loop for angular error and PD loop for angular rate error. * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch. * For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw, @@ -71,7 +72,7 @@ #include <systemlib/err.h> #include <systemlib/perf_counter.h> #include <systemlib/systemlib.h> -#include <mathlib/mathlib.h> +#include <lib/mathlib/mathlib.h> #include <lib/geo/geo.h> /** @@ -156,8 +157,11 @@ private: param_t yaw_rate_i; param_t yaw_rate_d; param_t yaw_ff; + param_t yaw_rate_max; - param_t rc_scale_yaw; + param_t man_roll_max; + param_t man_pitch_max; + param_t man_yaw_max; } _params_handles; /**< handles for interesting parameters */ struct { @@ -166,8 +170,11 @@ private: math::Vector<3> rate_i; /**< I gain for angular rate error */ math::Vector<3> rate_d; /**< D gain for angular rate error */ float yaw_ff; /**< yaw control feed-forward */ + float yaw_rate_max; /**< max yaw rate */ - float rc_scale_yaw; + float man_roll_max; + float man_pitch_max; + float man_yaw_max; } _params; /** @@ -221,9 +228,9 @@ private: static void task_main_trampoline(int argc, char *argv[]); /** - * Main sensor collection task. + * Main attitude control task. */ - void task_main() __attribute__((noreturn)); + void task_main(); }; namespace mc_att_control @@ -272,6 +279,11 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params.rate_p.zero(); _params.rate_i.zero(); _params.rate_d.zero(); + _params.yaw_ff = 0.0f; + _params.yaw_rate_max = 0.0f; + _params.man_roll_max = 0.0f; + _params.man_pitch_max = 0.0f; + _params.man_yaw_max = 0.0f; _rates_prev.zero(); _rates_sp.zero(); @@ -294,8 +306,10 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); _params_handles.yaw_ff = param_find("MC_YAW_FF"); - - _params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); + _params_handles.yaw_rate_max = param_find("MC_YAWRATE_MAX"); + _params_handles.man_roll_max = param_find("MC_MAN_R_MAX"); + _params_handles.man_pitch_max = param_find("MC_MAN_P_MAX"); + _params_handles.man_yaw_max = param_find("MC_MAN_Y_MAX"); /* fetch initial parameter values */ parameters_update(); @@ -330,7 +344,7 @@ MulticopterAttitudeControl::parameters_update() { float v; - /* roll */ + /* roll gains */ param_get(_params_handles.roll_p, &v); _params.att_p(0) = v; param_get(_params_handles.roll_rate_p, &v); @@ -340,7 +354,7 @@ MulticopterAttitudeControl::parameters_update() param_get(_params_handles.roll_rate_d, &v); _params.rate_d(0) = v; - /* pitch */ + /* pitch gains */ param_get(_params_handles.pitch_p, &v); _params.att_p(1) = v; param_get(_params_handles.pitch_rate_p, &v); @@ -350,7 +364,7 @@ MulticopterAttitudeControl::parameters_update() param_get(_params_handles.pitch_rate_d, &v); _params.rate_d(1) = v; - /* yaw */ + /* yaw gains */ param_get(_params_handles.yaw_p, &v); _params.att_p(2) = v; param_get(_params_handles.yaw_rate_p, &v); @@ -361,8 +375,16 @@ MulticopterAttitudeControl::parameters_update() _params.rate_d(2) = v; param_get(_params_handles.yaw_ff, &_params.yaw_ff); + param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max); + _params.yaw_rate_max = math::radians(_params.yaw_rate_max); - param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw); + /* manual control scale */ + param_get(_params_handles.man_roll_max, &_params.man_roll_max); + param_get(_params_handles.man_pitch_max, &_params.man_pitch_max); + param_get(_params_handles.man_yaw_max, &_params.man_yaw_max); + _params.man_roll_max = math::radians(_params.man_roll_max); + _params.man_pitch_max = math::radians(_params.man_pitch_max); + _params.man_yaw_max = math::radians(_params.man_yaw_max); return OK; } @@ -404,7 +426,6 @@ MulticopterAttitudeControl::vehicle_manual_poll() orb_check(_manual_control_sp_sub, &updated); if (updated) { - orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp); } } @@ -466,7 +487,7 @@ MulticopterAttitudeControl::control_attitude(float dt) if (!_v_control_mode.flag_control_climb_rate_enabled) { /* pass throttle directly if not in altitude stabilized mode */ - _v_att_sp.thrust = _manual_control_sp.throttle; + _v_att_sp.thrust = _manual_control_sp.z; publish_att_sp = true; } @@ -483,24 +504,19 @@ MulticopterAttitudeControl::control_attitude(float dt) // reset_yaw_sp = true; //} } else { - float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale_yaw; - - if (_params.rc_scale_yaw > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) { - /* move yaw setpoint */ - yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale_yaw; - - if (_manual_control_sp.yaw > 0.0f) { - yaw_sp_move_rate -= YAW_DEADZONE; - - } else { - yaw_sp_move_rate += YAW_DEADZONE; - } - - yaw_sp_move_rate *= _params.rc_scale_yaw; - _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt); - _v_att_sp.R_valid = false; - publish_att_sp = true; + /* move yaw setpoint */ + yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max; + _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.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.yaw_body - _v_att.yaw); + if (yaw_offs < - yaw_offs_max) { + _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max); + + } else if (yaw_offs > yaw_offs_max) { + _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max); } + _v_att_sp.R_valid = false; + publish_att_sp = true; } /* reset yaw setpint to current position if needed */ @@ -513,8 +529,8 @@ MulticopterAttitudeControl::control_attitude(float dt) if (!_v_control_mode.flag_control_velocity_enabled) { /* update attitude setpoint if not in position control mode */ - _v_att_sp.roll_body = _manual_control_sp.roll; - _v_att_sp.pitch_body = _manual_control_sp.pitch; + _v_att_sp.roll_body = _manual_control_sp.y * _params.man_roll_max; + _v_att_sp.pitch_body = -_manual_control_sp.x * _params.man_pitch_max; _v_att_sp.R_valid = false; publish_att_sp = true; } @@ -627,6 +643,9 @@ MulticopterAttitudeControl::control_attitude(float dt) /* calculate angular rates setpoint */ _rates_sp = _params.att_p.emult(e_R); + /* limit yaw rate */ + _rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, _params.yaw_rate_max); + /* feed forward yaw setpoint rate */ _rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff; } @@ -807,7 +826,7 @@ MulticopterAttitudeControl::start() _control_task = task_spawn_cmd("mc_att_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 2048, + 2000, (main_t)&MulticopterAttitudeControl::task_main_trampoline, nullptr); diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 488107d58..ad38a0a03 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -1,9 +1,6 @@ /**************************************************************************** * * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. - * Author: @author Tobias Naegeli <naegelit@student.ethz.ch> - * @author Lorenz Meier <lm@inf.ethz.ch> - * @author Anton Babushkin <anton.babushkin@me.com> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,6 +34,10 @@ /** * @file mc_att_control_params.c * Parameters for multicopter attitude controller. + * + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> */ #include <systemlib/param/param.h> @@ -173,3 +174,44 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); * @group Multicopter Attitude Control */ PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f); + +/** + * Max yaw rate + * + * Limit for yaw rate, has effect for large rotations in autonomous mode, to avoid large control output and mixer saturation. + * + * @unit deg/s + * @min 0.0 + * @max 360.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 120.0f); + +/** + * Max manual roll + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_MAN_R_MAX, 35.0f); + +/** + * Max manual pitch + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_MAN_P_MAX, 35.0f); + +/** + * Max manual yaw rate + * + * @unit deg/s + * @min 0.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX, 120.0f); 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 78d06ba5b..09960d106 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Anton Babushkin <anton.babushkin@me.com> + * 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 @@ -40,6 +39,8 @@ * 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 <anton.babushkin@me.com> */ #include <nuttx/config.h> @@ -62,9 +63,10 @@ #include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/actuator_armed.h> #include <uORB/topics/parameter_update.h> -#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/vehicle_global_velocity_setpoint.h> +#include <uORB/topics/vehicle_local_position_setpoint.h> #include <systemlib/param/param.h> #include <systemlib/err.h> #include <systemlib/systemlib.h> @@ -114,20 +116,21 @@ private: int _params_sub; /**< notification of parameter updates */ int _manual_sub; /**< notification of manual control updates */ int _arming_sub; /**< arming status of outputs */ - int _global_pos_sub; /**< vehicle local position */ + int _local_pos_sub; /**< vehicle local position */ int _pos_sp_triplet_sub; /**< position setpoint triplet */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ - orb_advert_t _pos_sp_triplet_pub; /**< position setpoint triplet publication */ - orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint */ + orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */ + orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint publication */ struct vehicle_attitude_s _att; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ struct actuator_armed_s _arming; /**< actuator arming status */ - struct vehicle_global_position_s _global_pos; /**< vehicle global position */ + struct vehicle_local_position_s _local_pos; /**< vehicle local position */ struct position_setpoint_triplet_s _pos_sp_triplet; /**< vehicle global position setpoint triplet */ + struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position setpoint */ struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */ struct { @@ -145,23 +148,17 @@ private: param_t xy_vel_d; param_t xy_vel_max; param_t xy_ff; - param_t tilt_max; + param_t tilt_max_air; param_t land_speed; - param_t land_tilt_max; - - param_t rc_scale_pitch; - param_t rc_scale_roll; + param_t tilt_max_land; } _params_handles; /**< handles for interesting parameters */ struct { float thr_min; float thr_max; - float tilt_max; + float tilt_max_air; float land_speed; - float land_tilt_max; - - float rc_scale_pitch; - float rc_scale_roll; + float tilt_max_land; math::Vector<3> pos_p; math::Vector<3> vel_p; @@ -172,14 +169,15 @@ private: math::Vector<3> sp_offs_max; } _params; - double _lat_sp; - double _lon_sp; - float _alt_sp; + struct map_projection_reference_s _ref_pos; + float _ref_alt; + hrt_abstime _ref_timestamp; - bool _reset_lat_lon_sp; + bool _reset_pos_sp; bool _reset_alt_sp; - bool _use_global_alt; /**< switch between global (AMSL) and barometric altitudes */ + 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 */ @@ -202,9 +200,13 @@ private: static float scale_control(float ctl, float end, float dz); /** - * Reset lat/lon to current position + * Update reference for local position projection + */ + void update_ref(); + /** + * Reset position setpoint to current position */ - void reset_lat_lon_sp(); + void reset_pos_sp(); /** * Reset altitude setpoint to current altitude @@ -224,7 +226,7 @@ private: /** * Main sensor collection task. */ - void task_main() __attribute__((noreturn)); + void task_main(); }; namespace pos_control @@ -252,31 +254,32 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_sub(-1), _manual_sub(-1), _arming_sub(-1), - _global_pos_sub(-1), + _local_pos_sub(-1), _pos_sp_triplet_sub(-1), /* publications */ _att_sp_pub(-1), - _pos_sp_triplet_pub(-1), + _local_pos_sp_pub(-1), _global_vel_sp_pub(-1), - _lat_sp(0.0), - _lon_sp(0.0), - _alt_sp(0.0f), + _ref_alt(0.0f), + _ref_timestamp(0), - _reset_lat_lon_sp(true), - _reset_alt_sp(true), - _use_global_alt(false) + _reset_pos_sp(true), + _reset_alt_sp(true) { memset(&_att, 0, sizeof(_att)); memset(&_att_sp, 0, sizeof(_att_sp)); memset(&_manual, 0, sizeof(_manual)); memset(&_control_mode, 0, sizeof(_control_mode)); memset(&_arming, 0, sizeof(_arming)); - memset(&_global_pos, 0, sizeof(_global_pos)); + memset(&_local_pos, 0, sizeof(_local_pos)); memset(&_pos_sp_triplet, 0, sizeof(_pos_sp_triplet)); + memset(&_local_pos_sp, 0, sizeof(_local_pos_sp)); memset(&_global_vel_sp, 0, sizeof(_global_vel_sp)); + memset(&_ref_pos, 0, sizeof(_ref_pos)); + _params.pos_p.zero(); _params.vel_p.zero(); _params.vel_i.zero(); @@ -285,6 +288,8 @@ MulticopterPositionControl::MulticopterPositionControl() : _params.vel_ff.zero(); _params.sp_offs_max.zero(); + _pos.zero(); + _pos_sp.zero(); _vel.zero(); _vel_sp.zero(); _vel_prev.zero(); @@ -303,11 +308,9 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_handles.xy_vel_d = param_find("MPC_XY_VEL_D"); _params_handles.xy_vel_max = param_find("MPC_XY_VEL_MAX"); _params_handles.xy_ff = param_find("MPC_XY_FF"); - _params_handles.tilt_max = param_find("MPC_TILT_MAX"); + _params_handles.tilt_max_air = param_find("MPC_TILTMAX_AIR"); _params_handles.land_speed = param_find("MPC_LAND_SPEED"); - _params_handles.land_tilt_max = param_find("MPC_LAND_TILT"); - _params_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH"); - _params_handles.rc_scale_roll = param_find("RC_SCALE_ROLL"); + _params_handles.tilt_max_land = param_find("MPC_TILTMAX_LND"); /* fetch initial parameter values */ parameters_update(true); @@ -345,17 +348,18 @@ MulticopterPositionControl::parameters_update(bool force) orb_check(_params_sub, &updated); - if (updated) + if (updated) { orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_upd); + } if (updated || force) { param_get(_params_handles.thr_min, &_params.thr_min); param_get(_params_handles.thr_max, &_params.thr_max); - param_get(_params_handles.tilt_max, &_params.tilt_max); + param_get(_params_handles.tilt_max_air, &_params.tilt_max_air); + _params.tilt_max_air = math::radians(_params.tilt_max_air); param_get(_params_handles.land_speed, &_params.land_speed); - param_get(_params_handles.land_tilt_max, &_params.land_tilt_max); - param_get(_params_handles.rc_scale_pitch, &_params.rc_scale_pitch); - param_get(_params_handles.rc_scale_roll, &_params.rc_scale_roll); + param_get(_params_handles.tilt_max_land, &_params.tilt_max_land); + _params.tilt_max_land = math::radians(_params.tilt_max_land); float v; param_get(_params_handles.xy_p, &v); @@ -402,33 +406,39 @@ MulticopterPositionControl::poll_subscriptions() orb_check(_att_sub, &updated); - if (updated) + if (updated) { orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + } orb_check(_att_sp_sub, &updated); - if (updated) + if (updated) { orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp); + } orb_check(_control_mode_sub, &updated); - if (updated) + if (updated) { orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + } orb_check(_manual_sub, &updated); - if (updated) + if (updated) { orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual); + } orb_check(_arming_sub, &updated); - if (updated) + if (updated) { orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming); + } - orb_check(_global_pos_sub, &updated); + orb_check(_local_pos_sub, &updated); - if (updated) - orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); + if (updated) { + orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos); + } } float @@ -452,40 +462,50 @@ MulticopterPositionControl::task_main_trampoline(int argc, char *argv[]) } void -MulticopterPositionControl::reset_lat_lon_sp() +MulticopterPositionControl::update_ref() { - if (_reset_lat_lon_sp) { - _reset_lat_lon_sp = false; - _lat_sp = _global_pos.lat; - _lon_sp = _global_pos.lon; - mavlink_log_info(_mavlink_fd, "[mpc] reset lat/lon sp: %.7f, %.7f", _lat_sp, _lon_sp); + if (_local_pos.ref_timestamp != _ref_timestamp) { + double lat_sp, lon_sp; + float alt_sp; + + 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.ref_lat, _local_pos.ref_lon); + _ref_alt = _local_pos.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.ref_timestamp; } } void -MulticopterPositionControl::reset_alt_sp() +MulticopterPositionControl::reset_pos_sp() { - if (_reset_alt_sp) { - _reset_alt_sp = false; - _alt_sp = _use_global_alt ? _global_pos.alt : _global_pos.baro_alt; - mavlink_log_info(_mavlink_fd, "[mpc] reset alt (%s) sp: %.2f", _use_global_alt ? "AMSL" : "baro", (double)_alt_sp); + if (_reset_pos_sp) { + _reset_pos_sp = false; + _pos_sp(0) = _pos(0); + _pos_sp(1) = _pos(1); + mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1)); } } void -MulticopterPositionControl::select_alt(bool global) +MulticopterPositionControl::reset_alt_sp() { - if (global != _use_global_alt) { - _use_global_alt = global; - - if (global) { - /* switch from barometric to global altitude */ - _alt_sp += _global_pos.alt - _global_pos.baro_alt; - - } else { - /* switch from global to barometric altitude */ - _alt_sp += _global_pos.baro_alt - _global_pos.alt; - } + if (_reset_alt_sp) { + _reset_alt_sp = false; + _pos_sp(2) = _pos(2); + mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2)); } } @@ -506,7 +526,7 @@ MulticopterPositionControl::task_main() _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); - _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); parameters_update(true); @@ -537,8 +557,7 @@ MulticopterPositionControl::task_main() /* wakeup source */ struct pollfd fds[1]; - /* Setup of loop */ - fds[0].fd = _global_pos_sub; + fds[0].fd = _local_pos_sub; fds[0].events = POLLIN; while (!_task_should_exit) { @@ -546,8 +565,9 @@ MulticopterPositionControl::task_main() int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500); /* timed out - periodic check for _task_should_exit */ - if (pret == 0) + if (pret == 0) { continue; + } /* this is undesirable but not much we can do */ if (pret < 0) { @@ -564,7 +584,7 @@ MulticopterPositionControl::task_main() if (_control_mode.flag_armed && !was_armed) { /* reset setpoints and integrals on arming */ - _reset_lat_lon_sp = true; + _reset_pos_sp = true; _reset_alt_sp = true; reset_int_z = true; reset_int_xy = true; @@ -572,44 +592,41 @@ MulticopterPositionControl::task_main() was_armed = _control_mode.flag_armed; + update_ref(); + if (_control_mode.flag_control_altitude_enabled || _control_mode.flag_control_position_enabled || _control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) { - _vel(0) = _global_pos.vel_n; - _vel(1) = _global_pos.vel_e; - _vel(2) = _global_pos.vel_d; + _pos(0) = _local_pos.x; + _pos(1) = _local_pos.y; + _pos(2) = _local_pos.z; - sp_move_rate.zero(); + _vel(0) = _local_pos.vx; + _vel(1) = _local_pos.vy; + _vel(2) = _local_pos.vz; - float alt = _global_pos.alt; + sp_move_rate.zero(); /* select control source */ if (_control_mode.flag_control_manual_enabled) { - /* select altitude source and update setpoint */ - select_alt(_global_pos.global_valid); - - if (!_use_global_alt) { - alt = _global_pos.baro_alt; - } - /* manual control */ if (_control_mode.flag_control_altitude_enabled) { /* reset alt setpoint to current altitude if needed */ reset_alt_sp(); /* move altitude setpoint with throttle stick */ - sp_move_rate(2) = -scale_control(_manual.throttle - 0.5f, 0.5f, alt_ctl_dz); + sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz); } if (_control_mode.flag_control_position_enabled) { - /* reset lat/lon setpoint to current position if needed */ - reset_lat_lon_sp(); + /* reset position setpoint to current position if needed */ + reset_pos_sp(); /* move position setpoint with roll/pitch stick */ - sp_move_rate(0) = scale_control(-_manual.pitch / _params.rc_scale_pitch, 1.0f, pos_ctl_dz); - sp_move_rate(1) = scale_control(_manual.roll / _params.rc_scale_roll, 1.0f, pos_ctl_dz); + sp_move_rate(0) = _manual.x; + sp_move_rate(1) = _manual.y; } /* limit setpoint move rate */ @@ -625,74 +642,47 @@ MulticopterPositionControl::task_main() sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max); /* move position setpoint */ - add_vector_to_global_position(_lat_sp, _lon_sp, sp_move_rate(0) * dt, sp_move_rate(1) * dt, &_lat_sp, &_lon_sp); - _alt_sp -= sp_move_rate(2) * dt; + _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.flag_control_position_enabled) { - get_vector_to_next_waypoint_fast(_global_pos.lat, _global_pos.lon, _lat_sp, _lon_sp, &pos_sp_offs.data[0], &pos_sp_offs.data[1]); - pos_sp_offs(0) /= _params.sp_offs_max(0); - pos_sp_offs(1) /= _params.sp_offs_max(1); + 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.flag_control_altitude_enabled) { - pos_sp_offs(2) = -(_alt_sp - alt) / _params.sp_offs_max(2); + 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; - add_vector_to_global_position(_global_pos.lat, _global_pos.lon, pos_sp_offs(0) * _params.sp_offs_max(0), pos_sp_offs(1) * _params.sp_offs_max(1), &_lat_sp, &_lon_sp); - _alt_sp = alt - pos_sp_offs(2) * _params.sp_offs_max(2); - } - - /* fill position setpoint triplet */ - _pos_sp_triplet.previous.valid = true; - _pos_sp_triplet.current.valid = true; - _pos_sp_triplet.next.valid = true; - - _pos_sp_triplet.nav_state = NAV_STATE_NONE; - _pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL; - _pos_sp_triplet.current.lat = _lat_sp; - _pos_sp_triplet.current.lon = _lon_sp; - _pos_sp_triplet.current.alt = _alt_sp; - _pos_sp_triplet.current.yaw = _att_sp.yaw_body; - _pos_sp_triplet.current.loiter_radius = 0.0f; - _pos_sp_triplet.current.loiter_direction = 1.0f; - _pos_sp_triplet.current.pitch_min = 0.0f; - - /* publish position setpoint triplet */ - if (_pos_sp_triplet_pub > 0) { - orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet); - - } else { - _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet); + _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); } } else { - /* always use AMSL altitude for AUTO */ - select_alt(true); - /* AUTO */ bool updated; orb_check(_pos_sp_triplet_sub, &updated); - if (updated) + if (updated) { orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); + } if (_pos_sp_triplet.current.valid) { /* in case of interrupted mission don't go to waypoint but stay at current position */ - _reset_lat_lon_sp = true; + _reset_pos_sp = true; _reset_alt_sp = true; - /* update position setpoint */ - _lat_sp = _pos_sp_triplet.current.lat; - _lon_sp = _pos_sp_triplet.current.lon; - _alt_sp = _pos_sp_triplet.current.alt; + /* project setpoint to local frame */ + map_projection_project(&_ref_pos, + _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon, + &_pos_sp.data[0], &_pos_sp.data[1]); + _pos_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt); /* update yaw setpoint if needed */ if (isfinite(_pos_sp_triplet.current.yaw)) { @@ -701,11 +691,25 @@ MulticopterPositionControl::task_main() } else { /* no waypoint, loiter, reset position setpoint if needed */ - reset_lat_lon_sp(); + reset_pos_sp(); reset_alt_sp(); } } + /* fill local position setpoint */ + _local_pos_sp.x = _pos_sp(0); + _local_pos_sp.y = _pos_sp(1); + _local_pos_sp.z = _pos_sp(2); + _local_pos_sp.yaw = _att_sp.yaw_body; + + /* publish local position setpoint */ + if (_local_pos_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp); + + } else { + _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp); + } + if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) { /* idle state, don't run controller and set zero thrust */ R.identity(); @@ -729,9 +733,7 @@ MulticopterPositionControl::task_main() } else { /* run position & altitude controllers, calculate velocity setpoint */ - math::Vector<3> pos_err; - get_vector_to_next_waypoint_fast(_global_pos.lat, _global_pos.lon, _lat_sp, _lon_sp, &pos_err.data[0], &pos_err.data[1]); - pos_err(2) = -(_alt_sp - alt); + math::Vector<3> pos_err = _pos_sp - _pos; _vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff); @@ -741,7 +743,7 @@ MulticopterPositionControl::task_main() } if (!_control_mode.flag_control_position_enabled) { - _reset_lat_lon_sp = true; + _reset_pos_sp = true; _vel_sp(0) = 0.0f; _vel_sp(1) = 0.0f; } @@ -780,7 +782,7 @@ MulticopterPositionControl::task_main() float i = _params.thr_min; if (reset_int_z_manual) { - i = _manual.throttle; + i = _manual.z; if (i < _params.thr_min) { i = _params.thr_min; @@ -839,16 +841,17 @@ MulticopterPositionControl::task_main() thr_min = 0.0f; } - float tilt_max = _params.tilt_max; + float tilt_max = _params.tilt_max_air; /* 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) { /* limit max tilt and min lift when landing */ - tilt_max = _params.land_tilt_max; + tilt_max = _params.tilt_max_land; - if (thr_min < 0.0f) + if (thr_min < 0.0f) { thr_min = 0.0f; + } } /* limit min lift */ @@ -939,8 +942,9 @@ MulticopterPositionControl::task_main() thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt; /* protection against flipping on ground when landing */ - if (thrust_int(2) > 0.0f) + if (thrust_int(2) > 0.0f) { thrust_int(2) = 0.0f; + } } /* calculate attitude setpoint from thrust vector */ @@ -999,6 +1003,18 @@ MulticopterPositionControl::task_main() _att_sp.roll_body = euler(0); _att_sp.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.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.yaw_body); + + /* copy rotation matrix to attitude setpoint topic */ + memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); + _att_sp.R_valid = true; + + _att_sp.roll_body = 0.0f; + _att_sp.pitch_body = 0.0f; } _att_sp.thrust = thrust_abs; @@ -1021,7 +1037,7 @@ MulticopterPositionControl::task_main() } else { /* position controller disabled, reset setpoints */ _reset_alt_sp = true; - _reset_lat_lon_sp = true; + _reset_pos_sp = true; reset_int_z = true; reset_int_xy = true; } @@ -1046,7 +1062,7 @@ MulticopterPositionControl::start() _control_task = task_spawn_cmd("mc_pos_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 2048, + 2000, (main_t)&MulticopterPositionControl::task_main_trampoline, nullptr); @@ -1060,18 +1076,21 @@ MulticopterPositionControl::start() int mc_pos_control_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 1) { errx(1, "usage: mc_pos_control {start|stop|status}"); + } if (!strcmp(argv[1], "start")) { - if (pos_control::g_control != nullptr) + if (pos_control::g_control != nullptr) { errx(1, "already running"); + } pos_control::g_control = new MulticopterPositionControl; - if (pos_control::g_control == nullptr) + if (pos_control::g_control == nullptr) { errx(1, "alloc failed"); + } if (OK != pos_control::g_control->start()) { delete pos_control::g_control; @@ -1083,8 +1102,9 @@ int mc_pos_control_main(int argc, char *argv[]) } if (!strcmp(argv[1], "stop")) { - if (pos_control::g_control == nullptr) + if (pos_control::g_control == nullptr) { errx(1, "not running"); + } delete pos_control::g_control; pos_control::g_control = nullptr; diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 0082a5e6a..05ab5769b 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -35,6 +35,8 @@ /** * @file mc_pos_control_params.c * Multicopter position controller parameters. + * + * @author Anton Babushkin <anton.babushkin@me.com> */ #include <systemlib/param/param.h> @@ -98,8 +100,9 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); /** * Maximum vertical velocity * - * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (SEATBELT, EASY). + * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTCTRL, POSCTRL). * + * @unit m/s * @min 0.0 * @group Multicopter Position Control */ @@ -108,7 +111,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f); /** * Vertical velocity feed forward * - * Feed forward weight for altitude control in stabilized modes (SEATBELT, EASY). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * Feed forward weight for altitude control in stabilized modes (ALTCTRL, POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. * * @min 0.0 * @max 1.0 @@ -153,8 +156,9 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f); /** * Maximum horizontal velocity * - * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (EASY). + * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSCTRL). * + * @unit m/s * @min 0.0 * @group Multicopter Position Control */ @@ -163,7 +167,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); /** * Horizontal velocity feed forward * - * Feed forward weight for position control in position control mode (EASY). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * 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 @@ -172,31 +176,35 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f); /** - * Maximum tilt + * Maximum tilt angle in air * - * Limits maximum tilt in AUTO and EASY modes. + * Limits maximum tilt in AUTO and POSCTRL modes during flight. * + * @unit deg * @min 0.0 - * @max 1.57 + * @max 90.0 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 1.0f); +PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR, 45.0f); /** - * Landing descend rate + * Maximum tilt during landing * + * Limits maximum tilt angle on landing. + * + * @unit deg * @min 0.0 + * @max 90.0 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 1.0f); +PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 15.0f); /** - * Maximum landing tilt - * - * Limits maximum tilt on landing. + * Landing descend rate * + * @unit m/s * @min 0.0 - * @max 1.57 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_LAND_TILT, 0.3f); +PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 1.0f); + diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index f452a85f7..bc8dbca50 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -1,8 +1,6 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Jean Cyr <jean.m.cyr@gmail.com> - * @author Thomas Gubler <thomasgubler@gmail.com> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +33,9 @@ /** * @file geofence.cpp * Provides functions for handling the geofence + * + * @author Jean Cyr <jean.m.cyr@gmail.com> + * @author Thomas Gubler <thomasgubler@gmail.com> */ #include "geofence.h" diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 9628b7271..2eb126ab5 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -1,8 +1,6 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Jean Cyr <jean.m.cyr@gmail.com> - * @author Thomas Gubler <thomasgubler@gmail.com> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +33,9 @@ /** * @file geofence.h * Provides functions for handling the geofence + * + * @author Jean Cyr <jean.m.cyr@gmail.com> + * @author Thomas Gubler <thomasgubler@gmail.com> */ #ifndef GEOFENCE_H_ diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index 5831a0ca9..653b1ad84 100644 --- a/src/modules/navigator/geofence_params.c +++ b/src/modules/navigator/geofence_params.c @@ -1,7 +1,6 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier <lm@inf.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index eaafa217d..2f45f2267 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -1,8 +1,6 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> - * @author Thomas Gubler <thomasgubler@student.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +33,9 @@ /** * @file mission_feasibility_checker.cpp * Provides checks if mission is feasible given the navigation capabilities + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Thomas Gubler <thomasgubler@student.ethz.ch> */ #include "mission_feasibility_checker.h" @@ -100,8 +101,8 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss /* Check if all mission items are inside the geofence (if we have a valid geofence) */ if (geofence.valid()) { for (size_t i = 0; i < nMissionItems; i++) { - static struct mission_item_s missionitem; - const ssize_t len = sizeof(struct mission_item_s); + struct mission_item_s missionitem; + const ssize_t len = sizeof(missionitem); if (dm_read(dm_current, i, &missionitem, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ @@ -125,8 +126,8 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size for (size_t i = 0; i < nMissionItems; i++) { - static struct mission_item_s missionitem; - const ssize_t len = sizeof(struct mission_item_s); + struct mission_item_s missionitem; + const ssize_t len = sizeof(missionitem); if (dm_read(dm_current, i, &missionitem, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ return false; diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 7a0b2a296..f98e28462 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -1,8 +1,6 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> - * @author Thomas Gubler <thomasgubler@student.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,7 +33,11 @@ /** * @file mission_feasibility_checker.h * Provides checks if mission is feasible given the navigation capabilities + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Thomas Gubler <thomasgubler@student.ethz.ch> */ + #ifndef MISSION_FEASIBILITY_CHECKER_H_ #define MISSION_FEASIBILITY_CHECKER_H_ diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index 55f8a4caa..6ea9dec2b 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -45,3 +45,5 @@ SRCS = navigator_main.cpp \ geofence_params.c INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink + +MODULE_STACKSIZE = 1200 diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c45cafc1b..87c893079 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1,10 +1,6 @@ /**************************************************************************** * * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> - * @author Jean Cyr <jean.m.cyr@gmail.com> - * @author Julian Oes <joes@student.ethz.ch> - * @author Anton Babushkin <anton.babushkin@me.com> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,7 +31,7 @@ * ****************************************************************************/ /** - * @file navigator_main.c + * @file navigator_main.cpp * Implementation of the main navigation state machine. * * Handles missions, geo fencing and failsafe navigation behavior. @@ -177,7 +173,7 @@ private: class Mission _mission; bool _mission_item_valid; /**< current mission item valid */ - bool _global_pos_valid; /**< track changes of global_position.global_valid flag */ + bool _global_pos_valid; /**< track changes of global_position */ bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */ bool _waypoint_position_reached; bool _waypoint_yaw_reached; @@ -510,7 +506,7 @@ Navigator::offboard_mission_update(bool isrotaryWing) { struct mission_s offboard_mission; - if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) { + if (orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &offboard_mission) == OK) { /* Check mission feasibility, for now do not handle the return value, * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ @@ -543,7 +539,7 @@ Navigator::onboard_mission_update() { struct mission_s onboard_mission; - if (orb_copy(ORB_ID(mission), _onboard_mission_sub, &onboard_mission) == OK) { + if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) { _mission.set_onboard_mission_count(onboard_mission.count); _mission.set_current_onboard_mission_index(onboard_mission.current_index); @@ -611,7 +607,7 @@ Navigator::task_main() * do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - _offboard_mission_sub = orb_subscribe(ORB_ID(mission)); + _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); @@ -690,84 +686,56 @@ Navigator::task_main() if (fds[6].revents & POLLIN) { vehicle_status_update(); - /* evaluate state machine from commander and set the navigator mode accordingly */ + /* evaluate state requested by commander */ if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { - bool stick_mode = false; - - if (!_vstatus.rc_signal_lost) { - /* RC signal available, use control switches to set mode */ - /* RETURN switch, overrides MISSION switch */ - if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { - /* switch to RTL if not already landed after RTL and home position set */ - if (!(_rtl_state == RTL_STATE_DESCEND && - (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && - _vstatus.condition_home_position_valid) { - dispatch(EVENT_RTL_REQUESTED); - } + /* publish position setpoint triplet on each status update if navigator active */ + _pos_sp_triplet_updated = true; - stick_mode = true; + if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { + /* commander requested new navigation mode, try to set it */ + switch (_vstatus.set_nav_state) { + case NAV_STATE_NONE: + /* nothing to do */ + break; - } else { - /* MISSION switch */ - if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { - request_loiter_or_ready(); - stick_mode = true; + case NAV_STATE_LOITER: + request_loiter_or_ready(); + break; - } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { - request_mission_if_available(); - stick_mode = true; - } + case NAV_STATE_MISSION: + request_mission_if_available(); + break; - if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { - /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ - request_mission_if_available(); - stick_mode = true; + case NAV_STATE_RTL: + if (!(_rtl_state == RTL_STATE_DESCEND && + (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && + _vstatus.condition_home_position_valid) { + dispatch(EVENT_RTL_REQUESTED); } - } - } - - if (!stick_mode) { - if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { - /* commander requested new navigation mode, try to set it */ - _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; - - switch (_vstatus.set_nav_state) { - case NAV_STATE_NONE: - /* nothing to do */ - break; - - case NAV_STATE_LOITER: - request_loiter_or_ready(); - break; - - case NAV_STATE_MISSION: - request_mission_if_available(); - break; - case NAV_STATE_RTL: - if (!(_rtl_state == RTL_STATE_DESCEND && - (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && - _vstatus.condition_home_position_valid) { - dispatch(EVENT_RTL_REQUESTED); - } + break; - break; + case NAV_STATE_LAND: + dispatch(EVENT_LAND_REQUESTED); - case NAV_STATE_LAND: - dispatch(EVENT_LAND_REQUESTED); + break; - break; + default: + warnx("ERROR: Requested navigation state not supported"); + break; + } - default: - warnx("ERROR: Requested navigation state not supported"); - break; - } + } else { + /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ + if (myState == NAV_STATE_NONE) { + request_mission_if_available(); + } + } - } else { - /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ - if (myState == NAV_STATE_NONE) { - request_mission_if_available(); - } + /* check if waypoint has been reached in MISSION, RTL and LAND modes */ + if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) { + if (check_mission_item_reached()) { + on_mission_item_reached(); } } @@ -775,6 +743,8 @@ Navigator::task_main() /* navigator shouldn't act */ dispatch(EVENT_NONE_REQUESTED); } + + _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; } /* parameters updated */ @@ -813,17 +783,15 @@ Navigator::task_main() if (fds[1].revents & POLLIN) { global_position_update(); - /* publish position setpoint triplet on each position update if navigator active */ if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { + /* publish position setpoint triplet on each position update if navigator active */ _pos_sp_triplet_updated = true; - if (myState == NAV_STATE_LAND && _global_pos.global_valid && !_global_pos_valid) { + if (myState == NAV_STATE_LAND && !_global_pos_valid) { /* got global position when landing, update setpoint */ start_land(); } - _global_pos_valid = _global_pos.global_valid; - /* check if waypoint has been reached in MISSION, RTL and LAND modes */ if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) { if (check_mission_item_reached()) { @@ -848,6 +816,8 @@ Navigator::task_main() } } + _global_pos_valid = _vstatus.condition_global_position_valid; + /* publish position setpoint triplet if updated */ if (_pos_sp_triplet_updated) { _pos_sp_triplet_updated = false; @@ -878,7 +848,7 @@ Navigator::start() _navigator_task = task_spawn_cmd("navigator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 2048, + 2000, (main_t)&Navigator::task_main_trampoline, nullptr); @@ -893,9 +863,9 @@ Navigator::start() void Navigator::status() { - warnx("Global position is %svalid", _global_pos.global_valid ? "" : "in"); + warnx("Global position: %svalid", _global_pos_valid ? "" : "in"); - if (_global_pos.global_valid) { + if (_global_pos_valid) { warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat); warnx("Altitude %5.5f meters, altitude above home %5.5f meters", (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt)); @@ -1317,7 +1287,7 @@ Navigator::set_rtl_item() _mission_item.yaw = NAN; _mission_item.loiter_radius = _parameters.loiter_radius; _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_TAKEOFF; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.acceptance_radius = _parameters.acceptance_radius; _mission_item.time_inside = 0.0f; _mission_item.pitch_min = 0.0f; @@ -1377,7 +1347,7 @@ Navigator::set_rtl_item() _mission_item.yaw = NAN; _mission_item.loiter_radius = _parameters.loiter_radius; _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; _mission_item.acceptance_radius = _parameters.acceptance_radius; _mission_item.time_inside = _parameters.rtl_land_delay < 0.0f ? 0.0f : _parameters.rtl_land_delay; _mission_item.pitch_min = 0.0f; @@ -1539,7 +1509,7 @@ Navigator::check_mission_item_reached() /* check yaw if defined only for rotary wing except takeoff */ float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw); - if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */ + if (fabsf(yaw_err) < 0.2f) { /* XXX get rid of magic number */ _waypoint_yaw_reached = true; } diff --git a/src/modules/navigator/navigator_mission.cpp b/src/modules/navigator/navigator_mission.cpp index 72dddebfe..49fc62785 100644 --- a/src/modules/navigator/navigator_mission.cpp +++ b/src/modules/navigator/navigator_mission.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Julian Oes <joes@student.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,6 +33,8 @@ /** * @file navigator_mission.cpp * Helper class to access missions + * + * @author Julian Oes <joes@student.ethz.ch> */ #include <string.h> diff --git a/src/modules/navigator/navigator_mission.h b/src/modules/navigator/navigator_mission.h index 2bd4da82e..b0f88e016 100644 --- a/src/modules/navigator/navigator_mission.h +++ b/src/modules/navigator/navigator_mission.h @@ -1,7 +1,6 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Julian Oes <joes@student.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,6 +33,8 @@ /** * @file navigator_mission.h * Helper class to access missions + * + * @author Julian Oes <joes@student.ethz.ch> */ #ifndef NAVIGATOR_MISSION_H diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 9ef359c6d..5139283b6 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -1,9 +1,6 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> - * @author Anton Babushkin <anton.babushkin@me.com> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/navigator/navigator_state.h b/src/modules/navigator/navigator_state.h index 6a1475c9b..476f93414 100644 --- a/src/modules/navigator/navigator_state.h +++ b/src/modules/navigator/navigator_state.h @@ -1,8 +1,42 @@ -/* - * navigator_state.h +/**************************************************************************** * - * Created on: 27.01.2014 - * Author: ton + * 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 navigator_state.h + * + * Navigator state + * + * @author Anton Babushkin <anton.babushkin@me.com> */ #ifndef NAVIGATOR_STATE_H_ diff --git a/src/modules/position_estimator/module.mk b/src/modules/position_estimator/module.mk deleted file mode 100644 index f64095d9d..000000000 --- a/src/modules/position_estimator/module.mk +++ /dev/null @@ -1,44 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 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. -# -############################################################################ - -# -# Makefile to build the position estimator -# - -MODULE_COMMAND = position_estimator - -# XXX this should be converted to a deamon, its a pretty bad example app -MODULE_PRIORITY = SCHED_PRIORITY_DEFAULT -MODULE_STACKSIZE = 4096 - -SRCS = position_estimator_main.c diff --git a/src/modules/position_estimator/position_estimator_main.c b/src/modules/position_estimator/position_estimator_main.c deleted file mode 100644 index e84945299..000000000 --- a/src/modules/position_estimator/position_estimator_main.c +++ /dev/null @@ -1,423 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli <naegelit@student.ethz.ch> - * Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> - * Lorenz Meier <lm@inf.ethz.ch> - * - * 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_main.c - * Model-identification based position estimator for multirotors - */ - -#include <nuttx/config.h> -#include <unistd.h> -#include <stdlib.h> -#include <stdio.h> -#include <stdbool.h> -#include <fcntl.h> -#include <float.h> -#include <nuttx/sched.h> -#include <sys/prctl.h> -#include <termios.h> -#include <errno.h> -#include <limits.h> -#include <math.h> -#include <uORB/uORB.h> -#include <uORB/topics/vehicle_status.h> -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_gps_position.h> -#include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/vehicle_local_position.h> -#include <poll.h> - -#define N_STATES 6 -#define ERROR_COVARIANCE_INIT 3 -#define R_EARTH 6371000.0 - -#define PROJECTION_INITIALIZE_COUNTER_LIMIT 5000 -#define REPROJECTION_COUNTER_LIMIT 125 - -__EXPORT int position_estimator_main(int argc, char *argv[]); - -static uint16_t position_estimator_counter_position_information; - -/* values for map projection */ -static double phi_1; -static double sin_phi_1; -static double cos_phi_1; -static double lambda_0; -static double scale; - -/** - * Initializes the map transformation. - * - * Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane - * @param lat in degrees (47.1234567°, not 471234567°) - * @param lon in degrees (8.1234567°, not 81234567°) - */ -static void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 -{ - /* notation and formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ - phi_1 = lat_0 / 180.0 * M_PI; - lambda_0 = lon_0 / 180.0 * M_PI; - - sin_phi_1 = sin(phi_1); - cos_phi_1 = cos(phi_1); - - /* calculate local scale by using the relation of true distance and the distance on plane */ //TODO: this is a quick solution, there are probably easier ways to determine the scale - - /* 1) calculate true distance d on sphere to a point: http://www.movable-type.co.uk/scripts/latlong.html */ - const double r_earth = 6371000; - - double lat1 = phi_1; - double lon1 = lambda_0; - - double lat2 = phi_1 + 0.5 / 180 * M_PI; - double lon2 = lambda_0 + 0.5 / 180 * M_PI; - double sin_lat_2 = sin(lat2); - double cos_lat_2 = cos(lat2); - double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * r_earth; - - /* 2) calculate distance rho on plane */ - double k_bar = 0; - double c = acos(sin_phi_1 * sin_lat_2 + cos_phi_1 * cos_lat_2 * cos(lon2 - lambda_0)); - - if (0 != c) - k_bar = c / sin(c); - - double x2 = k_bar * (cos_lat_2 * sin(lon2 - lambda_0)); //Projection of point 2 on plane - double y2 = k_bar * ((cos_phi_1 * sin_lat_2 - sin_phi_1 * cos_lat_2 * cos(lon2 - lambda_0))); - double rho = sqrt(pow(x2, 2) + pow(y2, 2)); - - scale = d / rho; - -} - -/** - * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane - * @param x north - * @param y east - * @param lat in degrees (47.1234567°, not 471234567°) - * @param lon in degrees (8.1234567°, not 81234567°) - */ -static void map_projection_project(double lat, double lon, float *x, float *y) -{ - /* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ - double phi = lat / 180.0 * M_PI; - double lambda = lon / 180.0 * M_PI; - - double sin_phi = sin(phi); - double cos_phi = cos(phi); - - double k_bar = 0; - /* using small angle approximation (formula in comment is without aproximation) */ - double c = acos(sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2)); //double c = acos( sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * cos(lambda - lambda_0) ); - - if (0 != c) - k_bar = c / sin(c); - - /* using small angle approximation (formula in comment is without aproximation) */ - *y = k_bar * (cos_phi * (lambda - lambda_0)) * scale;//*y = k_bar * (cos_phi * sin(lambda - lambda_0)) * scale; - *x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2))) * scale; // *x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * cos(lambda - lambda_0))) * scale; - -// printf("%phi_1=%.10f, lambda_0 =%.10f\n", phi_1, lambda_0); -} - -/** - * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system - * - * @param x north - * @param y east - * @param lat in degrees (47.1234567°, not 471234567°) - * @param lon in degrees (8.1234567°, not 81234567°) - */ -static void map_projection_reproject(float x, float y, double *lat, double *lon) -{ - /* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ - - double x_descaled = x / scale; - double y_descaled = y / scale; - - double c = sqrt(pow(x_descaled, 2) + pow(y_descaled, 2)); - double sin_c = sin(c); - double cos_c = cos(c); - - double lat_sphere = 0; - - if (c != 0) - lat_sphere = asin(cos_c * sin_phi_1 + (x_descaled * sin_c * cos_phi_1) / c); - else - lat_sphere = asin(cos_c * sin_phi_1); - -// printf("lat_sphere = %.10f\n",lat_sphere); - - double lon_sphere = 0; - - if (phi_1 == M_PI / 2) { - //using small angle approximation (formula in comment is without aproximation) - lon_sphere = (lambda_0 - y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(-y_descaled, x_descaled)); - - } else if (phi_1 == -M_PI / 2) { - //using small angle approximation (formula in comment is without aproximation) - lon_sphere = (lambda_0 + y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(y_descaled, x_descaled)); - - } else { - - lon_sphere = (lambda_0 + atan2(y_descaled * sin_c , c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c)); - //using small angle approximation -// double denominator = (c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c); -// if(denominator != 0) -// { -// lon_sphere = (lambda_0 + (y_descaled * sin_c) / denominator); -// } -// else -// { -// ... -// } - } - -// printf("lon_sphere = %.10f\n",lon_sphere); - - *lat = lat_sphere * 180.0 / M_PI; - *lon = lon_sphere * 180.0 / M_PI; - -} - -/**************************************************************************** - * main - ****************************************************************************/ - -int position_estimator_main(int argc, char *argv[]) -{ - - /* welcome user */ - printf("[multirotor position_estimator] started\n"); - - /* initialize values */ - static float u[2] = {0, 0}; - static float z[3] = {0, 0, 0}; - static float xapo[N_STATES] = {0, 0, 0, 0, 0, 0}; - static float Papo[N_STATES * N_STATES] = {ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0, - ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0, - ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0, - ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0, - ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0, - ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0 - }; - - static float xapo1[N_STATES]; - static float Papo1[36]; - - static float gps_covariance[3] = {0.0f, 0.0f, 0.0f}; - - static uint16_t counter = 0; - position_estimator_counter_position_information = 0; - - uint8_t predict_only = 1; - - bool gps_valid = false; - - bool new_initialization = true; - - static double lat_current = 0.0d;//[°]] --> 47.0 - static double lon_current = 0.0d; //[°]] -->8.5 - float alt_current = 0.0f; - - - //TODO: handle flight without gps but with estimator - - /* subscribe to vehicle status, attitude, gps */ - struct vehicle_gps_position_s gps; - gps.fix_type = 0; - struct vehicle_status_s vstatus; - struct vehicle_attitude_s att; - - int vehicle_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); - - /* subscribe to attitude at 100 Hz */ - int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - - /* wait until gps signal turns valid, only then can we initialize the projection */ - while (gps.fix_type < 3) { - struct pollfd fds[1] = { {.fd = vehicle_gps_sub, .events = POLLIN} }; - - /* wait for GPS updates, BUT READ VEHICLE STATUS (!) - * this choice is critical, since the vehicle status might not - * actually change, if this app is started after GPS lock was - * aquired. - */ - if (poll(fds, 1, 5000)) { - /* Wait for the GPS update to propagate (we have some time) */ - usleep(5000); - /* Read wether the vehicle status changed */ - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps); - gps_valid = (gps.fix_type > 2); - } - } - - /* get gps value for first initialization */ - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps); - lat_current = ((double)(gps.lat)) * 1e-7; - lon_current = ((double)(gps.lon)) * 1e-7; - alt_current = gps.alt * 1e-3; - - /* initialize coordinates */ - map_projection_init(lat_current, lon_current); - - /* publish global position messages only after first GPS message */ - struct vehicle_local_position_s local_pos = { - .x = 0, - .y = 0, - .z = 0 - }; - orb_advert_t local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos); - - printf("[multirotor position estimator] initialized projection with: lat: %.10f, lon:%.10f\n", lat_current, lon_current); - - while (1) { - - /*This runs at the rate of the sensors, if we have also a new gps update this is used in the position_estimator function */ - struct pollfd fds[1] = { {.fd = vehicle_attitude_sub, .events = POLLIN} }; - - if (poll(fds, 1, 5000) <= 0) { - /* error / timeout */ - } else { - - orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); - /* got attitude, updating pos as well */ - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps); - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus); - - /*copy attitude */ - u[0] = att.roll; - u[1] = att.pitch; - - /* initialize map projection with the last estimate (not at full rate) */ - if (gps.fix_type > 2) { - /* Project gps lat lon (Geographic coordinate system) to plane*/ - map_projection_project(((double)(gps.lat)) * 1e-7, ((double)(gps.lon)) * 1e-7, &(z[0]), &(z[1])); - - local_pos.x = z[0]; - local_pos.y = z[1]; - /* negative offset from initialization altitude */ - local_pos.z = alt_current - (gps.alt) * 1e-3; - - - orb_publish(ORB_ID(vehicle_local_position), local_pos_pub, &local_pos); - } - - - // gps_covariance[0] = gps.eph; //TODO: needs scaling - // gps_covariance[1] = gps.eph; - // gps_covariance[2] = gps.epv; - - // } else { - // /* we can not use the gps signal (it is of low quality) */ - // predict_only = 1; - // } - - // // predict_only = 0; //TODO: only for testing, removeme, XXX - // // z[0] = sinf(((float)counter)/180.0f*3.14159265f); //TODO: only for testing, removeme, XXX - // // usleep(100000); //TODO: only for testing, removeme, XXX - - - // /*Get new estimation (this is calculated in the plane) */ - // //TODO: if new_initialization == true: use 0,0,0, else use xapo - // if (true == new_initialization) { //TODO,XXX: uncomment! - // xapo[0] = 0; //we have a new plane initialization. the current estimate is in the center of the plane - // xapo[2] = 0; - // xapo[4] = 0; - // position_estimator(u, z, xapo, Papo, gps_covariance, predict_only, xapo1, Papo1); - - // } else { - // position_estimator(u, z, xapo, Papo, gps_covariance, predict_only, xapo1, Papo1); - // } - - - - // /* Copy values from xapo1 to xapo */ - // int i; - - // for (i = 0; i < N_STATES; i++) { - // xapo[i] = xapo1[i]; - // } - - // if ((counter % REPROJECTION_COUNTER_LIMIT == 0) || (counter % (PROJECTION_INITIALIZE_COUNTER_LIMIT - 1) == 0)) { - // /* Reproject from plane to geographic coordinate system */ - // // map_projection_reproject(xapo1[0], xapo1[2], map_scale, phi_1, lambda_0, &lat_current, &lon_current) //TODO,XXX: uncomment! - // map_projection_reproject(z[0], z[1], &lat_current, &lon_current); //do not use estimator for projection testing, removeme - // // //DEBUG - // // if(counter%500 == 0) - // // { - // // printf("phi_1: %.10f\n", phi_1); - // // printf("lambda_0: %.10f\n", lambda_0); - // // printf("lat_estimated: %.10f\n", lat_current); - // // printf("lon_estimated: %.10f\n", lon_current); - // // printf("z[0]=%.10f, z[1]=%.10f, z[2]=%f\n", z[0], z[1], z[2]); - // // fflush(stdout); - // // - // // } - - // // if(!isnan(lat_current) && !isnan(lon_current))// && !isnan(xapo1[4]) && !isnan(xapo1[1]) && !isnan(xapo1[3]) && !isnan(xapo1[5])) - // // { - // /* send out */ - - // global_pos.lat = lat_current; - // global_pos.lon = lon_current; - // global_pos.alt = xapo1[4]; - // global_pos.vx = xapo1[1]; - // global_pos.vy = xapo1[3]; - // global_pos.vz = xapo1[5]; - - /* publish current estimate */ - // orb_publish(ORB_ID(vehicle_global_position), global_pos_pub, &global_pos); - // } - // else - // { - // printf("[position estimator] ERROR: nan values, lat_current=%.4f, lon_current=%.4f, z[0]=%.4f z[1]=%.4f\n", lat_current, lon_current, z[0], z[1]); - // fflush(stdout); - // } - - // } - - counter++; - } - - } - - return 0; -} - - 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 368fa6ee2..368424853 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 Anton Babushkin. All rights reserved. - * Author: Anton Babushkin <rk3dov@gmail.com> + * 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 @@ -35,6 +34,8 @@ /** * @file position_estimator_inav_main.c * Model-identification based position estimator for multirotors + * + * @author Anton Babushkin <anton.babushkin@me.com> */ #include <unistd.h> @@ -57,6 +58,7 @@ #include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/vehicle_gps_position.h> +#include <uORB/topics/home_position.h> #include <uORB/topics/optical_flow.h> #include <mavlink/mavlink_log.h> #include <poll.h> @@ -95,8 +97,9 @@ static void usage(const char *reason); */ static void usage(const char *reason) { - if (reason) + if (reason) { fprintf(stderr, "%s\n", reason); + } fprintf(stderr, "usage: position_estimator_inav {start|stop|status} [-v]\n\n"); exit(1); @@ -112,8 +115,9 @@ static void usage(const char *reason) */ int position_estimator_inav_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 1) { usage("missing command"); + } if (!strcmp(argv[1], "start")) { if (thread_running) { @@ -125,8 +129,9 @@ int position_estimator_inav_main(int argc, char *argv[]) verbose_mode = false; if (argc > 1) - if (!strcmp(argv[2], "-v")) + if (!strcmp(argv[2], "-v")) { verbose_mode = true; + } thread_should_exit = false; position_estimator_inav_task = task_spawn_cmd("position_estimator_inav", @@ -163,16 +168,19 @@ int position_estimator_inav_main(int argc, char *argv[]) exit(1); } -void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3], float z_est[3], float corr_acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) { +void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3], float z_est[3], float x_est_prev[3], float y_est_prev[3], float z_est_prev[3], float corr_acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) +{ FILE *f = fopen("/fs/microsd/inav.log", "a"); + if (f) { char *s = malloc(256); - unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2]); + unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f] x_est_prev=[%.5f %.5f %.5f] y_est_prev=[%.5f %.5f %.5f] z_est_prev=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2], x_est_prev[0], x_est_prev[1], x_est_prev[2], y_est_prev[0], y_est_prev[1], y_est_prev[2], z_est_prev[0], z_est_prev[1], z_est_prev[2]); fwrite(s, 1, n, f); n = snprintf(s, 256, "\tacc_corr=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", corr_acc[0], corr_acc[1], corr_acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v); fwrite(s, 1, n, f); free(s); } + fsync(fileno(f)); fclose(f); } @@ -191,6 +199,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float y_est[3] = { 0.0f, 0.0f, 0.0f }; float z_est[3] = { 0.0f, 0.0f, 0.0f }; + float x_est_prev[3], y_est_prev[3], z_est_prev[3]; + memset(x_est_prev, 0, sizeof(x_est_prev)); + memset(y_est_prev, 0, sizeof(y_est_prev)); + memset(z_est_prev, 0, sizeof(z_est_prev)); + int baro_init_cnt = 0; int baro_init_num = 200; float baro_offset = 0.0f; // baro offset for reference altitude, initialized on start, then adjusted @@ -206,6 +219,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) bool ref_inited = false; hrt_abstime ref_init_start = 0; const hrt_abstime ref_init_delay = 1000000; // wait for 1s after 3D fix + struct map_projection_reference_s ref; + memset(&ref, 0, sizeof(ref)); + hrt_abstime home_timestamp = 0; uint16_t accel_updates = 0; uint16_t baro_updates = 0; @@ -238,7 +254,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float corr_flow[] = { 0.0f, 0.0f }; // N E float w_flow = 0.0f; + static float min_eph_epv = 2.0f; // min EPH/EPV, used for weight calculation + static float max_eph_epv = 10.0f; // max EPH/EPV acceptable for estimation + float sonar_prev = 0.0f; + hrt_abstime flow_prev = 0; // time of last flow measurement hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered) hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered) hrt_abstime xy_src_time = 0; // time of last available position data @@ -257,6 +277,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memset(&sensor, 0, sizeof(sensor)); struct vehicle_gps_position_s gps; memset(&gps, 0, sizeof(gps)); + struct home_position_s home; + memset(&home, 0, sizeof(home)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_local_position_s local_pos; @@ -274,10 +296,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow)); int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + int home_position_sub = orb_subscribe(ORB_ID(home_position)); /* advertise */ orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos); - orb_advert_t vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); + orb_advert_t vehicle_global_position_pub = -1; struct position_estimator_inav_params params; struct position_estimator_inav_param_handles pos_inav_param_handles; @@ -325,7 +348,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", baro_offset); local_pos.z_valid = true; local_pos.v_z_valid = true; - global_pos.baro_valid = true; } } } @@ -425,6 +447,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); + /* calculate time from previous update */ + float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f; + flow_prev = flow.flow_timestamp; + if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7 && flow.ground_distance_m != sonar_prev) { sonar_time = t; sonar_prev = flow.ground_distance_m; @@ -475,10 +501,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow && fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow; - /* convert raw flow to angular flow */ + /* convert raw flow to angular flow (rad/s) */ float flow_ang[2]; - flow_ang[0] = flow.flow_raw_x * params.flow_k; - flow_ang[1] = flow.flow_raw_y * params.flow_k; + flow_ang[0] = flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt; + flow_ang[1] = flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt; /* flow measurements vector */ float flow_m[3]; flow_m[0] = -flow_ang[0] * flow_dist; @@ -503,8 +529,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* if flow is not accurate, reduce weight for it */ // TODO make this more fuzzy - if (!flow_accurate) + if (!flow_accurate) { w_flow *= 0.05f; + } flow_valid = true; @@ -516,32 +543,73 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) flow_updates++; } + /* home position */ + orb_check(home_position_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(home_position), home_position_sub, &home); + + if (home.timestamp != home_timestamp) { + home_timestamp = home.timestamp; + + double est_lat, est_lon; + float est_alt; + + if (ref_inited) { + /* calculate current estimated position in global frame */ + est_alt = local_pos.ref_alt - local_pos.z; + map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon); + } + + /* update reference */ + map_projection_init(&ref, home.lat, home.lon); + + /* update baro offset */ + baro_offset += home.alt - local_pos.ref_alt; + + local_pos.ref_lat = home.lat; + local_pos.ref_lon = home.lon; + local_pos.ref_alt = home.alt; + local_pos.ref_timestamp = home.timestamp; + + if (ref_inited) { + /* reproject position estimate with new reference */ + map_projection_project(&ref, est_lat, est_lon, &x_est[0], &y_est[0]); + z_est[0] = -(est_alt - local_pos.ref_alt); + } + + ref_inited = true; + } + } + /* vehicle GPS position */ orb_check(vehicle_gps_position_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - if (gps.fix_type >= 3) { - /* hysteresis for GPS quality */ - if (gps_valid) { - if (gps.eph_m > 10.0f || gps.epv_m > 20.0f) { - gps_valid = false; - mavlink_log_info(mavlink_fd, "[inav] GPS signal lost"); - } + bool reset_est = false; - } else { - if (gps.eph_m < 5.0f && gps.epv_m < 10.0f) { - gps_valid = true; - mavlink_log_info(mavlink_fd, "[inav] GPS signal found"); - } + /* hysteresis for GPS quality */ + if (gps_valid) { + if (gps.eph_m > max_eph_epv * 1.5f || gps.epv_m > max_eph_epv * 1.5f || gps.fix_type < 3) { + gps_valid = false; + mavlink_log_info(mavlink_fd, "[inav] GPS signal lost"); } } else { - gps_valid = false; + if (gps.eph_m < max_eph_epv && gps.epv_m < max_eph_epv && gps.fix_type >= 3) { + gps_valid = true; + reset_est = true; + mavlink_log_info(mavlink_fd, "[inav] GPS signal found"); + } } if (gps_valid) { + double lat = gps.lat * 1e-7; + double lon = gps.lon * 1e-7; + float alt = gps.alt * 1e-3; + /* initialize reference position if needed */ if (!ref_inited) { if (ref_init_start == 0) { @@ -549,18 +617,25 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else if (t > ref_init_start + ref_init_delay) { ref_inited = true; - /* reference GPS position */ - double lat = gps.lat * 1e-7; - double lon = gps.lon * 1e-7; - float alt = gps.alt * 1e-3; - - local_pos.ref_lat = gps.lat; - local_pos.ref_lon = gps.lon; - local_pos.ref_alt = alt + z_est[0]; + /* update baro offset */ + baro_offset -= z_est[0]; + + /* set position estimate to (0, 0, 0), use GPS velocity for XY */ + x_est[0] = 0.0f; + x_est[1] = gps.vel_n_m_s; + x_est[2] = accel_NED[0]; + y_est[0] = 0.0f; + y_est[1] = gps.vel_e_m_s; + z_est[0] = 0.0f; + y_est[2] = accel_NED[1]; + + local_pos.ref_lat = lat; + local_pos.ref_lon = lon; + local_pos.ref_alt = alt; local_pos.ref_timestamp = t; /* initialize projection */ - map_projection_init(lat, lon); + map_projection_init(&ref, lat, lon); warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); } @@ -569,11 +644,22 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (ref_inited) { /* project GPS lat lon to plane */ float gps_proj[2]; - map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); + map_projection_project(&ref, lat, lon, &(gps_proj[0]), &(gps_proj[1])); + + /* reset position estimate when GPS becomes good */ + if (reset_est) { + x_est[0] = gps_proj[0]; + x_est[1] = gps.vel_n_m_s; + x_est[2] = accel_NED[0]; + y_est[0] = gps_proj[1]; + y_est[1] = gps.vel_e_m_s; + y_est[2] = accel_NED[1]; + } + /* calculate correction for position */ corr_gps[0][0] = gps_proj[0] - x_est[0]; corr_gps[1][0] = gps_proj[1] - y_est[0]; - corr_gps[2][0] = local_pos.ref_alt - gps.alt * 1e-3 - z_est[0]; + corr_gps[2][0] = local_pos.ref_alt - alt - z_est[0]; /* calculate correction for velocity */ if (gps.vel_ned_valid) { @@ -587,8 +673,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) corr_gps[2][1] = 0.0f; } - w_gps_xy = 2.0f / fmaxf(2.0f, gps.eph_m); - w_gps_z = 4.0f / fmaxf(4.0f, gps.epv_m); + w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph_m); + w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv_m); } } else { @@ -704,23 +790,34 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* inertial filter prediction for altitude */ inertial_filter_predict(dt, z_est); + if (!(isfinite(z_est[0]) && isfinite(z_est[1]) && isfinite(z_est[2]))) { + write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + memcpy(z_est, z_est_prev, sizeof(z_est)); + } + /* inertial filter correction for altitude */ inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro); inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); inertial_filter_correct(corr_acc[2], dt, z_est, 2, params.w_z_acc); - float x_est_prev[3], y_est_prev[3]; + if (!(isfinite(z_est[0]) && isfinite(z_est[1]) && isfinite(z_est[2]))) { + write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + memcpy(z_est, z_est_prev, sizeof(z_est)); + memset(corr_acc, 0, sizeof(corr_acc)); + memset(corr_gps, 0, sizeof(corr_gps)); + corr_baro = 0; - memcpy(x_est_prev, x_est, sizeof(x_est)); - memcpy(y_est_prev, y_est, sizeof(y_est)); + } else { + memcpy(z_est_prev, z_est, sizeof(z_est)); + } if (can_estimate_xy) { /* inertial filter prediction for position */ inertial_filter_predict(dt, x_est); inertial_filter_predict(dt, y_est); - if (!isfinite(x_est[0]) || !isfinite(y_est[0])) { - write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(x_est[2]) && isfinite(y_est[0]) && isfinite(y_est[1]) && isfinite(y_est[2]))) { + write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); memcpy(x_est, x_est_prev, sizeof(x_est)); memcpy(y_est, y_est_prev, sizeof(y_est)); } @@ -744,13 +841,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } - if (!isfinite(x_est[0]) || !isfinite(y_est[0])) { - write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(x_est[2]) && isfinite(y_est[0]) && isfinite(y_est[1]) && isfinite(y_est[2]))) { + write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); memcpy(x_est, x_est_prev, sizeof(x_est)); memcpy(y_est, y_est_prev, sizeof(y_est)); memset(corr_acc, 0, sizeof(corr_acc)); memset(corr_gps, 0, sizeof(corr_gps)); memset(corr_flow, 0, sizeof(corr_flow)); + + } else { + memcpy(x_est_prev, x_est, sizeof(x_est)); + memcpy(y_est_prev, y_est, sizeof(y_est)); } } @@ -763,7 +864,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float thrust = armed.armed ? actuator.control[3] : 0.0f; if (landed) { - if (alt_disp2 > land_disp2 && thrust > params.land_thr) { + if (alt_disp2 > land_disp2 || thrust > params.land_thr) { landed = false; landed_time = 0; } @@ -808,7 +909,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (t > pub_last + pub_interval) { pub_last = t; /* publish local position */ - local_pos.xy_valid = can_estimate_xy && use_gps_xy; + local_pos.xy_valid = can_estimate_xy; local_pos.v_xy_valid = can_estimate_xy; local_pos.xy_global = local_pos.xy_valid && use_gps_xy; local_pos.z_global = local_pos.z_valid && use_gps_z; @@ -831,40 +932,35 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); - /* publish global position */ - global_pos.global_valid = local_pos.xy_global; + if (local_pos.xy_global && local_pos.z_global) { + /* publish global position */ + global_pos.timestamp = t; + global_pos.time_gps_usec = gps.time_gps_usec; - if (local_pos.xy_global) { double est_lat, est_lon; - map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); + map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon); + global_pos.lat = est_lat; global_pos.lon = est_lon; - global_pos.time_gps_usec = gps.time_gps_usec; - } + global_pos.alt = local_pos.ref_alt - local_pos.z; - /* set valid values even if position is not valid */ - if (local_pos.v_xy_valid) { global_pos.vel_n = local_pos.vx; global_pos.vel_e = local_pos.vy; - } - - if (local_pos.z_global) { - global_pos.alt = local_pos.ref_alt - local_pos.z; - } - - if (local_pos.z_valid) { - global_pos.baro_alt = baro_offset - local_pos.z; - } - - if (local_pos.v_z_valid) { global_pos.vel_d = local_pos.vz; - } - global_pos.yaw = local_pos.yaw; + global_pos.yaw = local_pos.yaw; + + // TODO implement dead-reckoning + global_pos.eph = gps.eph_m; + global_pos.epv = gps.epv_m; - global_pos.timestamp = t; + if (vehicle_global_position_pub < 0) { + vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); - orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); + } else { + orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); + } + } } } 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 b71f9472f..2e4f26661 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -50,13 +50,13 @@ PARAM_DEFINE_FLOAT(INAV_W_XY_ACC, 20.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f); PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f); PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f); -PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.0165f); +PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.15f); PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.5f); PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f); PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f); PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f); -PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.3f); +PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f); int parameters_init(struct position_estimator_inav_param_handles *h) { diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe1.c deleted file mode 100755 index 977565b8e..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe1.c +++ /dev/null @@ -1,58 +0,0 @@ -/* - * kalman_dlqe1.c - * - * Code generation for function 'kalman_dlqe1' - * - * C source code generated on: Wed Feb 13 20:34:32 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "kalman_dlqe1.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void kalman_dlqe1(const real32_T A[9], const real32_T C[3], const real32_T K[3], - const real32_T x_aposteriori_k[3], real32_T z, real32_T - x_aposteriori[3]) -{ - printf("[dlqe input]: x_aposteriori_k %12.8f\t %12.8f\t %12.8f\t z:%12.8f\n", (double)(x_aposteriori_k[0]), (double)(x_aposteriori_k[1]), (double)(x_aposteriori_k[2]), (double)z); - printf("[dlqe input]: C[0]: %12.8f\tC[1] %12.8f\tC[2] %12.8f\n", (double)(C[0]), (double)(C[1]), (double)(C[2])); - real32_T y; - int32_T i0; - real32_T b_y[3]; - int32_T i1; - real32_T f0; - y = 0.0F; - for (i0 = 0; i0 < 3; i0++) { - b_y[i0] = 0.0F; - for (i1 = 0; i1 < 3; i1++) { - b_y[i0] += C[i1] * A[i1 + 3 * i0]; - } - - y += b_y[i0] * x_aposteriori_k[i0]; - } - - y = z - y; - for (i0 = 0; i0 < 3; i0++) { - f0 = 0.0F; - for (i1 = 0; i1 < 3; i1++) { - f0 += A[i0 + 3 * i1] * x_aposteriori_k[i1]; - } - - x_aposteriori[i0] = f0 + K[i0] * y; - } - //printf("[dlqe output]: x_aposteriori %12.8f\t %12.8f\t %12.8f\n", (double)(x_aposteriori[0]), (double)(x_aposteriori[1]), (double)(x_aposteriori[2])); -} - -/* End of code generation (kalman_dlqe1.c) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe1.h deleted file mode 100755 index 2f5330563..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe1.h +++ /dev/null @@ -1,30 +0,0 @@ -/* - * kalman_dlqe1.h - * - * Code generation for function 'kalman_dlqe1' - * - * C source code generated on: Wed Feb 13 20:34:32 2013 - * - */ - -#ifndef __KALMAN_DLQE1_H__ -#define __KALMAN_DLQE1_H__ -/* Include files */ -#include <stddef.h> -#include <stdlib.h> - -#include "rtwtypes.h" -#include "kalman_dlqe1_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void kalman_dlqe1(const real32_T A[9], const real32_T C[3], const real32_T K[3], const real32_T x_aposteriori_k[3], real32_T z, real32_T x_aposteriori[3]); -#endif -/* End of code generation (kalman_dlqe1.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.c deleted file mode 100755 index 6627f76e9..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.c +++ /dev/null @@ -1,31 +0,0 @@ -/* - * kalman_dlqe1_initialize.c - * - * Code generation for function 'kalman_dlqe1_initialize' - * - * C source code generated on: Wed Feb 13 20:34:31 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "kalman_dlqe1.h" -#include "kalman_dlqe1_initialize.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void kalman_dlqe1_initialize(void) -{ - rt_InitInfAndNaN(8U); -} - -/* End of code generation (kalman_dlqe1_initialize.c) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.h deleted file mode 100755 index a77eb5712..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.h +++ /dev/null @@ -1,30 +0,0 @@ -/* - * kalman_dlqe1_initialize.h - * - * Code generation for function 'kalman_dlqe1_initialize' - * - * C source code generated on: Wed Feb 13 20:34:31 2013 - * - */ - -#ifndef __KALMAN_DLQE1_INITIALIZE_H__ -#define __KALMAN_DLQE1_INITIALIZE_H__ -/* Include files */ -#include <stddef.h> -#include <stdlib.h> - -#include "rtwtypes.h" -#include "kalman_dlqe1_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void kalman_dlqe1_initialize(void); -#endif -/* End of code generation (kalman_dlqe1_initialize.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.c deleted file mode 100755 index a65536f79..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.c +++ /dev/null @@ -1,31 +0,0 @@ -/* - * kalman_dlqe1_terminate.c - * - * Code generation for function 'kalman_dlqe1_terminate' - * - * C source code generated on: Wed Feb 13 20:34:31 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "kalman_dlqe1.h" -#include "kalman_dlqe1_terminate.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void kalman_dlqe1_terminate(void) -{ - /* (no terminate code required) */ -} - -/* End of code generation (kalman_dlqe1_terminate.c) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.h deleted file mode 100755 index 100c7f76c..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.h +++ /dev/null @@ -1,30 +0,0 @@ -/* - * kalman_dlqe1_terminate.h - * - * Code generation for function 'kalman_dlqe1_terminate' - * - * C source code generated on: Wed Feb 13 20:34:32 2013 - * - */ - -#ifndef __KALMAN_DLQE1_TERMINATE_H__ -#define __KALMAN_DLQE1_TERMINATE_H__ -/* Include files */ -#include <stddef.h> -#include <stdlib.h> - -#include "rtwtypes.h" -#include "kalman_dlqe1_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void kalman_dlqe1_terminate(void); -#endif -/* End of code generation (kalman_dlqe1_terminate.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_types.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_types.h deleted file mode 100755 index d4b2c2d61..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_types.h +++ /dev/null @@ -1,16 +0,0 @@ -/* - * kalman_dlqe1_types.h - * - * Code generation for function 'kalman_dlqe1' - * - * C source code generated on: Wed Feb 13 20:34:31 2013 - * - */ - -#ifndef __KALMAN_DLQE1_TYPES_H__ -#define __KALMAN_DLQE1_TYPES_H__ - -/* Type Definitions */ - -#endif -/* End of code generation (kalman_dlqe1_types.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe2.c deleted file mode 100755 index 11b999064..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe2.c +++ /dev/null @@ -1,119 +0,0 @@ -/* - * kalman_dlqe2.c - * - * Code generation for function 'kalman_dlqe2' - * - * C source code generated on: Thu Feb 14 12:52:28 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "kalman_dlqe2.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -static real32_T rt_powf_snf(real32_T u0, real32_T u1); - -/* Function Definitions */ -static real32_T rt_powf_snf(real32_T u0, real32_T u1) -{ - real32_T y; - real32_T f1; - real32_T f2; - if (rtIsNaNF(u0) || rtIsNaNF(u1)) { - y = ((real32_T)rtNaN); - } else { - f1 = (real32_T)fabs(u0); - f2 = (real32_T)fabs(u1); - if (rtIsInfF(u1)) { - if (f1 == 1.0F) { - y = ((real32_T)rtNaN); - } else if (f1 > 1.0F) { - if (u1 > 0.0F) { - y = ((real32_T)rtInf); - } else { - y = 0.0F; - } - } else if (u1 > 0.0F) { - y = 0.0F; - } else { - y = ((real32_T)rtInf); - } - } else if (f2 == 0.0F) { - y = 1.0F; - } else if (f2 == 1.0F) { - if (u1 > 0.0F) { - y = u0; - } else { - y = 1.0F / u0; - } - } else if (u1 == 2.0F) { - y = u0 * u0; - } else if ((u1 == 0.5F) && (u0 >= 0.0F)) { - y = (real32_T)sqrt(u0); - } else if ((u0 < 0.0F) && (u1 > (real32_T)floor(u1))) { - y = ((real32_T)rtNaN); - } else { - y = (real32_T)pow(u0, u1); - } - } - - return y; -} - -void kalman_dlqe2(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const - real32_T x_aposteriori_k[3], real32_T z, real32_T - x_aposteriori[3]) -{ - //printf("[dqle2] dt: %12.8f\tvk1 %12.8f\tk2: %12.8f\tk3: %12.8f\n", (double)(dt), (double)(k1), (double)(k2), (double)(k3)); - //printf("[dqle2] dt: %8.4f\n", (double)(dt));//, (double)(k1), (double)(k2), (double)(k3)); - real32_T A[9]; - real32_T y; - int32_T i0; - static const int8_T iv0[3] = { 0, 0, 1 }; - - real32_T b_k1[3]; - int32_T i1; - static const int8_T iv1[3] = { 1, 0, 0 }; - - real32_T f0; - A[0] = 1.0F; - A[3] = dt; - A[6] = 0.5F * rt_powf_snf(dt, 2.0F); - A[1] = 0.0F; - A[4] = 1.0F; - A[7] = dt; - y = 0.0F; - for (i0 = 0; i0 < 3; i0++) { - A[2 + 3 * i0] = (real32_T)iv0[i0]; - b_k1[i0] = 0.0F; - for (i1 = 0; i1 < 3; i1++) { - b_k1[i0] += (real32_T)iv1[i1] * A[i1 + 3 * i0]; - } - - y += b_k1[i0] * x_aposteriori_k[i0]; - } - - y = z - y; - b_k1[0] = k1; - b_k1[1] = k2; - b_k1[2] = k3; - for (i0 = 0; i0 < 3; i0++) { - f0 = 0.0F; - for (i1 = 0; i1 < 3; i1++) { - f0 += A[i0 + 3 * i1] * x_aposteriori_k[i1]; - } - - x_aposteriori[i0] = f0 + b_k1[i0] * y; - } -} - -/* End of code generation (kalman_dlqe2.c) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe2.h deleted file mode 100755 index 30170ae22..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe2.h +++ /dev/null @@ -1,32 +0,0 @@ -/* - * kalman_dlqe2.h - * - * Code generation for function 'kalman_dlqe2' - * - * C source code generated on: Thu Feb 14 12:52:29 2013 - * - */ - -#ifndef __KALMAN_DLQE2_H__ -#define __KALMAN_DLQE2_H__ -/* Include files */ -#include <math.h> -#include <stddef.h> -#include <stdlib.h> -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "kalman_dlqe2_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void kalman_dlqe2(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const real32_T x_aposteriori_k[3], real32_T z, real32_T x_aposteriori[3]); -#endif -/* End of code generation (kalman_dlqe2.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.c deleted file mode 100755 index de5a1d8aa..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.c +++ /dev/null @@ -1,31 +0,0 @@ -/* - * kalman_dlqe2_initialize.c - * - * Code generation for function 'kalman_dlqe2_initialize' - * - * C source code generated on: Thu Feb 14 12:52:28 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "kalman_dlqe2.h" -#include "kalman_dlqe2_initialize.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void kalman_dlqe2_initialize(void) -{ - rt_InitInfAndNaN(8U); -} - -/* End of code generation (kalman_dlqe2_initialize.c) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.h deleted file mode 100755 index 3d507ff31..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.h +++ /dev/null @@ -1,32 +0,0 @@ -/* - * kalman_dlqe2_initialize.h - * - * Code generation for function 'kalman_dlqe2_initialize' - * - * C source code generated on: Thu Feb 14 12:52:28 2013 - * - */ - -#ifndef __KALMAN_DLQE2_INITIALIZE_H__ -#define __KALMAN_DLQE2_INITIALIZE_H__ -/* Include files */ -#include <math.h> -#include <stddef.h> -#include <stdlib.h> -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "kalman_dlqe2_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void kalman_dlqe2_initialize(void); -#endif -/* End of code generation (kalman_dlqe2_initialize.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.c deleted file mode 100755 index 0757c878c..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.c +++ /dev/null @@ -1,31 +0,0 @@ -/* - * kalman_dlqe2_terminate.c - * - * Code generation for function 'kalman_dlqe2_terminate' - * - * C source code generated on: Thu Feb 14 12:52:28 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "kalman_dlqe2.h" -#include "kalman_dlqe2_terminate.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void kalman_dlqe2_terminate(void) -{ - /* (no terminate code required) */ -} - -/* End of code generation (kalman_dlqe2_terminate.c) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.h deleted file mode 100755 index 23995020b..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.h +++ /dev/null @@ -1,32 +0,0 @@ -/* - * kalman_dlqe2_terminate.h - * - * Code generation for function 'kalman_dlqe2_terminate' - * - * C source code generated on: Thu Feb 14 12:52:28 2013 - * - */ - -#ifndef __KALMAN_DLQE2_TERMINATE_H__ -#define __KALMAN_DLQE2_TERMINATE_H__ -/* Include files */ -#include <math.h> -#include <stddef.h> -#include <stdlib.h> -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "kalman_dlqe2_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void kalman_dlqe2_terminate(void); -#endif -/* End of code generation (kalman_dlqe2_terminate.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_types.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_types.h deleted file mode 100755 index f7a04d908..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_types.h +++ /dev/null @@ -1,16 +0,0 @@ -/* - * kalman_dlqe2_types.h - * - * Code generation for function 'kalman_dlqe2' - * - * C source code generated on: Thu Feb 14 12:52:28 2013 - * - */ - -#ifndef __KALMAN_DLQE2_TYPES_H__ -#define __KALMAN_DLQE2_TYPES_H__ - -/* Type Definitions */ - -#endif -/* End of code generation (kalman_dlqe2_types.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe3.c deleted file mode 100755 index 9efe2ea7a..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3.c +++ /dev/null @@ -1,137 +0,0 @@ -/*
- * kalman_dlqe3.c
- *
- * Code generation for function 'kalman_dlqe3'
- *
- * C source code generated on: Tue Feb 19 15:26:31 2013
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "kalman_dlqe3.h"
-#include "randn.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-static real32_T rt_powf_snf(real32_T u0, real32_T u1);
-
-/* Function Definitions */
-static real32_T rt_powf_snf(real32_T u0, real32_T u1)
-{
- real32_T y;
- real32_T f1;
- real32_T f2;
- if (rtIsNaNF(u0) || rtIsNaNF(u1)) {
- y = ((real32_T)rtNaN);
- } else {
- f1 = (real32_T)fabs(u0);
- f2 = (real32_T)fabs(u1);
- if (rtIsInfF(u1)) {
- if (f1 == 1.0F) {
- y = ((real32_T)rtNaN);
- } else if (f1 > 1.0F) {
- if (u1 > 0.0F) {
- y = ((real32_T)rtInf);
- } else {
- y = 0.0F;
- }
- } else if (u1 > 0.0F) {
- y = 0.0F;
- } else {
- y = ((real32_T)rtInf);
- }
- } else if (f2 == 0.0F) {
- y = 1.0F;
- } else if (f2 == 1.0F) {
- if (u1 > 0.0F) {
- y = u0;
- } else {
- y = 1.0F / u0;
- }
- } else if (u1 == 2.0F) {
- y = u0 * u0;
- } else if ((u1 == 0.5F) && (u0 >= 0.0F)) {
- y = (real32_T)sqrt(u0);
- } else if ((u0 < 0.0F) && (u1 > (real32_T)floor(u1))) {
- y = ((real32_T)rtNaN);
- } else {
- y = (real32_T)pow(u0, u1);
- }
- }
-
- return y;
-}
-
-void kalman_dlqe3(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const
- real32_T x_aposteriori_k[3], real32_T z, real32_T posUpdate,
- real32_T addNoise, real32_T sigma, real32_T x_aposteriori[3])
-{
- real32_T A[9];
- int32_T i0;
- static const int8_T iv0[3] = { 0, 0, 1 };
-
- real_T b;
- real32_T y;
- real32_T b_y[3];
- int32_T i1;
- static const int8_T iv1[3] = { 1, 0, 0 };
-
- real32_T b_k1[3];
- real32_T f0;
- A[0] = 1.0F;
- A[3] = dt;
- A[6] = 0.5F * rt_powf_snf(dt, 2.0F);
- A[1] = 0.0F;
- A[4] = 1.0F;
- A[7] = dt;
- for (i0 = 0; i0 < 3; i0++) {
- A[2 + 3 * i0] = (real32_T)iv0[i0];
- }
-
- if (addNoise == 1.0F) {
- b = randn();
- z += sigma * (real32_T)b;
- }
-
- if (posUpdate != 0.0F) {
- y = 0.0F;
- for (i0 = 0; i0 < 3; i0++) {
- b_y[i0] = 0.0F;
- for (i1 = 0; i1 < 3; i1++) {
- b_y[i0] += (real32_T)iv1[i1] * A[i1 + 3 * i0];
- }
-
- y += b_y[i0] * x_aposteriori_k[i0];
- }
-
- y = z - y;
- b_k1[0] = k1;
- b_k1[1] = k2;
- b_k1[2] = k3;
- for (i0 = 0; i0 < 3; i0++) {
- f0 = 0.0F;
- for (i1 = 0; i1 < 3; i1++) {
- f0 += A[i0 + 3 * i1] * x_aposteriori_k[i1];
- }
-
- x_aposteriori[i0] = f0 + b_k1[i0] * y;
- }
- } else {
- for (i0 = 0; i0 < 3; i0++) {
- x_aposteriori[i0] = 0.0F;
- for (i1 = 0; i1 < 3; i1++) {
- x_aposteriori[i0] += A[i0 + 3 * i1] * x_aposteriori_k[i1];
- }
- }
- }
-}
-
-/* End of code generation (kalman_dlqe3.c) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe3.h deleted file mode 100755 index 9bbffbbb3..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3.h +++ /dev/null @@ -1,33 +0,0 @@ -/*
- * kalman_dlqe3.h
- *
- * Code generation for function 'kalman_dlqe3'
- *
- * C source code generated on: Tue Feb 19 15:26:32 2013
- *
- */
-
-#ifndef __KALMAN_DLQE3_H__
-#define __KALMAN_DLQE3_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "kalman_dlqe3_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void kalman_dlqe3(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const real32_T x_aposteriori_k[3], real32_T z, real32_T posUpdate, real32_T addNoise, real32_T sigma, real32_T x_aposteriori[3]);
-#endif
-/* End of code generation (kalman_dlqe3.h) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.c deleted file mode 100755 index 8f2275c13..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.c +++ /dev/null @@ -1,32 +0,0 @@ -/*
- * kalman_dlqe3_data.c
- *
- * Code generation for function 'kalman_dlqe3_data'
- *
- * C source code generated on: Tue Feb 19 15:26:31 2013
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "kalman_dlqe3.h"
-#include "kalman_dlqe3_data.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-uint32_T method;
-uint32_T state[2];
-uint32_T b_method;
-uint32_T b_state;
-uint32_T c_state[2];
-boolean_T state_not_empty;
-
-/* Function Declarations */
-
-/* Function Definitions */
-/* End of code generation (kalman_dlqe3_data.c) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.h deleted file mode 100755 index 952eb7b89..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.h +++ /dev/null @@ -1,38 +0,0 @@ -/*
- * kalman_dlqe3_data.h
- *
- * Code generation for function 'kalman_dlqe3_data'
- *
- * C source code generated on: Tue Feb 19 15:26:31 2013
- *
- */
-
-#ifndef __KALMAN_DLQE3_DATA_H__
-#define __KALMAN_DLQE3_DATA_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "kalman_dlqe3_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-extern uint32_T method;
-extern uint32_T state[2];
-extern uint32_T b_method;
-extern uint32_T b_state;
-extern uint32_T c_state[2];
-extern boolean_T state_not_empty;
-
-/* Variable Definitions */
-
-/* Function Declarations */
-#endif
-/* End of code generation (kalman_dlqe3_data.h) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.c deleted file mode 100755 index b87d604c4..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.c +++ /dev/null @@ -1,47 +0,0 @@ -/*
- * kalman_dlqe3_initialize.c
- *
- * Code generation for function 'kalman_dlqe3_initialize'
- *
- * C source code generated on: Tue Feb 19 15:26:31 2013
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "kalman_dlqe3.h"
-#include "kalman_dlqe3_initialize.h"
-#include "kalman_dlqe3_data.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void kalman_dlqe3_initialize(void)
-{
- int32_T i;
- static const uint32_T uv0[2] = { 362436069U, 0U };
-
- rt_InitInfAndNaN(8U);
- state_not_empty = FALSE;
- b_state = 1144108930U;
- b_method = 7U;
- method = 0U;
- for (i = 0; i < 2; i++) {
- c_state[i] = 362436069U + 158852560U * (uint32_T)i;
- state[i] = uv0[i];
- }
-
- if (state[1] == 0U) {
- state[1] = 521288629U;
- }
-}
-
-/* End of code generation (kalman_dlqe3_initialize.c) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.h deleted file mode 100755 index 9dee90f9e..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.h +++ /dev/null @@ -1,33 +0,0 @@ -/*
- * kalman_dlqe3_initialize.h
- *
- * Code generation for function 'kalman_dlqe3_initialize'
- *
- * C source code generated on: Tue Feb 19 15:26:31 2013
- *
- */
-
-#ifndef __KALMAN_DLQE3_INITIALIZE_H__
-#define __KALMAN_DLQE3_INITIALIZE_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "kalman_dlqe3_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void kalman_dlqe3_initialize(void);
-#endif
-/* End of code generation (kalman_dlqe3_initialize.h) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.c deleted file mode 100755 index b00858205..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.c +++ /dev/null @@ -1,31 +0,0 @@ -/*
- * kalman_dlqe3_terminate.c
- *
- * Code generation for function 'kalman_dlqe3_terminate'
- *
- * C source code generated on: Tue Feb 19 15:26:31 2013
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "kalman_dlqe3.h"
-#include "kalman_dlqe3_terminate.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void kalman_dlqe3_terminate(void)
-{
- /* (no terminate code required) */
-}
-
-/* End of code generation (kalman_dlqe3_terminate.c) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.h deleted file mode 100755 index 69cc85c76..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.h +++ /dev/null @@ -1,33 +0,0 @@ -/*
- * kalman_dlqe3_terminate.h
- *
- * Code generation for function 'kalman_dlqe3_terminate'
- *
- * C source code generated on: Tue Feb 19 15:26:31 2013
- *
- */
-
-#ifndef __KALMAN_DLQE3_TERMINATE_H__
-#define __KALMAN_DLQE3_TERMINATE_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "kalman_dlqe3_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void kalman_dlqe3_terminate(void);
-#endif
-/* End of code generation (kalman_dlqe3_terminate.h) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_types.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_types.h deleted file mode 100755 index f36ea4557..000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_types.h +++ /dev/null @@ -1,16 +0,0 @@ -/*
- * kalman_dlqe3_types.h
- *
- * Code generation for function 'kalman_dlqe3'
- *
- * C source code generated on: Tue Feb 19 15:26:30 2013
- *
- */
-
-#ifndef __KALMAN_DLQE3_TYPES_H__
-#define __KALMAN_DLQE3_TYPES_H__
-
-/* Type Definitions */
-
-#endif
-/* End of code generation (kalman_dlqe3_types.h) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.c deleted file mode 100755 index 5139848bc..000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.c +++ /dev/null @@ -1,136 +0,0 @@ -/* - * positionKalmanFilter1D.c - * - * Code generation for function 'positionKalmanFilter1D' - * - * C source code generated on: Fri Nov 30 14:26:11 2012 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "positionKalmanFilter1D.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void positionKalmanFilter1D(const real32_T A[9], const real32_T B[3], const - real32_T C[3], const real32_T x_aposteriori_k[3], const real32_T - P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, const real32_T - Q[9], real32_T R, real32_T thresh, real32_T decay, real32_T x_aposteriori[3], - real32_T P_aposteriori[9]) -{ - int32_T i0; - real32_T f0; - int32_T k; - real32_T b_A[9]; - int32_T i1; - real32_T P_apriori[9]; - real32_T y; - real32_T K[3]; - real32_T S; - int8_T I[9]; - - /* prediction */ - for (i0 = 0; i0 < 3; i0++) { - f0 = 0.0F; - for (k = 0; k < 3; k++) { - f0 += A[i0 + 3 * k] * x_aposteriori_k[k]; - } - - x_aposteriori[i0] = f0 + B[i0] * u; - } - - for (i0 = 0; i0 < 3; i0++) { - for (k = 0; k < 3; k++) { - b_A[i0 + 3 * k] = 0.0F; - for (i1 = 0; i1 < 3; i1++) { - b_A[i0 + 3 * k] += A[i0 + 3 * i1] * P_aposteriori_k[i1 + 3 * k]; - } - } - } - - for (i0 = 0; i0 < 3; i0++) { - for (k = 0; k < 3; k++) { - f0 = 0.0F; - for (i1 = 0; i1 < 3; i1++) { - f0 += b_A[i0 + 3 * i1] * A[k + 3 * i1]; - } - - P_apriori[i0 + 3 * k] = f0 + Q[i0 + 3 * k]; - } - } - - if ((real32_T)fabs(u) < thresh) { - x_aposteriori[1] *= decay; - } - - /* update */ - if (gps_update == 1) { - y = 0.0F; - for (k = 0; k < 3; k++) { - y += C[k] * x_aposteriori[k]; - K[k] = 0.0F; - for (i0 = 0; i0 < 3; i0++) { - K[k] += C[i0] * P_apriori[i0 + 3 * k]; - } - } - - y = z - y; - S = 0.0F; - for (k = 0; k < 3; k++) { - S += K[k] * C[k]; - } - - S += R; - for (i0 = 0; i0 < 3; i0++) { - f0 = 0.0F; - for (k = 0; k < 3; k++) { - f0 += P_apriori[i0 + 3 * k] * C[k]; - } - - K[i0] = f0 / S; - } - - for (i0 = 0; i0 < 3; i0++) { - x_aposteriori[i0] += K[i0] * y; - } - - for (i0 = 0; i0 < 9; i0++) { - I[i0] = 0; - } - - for (k = 0; k < 3; k++) { - I[k + 3 * k] = 1; - } - - for (i0 = 0; i0 < 3; i0++) { - for (k = 0; k < 3; k++) { - b_A[k + 3 * i0] = (real32_T)I[k + 3 * i0] - K[k] * C[i0]; - } - } - - for (i0 = 0; i0 < 3; i0++) { - for (k = 0; k < 3; k++) { - P_aposteriori[i0 + 3 * k] = 0.0F; - for (i1 = 0; i1 < 3; i1++) { - P_aposteriori[i0 + 3 * k] += b_A[i0 + 3 * i1] * P_apriori[i1 + 3 * k]; - } - } - } - } else { - for (i0 = 0; i0 < 9; i0++) { - P_aposteriori[i0] = P_apriori[i0]; - } - } -} - -/* End of code generation (positionKalmanFilter1D.c) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.h deleted file mode 100755 index 205c8eb4e..000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.h +++ /dev/null @@ -1,31 +0,0 @@ -/* - * positionKalmanFilter1D.h - * - * Code generation for function 'positionKalmanFilter1D' - * - * C source code generated on: Fri Nov 30 14:26:11 2012 - * - */ - -#ifndef __POSITIONKALMANFILTER1D_H__ -#define __POSITIONKALMANFILTER1D_H__ -/* Include files */ -#include <math.h> -#include <stddef.h> -#include <stdlib.h> - -#include "rtwtypes.h" -#include "positionKalmanFilter1D_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void positionKalmanFilter1D(const real32_T A[9], const real32_T B[3], const real32_T C[3], const real32_T x_aposteriori_k[3], const real32_T P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, const real32_T Q[9], real32_T R, real32_T thresh, real32_T decay, real32_T x_aposteriori[3], real32_T P_aposteriori[9]); -#endif -/* End of code generation (positionKalmanFilter1D.h) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.c deleted file mode 100755 index 4c535618a..000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.c +++ /dev/null @@ -1,157 +0,0 @@ -/* - * positionKalmanFilter1D_dT.c - * - * Code generation for function 'positionKalmanFilter1D_dT' - * - * C source code generated on: Fri Nov 30 17:37:33 2012 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "positionKalmanFilter1D_dT.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void positionKalmanFilter1D_dT(real32_T dT, const real32_T x_aposteriori_k[3], - const real32_T P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, - const real32_T Q[9], real_T R, real32_T thresh, real32_T decay, real32_T - x_aposteriori[3], real32_T P_aposteriori[9]) -{ - real32_T A[9]; - int32_T i; - static const int8_T iv0[3] = { 0, 0, 1 }; - - real32_T K[3]; - real32_T f0; - int32_T i0; - real32_T b_A[9]; - int32_T i1; - real32_T P_apriori[9]; - static const int8_T iv1[3] = { 1, 0, 0 }; - - real32_T fv0[3]; - real32_T y; - static const int8_T iv2[3] = { 1, 0, 0 }; - - real32_T S; - int8_T I[9]; - - /* dynamics */ - A[0] = 1.0F; - A[3] = dT; - A[6] = -0.5F * dT * dT; - A[1] = 0.0F; - A[4] = 1.0F; - A[7] = -dT; - for (i = 0; i < 3; i++) { - A[2 + 3 * i] = (real32_T)iv0[i]; - } - - /* prediction */ - K[0] = 0.5F * dT * dT; - K[1] = dT; - K[2] = 0.0F; - for (i = 0; i < 3; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 3; i0++) { - f0 += A[i + 3 * i0] * x_aposteriori_k[i0]; - } - - x_aposteriori[i] = f0 + K[i] * u; - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A[i + 3 * i0] = 0.0F; - for (i1 = 0; i1 < 3; i1++) { - b_A[i + 3 * i0] += A[i + 3 * i1] * P_aposteriori_k[i1 + 3 * i0]; - } - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - f0 = 0.0F; - for (i1 = 0; i1 < 3; i1++) { - f0 += b_A[i + 3 * i1] * A[i0 + 3 * i1]; - } - - P_apriori[i + 3 * i0] = f0 + Q[i + 3 * i0]; - } - } - - if ((real32_T)fabs(u) < thresh) { - x_aposteriori[1] *= decay; - } - - /* update */ - if (gps_update == 1) { - f0 = 0.0F; - for (i = 0; i < 3; i++) { - f0 += (real32_T)iv1[i] * x_aposteriori[i]; - fv0[i] = 0.0F; - for (i0 = 0; i0 < 3; i0++) { - fv0[i] += (real32_T)iv1[i0] * P_apriori[i0 + 3 * i]; - } - } - - y = z - f0; - f0 = 0.0F; - for (i = 0; i < 3; i++) { - f0 += fv0[i] * (real32_T)iv2[i]; - } - - S = f0 + (real32_T)R; - for (i = 0; i < 3; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 3; i0++) { - f0 += P_apriori[i + 3 * i0] * (real32_T)iv2[i0]; - } - - K[i] = f0 / S; - } - - for (i = 0; i < 3; i++) { - x_aposteriori[i] += K[i] * y; - } - - for (i = 0; i < 9; i++) { - I[i] = 0; - } - - for (i = 0; i < 3; i++) { - I[i + 3 * i] = 1; - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A[i0 + 3 * i] = (real32_T)I[i0 + 3 * i] - K[i0] * (real32_T)iv1[i]; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - P_aposteriori[i + 3 * i0] = 0.0F; - for (i1 = 0; i1 < 3; i1++) { - P_aposteriori[i + 3 * i0] += A[i + 3 * i1] * P_apriori[i1 + 3 * i0]; - } - } - } - } else { - for (i = 0; i < 9; i++) { - P_aposteriori[i] = P_apriori[i]; - } - } -} - -/* End of code generation (positionKalmanFilter1D_dT.c) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.h deleted file mode 100755 index 94cbe2ce6..000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.h +++ /dev/null @@ -1,31 +0,0 @@ -/* - * positionKalmanFilter1D_dT.h - * - * Code generation for function 'positionKalmanFilter1D_dT' - * - * C source code generated on: Fri Nov 30 17:37:33 2012 - * - */ - -#ifndef __POSITIONKALMANFILTER1D_DT_H__ -#define __POSITIONKALMANFILTER1D_DT_H__ -/* Include files */ -#include <math.h> -#include <stddef.h> -#include <stdlib.h> - -#include "rtwtypes.h" -#include "positionKalmanFilter1D_dT_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void positionKalmanFilter1D_dT(real32_T dT, const real32_T x_aposteriori_k[3], const real32_T P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, const real32_T Q[9], real_T R, real32_T thresh, real32_T decay, real32_T x_aposteriori[3], real32_T P_aposteriori[9]); -#endif -/* End of code generation (positionKalmanFilter1D_dT.h) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.c deleted file mode 100755 index aa89f8a9d..000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.c +++ /dev/null @@ -1,31 +0,0 @@ -/* - * positionKalmanFilter1D_dT_initialize.c - * - * Code generation for function 'positionKalmanFilter1D_dT_initialize' - * - * C source code generated on: Fri Nov 30 17:37:33 2012 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "positionKalmanFilter1D_dT.h" -#include "positionKalmanFilter1D_dT_initialize.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void positionKalmanFilter1D_dT_initialize(void) -{ - rt_InitInfAndNaN(8U); -} - -/* End of code generation (positionKalmanFilter1D_dT_initialize.c) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.h deleted file mode 100755 index 8d358a9a3..000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.h +++ /dev/null @@ -1,31 +0,0 @@ -/* - * positionKalmanFilter1D_dT_initialize.h - * - * Code generation for function 'positionKalmanFilter1D_dT_initialize' - * - * C source code generated on: Fri Nov 30 17:37:33 2012 - * - */ - -#ifndef __POSITIONKALMANFILTER1D_DT_INITIALIZE_H__ -#define __POSITIONKALMANFILTER1D_DT_INITIALIZE_H__ -/* Include files */ -#include <math.h> -#include <stddef.h> -#include <stdlib.h> - -#include "rtwtypes.h" -#include "positionKalmanFilter1D_dT_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void positionKalmanFilter1D_dT_initialize(void); -#endif -/* End of code generation (positionKalmanFilter1D_dT_initialize.h) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.c deleted file mode 100755 index 20ed2edbb..000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.c +++ /dev/null @@ -1,31 +0,0 @@ -/* - * positionKalmanFilter1D_dT_terminate.c - * - * Code generation for function 'positionKalmanFilter1D_dT_terminate' - * - * C source code generated on: Fri Nov 30 17:37:33 2012 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "positionKalmanFilter1D_dT.h" -#include "positionKalmanFilter1D_dT_terminate.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void positionKalmanFilter1D_dT_terminate(void) -{ - /* (no terminate code required) */ -} - -/* End of code generation (positionKalmanFilter1D_dT_terminate.c) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.h deleted file mode 100755 index 5eb5807a0..000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.h +++ /dev/null @@ -1,31 +0,0 @@ -/* - * positionKalmanFilter1D_dT_terminate.h - * - * Code generation for function 'positionKalmanFilter1D_dT_terminate' - * - * C source code generated on: Fri Nov 30 17:37:33 2012 - * - */ - -#ifndef __POSITIONKALMANFILTER1D_DT_TERMINATE_H__ -#define __POSITIONKALMANFILTER1D_DT_TERMINATE_H__ -/* Include files */ -#include <math.h> -#include <stddef.h> -#include <stdlib.h> - -#include "rtwtypes.h" -#include "positionKalmanFilter1D_dT_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void positionKalmanFilter1D_dT_terminate(void); -#endif -/* End of code generation (positionKalmanFilter1D_dT_terminate.h) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_types.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_types.h deleted file mode 100755 index 43e5f016c..000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_types.h +++ /dev/null @@ -1,16 +0,0 @@ -/* - * positionKalmanFilter1D_dT_types.h - * - * Code generation for function 'positionKalmanFilter1D_dT' - * - * C source code generated on: Fri Nov 30 17:37:33 2012 - * - */ - -#ifndef __POSITIONKALMANFILTER1D_DT_TYPES_H__ -#define __POSITIONKALMANFILTER1D_DT_TYPES_H__ - -/* Type Definitions */ - -#endif -/* End of code generation (positionKalmanFilter1D_dT_types.h) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.c deleted file mode 100755 index 5bd87c390..000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.c +++ /dev/null @@ -1,31 +0,0 @@ -/* - * positionKalmanFilter1D_initialize.c - * - * Code generation for function 'positionKalmanFilter1D_initialize' - * - * C source code generated on: Fri Nov 30 14:26:11 2012 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "positionKalmanFilter1D.h" -#include "positionKalmanFilter1D_initialize.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void positionKalmanFilter1D_initialize(void) -{ - rt_InitInfAndNaN(8U); -} - -/* End of code generation (positionKalmanFilter1D_initialize.c) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.h deleted file mode 100755 index 44bce472f..000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.h +++ /dev/null @@ -1,31 +0,0 @@ -/* - * positionKalmanFilter1D_initialize.h - * - * Code generation for function 'positionKalmanFilter1D_initialize' - * - * C source code generated on: Fri Nov 30 14:26:11 2012 - * - */ - -#ifndef __POSITIONKALMANFILTER1D_INITIALIZE_H__ -#define __POSITIONKALMANFILTER1D_INITIALIZE_H__ -/* Include files */ -#include <math.h> -#include <stddef.h> -#include <stdlib.h> - -#include "rtwtypes.h" -#include "positionKalmanFilter1D_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void positionKalmanFilter1D_initialize(void); -#endif -/* End of code generation (positionKalmanFilter1D_initialize.h) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.c deleted file mode 100755 index 41e11936f..000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.c +++ /dev/null @@ -1,31 +0,0 @@ -/* - * positionKalmanFilter1D_terminate.c - * - * Code generation for function 'positionKalmanFilter1D_terminate' - * - * C source code generated on: Fri Nov 30 14:26:11 2012 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "positionKalmanFilter1D.h" -#include "positionKalmanFilter1D_terminate.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void positionKalmanFilter1D_terminate(void) -{ - /* (no terminate code required) */ -} - -/* End of code generation (positionKalmanFilter1D_terminate.c) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.h deleted file mode 100755 index e84ea01bc..000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.h +++ /dev/null @@ -1,31 +0,0 @@ -/* - * positionKalmanFilter1D_terminate.h - * - * Code generation for function 'positionKalmanFilter1D_terminate' - * - * C source code generated on: Fri Nov 30 14:26:11 2012 - * - */ - -#ifndef __POSITIONKALMANFILTER1D_TERMINATE_H__ -#define __POSITIONKALMANFILTER1D_TERMINATE_H__ -/* Include files */ -#include <math.h> -#include <stddef.h> -#include <stdlib.h> - -#include "rtwtypes.h" -#include "positionKalmanFilter1D_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void positionKalmanFilter1D_terminate(void); -#endif -/* End of code generation (positionKalmanFilter1D_terminate.h) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_types.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_types.h deleted file mode 100755 index 4b473f56f..000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_types.h +++ /dev/null @@ -1,16 +0,0 @@ -/* - * positionKalmanFilter1D_types.h - * - * Code generation for function 'positionKalmanFilter1D' - * - * C source code generated on: Fri Nov 30 14:26:11 2012 - * - */ - -#ifndef __POSITIONKALMANFILTER1D_TYPES_H__ -#define __POSITIONKALMANFILTER1D_TYPES_H__ - -/* Type Definitions */ - -#endif -/* End of code generation (positionKalmanFilter1D_types.h) */ diff --git a/src/modules/position_estimator_mc/codegen/randn.c b/src/modules/position_estimator_mc/codegen/randn.c deleted file mode 100755 index 51aef7b76..000000000 --- a/src/modules/position_estimator_mc/codegen/randn.c +++ /dev/null @@ -1,524 +0,0 @@ -/*
- * randn.c
- *
- * Code generation for function 'randn'
- *
- * C source code generated on: Tue Feb 19 15:26:32 2013
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "kalman_dlqe3.h"
-#include "randn.h"
-#include "kalman_dlqe3_data.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-static uint32_T d_state[625];
-
-/* Function Declarations */
-static real_T b_genrandu(uint32_T mt[625]);
-static real_T eml_rand_mt19937ar(uint32_T e_state[625]);
-static real_T eml_rand_shr3cong(uint32_T e_state[2]);
-static void genrand_uint32_vector(uint32_T mt[625], uint32_T u[2]);
-static void genrandu(uint32_T s, uint32_T *e_state, real_T *r);
-static void twister_state_vector(uint32_T mt[625], real_T seed);
-
-/* Function Definitions */
-static real_T b_genrandu(uint32_T mt[625])
-{
- real_T r;
- int32_T exitg1;
- uint32_T u[2];
- boolean_T isvalid;
- int32_T k;
- boolean_T exitg2;
-
- /* <LEGAL> This is a uniform (0,1) pseudorandom number generator based on: */
- /* <LEGAL> */
- /* <LEGAL> A C-program for MT19937, with initialization improved 2002/1/26. */
- /* <LEGAL> Coded by Takuji Nishimura and Makoto Matsumoto. */
- /* <LEGAL> */
- /* <LEGAL> Copyright (C) 1997 - 2002, Makoto Matsumoto and Takuji Nishimura, */
- /* <LEGAL> All rights reserved. */
- /* <LEGAL> */
- /* <LEGAL> Redistribution and use in source and binary forms, with or without */
- /* <LEGAL> modification, are permitted provided that the following conditions */
- /* <LEGAL> are met: */
- /* <LEGAL> */
- /* <LEGAL> 1. Redistributions of source code must retain the above copyright */
- /* <LEGAL> notice, this list of conditions and the following disclaimer. */
- /* <LEGAL> */
- /* <LEGAL> 2. Redistributions in binary form must reproduce the above copyright */
- /* <LEGAL> notice, this list of conditions and the following disclaimer in the */
- /* <LEGAL> documentation and/or other materials provided with the distribution. */
- /* <LEGAL> */
- /* <LEGAL> 3. The names of its contributors may not be used to endorse or promote */
- /* <LEGAL> products derived from this software without specific prior written */
- /* <LEGAL> permission. */
- /* <LEGAL> */
- /* <LEGAL> THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS */
- /* <LEGAL> "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT */
- /* <LEGAL> LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR */
- /* <LEGAL> A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR */
- /* <LEGAL> CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, */
- /* <LEGAL> EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, */
- /* <LEGAL> PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR */
- /* <LEGAL> PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF */
- /* <LEGAL> LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING */
- /* <LEGAL> NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS */
- /* <LEGAL> SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */
- do {
- exitg1 = 0;
- genrand_uint32_vector(mt, u);
- r = 1.1102230246251565E-16 * ((real_T)(u[0] >> 5U) * 6.7108864E+7 + (real_T)
- (u[1] >> 6U));
- if (r == 0.0) {
- if ((mt[624] >= 1U) && (mt[624] < 625U)) {
- isvalid = TRUE;
- } else {
- isvalid = FALSE;
- }
-
- if (isvalid) {
- isvalid = FALSE;
- k = 1;
- exitg2 = FALSE;
- while ((exitg2 == FALSE) && (k < 625)) {
- if (mt[k - 1] == 0U) {
- k++;
- } else {
- isvalid = TRUE;
- exitg2 = TRUE;
- }
- }
- }
-
- if (!isvalid) {
- twister_state_vector(mt, 5489.0);
- }
- } else {
- exitg1 = 1;
- }
- } while (exitg1 == 0);
-
- return r;
-}
-
-static real_T eml_rand_mt19937ar(uint32_T e_state[625])
-{
- real_T r;
- int32_T exitg1;
- uint32_T u32[2];
- int32_T i;
- static const real_T dv1[257] = { 0.0, 0.215241895984875, 0.286174591792068,
- 0.335737519214422, 0.375121332878378, 0.408389134611989, 0.43751840220787,
- 0.46363433679088, 0.487443966139235, 0.50942332960209, 0.529909720661557,
- 0.549151702327164, 0.567338257053817, 0.584616766106378, 0.601104617755991,
- 0.61689699000775, 0.63207223638606, 0.646695714894993, 0.660822574244419,
- 0.674499822837293, 0.687767892795788, 0.700661841106814, 0.713212285190975,
- 0.725446140909999, 0.737387211434295, 0.749056662017815, 0.760473406430107,
- 0.771654424224568, 0.782615023307232, 0.793369058840623, 0.80392911698997,
- 0.814306670135215, 0.824512208752291, 0.834555354086381, 0.844444954909153,
- 0.854189171008163, 0.863795545553308, 0.87327106808886, 0.882622229585165,
- 0.891855070732941, 0.900975224461221, 0.909987953496718, 0.91889818364959,
- 0.927710533401999, 0.936429340286575, 0.945058684468165, 0.953602409881086,
- 0.96206414322304, 0.970447311064224, 0.978755155294224, 0.986990747099062,
- 0.99515699963509, 1.00325667954467, 1.01129241744, 1.01926671746548,
- 1.02718196603564, 1.03504043983344, 1.04284431314415, 1.05059566459093,
- 1.05829648333067, 1.06594867476212, 1.07355406579244, 1.0811144097034,
- 1.08863139065398, 1.09610662785202, 1.10354167942464, 1.11093804601357,
- 1.11829717411934, 1.12562045921553, 1.13290924865253, 1.14016484436815,
- 1.14738850542085, 1.15458145035993, 1.16174485944561, 1.16887987673083,
- 1.17598761201545, 1.18306914268269, 1.19012551542669, 1.19715774787944,
- 1.20416683014438, 1.2111537262437, 1.21811937548548, 1.22506469375653,
- 1.23199057474614, 1.23889789110569, 1.24578749554863, 1.2526602218949,
- 1.25951688606371, 1.26635828701823, 1.27318520766536, 1.27999841571382,
- 1.28679866449324, 1.29358669373695, 1.30036323033084, 1.30712898903073,
- 1.31388467315022, 1.32063097522106, 1.32736857762793, 1.33409815321936,
- 1.3408203658964, 1.34753587118059, 1.35424531676263, 1.36094934303328,
- 1.36764858359748, 1.37434366577317, 1.38103521107586, 1.38772383568998,
- 1.39441015092814, 1.40109476367925, 1.4077782768464, 1.41446128977547,
- 1.42114439867531, 1.42782819703026, 1.43451327600589, 1.44120022484872,
- 1.44788963128058, 1.45458208188841, 1.46127816251028, 1.46797845861808,
- 1.47468355569786, 1.48139403962819, 1.48811049705745, 1.49483351578049,
- 1.50156368511546, 1.50830159628131, 1.51504784277671, 1.521803020761,
- 1.52856772943771, 1.53534257144151, 1.542128153229, 1.54892508547417,
- 1.55573398346918, 1.56255546753104, 1.56939016341512, 1.57623870273591,
- 1.58310172339603, 1.58997987002419, 1.59687379442279, 1.60378415602609,
- 1.61071162236983, 1.61765686957301, 1.62462058283303, 1.63160345693487,
- 1.63860619677555, 1.64562951790478, 1.65267414708306, 1.65974082285818,
- 1.66683029616166, 1.67394333092612, 1.68108070472517, 1.68824320943719,
- 1.69543165193456, 1.70264685479992, 1.7098896570713, 1.71716091501782,
- 1.72446150294804, 1.73179231405296, 1.73915426128591, 1.74654827828172,
- 1.75397532031767, 1.76143636531891, 1.76893241491127, 1.77646449552452,
- 1.78403365954944, 1.79164098655216, 1.79928758454972, 1.80697459135082,
- 1.81470317596628, 1.82247454009388, 1.83028991968276, 1.83815058658281,
- 1.84605785028518, 1.8540130597602, 1.86201760539967, 1.87007292107127,
- 1.878180486293, 1.88634182853678, 1.8945585256707, 1.90283220855043,
- 1.91116456377125, 1.91955733659319, 1.92801233405266, 1.93653142827569,
- 1.94511656000868, 1.95376974238465, 1.96249306494436, 1.97128869793366,
- 1.98015889690048, 1.98910600761744, 1.99813247135842, 2.00724083056053,
- 2.0164337349062, 2.02571394786385, 2.03508435372962, 2.04454796521753,
- 2.05410793165065, 2.06376754781173, 2.07353026351874, 2.0833996939983,
- 2.09337963113879, 2.10347405571488, 2.11368715068665, 2.12402331568952,
- 2.13448718284602, 2.14508363404789, 2.15581781987674, 2.16669518035431,
- 2.17772146774029, 2.18890277162636, 2.20024554661128, 2.21175664288416,
- 2.22344334009251, 2.23531338492992, 2.24737503294739, 2.25963709517379,
- 2.27210899022838, 2.28480080272449, 2.29772334890286, 2.31088825060137,
- 2.32430801887113, 2.33799614879653, 2.35196722737914, 2.36623705671729,
- 2.38082279517208, 2.39574311978193, 2.41101841390112, 2.42667098493715,
- 2.44272531820036, 2.4592083743347, 2.47614993967052, 2.49358304127105,
- 2.51154444162669, 2.53007523215985, 2.54922155032478, 2.56903545268184,
- 2.58957598670829, 2.61091051848882, 2.63311639363158, 2.65628303757674,
- 2.68051464328574, 2.70593365612306, 2.73268535904401, 2.76094400527999,
- 2.79092117400193, 2.82287739682644, 2.85713873087322, 2.89412105361341,
- 2.93436686720889, 2.97860327988184, 3.02783779176959, 3.08352613200214,
- 3.147889289518, 3.2245750520478, 3.32024473383983, 3.44927829856143,
- 3.65415288536101, 3.91075795952492 };
-
- real_T u;
- static const real_T dv2[257] = { 1.0, 0.977101701267673, 0.959879091800108,
- 0.9451989534423, 0.932060075959231, 0.919991505039348, 0.908726440052131,
- 0.898095921898344, 0.887984660755834, 0.878309655808918, 0.869008688036857,
- 0.860033621196332, 0.851346258458678, 0.842915653112205, 0.834716292986884,
- 0.826726833946222, 0.818929191603703, 0.811307874312656, 0.803849483170964,
- 0.796542330422959, 0.789376143566025, 0.782341832654803, 0.775431304981187,
- 0.768637315798486, 0.761953346836795, 0.755373506507096, 0.748892447219157,
- 0.742505296340151, 0.736207598126863, 0.729995264561476, 0.72386453346863,
- 0.717811932630722, 0.711834248878248, 0.705928501332754, 0.700091918136512,
- 0.694321916126117, 0.688616083004672, 0.682972161644995, 0.677388036218774,
- 0.671861719897082, 0.66639134390875, 0.660975147776663, 0.655611470579697,
- 0.650298743110817, 0.645035480820822, 0.639820277453057, 0.634651799287624,
- 0.629528779924837, 0.624450015547027, 0.619414360605834, 0.614420723888914,
- 0.609468064925773, 0.604555390697468, 0.599681752619125, 0.594846243767987,
- 0.590047996332826, 0.585286179263371, 0.580559996100791, 0.575868682972354,
- 0.571211506735253, 0.566587763256165, 0.561996775814525, 0.557437893618766,
- 0.552910490425833, 0.548413963255266, 0.543947731190026, 0.539511234256952,
- 0.535103932380458, 0.530725304403662, 0.526374847171684, 0.522052074672322,
- 0.517756517229756, 0.513487720747327, 0.509245245995748, 0.505028667943468,
- 0.500837575126149, 0.49667156905249, 0.492530263643869, 0.488413284705458,
- 0.484320269426683, 0.480250865909047, 0.476204732719506, 0.47218153846773,
- 0.468180961405694, 0.464202689048174, 0.460246417812843, 0.456311852678716,
- 0.452398706861849, 0.448506701507203, 0.444635565395739, 0.440785034665804,
- 0.436954852547985, 0.433144769112652, 0.429354541029442, 0.425583931338022,
- 0.421832709229496, 0.418100649837848, 0.414387534040891, 0.410693148270188,
- 0.407017284329473, 0.403359739221114, 0.399720314980197, 0.396098818515832,
- 0.392495061459315, 0.388908860018789, 0.385340034840077, 0.381788410873393,
- 0.378253817245619, 0.374736087137891, 0.371235057668239, 0.367750569779032,
- 0.364282468129004, 0.360830600989648, 0.357394820145781, 0.353974980800077,
- 0.350570941481406, 0.347182563956794, 0.343809713146851, 0.340452257044522,
- 0.337110066637006, 0.333783015830718, 0.330470981379163, 0.327173842813601,
- 0.323891482376391, 0.320623784956905, 0.317370638029914, 0.314131931596337,
- 0.310907558126286, 0.307697412504292, 0.30450139197665, 0.301319396100803,
- 0.298151326696685, 0.294997087799962, 0.291856585617095, 0.288729728482183,
- 0.285616426815502, 0.282516593083708, 0.279430141761638, 0.276356989295668,
- 0.273297054068577, 0.270250256365875, 0.267216518343561, 0.264195763997261,
- 0.261187919132721, 0.258192911337619, 0.255210669954662, 0.252241126055942,
- 0.249284212418529, 0.246339863501264, 0.24340801542275, 0.240488605940501,
- 0.237581574431238, 0.23468686187233, 0.231804410824339, 0.228934165414681,
- 0.226076071322381, 0.223230075763918, 0.220396127480152, 0.217574176724331,
- 0.214764175251174, 0.211966076307031, 0.209179834621125, 0.206405406397881,
- 0.203642749310335, 0.200891822494657, 0.198152586545776, 0.195425003514135,
- 0.192709036903589, 0.190004651670465, 0.187311814223801, 0.1846304924268,
- 0.181960655599523, 0.179302274522848, 0.176655321443735, 0.174019770081839,
- 0.171395595637506, 0.168782774801212, 0.166181285764482, 0.163591108232366,
- 0.161012223437511, 0.158444614155925, 0.15588826472448, 0.153343161060263,
- 0.150809290681846, 0.148286642732575, 0.145775208005994, 0.143274978973514,
- 0.140785949814445, 0.138308116448551, 0.135841476571254, 0.133386029691669,
- 0.130941777173644, 0.12850872228, 0.126086870220186, 0.123676228201597,
- 0.12127680548479, 0.11888861344291, 0.116511665625611, 0.114145977827839,
- 0.111791568163838, 0.109448457146812, 0.107116667774684, 0.104796225622487,
- 0.102487158941935, 0.10018949876881, 0.0979032790388625, 0.095628536713009,
- 0.093365311912691, 0.0911136480663738, 0.0888735920682759,
- 0.0866451944505581, 0.0844285095703535, 0.082223595813203,
- 0.0800305158146631, 0.0778493367020961, 0.0756801303589272,
- 0.0735229737139814, 0.0713779490588905, 0.0692451443970068,
- 0.0671246538277886, 0.065016577971243, 0.0629210244377582, 0.06083810834954,
- 0.0587679529209339, 0.0567106901062031, 0.0546664613248891,
- 0.0526354182767924, 0.0506177238609479, 0.0486135532158687,
- 0.0466230949019305, 0.0446465522512946, 0.0426841449164746,
- 0.0407361106559411, 0.0388027074045262, 0.0368842156885674,
- 0.0349809414617162, 0.0330932194585786, 0.0312214171919203,
- 0.0293659397581334, 0.0275272356696031, 0.0257058040085489,
- 0.0239022033057959, 0.0221170627073089, 0.0203510962300445,
- 0.0186051212757247, 0.0168800831525432, 0.0151770883079353,
- 0.0134974506017399, 0.0118427578579079, 0.0102149714397015,
- 0.00861658276939875, 0.00705087547137324, 0.00552240329925101,
- 0.00403797259336304, 0.00260907274610216, 0.0012602859304986,
- 0.000477467764609386 };
-
- real_T x;
- do {
- exitg1 = 0;
- genrand_uint32_vector(e_state, u32);
- i = (int32_T)((u32[1] >> 24U) + 1U);
- r = (((real_T)(u32[0] >> 3U) * 1.6777216E+7 + (real_T)((int32_T)u32[1] &
- 16777215)) * 2.2204460492503131E-16 - 1.0) * dv1[i];
- if (fabs(r) <= dv1[i - 1]) {
- exitg1 = 1;
- } else if (i < 256) {
- u = b_genrandu(e_state);
- if (dv2[i] + u * (dv2[i - 1] - dv2[i]) < exp(-0.5 * r * r)) {
- exitg1 = 1;
- }
- } else {
- do {
- u = b_genrandu(e_state);
- x = log(u) * 0.273661237329758;
- u = b_genrandu(e_state);
- } while (!(-2.0 * log(u) > x * x));
-
- if (r < 0.0) {
- r = x - 3.65415288536101;
- } else {
- r = 3.65415288536101 - x;
- }
-
- exitg1 = 1;
- }
- } while (exitg1 == 0);
-
- return r;
-}
-
-static real_T eml_rand_shr3cong(uint32_T e_state[2])
-{
- real_T r;
- uint32_T icng;
- uint32_T jsr;
- uint32_T ui;
- int32_T j;
- static const real_T dv0[65] = { 0.340945, 0.4573146, 0.5397793, 0.6062427,
- 0.6631691, 0.7136975, 0.7596125, 0.8020356, 0.8417227, 0.8792102, 0.9148948,
- 0.9490791, 0.9820005, 1.0138492, 1.044781, 1.0749254, 1.1043917, 1.1332738,
- 1.161653, 1.189601, 1.2171815, 1.2444516, 1.2714635, 1.298265, 1.3249008,
- 1.3514125, 1.3778399, 1.4042211, 1.4305929, 1.4569915, 1.4834527, 1.5100122,
- 1.5367061, 1.5635712, 1.5906454, 1.617968, 1.6455802, 1.6735255, 1.7018503,
- 1.7306045, 1.7598422, 1.7896223, 1.8200099, 1.851077, 1.8829044, 1.9155831,
- 1.9492166, 1.9839239, 2.0198431, 2.0571356, 2.095993, 2.136645, 2.1793713,
- 2.2245175, 2.2725186, 2.3239338, 2.3795008, 2.4402218, 2.5075117, 2.5834658,
- 2.6713916, 2.7769942, 2.7769942, 2.7769942, 2.7769942 };
-
- real_T x;
- real_T y;
- real_T s;
- icng = 69069U * e_state[0] + 1234567U;
- jsr = e_state[1] ^ e_state[1] << 13U;
- jsr ^= jsr >> 17U;
- jsr ^= jsr << 5U;
- ui = icng + jsr;
- j = (int32_T)(ui & 63U);
- r = (real_T)(int32_T)ui * 4.6566128730773926E-10 * dv0[j + 1];
- if (fabs(r) <= dv0[j]) {
- } else {
- x = (fabs(r) - dv0[j]) / (dv0[j + 1] - dv0[j]);
- icng = 69069U * icng + 1234567U;
- jsr ^= jsr << 13U;
- jsr ^= jsr >> 17U;
- jsr ^= jsr << 5U;
- y = (real_T)(int32_T)(icng + jsr) * 2.328306436538696E-10;
- s = x + (0.5 + y);
- if (s > 1.301198) {
- if (r < 0.0) {
- r = 0.4878992 * x - 0.4878992;
- } else {
- r = 0.4878992 - 0.4878992 * x;
- }
- } else if (s <= 0.9689279) {
- } else {
- s = 0.4878992 * x;
- x = 0.4878992 - 0.4878992 * x;
- if (0.5 + y > 12.67706 - 12.37586 * exp(-0.5 * (0.4878992 - s) * x)) {
- if (r < 0.0) {
- r = -(0.4878992 - s);
- } else {
- r = 0.4878992 - s;
- }
- } else if (exp(-0.5 * dv0[j + 1] * dv0[j + 1]) + (0.5 + y) * 0.01958303 /
- dv0[j + 1] <= exp(-0.5 * r * r)) {
- } else {
- do {
- icng = 69069U * icng + 1234567U;
- jsr ^= jsr << 13U;
- jsr ^= jsr >> 17U;
- jsr ^= jsr << 5U;
- x = log(0.5 + (real_T)(int32_T)(icng + jsr) * 2.328306436538696E-10) /
- 2.776994;
- icng = 69069U * icng + 1234567U;
- jsr ^= jsr << 13U;
- jsr ^= jsr >> 17U;
- jsr ^= jsr << 5U;
- } while (!(-2.0 * log(0.5 + (real_T)(int32_T)(icng + jsr) *
- 2.328306436538696E-10) > x * x));
-
- if (r < 0.0) {
- r = x - 2.776994;
- } else {
- r = 2.776994 - x;
- }
- }
- }
- }
-
- e_state[0] = icng;
- e_state[1] = jsr;
- return r;
-}
-
-static void genrand_uint32_vector(uint32_T mt[625], uint32_T u[2])
-{
- int32_T i;
- uint32_T mti;
- int32_T kk;
- uint32_T y;
- uint32_T b_y;
- uint32_T c_y;
- uint32_T d_y;
- for (i = 0; i < 2; i++) {
- u[i] = 0U;
- }
-
- for (i = 0; i < 2; i++) {
- mti = mt[624] + 1U;
- if (mti >= 625U) {
- for (kk = 0; kk < 227; kk++) {
- y = (mt[kk] & 2147483648U) | (mt[1 + kk] & 2147483647U);
- if ((int32_T)(y & 1U) == 0) {
- b_y = y >> 1U;
- } else {
- b_y = y >> 1U ^ 2567483615U;
- }
-
- mt[kk] = mt[397 + kk] ^ b_y;
- }
-
- for (kk = 0; kk < 396; kk++) {
- y = (mt[227 + kk] & 2147483648U) | (mt[228 + kk] & 2147483647U);
- if ((int32_T)(y & 1U) == 0) {
- c_y = y >> 1U;
- } else {
- c_y = y >> 1U ^ 2567483615U;
- }
-
- mt[227 + kk] = mt[kk] ^ c_y;
- }
-
- y = (mt[623] & 2147483648U) | (mt[0] & 2147483647U);
- if ((int32_T)(y & 1U) == 0) {
- d_y = y >> 1U;
- } else {
- d_y = y >> 1U ^ 2567483615U;
- }
-
- mt[623] = mt[396] ^ d_y;
- mti = 1U;
- }
-
- y = mt[(int32_T)mti - 1];
- mt[624] = mti;
- y ^= y >> 11U;
- y ^= y << 7U & 2636928640U;
- y ^= y << 15U & 4022730752U;
- y ^= y >> 18U;
- u[i] = y;
- }
-}
-
-static void genrandu(uint32_T s, uint32_T *e_state, real_T *r)
-{
- int32_T hi;
- uint32_T test1;
- uint32_T test2;
- hi = (int32_T)(s / 127773U);
- test1 = 16807U * (s - (uint32_T)hi * 127773U);
- test2 = 2836U * (uint32_T)hi;
- if (test1 < test2) {
- *e_state = (test1 - test2) + 2147483647U;
- } else {
- *e_state = test1 - test2;
- }
-
- *r = (real_T)*e_state * 4.6566128752457969E-10;
-}
-
-static void twister_state_vector(uint32_T mt[625], real_T seed)
-{
- uint32_T r;
- int32_T mti;
- if (seed < 4.294967296E+9) {
- if (seed >= 0.0) {
- r = (uint32_T)seed;
- } else {
- r = 0U;
- }
- } else if (seed >= 4.294967296E+9) {
- r = MAX_uint32_T;
- } else {
- r = 0U;
- }
-
- mt[0] = r;
- for (mti = 0; mti < 623; mti++) {
- r = (r ^ r >> 30U) * 1812433253U + (uint32_T)(1 + mti);
- mt[1 + mti] = r;
- }
-
- mt[624] = 624U;
-}
-
-real_T randn(void)
-{
- real_T r;
- uint32_T e_state;
- real_T t;
- real_T b_r;
- uint32_T f_state;
- if (method == 0U) {
- if (b_method == 4U) {
- do {
- genrandu(b_state, &e_state, &r);
- genrandu(e_state, &b_state, &t);
- b_r = 2.0 * r - 1.0;
- t = 2.0 * t - 1.0;
- t = t * t + b_r * b_r;
- } while (!(t <= 1.0));
-
- r = (2.0 * r - 1.0) * sqrt(-2.0 * log(t) / t);
- } else if (b_method == 5U) {
- r = eml_rand_shr3cong(c_state);
- } else {
- if (!state_not_empty) {
- memset(&d_state[0], 0, 625U * sizeof(uint32_T));
- twister_state_vector(d_state, 5489.0);
- state_not_empty = TRUE;
- }
-
- r = eml_rand_mt19937ar(d_state);
- }
- } else if (method == 4U) {
- e_state = state[0];
- do {
- genrandu(e_state, &f_state, &r);
- genrandu(f_state, &e_state, &t);
- b_r = 2.0 * r - 1.0;
- t = 2.0 * t - 1.0;
- t = t * t + b_r * b_r;
- } while (!(t <= 1.0));
-
- state[0] = e_state;
- r = (2.0 * r - 1.0) * sqrt(-2.0 * log(t) / t);
- } else {
- r = eml_rand_shr3cong(state);
- }
-
- return r;
-}
-
-/* End of code generation (randn.c) */
diff --git a/src/modules/position_estimator_mc/codegen/randn.h b/src/modules/position_estimator_mc/codegen/randn.h deleted file mode 100755 index 8a2aa9277..000000000 --- a/src/modules/position_estimator_mc/codegen/randn.h +++ /dev/null @@ -1,33 +0,0 @@ -/*
- * randn.h
- *
- * Code generation for function 'randn'
- *
- * C source code generated on: Tue Feb 19 15:26:32 2013
- *
- */
-
-#ifndef __RANDN_H__
-#define __RANDN_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "kalman_dlqe3_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern real_T randn(void);
-#endif
-/* End of code generation (randn.h) */
diff --git a/src/modules/position_estimator_mc/codegen/rtGetInf.c b/src/modules/position_estimator_mc/codegen/rtGetInf.c deleted file mode 100755 index c6fa7884e..000000000 --- a/src/modules/position_estimator_mc/codegen/rtGetInf.c +++ /dev/null @@ -1,139 +0,0 @@ -/* - * rtGetInf.c - * - * Code generation for function 'kalman_dlqe2' - * - * C source code generated on: Thu Feb 14 12:52:29 2013 - * - */ - -/* - * Abstract: - * MATLAB for code generation function to initialize non-finite, Inf and MinusInf - */ -#include "rtGetInf.h" -#define NumBitsPerChar 8U - -/* Function: rtGetInf ================================================== - * Abstract: - * Initialize rtInf needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -real_T rtGetInf(void) -{ - size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); - real_T inf = 0.0; - if (bitsPerReal == 32U) { - inf = rtGetInfF(); - } else { - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0x7FF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - inf = tmpVal.fltVal; - break; - } - - case BigEndian: - { - union { - BigEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0x7FF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - inf = tmpVal.fltVal; - break; - } - } - } - - return inf; -} - -/* Function: rtGetInfF ================================================== - * Abstract: - * Initialize rtInfF needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -real32_T rtGetInfF(void) -{ - IEEESingle infF; - infF.wordL.wordLuint = 0x7F800000U; - return infF.wordL.wordLreal; -} - -/* Function: rtGetMinusInf ================================================== - * Abstract: - * Initialize rtMinusInf needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -real_T rtGetMinusInf(void) -{ - size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); - real_T minf = 0.0; - if (bitsPerReal == 32U) { - minf = rtGetMinusInfF(); - } else { - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0xFFF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - minf = tmpVal.fltVal; - break; - } - - case BigEndian: - { - union { - BigEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0xFFF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - minf = tmpVal.fltVal; - break; - } - } - } - - return minf; -} - -/* Function: rtGetMinusInfF ================================================== - * Abstract: - * Initialize rtMinusInfF needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -real32_T rtGetMinusInfF(void) -{ - IEEESingle minfF; - minfF.wordL.wordLuint = 0xFF800000U; - return minfF.wordL.wordLreal; -} - -/* End of code generation (rtGetInf.c) */ diff --git a/src/modules/position_estimator_mc/codegen/rtGetInf.h b/src/modules/position_estimator_mc/codegen/rtGetInf.h deleted file mode 100755 index e7b2a2d1c..000000000 --- a/src/modules/position_estimator_mc/codegen/rtGetInf.h +++ /dev/null @@ -1,23 +0,0 @@ -/* - * rtGetInf.h - * - * Code generation for function 'kalman_dlqe2' - * - * C source code generated on: Thu Feb 14 12:52:29 2013 - * - */ - -#ifndef __RTGETINF_H__ -#define __RTGETINF_H__ - -#include <stddef.h> -#include "rtwtypes.h" -#include "rt_nonfinite.h" - -extern real_T rtGetInf(void); -extern real32_T rtGetInfF(void); -extern real_T rtGetMinusInf(void); -extern real32_T rtGetMinusInfF(void); - -#endif -/* End of code generation (rtGetInf.h) */ diff --git a/src/modules/position_estimator_mc/codegen/rtGetNaN.c b/src/modules/position_estimator_mc/codegen/rtGetNaN.c deleted file mode 100755 index 552770149..000000000 --- a/src/modules/position_estimator_mc/codegen/rtGetNaN.c +++ /dev/null @@ -1,96 +0,0 @@ -/* - * rtGetNaN.c - * - * Code generation for function 'kalman_dlqe2' - * - * C source code generated on: Thu Feb 14 12:52:29 2013 - * - */ - -/* - * Abstract: - * MATLAB for code generation function to initialize non-finite, NaN - */ -#include "rtGetNaN.h" -#define NumBitsPerChar 8U - -/* Function: rtGetNaN ================================================== - * Abstract: - * Initialize rtNaN needed by the generated code. - * NaN is initialized as non-signaling. Assumes IEEE. - */ -real_T rtGetNaN(void) -{ - size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); - real_T nan = 0.0; - if (bitsPerReal == 32U) { - nan = rtGetNaNF(); - } else { - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0xFFF80000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - nan = tmpVal.fltVal; - break; - } - - case BigEndian: - { - union { - BigEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0x7FFFFFFFU; - tmpVal.bitVal.words.wordL = 0xFFFFFFFFU; - nan = tmpVal.fltVal; - break; - } - } - } - - return nan; -} - -/* Function: rtGetNaNF ================================================== - * Abstract: - * Initialize rtNaNF needed by the generated code. - * NaN is initialized as non-signaling. Assumes IEEE. - */ -real32_T rtGetNaNF(void) -{ - IEEESingle nanF = { { 0 } }; - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - nanF.wordL.wordLuint = 0xFFC00000U; - break; - } - - case BigEndian: - { - nanF.wordL.wordLuint = 0x7FFFFFFFU; - break; - } - } - - return nanF.wordL.wordLreal; -} - -/* End of code generation (rtGetNaN.c) */ diff --git a/src/modules/position_estimator_mc/codegen/rtGetNaN.h b/src/modules/position_estimator_mc/codegen/rtGetNaN.h deleted file mode 100755 index 5acdd9790..000000000 --- a/src/modules/position_estimator_mc/codegen/rtGetNaN.h +++ /dev/null @@ -1,21 +0,0 @@ -/* - * rtGetNaN.h - * - * Code generation for function 'kalman_dlqe2' - * - * C source code generated on: Thu Feb 14 12:52:29 2013 - * - */ - -#ifndef __RTGETNAN_H__ -#define __RTGETNAN_H__ - -#include <stddef.h> -#include "rtwtypes.h" -#include "rt_nonfinite.h" - -extern real_T rtGetNaN(void); -extern real32_T rtGetNaNF(void); - -#endif -/* End of code generation (rtGetNaN.h) */ diff --git a/src/modules/position_estimator_mc/codegen/rt_nonfinite.c b/src/modules/position_estimator_mc/codegen/rt_nonfinite.c deleted file mode 100755 index de121c4a0..000000000 --- a/src/modules/position_estimator_mc/codegen/rt_nonfinite.c +++ /dev/null @@ -1,87 +0,0 @@ -/* - * rt_nonfinite.c - * - * Code generation for function 'kalman_dlqe2' - * - * C source code generated on: Thu Feb 14 12:52:29 2013 - * - */ - -/* - * Abstract: - * MATLAB for code generation function to initialize non-finites, - * (Inf, NaN and -Inf). - */ -#include "rt_nonfinite.h" -#include "rtGetNaN.h" -#include "rtGetInf.h" - -real_T rtInf; -real_T rtMinusInf; -real_T rtNaN; -real32_T rtInfF; -real32_T rtMinusInfF; -real32_T rtNaNF; - -/* Function: rt_InitInfAndNaN ================================================== - * Abstract: - * Initialize the rtInf, rtMinusInf, and rtNaN needed by the - * generated code. NaN is initialized as non-signaling. Assumes IEEE. - */ -void rt_InitInfAndNaN(size_t realSize) -{ - (void) (realSize); - rtNaN = rtGetNaN(); - rtNaNF = rtGetNaNF(); - rtInf = rtGetInf(); - rtInfF = rtGetInfF(); - rtMinusInf = rtGetMinusInf(); - rtMinusInfF = rtGetMinusInfF(); -} - -/* Function: rtIsInf ================================================== - * Abstract: - * Test if value is infinite - */ -boolean_T rtIsInf(real_T value) -{ - return ((value==rtInf || value==rtMinusInf) ? 1U : 0U); -} - -/* Function: rtIsInfF ================================================= - * Abstract: - * Test if single-precision value is infinite - */ -boolean_T rtIsInfF(real32_T value) -{ - return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U); -} - -/* Function: rtIsNaN ================================================== - * Abstract: - * Test if value is not a number - */ -boolean_T rtIsNaN(real_T value) -{ -#if defined(_MSC_VER) && (_MSC_VER <= 1200) - return _isnan(value)? TRUE:FALSE; -#else - return (value!=value)? 1U:0U; -#endif -} - -/* Function: rtIsNaNF ================================================= - * Abstract: - * Test if single-precision value is not a number - */ -boolean_T rtIsNaNF(real32_T value) -{ -#if defined(_MSC_VER) && (_MSC_VER <= 1200) - return _isnan((real_T)value)? true:false; -#else - return (value!=value)? 1U:0U; -#endif -} - - -/* End of code generation (rt_nonfinite.c) */ diff --git a/src/modules/position_estimator_mc/codegen/rt_nonfinite.h b/src/modules/position_estimator_mc/codegen/rt_nonfinite.h deleted file mode 100755 index 3bbcfd087..000000000 --- a/src/modules/position_estimator_mc/codegen/rt_nonfinite.h +++ /dev/null @@ -1,53 +0,0 @@ -/* - * rt_nonfinite.h - * - * Code generation for function 'kalman_dlqe2' - * - * C source code generated on: Thu Feb 14 12:52:29 2013 - * - */ - -#ifndef __RT_NONFINITE_H__ -#define __RT_NONFINITE_H__ - -#if defined(_MSC_VER) && (_MSC_VER <= 1200) -#include <float.h> -#endif -#include <stddef.h> -#include "rtwtypes.h" - -extern real_T rtInf; -extern real_T rtMinusInf; -extern real_T rtNaN; -extern real32_T rtInfF; -extern real32_T rtMinusInfF; -extern real32_T rtNaNF; -extern void rt_InitInfAndNaN(size_t realSize); -extern boolean_T rtIsInf(real_T value); -extern boolean_T rtIsInfF(real32_T value); -extern boolean_T rtIsNaN(real_T value); -extern boolean_T rtIsNaNF(real32_T value); - -typedef struct { - struct { - uint32_T wordH; - uint32_T wordL; - } words; -} BigEndianIEEEDouble; - -typedef struct { - struct { - uint32_T wordL; - uint32_T wordH; - } words; -} LittleEndianIEEEDouble; - -typedef struct { - union { - real32_T wordLreal; - uint32_T wordLuint; - } wordL; -} IEEESingle; - -#endif -/* End of code generation (rt_nonfinite.h) */ diff --git a/src/modules/position_estimator_mc/codegen/rtwtypes.h b/src/modules/position_estimator_mc/codegen/rtwtypes.h deleted file mode 100755 index 8916e8572..000000000 --- a/src/modules/position_estimator_mc/codegen/rtwtypes.h +++ /dev/null @@ -1,159 +0,0 @@ -/* - * rtwtypes.h - * - * Code generation for function 'kalman_dlqe2' - * - * C source code generated on: Thu Feb 14 12:52:29 2013 - * - */ - -#ifndef __RTWTYPES_H__ -#define __RTWTYPES_H__ -#ifndef TRUE -# define TRUE (1U) -#endif -#ifndef FALSE -# define FALSE (0U) -#endif -#ifndef __TMWTYPES__ -#define __TMWTYPES__ - -#include <limits.h> - -/*=======================================================================* - * Target hardware information - * Device type: Generic->MATLAB Host Computer - * Number of bits: char: 8 short: 16 int: 32 - * long: 32 native word size: 32 - * Byte ordering: LittleEndian - * Signed integer division rounds to: Undefined - * Shift right on a signed integer as arithmetic shift: off - *=======================================================================*/ - -/*=======================================================================* - * Fixed width word size data types: * - * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * - * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * - * real32_T, real64_T - 32 and 64 bit floating point numbers * - *=======================================================================*/ - -typedef signed char int8_T; -typedef unsigned char uint8_T; -typedef short int16_T; -typedef unsigned short uint16_T; -typedef int int32_T; -typedef unsigned int uint32_T; -typedef float real32_T; -typedef double real64_T; - -/*===========================================================================* - * Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T, * - * ulong_T, char_T and byte_T. * - *===========================================================================*/ - -typedef double real_T; -typedef double time_T; -typedef unsigned char boolean_T; -typedef int int_T; -typedef unsigned int uint_T; -typedef unsigned long ulong_T; -typedef char char_T; -typedef char_T byte_T; - -/*===========================================================================* - * Complex number type definitions * - *===========================================================================*/ -#define CREAL_T - typedef struct { - real32_T re; - real32_T im; - } creal32_T; - - typedef struct { - real64_T re; - real64_T im; - } creal64_T; - - typedef struct { - real_T re; - real_T im; - } creal_T; - - typedef struct { - int8_T re; - int8_T im; - } cint8_T; - - typedef struct { - uint8_T re; - uint8_T im; - } cuint8_T; - - typedef struct { - int16_T re; - int16_T im; - } cint16_T; - - typedef struct { - uint16_T re; - uint16_T im; - } cuint16_T; - - typedef struct { - int32_T re; - int32_T im; - } cint32_T; - - typedef struct { - uint32_T re; - uint32_T im; - } cuint32_T; - - -/*=======================================================================* - * Min and Max: * - * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * - * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * - *=======================================================================*/ - -#define MAX_int8_T ((int8_T)(127)) -#define MIN_int8_T ((int8_T)(-128)) -#define MAX_uint8_T ((uint8_T)(255)) -#define MIN_uint8_T ((uint8_T)(0)) -#define MAX_int16_T ((int16_T)(32767)) -#define MIN_int16_T ((int16_T)(-32768)) -#define MAX_uint16_T ((uint16_T)(65535)) -#define MIN_uint16_T ((uint16_T)(0)) -#define MAX_int32_T ((int32_T)(2147483647)) -#define MIN_int32_T ((int32_T)(-2147483647-1)) -#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) -#define MIN_uint32_T ((uint32_T)(0)) - -/* Logical type definitions */ -#if !defined(__cplusplus) && !defined(__true_false_are_keywords) -# ifndef false -# define false (0U) -# endif -# ifndef true -# define true (1U) -# endif -#endif - -/* - * MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation - * for signed integer values. - */ -#if ((SCHAR_MIN + 1) != -SCHAR_MAX) -#error "This code must be compiled using a 2's complement representation for signed integer values" -#endif - -/* - * Maximum length of a MATLAB identifier (function/variable) - * including the null-termination character. Referenced by - * rt_logging.c and rt_matrx.c. - */ -#define TMW_NAME_LENGTH_MAX 64 - -#endif -#endif -/* End of code generation (rtwtypes.h) */ diff --git a/src/modules/position_estimator_mc/kalman_dlqe1.m b/src/modules/position_estimator_mc/kalman_dlqe1.m deleted file mode 100755 index ff939d029..000000000 --- a/src/modules/position_estimator_mc/kalman_dlqe1.m +++ /dev/null @@ -1,3 +0,0 @@ -function [x_aposteriori] = kalman_dlqe1(A,C,K,x_aposteriori_k,z) - x_aposteriori=A*x_aposteriori_k+K*(z-C*A*x_aposteriori_k); -end
\ No newline at end of file diff --git a/src/modules/position_estimator_mc/kalman_dlqe2.m b/src/modules/position_estimator_mc/kalman_dlqe2.m deleted file mode 100755 index 2a50164ef..000000000 --- a/src/modules/position_estimator_mc/kalman_dlqe2.m +++ /dev/null @@ -1,9 +0,0 @@ -function [x_aposteriori] = kalman_dlqe2(dt,k1,k2,k3,x_aposteriori_k,z) - st = 1/2*dt^2; - A = [1,dt,st; - 0,1,dt; - 0,0,1]; - C=[1,0,0]; - K = [k1;k2;k3]; - x_aposteriori=A*x_aposteriori_k+K*(z-C*A*x_aposteriori_k); -end
\ No newline at end of file diff --git a/src/modules/position_estimator_mc/kalman_dlqe3.m b/src/modules/position_estimator_mc/kalman_dlqe3.m deleted file mode 100755 index 4c6421b7f..000000000 --- a/src/modules/position_estimator_mc/kalman_dlqe3.m +++ /dev/null @@ -1,17 +0,0 @@ -function [x_aposteriori] = kalman_dlqe3(dt,k1,k2,k3,x_aposteriori_k,z,posUpdate,addNoise,sigma)
- st = 1/2*dt^2;
- A = [1,dt,st;
- 0,1,dt;
- 0,0,1];
- C=[1,0,0];
- K = [k1;k2;k3];
- if addNoise==1
- noise = sigma*randn(1,1);
- z = z + noise;
- end
- if(posUpdate)
- x_aposteriori=A*x_aposteriori_k+K*(z-C*A*x_aposteriori_k);
- else
- x_aposteriori=A*x_aposteriori_k;
- end
-end
\ No newline at end of file diff --git a/src/modules/position_estimator_mc/module.mk b/src/modules/position_estimator_mc/module.mk deleted file mode 100644 index 40b135ea4..000000000 --- a/src/modules/position_estimator_mc/module.mk +++ /dev/null @@ -1,60 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 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. -# -############################################################################ - -# -# Makefile to build the position estimator -# - -MODULE_COMMAND = position_estimator_mc - -SRCS = position_estimator_mc_main.c \ - position_estimator_mc_params.c \ - codegen/positionKalmanFilter1D_initialize.c \ - codegen/positionKalmanFilter1D_terminate.c \ - codegen/positionKalmanFilter1D.c \ - codegen/rt_nonfinite.c \ - codegen/rtGetInf.c \ - codegen/rtGetNaN.c \ - codegen/positionKalmanFilter1D_dT_initialize.c \ - codegen/positionKalmanFilter1D_dT_terminate.c \ - codegen/kalman_dlqe1.c \ - codegen/kalman_dlqe1_initialize.c \ - codegen/kalman_dlqe1_terminate.c \ - codegen/kalman_dlqe2.c \ - codegen/kalman_dlqe2_initialize.c \ - codegen/kalman_dlqe2_terminate.c \ - codegen/kalman_dlqe3.c \ - codegen/kalman_dlqe3_initialize.c \ - codegen/kalman_dlqe3_terminate.c \ - codegen/kalman_dlqe3_data.c \ - codegen/randn.c diff --git a/src/modules/position_estimator_mc/positionKalmanFilter1D.m b/src/modules/position_estimator_mc/positionKalmanFilter1D.m deleted file mode 100755 index 144ff8c7c..000000000 --- a/src/modules/position_estimator_mc/positionKalmanFilter1D.m +++ /dev/null @@ -1,19 +0,0 @@ -function [x_aposteriori,P_aposteriori]=positionKalmanFilter1D(A,B,C,x_aposteriori_k,P_aposteriori_k,u,z,gps_update,Q,R,thresh,decay) -%prediction - x_apriori=A*x_aposteriori_k+B*u; - P_apriori=A*P_aposteriori_k*A'+Q; - if abs(u)<thresh - x_apriori(2)=decay*x_apriori(2); - end - %update - if gps_update==1 - y=z-C*x_apriori; - S=C*P_apriori*C'+R; - K=(P_apriori*C')/S; - x_aposteriori=x_apriori+K*y; - P_aposteriori=(eye(size(P_apriori))-K*C)*P_apriori; - else - x_aposteriori=x_apriori; - P_aposteriori=P_apriori; - end -end diff --git a/src/modules/position_estimator_mc/positionKalmanFilter1D_dT.m b/src/modules/position_estimator_mc/positionKalmanFilter1D_dT.m deleted file mode 100755 index f94cce1fb..000000000 --- a/src/modules/position_estimator_mc/positionKalmanFilter1D_dT.m +++ /dev/null @@ -1,26 +0,0 @@ -function [x_aposteriori,P_aposteriori]=positionKalmanFilter1D_dT(dT,x_aposteriori_k,P_aposteriori_k,u,z,gps_update,Q,R,thresh,decay) - %dynamics - A = [1 dT -0.5*dT*dT; - 0 1 -dT; - 0 0 1]; - B = [0.5*dT*dT; dT; 0]; - C = [1 0 0]; - %prediction - x_apriori=A*x_aposteriori_k+B*u; - P_apriori=A*P_aposteriori_k*A'+Q; - if abs(u)<thresh - x_apriori(2)=decay*x_apriori(2); - end - %update - if gps_update==1 - y=z-C*x_apriori; - S=C*P_apriori*C'+R; - K=(P_apriori*C')/S; - x_aposteriori=x_apriori+K*y; - P_aposteriori=(eye(size(P_apriori))-K*C)*P_apriori; - else - x_aposteriori=x_apriori; - P_aposteriori=P_apriori; - end -end - diff --git a/src/modules/position_estimator_mc/position_estimator_mc_main.c b/src/modules/position_estimator_mc/position_estimator_mc_main.c deleted file mode 100755 index 363961819..000000000 --- a/src/modules/position_estimator_mc/position_estimator_mc_main.c +++ /dev/null @@ -1,515 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: Damian Aregger <daregger@student.ethz.ch> - * Tobias Naegeli <naegelit@student.ethz.ch> -* Lorenz Meier <lm@inf.ethz.ch> - * - * 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_main.c - * Model-identification based position estimator for multirotors - */ - -#include <unistd.h> -#include <stdlib.h> -#include <stdio.h> -#include <stdbool.h> -#include <fcntl.h> -#include <float.h> -#include <string.h> -#include <nuttx/config.h> -#include <nuttx/sched.h> -#include <sys/prctl.h> -#include <termios.h> -#include <errno.h> -#include <limits.h> -#include <math.h> -#include <uORB/uORB.h> -#include <uORB/topics/sensor_combined.h> -#include <uORB/topics/parameter_update.h> -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/actuator_outputs.h> -#include <uORB/topics/actuator_controls_effective.h> -#include <uORB/topics/vehicle_status.h> -#include <uORB/topics/vehicle_vicon_position.h> -#include <uORB/topics/vehicle_local_position.h> -#include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/vehicle_local_position_setpoint.h> -#include <uORB/topics/vehicle_gps_position.h> -#include <mavlink/mavlink_log.h> -#include <poll.h> -#include <systemlib/geo/geo.h> -#include <systemlib/err.h> -#include <systemlib/systemlib.h> - -#include <drivers/drv_hrt.h> - -#include "position_estimator_mc_params.h" -//#include <uORB/topics/debug_key_value.h> -#include "codegen/kalman_dlqe2.h" -#include "codegen/kalman_dlqe2_initialize.h" -#include "codegen/kalman_dlqe3.h" -#include "codegen/kalman_dlqe3_initialize.h" - -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int position_estimator_mc_task; /**< Handle of deamon task / thread */ - -__EXPORT int position_estimator_mc_main(int argc, char *argv[]); - -int position_estimator_mc_thread_main(int argc, char *argv[]); -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -static void -usage(const char *reason) -{ - if (reason) - warnx("%s\n", reason); - warnx("usage: position_estimator_mc {start|stop|status}"); - exit(1); -} - -/** - * The position_estimator_mc_thread 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(). - */ -int position_estimator_mc_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - printf("position_estimator_mc already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - position_estimator_mc_task = task_spawn_cmd("position_estimator_mc", - SCHED_RR, - SCHED_PRIORITY_MAX - 5, - 4096, - position_estimator_mc_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - exit(0); - } - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("position_estimator_mc is running"); - } else { - warnx("position_estimator_mc not started"); - } - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -/**************************************************************************** - * main - ****************************************************************************/ -int position_estimator_mc_thread_main(int argc, char *argv[]) -{ - /* welcome user */ - warnx("[position_estimator_mc] started"); - int mavlink_fd; - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(mavlink_fd, "[position_estimator_mc] started"); - - /* initialize values */ - float z[3] = {0, 0, 0}; /* output variables from tangent plane mapping */ - // float rotMatrix[4] = {1.0f, 0.0f, 0.0f, 1.0f}; - float x_x_aposteriori_k[3] = {1.0f, 0.0f, 0.0f}; - float x_y_aposteriori_k[3] = {1.0f, 0.0f, 0.0f}; - float x_z_aposteriori_k[3] = {1.0f, 0.0f, 0.0f}; - float x_x_aposteriori[3] = {0.0f, 0.0f, 0.0f}; - float x_y_aposteriori[3] = {1.0f, 0.0f, 0.0f}; - float x_z_aposteriori[3] = {1.0f, 0.0f, 0.0f}; - - // XXX this is terribly wrong and should actual dT instead - const float dT_const_50 = 1.0f/50.0f; - - float addNoise = 0.0f; - float sigma = 0.0f; - //computed from dlqe in matlab - const float K_vicon_50Hz[3] = {0.5297f, 0.9873f, 0.9201f}; - // XXX implement baro filter - const float K_baro[3] = {0.0248f, 0.0377f, 0.0287f}; - float K[3] = {0.0f, 0.0f, 0.0f}; - int baro_loop_cnt = 0; - int baro_loop_end = 70; /* measurement for 1 second */ - float p0_Pa = 0.0f; /* to determin while start up */ - float rho0 = 1.293f; /* standard pressure */ - const float const_earth_gravity = 9.81f; - - float posX = 0.0f; - float posY = 0.0f; - float posZ = 0.0f; - - double lat_current; - double lon_current; - float alt_current; - - float gps_origin_altitude = 0.0f; - - /* Initialize filter */ - kalman_dlqe2_initialize(); - kalman_dlqe3_initialize(); - - /* declare and safely initialize all structs */ - struct sensor_combined_s sensor; - memset(&sensor, 0, sizeof(sensor)); - struct vehicle_attitude_s att; - memset(&att, 0, sizeof(att)); - struct vehicle_status_s vehicle_status; - memset(&vehicle_status, 0, sizeof(vehicle_status)); /* make sure that baroINITdone = false */ - struct vehicle_vicon_position_s vicon_pos; - memset(&vicon_pos, 0, sizeof(vicon_pos)); - struct actuator_controls_effective_s act_eff; - memset(&act_eff, 0, sizeof(act_eff)); - struct vehicle_gps_position_s gps; - memset(&gps, 0, sizeof(gps)); - struct vehicle_local_position_s local_pos_est; - memset(&local_pos_est, 0, sizeof(local_pos_est)); - struct vehicle_global_position_s global_pos_est; - memset(&global_pos_est, 0, sizeof(global_pos_est)); - - /* subscribe */ - int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - int sub_params = orb_subscribe(ORB_ID(parameter_update)); - int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); - int actuator_eff_sub = orb_subscribe(ORB_ID(actuator_controls_effective_0)); - int vehicle_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); - - /* advertise */ - orb_advert_t local_pos_est_pub = 0; - orb_advert_t global_pos_est_pub = 0; - - struct position_estimator_mc_params pos1D_params; - struct position_estimator_mc_param_handles pos1D_param_handles; - /* initialize parameter handles */ - parameters_init(&pos1D_param_handles); - - bool flag_use_gps = false; - bool flag_use_baro = false; - bool flag_baro_initialized = false; /* in any case disable baroINITdone */ - /* FIRST PARAMETER READ at START UP*/ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), sub_params, &update); /* read from param to clear updated flag */ - /* FIRST PARAMETER UPDATE */ - parameters_update(&pos1D_param_handles, &pos1D_params); - flag_use_baro = pos1D_params.baro; - sigma = pos1D_params.sigma; - addNoise = pos1D_params.addNoise; - /* END FIRST PARAMETER UPDATE */ - - /* try to grab a vicon message - if it fails, go for GPS. */ - - /* make sure the next orb_check() can't return true unless we get a timely update */ - orb_copy(ORB_ID(vehicle_vicon_position), vicon_pos_sub, &vicon_pos); - /* allow 200 ms for vicon to come in */ - usleep(200000); - /* check if we got vicon */ - bool update_check; - orb_check(vicon_pos_sub, &update_check); - /* if no update was available, use GPS */ - flag_use_gps = !update_check; - - if (flag_use_gps) { - mavlink_log_info(mavlink_fd, "[pos_est_mc] GPS locked"); - /* wait until gps signal turns valid, only then can we initialize the projection */ - - // XXX magic number - float hdop_threshold_m = 4.0f; - float vdop_threshold_m = 8.0f; - - /* - * If horizontal dilution of precision (hdop / eph) - * and vertical diluation of precision (vdop / epv) - * are below a certain threshold (e.g. 4 m), AND - * home position is not yet set AND the last GPS - * GPS measurement is not older than two seconds AND - * the system is currently not armed, set home - * position to the current position. - */ - - while (!(gps.fix_type == 3 - && (gps.eph_m < hdop_threshold_m) - && (gps.epv_m < vdop_threshold_m) - && (hrt_absolute_time() - gps.timestamp_position < 2000000))) { - - struct pollfd fds1[2] = { - { .fd = vehicle_gps_sub, .events = POLLIN }, - { .fd = sub_params, .events = POLLIN }, - }; - - /* wait for GPS updates, BUT READ VEHICLE STATUS (!) - * this choice is critical, since the vehicle status might not - * actually change, if this app is started after GPS lock was - * aquired. - */ - if (poll(fds1, 2, 5000)) { - if (fds1[0].revents & POLLIN){ - /* Read gps position */ - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps); - } - if (fds1[1].revents & POLLIN){ - /* Read out parameters to check for an update there, e.g. useGPS variable */ - /* read from param to clear updated flag */ - struct parameter_update_s updated; - orb_copy(ORB_ID(parameter_update), sub_params, &updated); - /* update parameters */ - parameters_update(&pos1D_param_handles, &pos1D_params); - } - } - static int printcounter = 0; - if (printcounter == 100) { - printcounter = 0; - warnx("[pos_est_mc] wait for GPS fix"); - } - printcounter++; - } - - /* get gps value for first initialization */ - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps); - lat_current = ((double)(gps.lat)) * 1e-7d; - lon_current = ((double)(gps.lon)) * 1e-7d; - alt_current = gps.alt * 1e-3f; - gps_origin_altitude = alt_current; - /* initialize coordinates */ - map_projection_init(lat_current, lon_current); - /* publish global position messages only after first GPS message */ - printf("[pos_est_mc] initialized projection with: lat: %.10f, lon:%.10f\n", lat_current, lon_current); - - } else { - mavlink_log_info(mavlink_fd, "[pos_est_mc] I'm NOT using GPS - I use VICON"); - /* onboard calculated position estimations */ - } - thread_running = true; - - struct pollfd fds2[3] = { - { .fd = vehicle_gps_sub, .events = POLLIN }, - { .fd = vicon_pos_sub, .events = POLLIN }, - { .fd = sub_params, .events = POLLIN }, - }; - - bool vicon_updated = false; - bool gps_updated = false; - - /**< main_loop */ - while (!thread_should_exit) { - int ret = poll(fds2, 3, 20); //wait maximal this 20 ms = 50 Hz minimum rate - if (ret < 0) { - /* poll error */ - } else { - if (fds2[2].revents & POLLIN){ - /* new parameter */ - /* read from param to clear updated flag */ - struct parameter_update_s updated; - orb_copy(ORB_ID(parameter_update), sub_params, &updated); - /* update parameters */ - parameters_update(&pos1D_param_handles, &pos1D_params); - flag_use_baro = pos1D_params.baro; - sigma = pos1D_params.sigma; - addNoise = pos1D_params.addNoise; - } - vicon_updated = false; /* default is no vicon_updated */ - if (fds2[1].revents & POLLIN) { - /* new vicon position */ - orb_copy(ORB_ID(vehicle_vicon_position), vicon_pos_sub, &vicon_pos); - posX = vicon_pos.x; - posY = vicon_pos.y; - posZ = vicon_pos.z; - vicon_updated = true; /* set flag for vicon update */ - } /* end of poll call for vicon updates */ - gps_updated = false; - if (fds2[0].revents & POLLIN) { - /* new GPS value */ - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps); - /* Project gps lat lon (Geographic coordinate system) to plane*/ - map_projection_project(((double)(gps.lat)) * 1e-7d, ((double)(gps.lon)) * 1e-7d, &(z[0]), &(z[1])); - posX = z[0]; - posY = z[1]; - posZ = (float)(gps.alt * 1e-3f); - gps_updated = true; - } - - /* Main estimator loop */ - orb_copy(ORB_ID(actuator_controls_effective_0), actuator_eff_sub, &act_eff); - orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); - orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensor); - // barometric pressure estimation at start up - if (!flag_baro_initialized){ - // mean calculation over several measurements - if (baro_loop_cnt<baro_loop_end) { - p0_Pa += (sensor.baro_pres_mbar*100); - baro_loop_cnt++; - } else { - p0_Pa /= (float)(baro_loop_cnt); - flag_baro_initialized = true; - char *baro_m_start = "barometer initialized with p0 = "; - char p0_char[15]; - sprintf(p0_char, "%8.2f", (double)(p0_Pa/100)); - char *baro_m_end = " mbar"; - char str[80]; - strcpy(str,baro_m_start); - strcat(str,p0_char); - strcat(str,baro_m_end); - mavlink_log_info(mavlink_fd, str); - } - } - if (flag_use_gps) { - /* initialize map projection with the last estimate (not at full rate) */ - if (gps.fix_type > 2) { - /* x-y-position/velocity estimation in earth frame = gps frame */ - kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_x_aposteriori_k,posX,gps_updated,0.0f,0.0f,x_x_aposteriori); - memcpy(x_x_aposteriori_k, x_x_aposteriori, sizeof(x_x_aposteriori)); - kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_y_aposteriori_k,posY,gps_updated,0.0f,0.0f,x_y_aposteriori); - memcpy(x_y_aposteriori_k, x_y_aposteriori, sizeof(x_y_aposteriori)); - /* z-position/velocity estimation in earth frame = vicon frame */ - float z_est = 0.0f; - if (flag_baro_initialized && flag_use_baro) { - z_est = -p0_Pa*logf(p0_Pa/(sensor.baro_pres_mbar*100))/(rho0*const_earth_gravity); - K[0] = K_vicon_50Hz[0]; - K[1] = K_vicon_50Hz[1]; - K[2] = K_vicon_50Hz[2]; - gps_updated = 1.0f; /* always enable the update, cause baro update = 200 Hz */ - } else { - z_est = posZ; - K[0] = K_vicon_50Hz[0]; - K[1] = K_vicon_50Hz[1]; - K[2] = K_vicon_50Hz[2]; - } - - kalman_dlqe3(dT_const_50,K[0],K[1],K[2],x_z_aposteriori_k,z_est,gps_updated,0.0f,0.0f,x_z_aposteriori); - memcpy(x_z_aposteriori_k, x_z_aposteriori, sizeof(x_z_aposteriori)); - local_pos_est.x = x_x_aposteriori_k[0]; - local_pos_est.vx = x_x_aposteriori_k[1]; - local_pos_est.y = x_y_aposteriori_k[0]; - local_pos_est.vy = x_y_aposteriori_k[1]; - local_pos_est.z = x_z_aposteriori_k[0]; - local_pos_est.vz = x_z_aposteriori_k[1]; - local_pos_est.timestamp = hrt_absolute_time(); - if ((isfinite(x_x_aposteriori_k[0])) && (isfinite(x_x_aposteriori_k[1])) && (isfinite(x_y_aposteriori_k[0])) && (isfinite(x_y_aposteriori_k[1])) && (isfinite(x_z_aposteriori_k[0])) && (isfinite(x_z_aposteriori_k[1]))) { - /* publish local position estimate */ - if (local_pos_est_pub > 0) { - orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est); - } else { - local_pos_est_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos_est); - } - /* publish on GPS updates */ - if (gps_updated) { - - double lat, lon; - float alt = z_est + gps_origin_altitude; - - map_projection_reproject(local_pos_est.x, local_pos_est.y, &lat, &lon); - - global_pos_est.lat = lat; - global_pos_est.lon = lon; - global_pos_est.alt = alt; - - if (global_pos_est_pub > 0) { - orb_publish(ORB_ID(vehicle_global_position), global_pos_est_pub, &global_pos_est); - } else { - global_pos_est_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos_est); - } - } - } - } - } else { - /* x-y-position/velocity estimation in earth frame = vicon frame */ - kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_x_aposteriori_k,posX,vicon_updated,addNoise,sigma,x_x_aposteriori); - memcpy(x_x_aposteriori_k, x_x_aposteriori, sizeof(x_x_aposteriori)); - kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_y_aposteriori_k,posY,vicon_updated,addNoise,sigma,x_y_aposteriori); - memcpy(x_y_aposteriori_k, x_y_aposteriori, sizeof(x_y_aposteriori)); - /* z-position/velocity estimation in earth frame = vicon frame */ - float z_est = 0.0f; - float local_sigma = 0.0f; - if (flag_baro_initialized && flag_use_baro) { - z_est = -p0_Pa*logf(p0_Pa/(sensor.baro_pres_mbar*100.0f))/(rho0*const_earth_gravity); - K[0] = K_vicon_50Hz[0]; - K[1] = K_vicon_50Hz[1]; - K[2] = K_vicon_50Hz[2]; - vicon_updated = 1; /* always enable the update, cause baro update = 200 Hz */ - local_sigma = 0.0f; /* don't add noise on barometer in any case */ - } else { - z_est = posZ; - K[0] = K_vicon_50Hz[0]; - K[1] = K_vicon_50Hz[1]; - K[2] = K_vicon_50Hz[2]; - local_sigma = sigma; - } - kalman_dlqe3(dT_const_50,K[0],K[1],K[2],x_z_aposteriori_k,z_est,vicon_updated,addNoise,local_sigma,x_z_aposteriori); - memcpy(x_z_aposteriori_k, x_z_aposteriori, sizeof(x_z_aposteriori)); - local_pos_est.x = x_x_aposteriori_k[0]; - local_pos_est.vx = x_x_aposteriori_k[1]; - local_pos_est.y = x_y_aposteriori_k[0]; - local_pos_est.vy = x_y_aposteriori_k[1]; - local_pos_est.z = x_z_aposteriori_k[0]; - local_pos_est.vz = x_z_aposteriori_k[1]; - local_pos_est.timestamp = hrt_absolute_time(); - if ((isfinite(x_x_aposteriori_k[0])) && (isfinite(x_x_aposteriori_k[1])) && (isfinite(x_y_aposteriori_k[0])) && (isfinite(x_y_aposteriori_k[1])) && (isfinite(x_z_aposteriori_k[0])) && (isfinite(x_z_aposteriori_k[1]))){ - if(local_pos_est_pub > 0) - orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est); - else - local_pos_est_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos_est); - //char buf[0xff]; sprintf(buf,"[pos_est_mc] x:%f, y:%f, z:%f",x_x_aposteriori_k[0],x_y_aposteriori_k[0],x_z_aposteriori_k[0]); - //mavlink_log_info(mavlink_fd, buf); - } - } - } /* end of poll return value check */ - } - - printf("[pos_est_mc] exiting.\n"); - mavlink_log_info(mavlink_fd, "[pos_est_mc] exiting"); - thread_running = false; - return 0; -} diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 941500f0d..62e6b12cb 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -134,8 +134,6 @@ controls_tick() { perf_begin(c_gather_sbus); - bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS); - bool sbus_failsafe, sbus_frame_drop; bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS); @@ -201,94 +199,105 @@ controls_tick() { /* update RC-received timestamp */ system_state.rc_channels_timestamp_received = hrt_absolute_time(); - /* do not command anything in failsafe, kick in the RC loss counter */ - if (!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { - - /* update RC-received timestamp */ - system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received; - - /* map raw inputs to mapped inputs */ - /* XXX mapping should be atomic relative to protocol */ - for (unsigned i = 0; i < r_raw_rc_count; i++) { - - /* map the input channel */ - uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; - - if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { - - uint16_t raw = r_raw_rc_values[i]; - - int16_t scaled; - - /* - * 1) Constrain to min/max values, as later processing depends on bounds. - */ - if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) - raw = conf[PX4IO_P_RC_CONFIG_MIN]; - if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) - raw = conf[PX4IO_P_RC_CONFIG_MAX]; - - /* - * 2) Scale around the mid point differently for lower and upper range. - * - * This is necessary as they don't share the same endpoints and slope. - * - * First normalize to 0..1 range with correct sign (below or above center), - * then scale to 20000 range (if center is an actual center, -10000..10000, - * if parameters only support half range, scale to 10000 range, e.g. if - * center == min 0..10000, if center == max -10000..0). - * - * As the min and max bounds were enforced in step 1), division by zero - * cannot occur, as for the case of center == min or center == max the if - * statement is mutually exclusive with the arithmetic NaN case. - * - * DO NOT REMOVE OR ALTER STEP 1! - */ - if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); - - } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); - - } else { - /* in the configured dead zone, output zero */ - scaled = 0; - } - - /* invert channel if requested */ - if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) - scaled = -scaled; + /* update RC-received timestamp */ + system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received; + + /* map raw inputs to mapped inputs */ + /* XXX mapping should be atomic relative to protocol */ + for (unsigned i = 0; i < r_raw_rc_count; i++) { + + /* map the input channel */ + uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; + + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { + + uint16_t raw = r_raw_rc_values[i]; + + int16_t scaled; + + /* + * 1) Constrain to min/max values, as later processing depends on bounds. + */ + if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) + raw = conf[PX4IO_P_RC_CONFIG_MIN]; + if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) + raw = conf[PX4IO_P_RC_CONFIG_MAX]; + + /* + * 2) Scale around the mid point differently for lower and upper range. + * + * This is necessary as they don't share the same endpoints and slope. + * + * First normalize to 0..1 range with correct sign (below or above center), + * then scale to 20000 range (if center is an actual center, -10000..10000, + * if parameters only support half range, scale to 10000 range, e.g. if + * center == min 0..10000, if center == max -10000..0). + * + * As the min and max bounds were enforced in step 1), division by zero + * cannot occur, as for the case of center == min or center == max the if + * statement is mutually exclusive with the arithmetic NaN case. + * + * DO NOT REMOVE OR ALTER STEP 1! + */ + if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); + + } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); + + } else { + /* in the configured dead zone, output zero */ + scaled = 0; + } - /* and update the scaled/mapped version */ - unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; - if (mapped < PX4IO_CONTROL_CHANNELS) { + /* invert channel if requested */ + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) { + scaled = -scaled; + } - /* invert channel if pitch - pulling the lever down means pitching up by convention */ - if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ - scaled = -scaled; + /* and update the scaled/mapped version */ + unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; + if (mapped < PX4IO_CONTROL_CHANNELS) { - r_rc_values[mapped] = SIGNED_TO_REG(scaled); - assigned_channels |= (1 << mapped); + /* invert channel if pitch - pulling the lever down means pitching up by convention */ + if (mapped == 1) { + /* roll, pitch, yaw, throttle, override is the standard order */ + scaled = -scaled; + } + if (mapped == 3 && r_setup_rc_thr_failsafe) { + /* throttle failsafe detection */ + if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < r_setup_rc_thr_failsafe)) || + ((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > r_setup_rc_thr_failsafe))) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + } } + + r_rc_values[mapped] = SIGNED_TO_REG(scaled); + assigned_channels |= (1 << mapped); + } } + } - /* set un-assigned controls to zero */ - for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { - if (!(assigned_channels & (1 << i))) - r_rc_values[i] = 0; + /* set un-assigned controls to zero */ + for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { + if (!(assigned_channels & (1 << i))) { + r_rc_values[i] = 0; } + } - /* set RC OK flag, as we got an update */ - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; + /* set RC OK flag, as we got an update */ + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_OK; - /* if we have enough channels (5) to control the vehicle, the mapping is ok */ - if (assigned_channels > 4) { - r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK; - } else { - r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK); - } + /* if we have enough channels (5) to control the vehicle, the mapping is ok */ + if (assigned_channels > 4) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK; + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK); } /* @@ -316,37 +325,42 @@ controls_tick() { * Handle losing RC input */ - /* this kicks in if the receiver is gone or the system went to failsafe */ - if (rc_input_lost || (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { + /* if we are in failsafe, clear the override flag */ + if (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) { + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE); + } + + /* this kicks in if the receiver is gone, but there is not on failsafe (indicated by separate flag) */ + if (rc_input_lost) { /* Clear the RC input status flag, clear manual override flag */ r_status_flags &= ~( PX4IO_P_STATUS_FLAGS_OVERRIDE | PX4IO_P_STATUS_FLAGS_RC_OK); + /* flag raw RC as lost */ + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_RC_OK); + /* Mark all channels as invalid, as we just lost the RX */ r_rc_valid = 0; + /* Set raw channel count to zero */ + r_raw_rc_count = 0; + /* Set the RC_LOST alarm */ r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST; } - /* this kicks in if the receiver is completely gone */ - if (rc_input_lost) { - - /* Set channel count to zero */ - r_raw_rc_count = 0; - } - /* * Check for manual override. * * The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag must be set, and we - * must have R/C input. + * must have R/C input (NO FAILSAFE!). * Override is enabled if either the hardcoded channel / value combination * is selected, or the AP has requested it. */ if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && + !(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { bool override = false; @@ -369,10 +383,10 @@ controls_tick() { mixer_tick(); } else { - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE; + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE); } } else { - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE; + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE); } } @@ -395,8 +409,9 @@ 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++) { values[i] = ppm_buffer[i]; + } /* clear validity */ ppm_last_valid_decode = 0; diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index b044bbf13..ebf4f3e8e 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -173,16 +173,15 @@ mixer_tick(void) * here. */ should_arm = ( - (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)/* IO initialised without error */ - && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)/* and IO is armed */ - && ( - ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)/* and FMU is armed */ - && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK))/* and there is valid input via or mixer */ - || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)/* or direct PWM is set */ - || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) - && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK))/* or failsafe was set manually */ - ) - ); + /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + /* and FMU is armed */ && ( + ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) + /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ) + /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) + /* or failsafe was set manually */ || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) + ) + ); should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) @@ -255,10 +254,25 @@ mixer_tick(void) for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) up_pwm_servo_set(i, r_page_servos[i]); + /* set S.BUS1 or S.BUS2 outputs */ + + if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) { + sbus2_output(r_page_servos, PX4IO_SERVO_COUNT); + } else if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) { + sbus1_output(r_page_servos, PX4IO_SERVO_COUNT); + } + } else if (mixer_servos_armed && should_always_enable_pwm) { /* set the disarmed servo outputs. */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) up_pwm_servo_set(i, r_page_servo_disarmed[i]); + + /* set S.BUS1 or S.BUS2 outputs */ + if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) + sbus1_output(r_page_servos, PX4IO_SERVO_COUNT); + + if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) + sbus2_output(r_page_servos, PX4IO_SERVO_COUNT); } } diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index d48c6c529..91975f2a0 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -142,6 +142,7 @@ #define PX4IO_P_RAW_RC_FLAGS_FAILSAFE (1 << 1) /* receiver is in failsafe mode */ #define PX4IO_P_RAW_RC_FLAGS_RC_DSM11 (1 << 2) /* DSM decoding is 11 bit mode */ #define PX4IO_P_RAW_RC_FLAGS_MAPPING_OK (1 << 3) /* Channel mapping is ok */ +#define PX4IO_P_RAW_RC_FLAGS_RC_OK (1 << 4) /* RC reception ok */ #define PX4IO_P_RAW_RC_NRSSI 2 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */ #define PX4IO_P_RAW_RC_DATA 3 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */ @@ -164,10 +165,10 @@ /* setup page */ #define PX4IO_PAGE_SETUP 50 #define PX4IO_P_SETUP_FEATURES 0 -#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT (1 << 0) /* enable S.Bus v1 output */ -#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /* enable S.Bus v2 output */ -#define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /* enable PWM RSSI parsing */ -#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /* enable ADC RSSI parsing */ +#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT (1 << 0) /**< enable S.Bus v1 output */ +#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /**< enable S.Bus v2 output */ +#define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /**< enable PWM RSSI parsing */ +#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /**< enable ADC RSSI parsing */ #define PX4IO_P_SETUP_ARMING 1 /* arming controls */ #define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */ @@ -201,13 +202,19 @@ enum { /* DSM bind states */ dsm_bind_send_pulses, dsm_bind_reinit_uart }; - /* 8 */ -#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ + /* 8 */ +#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ -#define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */ -#define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */ +#define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */ +#define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */ -#define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */ +#define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */ + /* 12 occupied by CRC */ +#define PX4IO_P_SETUP_RC_THR_FAILSAFE_US 13 /**< the throttle failsafe pulse length in microseconds */ + +#define PX4IO_P_SETUP_FORCE_SAFETY_OFF 12 /* force safety switch into + 'armed' (PWM enabled) state */ +#define PX4IO_FORCE_SAFETY_MAGIC 22027 /* required argument for force safety (random) */ /* autopilot control values, -10000..10000 */ #define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */ @@ -217,10 +224,10 @@ enum { /* DSM bind states */ #define PX4IO_P_CONTROLS_GROUP_3 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 3) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ #define PX4IO_P_CONTROLS_GROUP_VALID 64 -#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0 (1 << 0) /* group 0 is valid / received */ -#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1 (1 << 1) /* group 1 is valid / received */ -#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2 (1 << 2) /* group 2 is valid / received */ -#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3 (1 << 3) /* group 3 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0 (1 << 0) /**< group 0 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1 (1 << 1) /**< group 1 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2 (1 << 2) /**< group 2 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3 (1 << 3) /**< group 3 is valid / received */ /* raw text load to the mixer parser - ignores offset */ #define PX4IO_PAGE_MIXERLOAD 52 diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 54c5663a5..ca175bfbc 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -108,6 +108,7 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ #ifdef CONFIG_ARCH_BOARD_PX4IO_V1 #define r_setup_relays r_page_setup[PX4IO_P_SETUP_RELAYS] #endif +#define r_setup_rc_thr_failsafe r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] #define r_control_values (&r_page_controls[0]) @@ -218,6 +219,8 @@ extern bool dsm_input(uint16_t *values, uint16_t *num_values); extern void dsm_bind(uint16_t cmd, int pulses); extern int sbus_init(const char *device); extern bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels); +extern bool sbus1_output(uint16_t *values, uint16_t num_values); +extern bool sbus2_output(uint16_t *values, uint16_t num_values); /** global debug level for isr_debug() */ extern volatile uint8_t debug_level; diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 97d25bbfa..fd7c6081f 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -169,6 +169,7 @@ volatile uint16_t r_page_setup[] = [PX4IO_P_SETUP_SET_DEBUG] = 0, [PX4IO_P_SETUP_REBOOT_BL] = 0, [PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0, + [PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = 0, }; #ifdef CONFIG_ARCH_BOARD_PX4IO_V2 @@ -462,9 +463,18 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) #ifdef ENABLE_SBUS_OUT ENABLE_SBUS_OUT(value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT)); - /* disable the conflicting options */ - if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) { - value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | PX4IO_P_SETUP_FEATURES_ADC_RSSI); + /* disable the conflicting options with SBUS 1 */ + if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT)) { + value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | + PX4IO_P_SETUP_FEATURES_ADC_RSSI | + PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + } + + /* disable the conflicting options with SBUS 2 */ + if (value & (PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) { + value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | + PX4IO_P_SETUP_FEATURES_ADC_RSSI | + PX4IO_P_SETUP_FEATURES_SBUS1_OUT); } #endif @@ -569,6 +579,12 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) dsm_bind(value & 0x0f, (value >> 4) & 0xF); break; + case PX4IO_P_SETUP_FORCE_SAFETY_OFF: + if (value == PX4IO_FORCE_SAFETY_MAGIC) { + r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF; + } + break; + default: return -1; } diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index f6ec542eb..0e7dc621c 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -93,7 +93,7 @@ int sbus_init(const char *device) { if (sbus_fd < 0) - sbus_fd = open(device, O_RDONLY | O_NONBLOCK); + sbus_fd = open(device, O_RDWR | O_NONBLOCK); if (sbus_fd >= 0) { struct termios t; @@ -113,11 +113,22 @@ sbus_init(const char *device) } else { debug("S.Bus: open failed"); } - return sbus_fd; } bool +sbus1_output(uint16_t *values, uint16_t num_values) +{ + write(sbus_fd, 'A', 1); +} + +bool +sbus2_output(uint16_t *values, uint16_t num_values) +{ + write(sbus_fd, 'B', 1); +} + +bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels) { ssize_t ret; diff --git a/src/modules/sdlog/module.mk b/src/modules/sdlog/module.mk deleted file mode 100644 index 89da2d827..000000000 --- a/src/modules/sdlog/module.mk +++ /dev/null @@ -1,43 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 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. -# -############################################################################ - -# -# sdlog Application -# - -MODULE_COMMAND = sdlog -# The main thread only buffers to RAM, needs a high priority -MODULE_PRIORITY = "SCHED_PRIORITY_MAX-30" - -SRCS = sdlog.c \ - sdlog_ringbuffer.c diff --git a/src/modules/sdlog/sdlog.c b/src/modules/sdlog/sdlog.c deleted file mode 100644 index c22523bf2..000000000 --- a/src/modules/sdlog/sdlog.c +++ /dev/null @@ -1,840 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier <lm@inf.ethz.ch> - * - * 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 sdlog.c - * @author Lorenz Meier <lm@inf.ethz.ch> - * - * Simple SD logger for flight data. Buffers new sensor values and - * does the heavy SD I/O in a low-priority worker thread. - */ - -#include <nuttx/config.h> -#include <sys/types.h> -#include <sys/stat.h> -#include <sys/prctl.h> -#include <fcntl.h> -#include <errno.h> -#include <unistd.h> -#include <stdio.h> -#include <poll.h> -#include <stdlib.h> -#include <string.h> -#include <ctype.h> -#include <systemlib/err.h> -#include <unistd.h> -#include <drivers/drv_hrt.h> - -#include <uORB/uORB.h> -#include <uORB/topics/sensor_combined.h> -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_attitude_setpoint.h> -#include <uORB/topics/actuator_outputs.h> -#include <uORB/topics/actuator_controls.h> -#include <uORB/topics/actuator_controls_effective.h> -#include <uORB/topics/vehicle_command.h> -#include <uORB/topics/vehicle_local_position.h> -#include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/vehicle_gps_position.h> -#include <uORB/topics/vehicle_vicon_position.h> -#include <uORB/topics/optical_flow.h> -#include <uORB/topics/battery_status.h> -#include <uORB/topics/differential_pressure.h> -#include <uORB/topics/airspeed.h> - -#include <systemlib/systemlib.h> - -#include <mavlink/mavlink_log.h> - -#include "sdlog_ringbuffer.h" - -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int deamon_task; /**< Handle of deamon task / thread */ -static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log folders */ - -static const char *mountpoint = "/fs/microsd"; -static const char *mfile_in = "/etc/logging/logconv.m"; -int sysvector_file = -1; -int mavlink_fd = -1; -struct sdlog_logbuffer lb; - -/* mutex / condition to synchronize threads */ -pthread_mutex_t sysvector_mutex; -pthread_cond_t sysvector_cond; - -/** - * System state vector log buffer writing - */ -static void *sdlog_sysvector_write_thread(void *arg); - -/** - * Create the thread to write the system vector - */ -pthread_t sysvector_write_start(struct sdlog_logbuffer *logbuf); - -/** - * SD log management function. - */ -__EXPORT int sdlog_main(int argc, char *argv[]); - -/** - * Mainloop of sd log deamon. - */ -int sdlog_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -static int file_exist(const char *filename); - -static int file_copy(const char *file_old, const char *file_new); - -static void handle_command(struct vehicle_command_s *cmd); - -/** - * Print the current status. - */ -static void print_sdlog_status(void); - -/** - * Create folder for current logging session. - */ -static int create_logfolder(char *folder_path); - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - errx(1, "usage: sdlog {start|stop|status} [-s <number of skipped lines>]\n\n"); -} - -// XXX turn this into a C++ class -unsigned sensor_combined_bytes = 0; -unsigned actuator_outputs_bytes = 0; -unsigned actuator_controls_bytes = 0; -unsigned sysvector_bytes = 0; -unsigned blackbox_file_bytes = 0; -uint64_t starttime = 0; - -/* logging on or off, default to true */ -bool logging_enabled = true; - -/** - * The sd log 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_spawn_cmd(). - */ -int sdlog_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - printf("sdlog already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - deamon_task = task_spawn_cmd("sdlog", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT - 30, - 4096, - sdlog_thread_main, - (const char **)argv); - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - if (!thread_running) { - printf("\tsdlog is not started\n"); - } - - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - print_sdlog_status(); - - } else { - printf("\tsdlog not started\n"); - } - - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -int create_logfolder(char *folder_path) -{ - /* make folder on sdcard */ - uint16_t foldernumber = 1; // start with folder 0001 - int mkdir_ret; - - /* look for the next folder that does not exist */ - while (foldernumber < MAX_NO_LOGFOLDER) { - /* set up file path: e.g. /mnt/sdcard/sensorfile0001.txt */ - sprintf(folder_path, "%s/session%04u", mountpoint, foldernumber); - mkdir_ret = mkdir(folder_path, S_IRWXU | S_IRWXG | S_IRWXO); - /* the result is -1 if the folder exists */ - - if (mkdir_ret == 0) { - /* folder does not exist, success */ - - /* now copy the Matlab/Octave file */ - char mfile_out[100]; - sprintf(mfile_out, "%s/session%04u/run_to_plot_data.m", mountpoint, foldernumber); - int ret = file_copy(mfile_in, mfile_out); - - if (!ret) { - warnx("copied m file to %s", mfile_out); - - } else { - warnx("failed copying m file from %s to\n %s", mfile_in, mfile_out); - } - - break; - - } else if (mkdir_ret == -1) { - /* folder exists already */ - foldernumber++; - continue; - - } else { - warn("failed creating new folder"); - return -1; - } - } - - if (foldernumber >= MAX_NO_LOGFOLDER) { - /* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */ - warn("all %d possible folders exist already", MAX_NO_LOGFOLDER); - return -1; - } - - return 0; -} - - -static void * -sdlog_sysvector_write_thread(void *arg) -{ - /* set name */ - prctl(PR_SET_NAME, "sdlog microSD I/O", 0); - - struct sdlog_logbuffer *logbuf = (struct sdlog_logbuffer *)arg; - - int poll_count = 0; - struct sdlog_sysvector sysvect; - memset(&sysvect, 0, sizeof(sysvect)); - - while (!thread_should_exit) { - - /* make sure threads are synchronized */ - pthread_mutex_lock(&sysvector_mutex); - - /* only wait if no data is available to process */ - if (sdlog_logbuffer_is_empty(logbuf)) { - /* blocking wait for new data at this line */ - pthread_cond_wait(&sysvector_cond, &sysvector_mutex); - } - - /* only quickly load data, do heavy I/O a few lines down */ - int ret = sdlog_logbuffer_read(logbuf, &sysvect); - /* continue */ - pthread_mutex_unlock(&sysvector_mutex); - - if (ret == OK) { - sysvector_bytes += write(sysvector_file, (const char *)&sysvect, sizeof(sysvect)); - } - - if (poll_count % 100 == 0) { - fsync(sysvector_file); - } - - poll_count++; - } - - fsync(sysvector_file); - - return OK; -} - -pthread_t -sysvector_write_start(struct sdlog_logbuffer *logbuf) -{ - pthread_attr_t receiveloop_attr; - pthread_attr_init(&receiveloop_attr); - - struct sched_param param; - /* low priority, as this is expensive disk I/O */ - param.sched_priority = SCHED_PRIORITY_DEFAULT - 40; - (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); - - pthread_attr_setstacksize(&receiveloop_attr, 2048); - - pthread_t thread; - pthread_create(&thread, &receiveloop_attr, sdlog_sysvector_write_thread, logbuf); - return thread; - - // XXX we have to destroy the attr at some point -} - - -int sdlog_thread_main(int argc, char *argv[]) -{ - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - - if (mavlink_fd < 0) { - warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.\n"); - } - - /* log every n'th value (skip three per default) */ - int skip_value = 3; - - /* work around some stupidity in task_create's argv handling */ - argc -= 2; - argv += 2; - int ch; - - while ((ch = getopt(argc, argv, "s:r")) != EOF) { - switch (ch) { - case 's': - { - /* log only every n'th (gyro clocked) value */ - unsigned s = strtoul(optarg, NULL, 10); - - if (s < 1 || s > 250) { - errx(1, "Wrong skip value of %d, out of range (1..250)\n", s); - } else { - skip_value = s; - } - } - break; - - case 'r': - /* log only on request, disable logging per default */ - logging_enabled = false; - break; - - case '?': - if (optopt == 'c') { - warnx("Option -%c requires an argument.\n", optopt); - } else if (isprint(optopt)) { - warnx("Unknown option `-%c'.\n", optopt); - } else { - warnx("Unknown option character `\\x%x'.\n", optopt); - } - - default: - usage("unrecognized flag"); - errx(1, "exiting."); - } - } - - if (file_exist(mountpoint) != OK) { - errx(1, "logging mount point %s not present, exiting.", mountpoint); - } - - char folder_path[64]; - - if (create_logfolder(folder_path)) - errx(1, "unable to create logging folder, exiting."); - - FILE *gpsfile; - FILE *blackbox_file; - - /* string to hold the path to the sensorfile */ - char path_buf[64] = ""; - - /* only print logging path, important to find log file later */ - warnx("logging to directory %s\n", folder_path); - - /* set up file path: e.g. /mnt/sdcard/session0001/actuator_controls0.bin */ - sprintf(path_buf, "%s/%s.bin", folder_path, "sysvector"); - - if (0 == (sysvector_file = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) { - errx(1, "opening %s failed.\n", path_buf); - } - - /* set up file path: e.g. /mnt/sdcard/session0001/gps.txt */ - sprintf(path_buf, "%s/%s.txt", folder_path, "gps"); - - if (NULL == (gpsfile = fopen(path_buf, "w"))) { - errx(1, "opening %s failed.\n", path_buf); - } - - int gpsfile_no = fileno(gpsfile); - - /* set up file path: e.g. /mnt/sdcard/session0001/blackbox.txt */ - sprintf(path_buf, "%s/%s.txt", folder_path, "blackbox"); - - if (NULL == (blackbox_file = fopen(path_buf, "w"))) { - errx(1, "opening %s failed.\n", path_buf); - } - - // XXX for fsync() calls - int blackbox_file_no = fileno(blackbox_file); - - /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ - /* number of messages */ - const ssize_t fdsc = 25; - /* Sanity check variable and index */ - ssize_t fdsc_count = 0; - /* file descriptors to wait for */ - struct pollfd fds[fdsc]; - - - struct { - struct sensor_combined_s raw; - struct vehicle_attitude_s att; - struct vehicle_attitude_setpoint_s att_sp; - struct actuator_outputs_s act_outputs; - struct actuator_controls_s act_controls; - struct actuator_controls_effective_s act_controls_effective; - struct vehicle_command_s cmd; - struct vehicle_local_position_s local_pos; - struct vehicle_global_position_s global_pos; - struct vehicle_gps_position_s gps_pos; - struct vehicle_vicon_position_s vicon_pos; - struct optical_flow_s flow; - struct battery_status_s batt; - struct differential_pressure_s diff_pres; - struct airspeed_s airspeed; - } buf; - memset(&buf, 0, sizeof(buf)); - - struct { - int cmd_sub; - int sensor_sub; - int att_sub; - int spa_sub; - int act_0_sub; - int controls_0_sub; - int controls_effective_0_sub; - int local_pos_sub; - int global_pos_sub; - int gps_pos_sub; - int vicon_pos_sub; - int flow_sub; - int batt_sub; - int diff_pres_sub; - int airspeed_sub; - } subs; - - /* --- MANAGEMENT - LOGGING COMMAND --- */ - /* subscribe to ORB for vehicle command */ - subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); - fds[fdsc_count].fd = subs.cmd_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- GPS POSITION --- */ - /* subscribe to ORB for global position */ - subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - fds[fdsc_count].fd = subs.gps_pos_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- SENSORS RAW VALUE --- */ - /* subscribe to ORB for sensors raw */ - subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - fds[fdsc_count].fd = subs.sensor_sub; - /* do not rate limit, instead use skip counter (aliasing on rate limit) */ - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- ATTITUDE VALUE --- */ - /* subscribe to ORB for attitude */ - subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - fds[fdsc_count].fd = subs.att_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- ATTITUDE SETPOINT VALUE --- */ - /* subscribe to ORB for attitude setpoint */ - /* struct already allocated */ - subs.spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - fds[fdsc_count].fd = subs.spa_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /** --- ACTUATOR OUTPUTS --- */ - subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0)); - fds[fdsc_count].fd = subs.act_0_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- ACTUATOR CONTROL VALUE --- */ - /* subscribe to ORB for actuator control */ - subs.controls_0_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - fds[fdsc_count].fd = subs.controls_0_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- ACTUATOR CONTROL EFFECTIVE VALUE --- */ - /* subscribe to ORB for actuator control */ - subs.controls_effective_0_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); - fds[fdsc_count].fd = subs.controls_effective_0_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- LOCAL POSITION --- */ - /* subscribe to ORB for local position */ - subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - fds[fdsc_count].fd = subs.local_pos_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- GLOBAL POSITION --- */ - /* subscribe to ORB for global position */ - subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - fds[fdsc_count].fd = subs.global_pos_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- VICON POSITION --- */ - /* subscribe to ORB for vicon position */ - subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); - fds[fdsc_count].fd = subs.vicon_pos_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- FLOW measurements --- */ - /* subscribe to ORB for flow measurements */ - subs.flow_sub = orb_subscribe(ORB_ID(optical_flow)); - fds[fdsc_count].fd = subs.flow_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- BATTERY STATUS --- */ - /* subscribe to ORB for flow measurements */ - subs.batt_sub = orb_subscribe(ORB_ID(battery_status)); - fds[fdsc_count].fd = subs.batt_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- DIFFERENTIAL PRESSURE --- */ - /* subscribe to ORB for flow measurements */ - subs.diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); - fds[fdsc_count].fd = subs.diff_pres_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- AIRSPEED --- */ - /* subscribe to ORB for airspeed */ - subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); - fds[fdsc_count].fd = subs.airspeed_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* WARNING: If you get the error message below, - * then the number of registered messages (fdsc) - * differs from the number of messages in the above list. - */ - if (fdsc_count > fdsc) { - warn("WARNING: Not enough space for poll fds allocated. Check %s:%d.\n", __FILE__, __LINE__); - fdsc_count = fdsc; - } - - /* - * set up poll to block for new data, - * wait for a maximum of 1000 ms (1 second) - */ - // const int timeout = 1000; - - thread_running = true; - - /* initialize log buffer with a size of 10 */ - sdlog_logbuffer_init(&lb, 10); - - /* initialize thread synchronization */ - pthread_mutex_init(&sysvector_mutex, NULL); - pthread_cond_init(&sysvector_cond, NULL); - - /* start logbuffer emptying thread */ - pthread_t sysvector_pthread = sysvector_write_start(&lb); - - starttime = hrt_absolute_time(); - - /* track skipping */ - int skip_count = 0; - - while (!thread_should_exit) { - - /* only poll for commands, gps and sensor_combined */ - int poll_ret = poll(fds, 3, 1000); - - /* handle the poll result */ - if (poll_ret == 0) { - /* XXX this means none of our providers is giving us data - might be an error? */ - } else if (poll_ret < 0) { - /* XXX this is seriously bad - should be an emergency */ - } else { - - int ifds = 0; - - /* --- VEHICLE COMMAND VALUE --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy command into local buffer */ - orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd); - - /* always log to blackbox, even when logging disabled */ - blackbox_file_bytes += fprintf(blackbox_file, "[%10.4f\tVCMD] CMD #%d [%f\t%f\t%f\t%f\t%f\t%f\t%f]\n", hrt_absolute_time()/1000000.0d, - buf.cmd.command, (double)buf.cmd.param1, (double)buf.cmd.param2, (double)buf.cmd.param3, (double)buf.cmd.param4, - (double)buf.cmd.param5, (double)buf.cmd.param6, (double)buf.cmd.param7); - - handle_command(&buf.cmd); - } - - /* --- VEHICLE GPS VALUE --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy gps position into local buffer */ - orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); - - /* if logging disabled, continue */ - if (logging_enabled) { - - /* write KML line */ - } - } - - /* --- SENSORS RAW VALUE --- */ - if (fds[ifds++].revents & POLLIN) { - - // /* copy sensors raw data into local buffer */ - // orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw); - // /* write out */ - // sensor_combined_bytes += write(sensorfile, (const char*)&(buf.raw), sizeof(buf.raw)); - - /* always copy sensors raw data into local buffer, since poll flags won't clear else */ - orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw); - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.controls_0_sub, &buf.act_controls); - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.controls_effective_0_sub, &buf.act_controls_effective); - orb_copy(ORB_ID(actuator_outputs_0), subs.act_0_sub, &buf.act_outputs); - orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp); - orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); - orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos); - orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos); - orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att); - orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos); - orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow); - orb_copy(ORB_ID(differential_pressure), subs.diff_pres_sub, &buf.diff_pres); - orb_copy(ORB_ID(airspeed), subs.airspeed_sub, &buf.airspeed); - orb_copy(ORB_ID(battery_status), subs.batt_sub, &buf.batt); - - /* if skipping is on or logging is disabled, ignore */ - if (skip_count < skip_value || !logging_enabled) { - skip_count++; - /* do not log data */ - continue; - } else { - /* log data, reset */ - skip_count = 0; - } - - struct sdlog_sysvector sysvect = { - .timestamp = buf.raw.timestamp, - .gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]}, - .accel = {buf.raw.accelerometer_m_s2[0], buf.raw.accelerometer_m_s2[1], buf.raw.accelerometer_m_s2[2]}, - .mag = {buf.raw.magnetometer_ga[0], buf.raw.magnetometer_ga[1], buf.raw.magnetometer_ga[2]}, - .baro = buf.raw.baro_pres_mbar, - .baro_alt = buf.raw.baro_alt_meter, - .baro_temp = buf.raw.baro_temp_celcius, - .control = {buf.act_controls.control[0], buf.act_controls.control[1], buf.act_controls.control[2], buf.act_controls.control[3]}, - .actuators = { - buf.act_outputs.output[0], buf.act_outputs.output[1], buf.act_outputs.output[2], buf.act_outputs.output[3], - buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7] - }, - .vbat = buf.batt.voltage_v, - .bat_current = buf.batt.current_a, - .bat_discharged = buf.batt.discharged_mah, - .adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2], buf.raw.adc_voltage_v[3]}, - .local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z}, - .gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt}, - .attitude = {buf.att.pitch, buf.att.roll, buf.att.yaw}, - .rotMatrix = {buf.att.R[0][0], buf.att.R[0][1], buf.att.R[0][2], buf.att.R[1][0], buf.att.R[1][1], buf.att.R[1][2], buf.att.R[2][0], buf.att.R[2][1], buf.att.R[2][2]}, - .vicon = {buf.vicon_pos.x, buf.vicon_pos.y, buf.vicon_pos.z, buf.vicon_pos.roll, buf.vicon_pos.pitch, buf.vicon_pos.yaw}, - .control_effective = {buf.act_controls_effective.control_effective[0], buf.act_controls_effective.control_effective[1], buf.act_controls_effective.control_effective[2], buf.act_controls_effective.control_effective[3]}, - .flow = {buf.flow.flow_raw_x, buf.flow.flow_raw_y, buf.flow.flow_comp_x_m, buf.flow.flow_comp_y_m, buf.flow.ground_distance_m, buf.flow.quality}, - .diff_pressure = buf.diff_pres.differential_pressure_pa, - .ind_airspeed = buf.airspeed.indicated_airspeed_m_s, - .true_airspeed = buf.airspeed.true_airspeed_m_s - }; - - /* put into buffer for later IO */ - pthread_mutex_lock(&sysvector_mutex); - sdlog_logbuffer_write(&lb, &sysvect); - /* signal the other thread new data, but not yet unlock */ - if ((unsigned)lb.count > (lb.size / 2)) { - /* only request write if several packets can be written at once */ - pthread_cond_signal(&sysvector_cond); - } - /* unlock, now the writer thread may run */ - pthread_mutex_unlock(&sysvector_mutex); - } - - } - - } - - print_sdlog_status(); - - /* wake up write thread one last time */ - pthread_mutex_lock(&sysvector_mutex); - pthread_cond_signal(&sysvector_cond); - /* unlock, now the writer thread may return */ - pthread_mutex_unlock(&sysvector_mutex); - - /* wait for write thread to return */ - (void)pthread_join(sysvector_pthread, NULL); - - pthread_mutex_destroy(&sysvector_mutex); - pthread_cond_destroy(&sysvector_cond); - - warnx("exiting.\n\n"); - - /* finish KML file */ - // XXX - fclose(gpsfile); - fclose(blackbox_file); - - thread_running = false; - - return 0; -} - -void print_sdlog_status() -{ - unsigned bytes = sysvector_bytes + sensor_combined_bytes + actuator_outputs_bytes + blackbox_file_bytes + actuator_controls_bytes; - float mebibytes = bytes / 1024.0f / 1024.0f; - float seconds = ((float)(hrt_absolute_time() - starttime)) / 1000000.0f; - - warnx("wrote %4.2f MiB (average %5.3f MiB/s).\n", (double)mebibytes, (double)(mebibytes / seconds)); -} - -/** - * @return 0 if file exists - */ -int file_exist(const char *filename) -{ - struct stat buffer; - return stat(filename, &buffer); -} - -int file_copy(const char *file_old, const char *file_new) -{ - FILE *source, *target; - source = fopen(file_old, "r"); - int ret = 0; - - if (source == NULL) { - warnx("failed opening input file to copy"); - return 1; - } - - target = fopen(file_new, "w"); - - if (target == NULL) { - fclose(source); - warnx("failed to open output file to copy"); - return 1; - } - - char buf[128]; - int nread; - - while ((nread = fread(buf, 1, sizeof(buf), source)) > 0) { - int ret = fwrite(buf, 1, nread, target); - - if (ret <= 0) { - warnx("error writing file"); - ret = 1; - break; - } - } - - fsync(fileno(target)); - - fclose(source); - fclose(target); - - return ret; -} - -void handle_command(struct vehicle_command_s *cmd) -{ - /* result of the command */ - uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; - - /* request to set different system mode */ - switch (cmd->command) { - - case VEHICLE_CMD_PREFLIGHT_STORAGE: - - if (((int)(cmd->param3)) == 1) { - - /* enable logging */ - mavlink_log_info(mavlink_fd, "[log] file:"); - mavlink_log_info(mavlink_fd, "logdir"); - logging_enabled = true; - } - if (((int)(cmd->param3)) == 0) { - - /* disable logging */ - mavlink_log_info(mavlink_fd, "[log] stopped."); - logging_enabled = false; - } - break; - - default: - /* silently ignore */ - break; - } - -} diff --git a/src/modules/sdlog/sdlog_ringbuffer.c b/src/modules/sdlog/sdlog_ringbuffer.c deleted file mode 100644 index d7c8a4759..000000000 --- a/src/modules/sdlog/sdlog_ringbuffer.c +++ /dev/null @@ -1,91 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier <lm@inf.ethz.ch> - * - * 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 sdlog_log.c - * MAVLink text logging. - * - * @author Lorenz Meier <lm@inf.ethz.ch> - */ - -#include <string.h> -#include <stdlib.h> - -#include "sdlog_ringbuffer.h" - -void sdlog_logbuffer_init(struct sdlog_logbuffer *lb, int size) -{ - lb->size = size; - lb->start = 0; - lb->count = 0; - lb->elems = (struct sdlog_sysvector *)calloc(lb->size, sizeof(struct sdlog_sysvector)); -} - -int sdlog_logbuffer_is_full(struct sdlog_logbuffer *lb) -{ - return lb->count == (int)lb->size; -} - -int sdlog_logbuffer_is_empty(struct sdlog_logbuffer *lb) -{ - return lb->count == 0; -} - - -// XXX make these functions thread-safe -void sdlog_logbuffer_write(struct sdlog_logbuffer *lb, const struct sdlog_sysvector *elem) -{ - int end = (lb->start + lb->count) % lb->size; - memcpy(&(lb->elems[end]), elem, sizeof(struct sdlog_sysvector)); - - if (sdlog_logbuffer_is_full(lb)) { - lb->start = (lb->start + 1) % lb->size; /* full, overwrite */ - - } else { - ++lb->count; - } -} - -int sdlog_logbuffer_read(struct sdlog_logbuffer *lb, struct sdlog_sysvector *elem) -{ - if (!sdlog_logbuffer_is_empty(lb)) { - memcpy(elem, &(lb->elems[lb->start]), sizeof(struct sdlog_sysvector)); - lb->start = (lb->start + 1) % lb->size; - --lb->count; - return 0; - - } else { - return 1; - } -} diff --git a/src/modules/sdlog/sdlog_ringbuffer.h b/src/modules/sdlog/sdlog_ringbuffer.h deleted file mode 100644 index b65916459..000000000 --- a/src/modules/sdlog/sdlog_ringbuffer.h +++ /dev/null @@ -1,91 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier <lm@inf.ethz.ch> - * - * 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 sdlog_ringbuffer.h - * microSD logging - * - * @author Lorenz Meier <lm@inf.ethz.ch> - */ - -#ifndef SDLOG_RINGBUFFER_H_ -#define SDLOG_RINGBUFFER_H_ - -#pragma pack(push, 1) -struct sdlog_sysvector { - uint64_t timestamp; /**< time [us] */ - float gyro[3]; /**< [rad/s] */ - float accel[3]; /**< [m/s^2] */ - float mag[3]; /**< [gauss] */ - float baro; /**< pressure [millibar] */ - float baro_alt; /**< altitude above MSL [meter] */ - float baro_temp; /**< [degree celcius] */ - float control[4]; /**< roll, pitch, yaw [-1..1], thrust [0..1] */ - float actuators[8]; /**< motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512) */ - float vbat; /**< battery voltage in [volt] */ - float bat_current; /**< battery discharge current */ - float bat_discharged; /**< discharged energy in mAh */ - float adc[4]; /**< ADC ports [volt] */ - float local_position[3]; /**< tangent plane mapping into x,y,z [m] */ - int32_t gps_raw_position[3]; /**< latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter] */ - float attitude[3]; /**< roll, pitch, yaw [rad] */ - float rotMatrix[9]; /**< unitvectors */ - float vicon[6]; /**< Vicon ground truth x, y, z and roll, pitch, yaw */ - float control_effective[4]; /**< roll, pitch, yaw [-1..1], thrust [0..1] */ - float flow[6]; /**< flow raw x, y, flow metric x, y, flow ground dist, flow quality */ - float diff_pressure; /**< differential pressure */ - float ind_airspeed; /**< indicated airspeed */ - float true_airspeed; /**< true airspeed */ -}; -#pragma pack(pop) - -struct sdlog_logbuffer { - unsigned int start; - // unsigned int end; - unsigned int size; - int count; - struct sdlog_sysvector *elems; -}; - -void sdlog_logbuffer_init(struct sdlog_logbuffer *lb, int size); - -int sdlog_logbuffer_is_full(struct sdlog_logbuffer *lb); - -int sdlog_logbuffer_is_empty(struct sdlog_logbuffer *lb); - -void sdlog_logbuffer_write(struct sdlog_logbuffer *lb, const struct sdlog_sysvector *elem); - -int sdlog_logbuffer_read(struct sdlog_logbuffer *lb, struct sdlog_sysvector *elem); - -#endif diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk index f53129688..a28d43e72 100644 --- a/src/modules/sdlog2/module.mk +++ b/src/modules/sdlog2/module.mk @@ -41,3 +41,5 @@ MODULE_PRIORITY = "SCHED_PRIORITY_MAX-30" SRCS = sdlog2.c \ logbuffer.c + +MODULE_STACKSIZE = 1200 diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index ad709d27d..39f433eb5 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -83,6 +83,9 @@ #include <uORB/topics/rc_channels.h> #include <uORB/topics/esc_status.h> #include <uORB/topics/telemetry_status.h> +#include <uORB/topics/estimator_status.h> +#include <uORB/topics/system_power.h> +#include <uORB/topics/servorail_status.h> #include <systemlib/systemlib.h> #include <systemlib/param/param.h> @@ -223,11 +226,11 @@ sdlog2_usage(const char *reason) } errx(1, "usage: sdlog2 {start|stop|status} [-r <log rate>] [-b <buffer size>] -e -a -t\n" - "\t-r\tLog rate in Hz, 0 means unlimited rate\n" - "\t-b\tLog buffer size in KiB, default is 8\n" - "\t-e\tEnable logging by default (if not, can be started by command)\n" - "\t-a\tLog only when armed (can be still overriden by command)\n" - "\t-t\tUse date/time for naming log directories and files\n"); + "\t-r\tLog rate in Hz, 0 means unlimited rate\n" + "\t-b\tLog buffer size in KiB, default is 8\n" + "\t-e\tEnable logging by default (if not, can be started by command)\n" + "\t-a\tLog only when armed (can be still overriden by command)\n" + "\t-t\tUse date/time for naming log directories and files\n"); } /** @@ -254,11 +257,11 @@ int sdlog2_main(int argc, char *argv[]) main_thread_should_exit = false; deamon_task = task_spawn_cmd("sdlog2", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT - 30, - 3000, - sdlog2_thread_main, - (const char **)argv); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT - 30, + 3000, + sdlog2_thread_main, + (const char **)argv); exit(0); } @@ -681,7 +684,7 @@ int sdlog2_thread_main(int argc, char *argv[]) case 'r': { unsigned long r = strtoul(optarg, NULL, 10); - if (r <= 0) { + if (r == 0) { r = 1; } @@ -794,6 +797,9 @@ int sdlog2_thread_main(int argc, char *argv[]) struct battery_status_s battery; struct telemetry_status_s telemetry; struct range_finder_report range_finder; + struct estimator_status_report estimator_status; + struct system_power_s system_power; + struct servorail_status_s servorail_status; } buf; memset(&buf, 0, sizeof(buf)); @@ -825,6 +831,11 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_BATT_s log_BATT; struct log_DIST_s log_DIST; struct log_TELE_s log_TELE; + struct log_ESTM_s log_ESTM; + struct log_PWR_s log_PWR; + struct log_VICN_s log_VICN; + struct log_GSN0_s log_GSN0; + struct log_GSN1_s log_GSN1; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -855,6 +866,9 @@ int sdlog2_thread_main(int argc, char *argv[]) int battery_sub; int telemetry_sub; int range_finder_sub; + int estimator_status_sub; + int system_power_sub; + int servorail_status_sub; } subs; subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -879,6 +893,9 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.battery_sub = orb_subscribe(ORB_ID(battery_status)); subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status)); subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); + subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status)); + subs.system_power_sub = orb_subscribe(ORB_ID(system_power)); + subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status)); thread_running = true; @@ -893,9 +910,6 @@ int sdlog2_thread_main(int argc, char *argv[]) hrt_abstime barometer_timestamp = 0; hrt_abstime differential_pressure_timestamp = 0; - /* track changes in distance status */ - bool dist_bottom_present = false; - /* enable logging on start if needed */ if (log_on_start) { /* check GPS topic to get GPS time */ @@ -948,6 +962,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_STAT_MSG; log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; + log_msg.body.log_STAT.failsafe_state = (uint8_t) buf_status.failsafe_state; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning; log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed; @@ -969,6 +984,18 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_GPS.vel_d = buf_gps_pos.vel_d_m_s; log_msg.body.log_GPS.cog = buf_gps_pos.cog_rad; LOGBUFFER_WRITE_AND_COUNT(GPS); + + /* log the SNR of each satellite for a detailed view of signal quality */ + log_msg.msg_type = LOG_GSN0_MSG; + /* pick the smaller number so we do not overflow any of the arrays */ + unsigned gps_msg_max_snr = sizeof(buf_gps_pos.satellite_snr) / sizeof(buf_gps_pos.satellite_snr[0]); + unsigned log_max_snr = sizeof(log_msg.body.log_GSN0.satellite_snr) / sizeof(log_msg.body.log_GSN0.satellite_snr[0]); + unsigned sat_max_snr = (gps_msg_max_snr < log_max_snr) ? gps_msg_max_snr : log_max_snr; + + for (unsigned i = 0; i < sat_max_snr; i++) { + log_msg.body.log_GSN0.satellite_snr[i] = buf_gps_pos.satellite_snr[i]; + } + LOGBUFFER_WRITE_AND_COUNT(GSN0); } /* --- SENSOR COMBINED --- */ @@ -1084,28 +1111,19 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_LPOS.x = buf.local_pos.x; log_msg.body.log_LPOS.y = buf.local_pos.y; log_msg.body.log_LPOS.z = buf.local_pos.z; + log_msg.body.log_LPOS.ground_dist = buf.local_pos.dist_bottom; + log_msg.body.log_LPOS.ground_dist_rate = buf.local_pos.dist_bottom_rate; log_msg.body.log_LPOS.vx = buf.local_pos.vx; log_msg.body.log_LPOS.vy = buf.local_pos.vy; log_msg.body.log_LPOS.vz = buf.local_pos.vz; - log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat; - log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon; + log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat * 1e7; + log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon * 1e7; log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt; log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0); log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 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); LOGBUFFER_WRITE_AND_COUNT(LPOS); - - if (buf.local_pos.dist_bottom_valid) { - dist_bottom_present = true; - } - - if (dist_bottom_present) { - log_msg.msg_type = LOG_DIST_MSG; - log_msg.body.log_DIST.bottom = buf.local_pos.dist_bottom; - log_msg.body.log_DIST.bottom_rate = buf.local_pos.dist_bottom_rate; - log_msg.body.log_DIST.flags = (buf.local_pos.dist_bottom_valid ? 1 : 0); - LOGBUFFER_WRITE_AND_COUNT(DIST); - } } /* --- LOCAL POSITION SETPOINT --- */ @@ -1127,8 +1145,8 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_GPOS.vel_n = buf.global_pos.vel_n; log_msg.body.log_GPOS.vel_e = buf.global_pos.vel_e; log_msg.body.log_GPOS.vel_d = buf.global_pos.vel_d; - log_msg.body.log_GPOS.baro_alt = buf.global_pos.baro_alt; - log_msg.body.log_GPOS.flags = (buf.global_pos.baro_valid ? 1 : 0) | (buf.global_pos.global_valid ? 2 : 0); + log_msg.body.log_GPOS.eph = buf.global_pos.eph; + log_msg.body.log_GPOS.epv = buf.global_pos.epv; LOGBUFFER_WRITE_AND_COUNT(GPOS); } @@ -1149,7 +1167,14 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- VICON POSITION --- */ if (copy_if_updated(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos)) { - // TODO not implemented yet + log_msg.msg_type = LOG_VICN_MSG; + log_msg.body.log_VICN.x = buf.vicon_pos.x; + log_msg.body.log_VICN.y = buf.vicon_pos.y; + log_msg.body.log_VICN.z = buf.vicon_pos.z; + log_msg.body.log_VICN.pitch = buf.vicon_pos.pitch; + log_msg.body.log_VICN.roll = buf.vicon_pos.roll; + log_msg.body.log_VICN.yaw = buf.vicon_pos.yaw; + LOGBUFFER_WRITE_AND_COUNT(VICN); } /* --- FLOW --- */ @@ -1171,6 +1196,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* Copy only the first 8 channels of 14 */ memcpy(log_msg.body.log_RC.channel, buf.rc.chan, sizeof(log_msg.body.log_RC.channel)); log_msg.body.log_RC.channel_count = buf.rc.chan_count; + log_msg.body.log_RC.signal_lost = buf.rc.signal_lost; LOGBUFFER_WRITE_AND_COUNT(RC); } @@ -1179,6 +1205,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_AIRS_MSG; log_msg.body.log_AIRS.indicated_airspeed = buf.airspeed.indicated_airspeed_m_s; log_msg.body.log_AIRS.true_airspeed = buf.airspeed.true_airspeed_m_s; + log_msg.body.log_AIRS.air_temperature_celsius = buf.airspeed.air_temperature_celsius; LOGBUFFER_WRITE_AND_COUNT(AIRS); } @@ -1221,6 +1248,24 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(BATT); } + /* --- SYSTEM POWER RAILS --- */ + if (copy_if_updated(ORB_ID(system_power), subs.system_power_sub, &buf.system_power)) { + log_msg.msg_type = LOG_PWR_MSG; + log_msg.body.log_PWR.peripherals_5v = buf.system_power.voltage5V_v; + log_msg.body.log_PWR.usb_ok = buf.system_power.usb_connected; + log_msg.body.log_PWR.brick_ok = buf.system_power.brick_valid; + log_msg.body.log_PWR.servo_ok = buf.system_power.servo_valid; + log_msg.body.log_PWR.low_power_rail_overcurrent = buf.system_power.periph_5V_OC; + log_msg.body.log_PWR.high_power_rail_overcurrent = buf.system_power.hipower_5V_OC; + + /* copy servo rail status topic here too */ + orb_copy(ORB_ID(servorail_status), subs.servorail_status_sub, &buf.servorail_status); + log_msg.body.log_PWR.servo_rail_5v = buf.servorail_status.voltage_v; + log_msg.body.log_PWR.servo_rssi = buf.servorail_status.rssi_v; + + LOGBUFFER_WRITE_AND_COUNT(PWR); + } + /* --- TELEMETRY --- */ if (copy_if_updated(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry)) { log_msg.msg_type = LOG_TELE_MSG; @@ -1243,6 +1288,19 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(DIST); } + /* --- ESTIMATOR STATUS --- */ + if (copy_if_updated(ORB_ID(estimator_status), subs.estimator_status_sub, &buf.estimator_status)) { + log_msg.msg_type = LOG_ESTM_MSG; + unsigned maxcopy = (sizeof(buf.estimator_status.states) < sizeof(log_msg.body.log_ESTM.s)) ? sizeof(buf.estimator_status.states) : sizeof(log_msg.body.log_ESTM.s); + memset(&(log_msg.body.log_ESTM.s), 0, sizeof(log_msg.body.log_ESTM.s)); + memcpy(&(log_msg.body.log_ESTM.s), buf.estimator_status.states, maxcopy); + log_msg.body.log_ESTM.n_states = buf.estimator_status.n_states; + log_msg.body.log_ESTM.states_nan = buf.estimator_status.states_nan; + log_msg.body.log_ESTM.covariance_nan = buf.estimator_status.covariance_nan; + log_msg.body.log_ESTM.kalman_gain_nan = buf.estimator_status.kalman_gain_nan; + LOGBUFFER_WRITE_AND_COUNT(ESTM); + } + /* signal the other thread new data, but not yet unlock */ if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { /* only request write if several packets can be written at once */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index fe500ad5f..0c6188657 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -101,6 +101,8 @@ struct log_LPOS_s { float x; float y; float z; + float ground_dist; + float ground_dist_rate; float vx; float vy; float vz; @@ -110,6 +112,7 @@ struct log_LPOS_s { uint8_t xy_flags; uint8_t z_flags; uint8_t landed; + uint8_t ground_dist_flags; }; /* --- LPSP - LOCAL POSITION SETPOINT --- */ @@ -151,6 +154,7 @@ struct log_ATTC_s { struct log_STAT_s { uint8_t main_state; uint8_t arming_state; + uint8_t failsafe_state; float battery_remaining; uint8_t battery_warning; uint8_t landed; @@ -161,6 +165,7 @@ struct log_STAT_s { struct log_RC_s { float channel[8]; uint8_t channel_count; + uint8_t signal_lost; }; /* --- OUT0 - ACTUATOR_0 OUTPUT --- */ @@ -174,6 +179,7 @@ struct log_OUT0_s { struct log_AIRS_s { float indicated_airspeed; float true_airspeed; + float air_temperature_celsius; }; /* --- ARSP - ATTITUDE RATE SET POINT --- */ @@ -205,8 +211,8 @@ struct log_GPOS_s { float vel_n; float vel_e; float vel_d; - float baro_alt; - uint8_t flags; + float eph; + float epv; }; /* --- GPSP - GLOBAL POSITION SETPOINT --- */ @@ -277,6 +283,52 @@ struct log_TELE_s { uint8_t txbuf; }; +/* --- ESTM - ESTIMATOR STATUS --- */ +#define LOG_ESTM_MSG 23 +struct log_ESTM_s { + float s[10]; + uint8_t n_states; + uint8_t states_nan; + uint8_t covariance_nan; + uint8_t kalman_gain_nan; +}; + +/* --- PWR - ONBOARD POWER SYSTEM --- */ +#define LOG_PWR_MSG 24 +struct log_PWR_s { + float peripherals_5v; + float servo_rail_5v; + float servo_rssi; + uint8_t usb_ok; + uint8_t brick_ok; + uint8_t servo_ok; + uint8_t low_power_rail_overcurrent; + uint8_t high_power_rail_overcurrent; +}; + +/* --- VICN - VICON POSITION --- */ +#define LOG_VICN_MSG 25 +struct log_VICN_s { + float x; + float y; + float z; + float roll; + float pitch; + float yaw; +}; + +/* --- GSN0 - GPS SNR #0 --- */ +#define LOG_GSN0_MSG 26 +struct log_GSN0_s { + uint8_t satellite_snr[16]; /**< Signal to noise ratio of satellite. 0 for none, 255 for max. */ +}; + +/* --- GSN1 - GPS SNR #1 --- */ +#define LOG_GSN1_MSG 27 +struct log_GSN1_s { + uint8_t satellite_snr[16]; /**< Signal to noise ratio of satellite. 0 for none, 255 for max. */ +}; + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -304,35 +356,40 @@ struct log_PARM_s { /* construct list of all message formats */ static const struct log_format_s log_formats[] = { /* business-level messages, ID < 0x80 */ - LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), - LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), - LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), - LOG_FORMAT(SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), - LOG_FORMAT(LPOS, "ffffffLLfBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,XYFlags,ZFlags,Landed"), - LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), - LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), - LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), - LOG_FORMAT(STAT, "BBfBB", "MainState,ArmState,BatRem,BatWarn,Landed"), - LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"), - LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), - LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), - LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), - LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), - LOG_FORMAT(GPOS, "LLfffffB", "Lat,Lon,Alt,VelN,VelE,VelD,BaroAlt,Flags"), - LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), - LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), - LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), - LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), - LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), - LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"), + LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), + LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), + LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), + LOG_FORMAT(SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), + LOG_FORMAT(LPOS, "ffffffffLLfBBBB", "X,Y,Z,dist,distR,VX,VY,VZ,RLat,RLon,RAlt,XYFlg,ZFlg,LFlg,GFlg"), + LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), + LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), + LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), + LOG_FORMAT(STAT, "BBBfBB", "MainState,ArmState,FailsafeState,BatRem,BatWarn,Landed"), + 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"), + LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), + LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), + LOG_FORMAT(GPOS, "LLffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV"), + LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), + LOG_FORMAT(ESC, "HBBBHHHHHHfH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), + LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), + LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), + LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), + LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"), + LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,nStat,statNaN,covNaN,kGainNaN"), + LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"), + LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), + LOG_FORMAT(GSN0, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), + LOG_FORMAT(GSN1, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), /* system-level messages, ID >= 0x80 */ - // FMT: don't write format of format message, it's useless + /* FMT: don't write format of format message, it's useless */ LOG_FORMAT(TIME, "Q", "StartTime"), LOG_FORMAT(VER, "NZ", "Arch,FwGit"), - LOG_FORMAT(PARM, "Nf", "Name,Value"), + LOG_FORMAT(PARM, "Nf", "Name,Value") }; -static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); +static const unsigned log_formats_num = sizeof(log_formats) / sizeof(log_formats[0]); #endif /* SDLOG2_MESSAGES_H_ */ diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp index 96a443c6e..91230a37c 100644 --- a/src/modules/segway/BlockSegwayController.cpp +++ b/src/modules/segway/BlockSegwayController.cpp @@ -35,8 +35,8 @@ void BlockSegwayController::update() { // handle autopilot modes if (_status.main_state == MAIN_STATE_AUTO || - _status.main_state == MAIN_STATE_SEATBELT || - _status.main_state == MAIN_STATE_EASY) { + _status.main_state == MAIN_STATE_ALTCTL || + _status.main_state == MAIN_STATE_POSCTL) { _actuators.control[0] = spdCmd; _actuators.control[1] = spdCmd; diff --git a/src/modules/sensors/module.mk b/src/modules/sensors/module.mk index aa538fd6b..5b1bc5e86 100644 --- a/src/modules/sensors/module.mk +++ b/src/modules/sensors/module.mk @@ -40,3 +40,5 @@ MODULE_PRIORITY = "SCHED_PRIORITY_MAX-5" SRCS = sensors.cpp \ sensor_params.c + +MODULE_STACKSIZE = 1200 diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 0823f9e70..ae0ff625b 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -488,6 +488,15 @@ PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000); * @group Battery Calibration */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); +#elif CONFIG_ARCH_BOARD_AEROCORE +/** + * Scaling factor for battery voltage sensor on AeroCore. + * + * For R70 = 133K, R71 = 10K --> scale = 1.8 * 143 / (4096*10) = 0.0063 + * + * @group Battery Calibration + */ +PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0063f); #else /** * Scaling factor for battery voltage sensor on FMU v1. @@ -536,6 +545,20 @@ PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); PARAM_DEFINE_INT32(RC_MAP_PITCH, 2); /** + * Failsafe channel mapping. + * + * The RC mapping index indicates which channel is used for failsafe + * If 0, whichever channel is mapped to throttle is used + * otherwise the value indicates the specific rc channel to use + * + * @min 0 + * @max 18 + * + * + */ +PARAM_DEFINE_INT32(RC_MAP_FAILSAFE, 0); //Default to throttle function + +/** * Throttle control channel mapping. * * The channel index (starting from 1 for channel 1) indicates @@ -585,22 +608,30 @@ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0); PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); /** - * Assist switch channel mapping. + * Posctl switch channel mapping. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_MAP_POSCTL_SW, 0); + +/** + * Loiter switch channel mapping. * * @min 0 * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_LOITER_SW, 0); /** - * Mission switch channel mapping. + * Offboard switch channel mapping. * * @min 0 * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); /** @@ -647,53 +678,106 @@ PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); /** - * Roll scaling factor + * Failsafe channel PWM threshold. * + * @min 800 + * @max 2200 * @group Radio Calibration */ -PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f); +PARAM_DEFINE_INT32(RC_FAILS_THR, 0); /** - * Pitch scaling factor + * Threshold for selecting assist mode + * + * min:-1 + * max:+1 + * + * 0-1 indicate where in the full channel range the threshold sits + * 0 : min + * 1 : max + * sign indicates polarity of comparison + * positive : true when channel>th + * negative : true when channel<th * - * @group Radio Calibration */ -PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f); +PARAM_DEFINE_FLOAT(RC_ASSIST_TH, 0.25f); /** - * Yaw scaling factor + * Threshold for selecting auto mode + * + * min:-1 + * max:+1 + * + * 0-1 indicate where in the full channel range the threshold sits + * 0 : min + * 1 : max + * sign indicates polarity of comparison + * positive : true when channel>th + * negative : true when channel<th * - * @group Radio Calibration */ -PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f); +PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f); +/** + * Threshold for selecting posctl mode + * + * min:-1 + * max:+1 + * + * 0-1 indicate where in the full channel range the threshold sits + * 0 : min + * 1 : max + * sign indicates polarity of comparison + * positive : true when channel>th + * negative : true when channel<th + * + */ +PARAM_DEFINE_FLOAT(RC_POSCTL_TH, 0.5f); /** - * Failsafe channel mapping. + * Threshold for selecting return to launch mode + * + * min:-1 + * max:+1 + * + * 0-1 indicate where in the full channel range the threshold sits + * 0 : min + * 1 : max + * sign indicates polarity of comparison + * positive : true when channel>th + * negative : true when channel<th * - * @min 0 - * @max 18 - * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_FS_CH, 0); +PARAM_DEFINE_FLOAT(RC_RETURN_TH, 0.5f); /** - * Failsafe channel mode. + * Threshold for selecting loiter mode * - * 0 = too low means signal loss, - * 1 = too high means signal loss + * min:-1 + * max:+1 + * + * 0-1 indicate where in the full channel range the threshold sits + * 0 : min + * 1 : max + * sign indicates polarity of comparison + * positive : true when channel>th + * negative : true when channel<th * - * @min 0 - * @max 1 - * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_FS_MODE, 0); +PARAM_DEFINE_FLOAT(RC_LOITER_TH, 0.5f); /** - * Failsafe channel PWM threshold. + * Threshold for selecting offboard mode + * + * min:-1 + * max:+1 + * + * 0-1 indicate where in the full channel range the threshold sits + * 0 : min + * 1 : max + * sign indicates polarity of comparison + * positive : true when channel>th + * negative : true when channel<th * - * @min 0 - * @max 1 - * @group Radio Calibration */ -PARAM_DEFINE_FLOAT(RC_FS_THR, 800); +PARAM_DEFINE_FLOAT(RC_OFFB_TH, 0.5f); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 45e7035de..f34263a96 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -36,6 +36,8 @@ * Sensor readout process. * * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Thomas Gubler <thomasgubler@student.ethz.ch> */ #include <nuttx/config.h> @@ -124,6 +126,12 @@ #define ADC_AIRSPEED_VOLTAGE_CHANNEL 15 #endif +#ifdef CONFIG_ARCH_BOARD_AEROCORE +#define ADC_BATTERY_VOLTAGE_CHANNEL 10 +#define ADC_BATTERY_CURRENT_CHANNEL -1 +#define ADC_AIRSPEED_VOLTAGE_CHANNEL -1 +#endif + #define BATT_V_LOWPASS 0.001f #define BATT_V_IGNORE_THRESHOLD 3.5f @@ -133,7 +141,7 @@ */ #define PCB_TEMP_ESTIMATE_DEG 5.0f -#define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg) +#define STICK_ON_OFF_LIMIT 0.75f /** * Sensor app start / stop handling function @@ -165,7 +173,16 @@ public: private: static const unsigned _rc_max_chan_count = RC_INPUT_MAX_CHANNELS; /**< maximum number of r/c channels we handle */ - hrt_abstime _rc_last_valid; /**< last time we got a valid RC signal */ + /** + * Get and limit value for specified RC function. Returns NAN if not mapped. + */ + float get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value); + + /** + * Get switch position for specified function. + */ + switch_pos_t get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv, float mid_th, bool mid_inv); + switch_pos_t get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv); /** * Gather and publish RC input data. @@ -209,8 +226,8 @@ private: struct differential_pressure_s _diff_pres; struct airspeed_s _airspeed; - math::Matrix<3,3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ - math::Matrix<3,3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ + math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ + math::Matrix<3, 3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ bool _mag_is_external; /**< true if the active mag is on an external board */ uint64_t _battery_discharged; /**< battery discharged current in mA*ms */ @@ -240,11 +257,12 @@ private: int rc_map_pitch; int rc_map_yaw; int rc_map_throttle; + int rc_map_failsafe; int rc_map_mode_sw; int rc_map_return_sw; - int rc_map_assisted_sw; - int rc_map_mission_sw; + int rc_map_posctl_sw; + int rc_map_loiter_sw; int rc_map_offboard_sw; int rc_map_flaps; @@ -255,14 +273,19 @@ private: int rc_map_aux4; int rc_map_aux5; - float rc_scale_roll; - float rc_scale_pitch; - float rc_scale_yaw; - float rc_scale_flaps; - - int rc_fs_ch; - int rc_fs_mode; - float rc_fs_thr; + int32_t rc_fails_thr; + float rc_assist_th; + float rc_auto_th; + float rc_posctl_th; + float rc_return_th; + float rc_loiter_th; + float rc_offboard_th; + bool rc_assist_inv; + bool rc_auto_inv; + bool rc_posctl_inv; + bool rc_return_inv; + bool rc_loiter_inv; + bool rc_offboard_inv; float battery_voltage_scaling; float battery_current_scaling; @@ -289,11 +312,12 @@ private: param_t rc_map_pitch; param_t rc_map_yaw; param_t rc_map_throttle; + param_t rc_map_failsafe; param_t rc_map_mode_sw; param_t rc_map_return_sw; - param_t rc_map_assisted_sw; - param_t rc_map_mission_sw; + param_t rc_map_posctl_sw; + param_t rc_map_loiter_sw; param_t rc_map_offboard_sw; param_t rc_map_flaps; @@ -304,14 +328,13 @@ private: param_t rc_map_aux4; param_t rc_map_aux5; - param_t rc_scale_roll; - param_t rc_scale_pitch; - param_t rc_scale_yaw; - param_t rc_scale_flaps; - - param_t rc_fs_ch; - param_t rc_fs_mode; - param_t rc_fs_thr; + param_t rc_fails_thr; + param_t rc_assist_th; + param_t rc_auto_th; + param_t rc_posctl_th; + param_t rc_return_th; + param_t rc_loiter_th; + param_t rc_offboard_th; param_t battery_voltage_scaling; param_t battery_current_scaling; @@ -418,7 +441,7 @@ private: /** * Main sensor collection task. */ - void task_main() __attribute__((noreturn)); + void task_main(); }; namespace sensors @@ -434,8 +457,6 @@ Sensors *g_sensors = nullptr; } Sensors::Sensors() : - _rc_last_valid(0), - _fd_adc(-1), _last_adc(0), @@ -470,6 +491,7 @@ Sensors::Sensors() : _battery_discharged(0), _battery_current_timestamp(0) { + memset(&_rc, 0, sizeof(_rc)); /* basic r/c parameters */ for (unsigned i = 0; i < _rc_max_chan_count; i++) { @@ -502,6 +524,7 @@ Sensors::Sensors() : _parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH"); _parameter_handles.rc_map_yaw = param_find("RC_MAP_YAW"); _parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE"); + _parameter_handles.rc_map_failsafe = param_find("RC_MAP_FAILSAFE"); /* mandatory mode switches, mapped to channel 5 and 6 per default */ _parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW"); @@ -510,8 +533,8 @@ Sensors::Sensors() : _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); /* optional mode switches, not mapped per default */ - _parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW"); - _parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW"); + _parameter_handles.rc_map_posctl_sw = param_find("RC_MAP_POSCTL_SW"); + _parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW"); _parameter_handles.rc_map_offboard_sw = param_find("RC_MAP_OFFB_SW"); _parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1"); @@ -520,15 +543,14 @@ Sensors::Sensors() : _parameter_handles.rc_map_aux4 = param_find("RC_MAP_AUX4"); _parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5"); - _parameter_handles.rc_scale_roll = param_find("RC_SCALE_ROLL"); - _parameter_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH"); - _parameter_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); - _parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS"); - - /* RC failsafe */ - _parameter_handles.rc_fs_ch = param_find("RC_FS_CH"); - _parameter_handles.rc_fs_mode = param_find("RC_FS_MODE"); - _parameter_handles.rc_fs_thr = param_find("RC_FS_THR"); + /* RC thresholds */ + _parameter_handles.rc_fails_thr = param_find("RC_FAILS_THR"); + _parameter_handles.rc_assist_th = param_find("RC_ASSIST_TH"); + _parameter_handles.rc_auto_th = param_find("RC_AUTO_TH"); + _parameter_handles.rc_posctl_th = param_find("RC_POSCTL_TH"); + _parameter_handles.rc_return_th = param_find("RC_RETURN_TH"); + _parameter_handles.rc_loiter_th = param_find("RC_LOITER_TH"); + _parameter_handles.rc_offboard_th = param_find("RC_OFFB_TH"); /* gyro offsets */ _parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF"); @@ -629,50 +651,55 @@ Sensors::parameters_update() } /* handle wrong values */ - if (!rc_valid) + if (!rc_valid) { warnx("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n"); + } const char *paramerr = "FAIL PARM LOAD"; /* channel mapping */ if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_pitch, &(_parameters.rc_map_pitch)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_yaw, &(_parameters.rc_map_yaw)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); + } + + if (param_get(_parameter_handles.rc_map_failsafe, &(_parameters.rc_map_failsafe)) != OK) { + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_return_sw, &(_parameters.rc_map_return_sw)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } - if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) { - warnx(paramerr); + if (param_get(_parameter_handles.rc_map_posctl_sw, &(_parameters.rc_map_posctl_sw)) != OK) { + warnx("%s", paramerr); } - if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) { - warnx(paramerr); + if (param_get(_parameter_handles.rc_map_loiter_sw, &(_parameters.rc_map_loiter_sw)) != OK) { + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_offboard_sw, &(_parameters.rc_map_offboard_sw)) != OK) { - warnx("Failed getting offboard sw chan index"); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)); @@ -680,13 +707,25 @@ Sensors::parameters_update() param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)); param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)); param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)); - param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll)); - param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch)); - param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw)); - param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps)); - param_get(_parameter_handles.rc_fs_ch, &(_parameters.rc_fs_ch)); - param_get(_parameter_handles.rc_fs_mode, &(_parameters.rc_fs_mode)); - param_get(_parameter_handles.rc_fs_thr, &(_parameters.rc_fs_thr)); + param_get(_parameter_handles.rc_fails_thr, &(_parameters.rc_fails_thr)); + param_get(_parameter_handles.rc_assist_th, &(_parameters.rc_assist_th)); + _parameters.rc_assist_inv = (_parameters.rc_assist_th < 0); + _parameters.rc_assist_th = fabs(_parameters.rc_assist_th); + param_get(_parameter_handles.rc_auto_th, &(_parameters.rc_auto_th)); + _parameters.rc_auto_inv = (_parameters.rc_auto_th < 0); + _parameters.rc_auto_th = fabs(_parameters.rc_auto_th); + param_get(_parameter_handles.rc_posctl_th, &(_parameters.rc_posctl_th)); + _parameters.rc_posctl_inv = (_parameters.rc_posctl_th < 0); + _parameters.rc_posctl_th = fabs(_parameters.rc_posctl_th); + param_get(_parameter_handles.rc_return_th, &(_parameters.rc_return_th)); + _parameters.rc_return_inv = (_parameters.rc_return_th < 0); + _parameters.rc_return_th = fabs(_parameters.rc_return_th); + param_get(_parameter_handles.rc_loiter_th, &(_parameters.rc_loiter_th)); + _parameters.rc_loiter_inv = (_parameters.rc_loiter_th < 0); + _parameters.rc_loiter_th = fabs(_parameters.rc_loiter_th); + param_get(_parameter_handles.rc_offboard_th, &(_parameters.rc_offboard_th)); + _parameters.rc_offboard_inv = (_parameters.rc_offboard_th < 0); + _parameters.rc_offboard_th = fabs(_parameters.rc_offboard_th); /* update RC function mappings */ _rc.function[THROTTLE] = _parameters.rc_map_throttle - 1; @@ -696,9 +735,9 @@ Sensors::parameters_update() _rc.function[MODE] = _parameters.rc_map_mode_sw - 1; _rc.function[RETURN] = _parameters.rc_map_return_sw - 1; - _rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1; - _rc.function[MISSION] = _parameters.rc_map_mission_sw - 1; - _rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_sw - 1; + _rc.function[POSCTL] = _parameters.rc_map_posctl_sw - 1; + _rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1; + _rc.function[OFFBOARD] = _parameters.rc_map_offboard_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; @@ -739,12 +778,12 @@ Sensors::parameters_update() /* scaling of ADC ticks to battery voltage */ if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } /* scaling of ADC ticks to battery current */ if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation)); @@ -779,7 +818,7 @@ Sensors::accel_init() /* set the driver to poll at 1000Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 1000); -#elif CONFIG_ARCH_BOARD_PX4FMU_V2 +#elif CONFIG_ARCH_BOARD_PX4FMU_V2 || CONFIG_ARCH_BOARD_AEROCORE /* set the accel internal sampling rate up to at leat 800Hz */ ioctl(fd, ACCELIOCSSAMPLERATE, 800); @@ -788,7 +827,7 @@ Sensors::accel_init() ioctl(fd, SENSORIOCSPOLLRATE, 800); #else -#error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2 +#error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1, CONFIG_ARCH_BOARD_PX4FMU_V2 or CONFIG_ARCH_BOARD_AEROCORE #endif @@ -814,12 +853,14 @@ Sensors::gyro_init() #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 /* set the gyro internal sampling rate up to at least 1000Hz */ - if (ioctl(fd, GYROIOCSSAMPLERATE, 1000) != OK) + if (ioctl(fd, GYROIOCSSAMPLERATE, 1000) != OK) { ioctl(fd, GYROIOCSSAMPLERATE, 800); + } /* set the driver to poll at 1000Hz */ - if (ioctl(fd, SENSORIOCSPOLLRATE, 1000) != OK) + if (ioctl(fd, SENSORIOCSPOLLRATE, 1000) != OK) { ioctl(fd, SENSORIOCSPOLLRATE, 800); + } #else @@ -874,12 +915,15 @@ Sensors::mag_init() ret = ioctl(fd, MAGIOCGEXTERNAL, 0); - if (ret < 0) + if (ret < 0) { errx(1, "FATAL: unknown if magnetometer is external or onboard"); - else if (ret == 1) + + } else if (ret == 1) { _mag_is_external = true; - else + + } else { _mag_is_external = false; + } close(fd); } @@ -979,10 +1023,12 @@ Sensors::mag_poll(struct sensor_combined_s &raw) math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); - if (_mag_is_external) + if (_mag_is_external) { vect = _external_mag_rotation * vect; - else + + } else { vect = _board_rotation * vect; + } raw.magnetometer_ga[0] = vect(0); raw.magnetometer_ga[1] = vect(1); @@ -1027,10 +1073,13 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) raw.differential_pressure_timestamp = _diff_pres.timestamp; raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa; + float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); + _airspeed.timestamp = _diff_pres.timestamp; _airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa); _airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f, - raw.baro_pres_mbar * 1e2f, raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); + raw.baro_pres_mbar * 1e2f, air_temperature_celsius); + _airspeed.air_temperature_celsius = air_temperature_celsius; /* announce the airspeed if needed, just publish else */ if (_airspeed_pub > 0) { @@ -1097,8 +1146,9 @@ Sensors::parameter_update_poll(bool forced) _parameters.gyro_scale[2], }; - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) { warn("WARNING: failed to set scale / offsets for gyro"); + } close(fd); @@ -1112,8 +1162,9 @@ Sensors::parameter_update_poll(bool forced) _parameters.accel_scale[2], }; - if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) + if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) { warn("WARNING: failed to set scale / offsets for accel"); + } close(fd); @@ -1127,8 +1178,9 @@ Sensors::parameter_update_poll(bool forced) _parameters.mag_scale[2], }; - if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) + if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) { warn("WARNING: failed to set scale / offsets for mag"); + } close(fd); @@ -1142,8 +1194,10 @@ Sensors::parameter_update_poll(bool forced) 1.0f, }; - if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) + if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { warn("WARNING: failed to set scale / offsets for airspeed sensor"); + } + close(fd); } @@ -1161,22 +1215,27 @@ void Sensors::adc_poll(struct sensor_combined_s &raw) { /* only read if publishing */ - if (!_publishing) + if (!_publishing) { return; + } hrt_abstime t = hrt_absolute_time(); + /* rate limit to 100 Hz */ if (t - _last_adc >= 10000) { - /* make space for a maximum of eight channels */ - struct adc_msg_s buf_adc[8]; + /* make space for a maximum of twelve channels (to ensure reading all channels at once) */ + struct adc_msg_s buf_adc[12]; /* read all channels available */ int ret = read(_fd_adc, &buf_adc, sizeof(buf_adc)); if (ret >= (int)sizeof(buf_adc[0])) { - for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) { + + /* Read add channels we got */ + for (unsigned i = 0; i < ret / sizeof(buf_adc[0]); i++) { /* Save raw voltage values */ - if (i < (sizeof(raw.adc_voltage_v)) / sizeof(raw.adc_voltage_v[0])) { + if (i < (sizeof(raw.adc_voltage_v) / sizeof(raw.adc_voltage_v[0]))) { raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f); + raw.adc_mapping[i] = buf_adc[i].am_channel; } /* look for specific channels and process the raw voltage to measurement data */ @@ -1186,6 +1245,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) if (voltage > BATT_V_IGNORE_THRESHOLD) { _battery_status.voltage_v = voltage; + /* one-time initialization of low-pass value to avoid long init delays */ if (_battery_status.voltage_filtered_v < BATT_V_IGNORE_THRESHOLD) { _battery_status.voltage_filtered_v = voltage; @@ -1204,19 +1264,24 @@ Sensors::adc_poll(struct sensor_combined_s &raw) /* handle current only if voltage is valid */ if (_battery_status.voltage_v > 0.0f) { float current = (buf_adc[i].am_data * _parameters.battery_current_scaling); + /* check measured current value */ if (current >= 0.0f) { _battery_status.timestamp = t; _battery_status.current_a = current; + if (_battery_current_timestamp != 0) { /* initialize discharged value */ - if (_battery_status.discharged_mah < 0.0f) + if (_battery_status.discharged_mah < 0.0f) { _battery_status.discharged_mah = 0.0f; + } + _battery_discharged += current * (t - _battery_current_timestamp); _battery_status.discharged_mah = ((float) _battery_discharged) / 3600000.0f; } } } + _battery_current_timestamp = t; } else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { @@ -1235,6 +1300,8 @@ Sensors::adc_poll(struct sensor_combined_s &raw) _diff_pres.timestamp = t; _diff_pres.differential_pressure_pa = diff_pres_pa; + _diff_pres.differential_pressure_filtered_pa = diff_pres_pa; + _diff_pres.temperature = -1000.0f; _diff_pres.voltage = voltage; /* announce the airspeed if needed, just publish else */ @@ -1247,8 +1314,10 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } } } + _last_adc = t; - if (_battery_status.voltage_v > 0.0f) { + + if (_battery_status.voltage_filtered_v > BATT_V_IGNORE_THRESHOLD) { /* announce the battery status if needed, just publish else */ if (_battery_pub > 0) { orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status); @@ -1261,6 +1330,66 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } } +float +Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value) +{ + if (_rc.function[func] >= 0) { + float value = _rc.chan[_rc.function[func]].scaled; + + if (value < min_value) { + return min_value; + + } else if (value > max_value) { + return max_value; + + } else { + return value; + } + + } else { + return 0.0f; + } +} + +switch_pos_t +Sensors::get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv, float mid_th, bool mid_inv) +{ + if (_rc.function[func] >= 0) { + float value = 0.5f * _rc.chan[_rc.function[func]].scaled + 0.5f; + + if (on_inv ? value < on_th : value > on_th) { + return SWITCH_POS_ON; + + } else if (mid_inv ? value < mid_th : value > mid_th) { + return SWITCH_POS_MIDDLE; + + } else { + return SWITCH_POS_OFF; + } + + } else { + return SWITCH_POS_NONE; + } +} + +switch_pos_t +Sensors::get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv) +{ + if (_rc.function[func] >= 0) { + float value = 0.5f * _rc.chan[_rc.function[func]].scaled + 0.5f; + + if (on_inv ? value < on_th : value > on_th) { + return SWITCH_POS_ON; + + } else { + return SWITCH_POS_OFF; + } + + } else { + return SWITCH_POS_NONE; + } +} + void Sensors::rc_poll() { @@ -1269,70 +1398,58 @@ Sensors::rc_poll() if (rc_updated) { /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ - struct rc_input_values rc_input; + struct rc_input_values rc_input; orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); - if (rc_input.rc_lost) - return; - - struct manual_control_setpoint_s manual_control; - struct actuator_controls_s actuator_group_3; - - /* initialize to default values */ - manual_control.roll = NAN; - manual_control.pitch = NAN; - manual_control.yaw = NAN; - manual_control.throttle = NAN; - - manual_control.mode_switch = NAN; - manual_control.return_switch = NAN; - manual_control.assisted_switch = NAN; - manual_control.mission_switch = NAN; - manual_control.offboard_switch = NAN; - - manual_control.flaps = NAN; - manual_control.aux1 = NAN; - manual_control.aux2 = NAN; - manual_control.aux3 = NAN; - manual_control.aux4 = NAN; - manual_control.aux5 = NAN; - - /* require at least four channels to consider the signal valid */ - if (rc_input.channel_count < 4) - return; - - /* failsafe check */ - if (_parameters.rc_fs_ch != 0) { - if (_parameters.rc_fs_mode == 0) { - if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr) - return; - - } else if (_parameters.rc_fs_mode == 1) { - if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr) - return; + /* detect RC signal loss */ + bool signal_lost; + + /* check flags and require at least four channels to consider the signal valid */ + if (rc_input.rc_lost || rc_input.rc_failsafe || rc_input.channel_count < 4) { + /* signal is lost or no enough channels */ + signal_lost = true; + + } else { + /* signal looks good */ + signal_lost = false; + + /* check failsafe */ + int8_t fs_ch = _rc.function[_parameters.rc_map_failsafe]; // get channel mapped to throttle + + if (_parameters.rc_map_failsafe > 0) { // if not 0, use channel number instead of rc.function mapping + fs_ch = _parameters.rc_map_failsafe - 1; + } + + if (_parameters.rc_fails_thr > 0 && fs_ch >= 0) { + /* failsafe configured */ + if ((_parameters.rc_fails_thr < _parameters.min[fs_ch] && rc_input.values[fs_ch] < _parameters.rc_fails_thr) || + (_parameters.rc_fails_thr > _parameters.max[fs_ch] && rc_input.values[fs_ch] > _parameters.rc_fails_thr)) { + /* failsafe triggered, signal is lost by receiver */ + signal_lost = true; + } } } unsigned channel_limit = rc_input.channel_count; - if (channel_limit > _rc_max_chan_count) + if (channel_limit > _rc_max_chan_count) { channel_limit = _rc_max_chan_count; + } - /* we are accepting this message */ - _rc_last_valid = rc_input.timestamp_last_signal; - - /* Read out values from raw message */ + /* read out and scale values from raw message even if signal is invalid */ for (unsigned int i = 0; i < channel_limit; i++) { /* * 1) Constrain to min/max values, as later processing depends on bounds. */ - if (rc_input.values[i] < _parameters.min[i]) + if (rc_input.values[i] < _parameters.min[i]) { rc_input.values[i] = _parameters.min[i]; + } - if (rc_input.values[i] > _parameters.max[i]) + if (rc_input.values[i] > _parameters.max[i]) { rc_input.values[i] = _parameters.max[i]; + } /* * 2) Scale around the mid point differently for lower and upper range. @@ -1364,136 +1481,82 @@ Sensors::rc_poll() _rc.chan[i].scaled *= _parameters.rev[i]; /* handle any parameter-induced blowups */ - if (!isfinite(_rc.chan[i].scaled)) + if (!isfinite(_rc.chan[i].scaled)) { _rc.chan[i].scaled = 0.0f; + } } _rc.chan_count = rc_input.channel_count; + _rc.rssi = rc_input.rssi; + _rc.signal_lost = signal_lost; _rc.timestamp = rc_input.timestamp_last_signal; - manual_control.timestamp = rc_input.timestamp_last_signal; - - /* roll input - rolling right is stick-wise and rotation-wise positive */ - manual_control.roll = limit_minus_one_to_one(_rc.chan[_rc.function[ROLL]].scaled); - /* - * pitch input - stick down is negative, but stick down is pitching up (pos) in NED, - * so reverse sign. - */ - manual_control.pitch = limit_minus_one_to_one(-1.0f * _rc.chan[_rc.function[PITCH]].scaled); - /* yaw input - stick right is positive and positive rotation */ - manual_control.yaw = limit_minus_one_to_one(_rc.chan[_rc.function[YAW]].scaled); - /* throttle input */ - manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled; - - if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f; - - if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f; - - /* scale output */ - if (isfinite(_parameters.rc_scale_roll) && _parameters.rc_scale_roll > 0.0f) { - manual_control.roll *= _parameters.rc_scale_roll; - } - - if (isfinite(_parameters.rc_scale_pitch) && _parameters.rc_scale_pitch > 0.0f) { - manual_control.pitch *= _parameters.rc_scale_pitch; - } - - if (isfinite(_parameters.rc_scale_yaw) && _parameters.rc_scale_yaw > 0.0f) { - manual_control.yaw *= _parameters.rc_scale_yaw; - } - - /* flaps */ - if (_rc.function[FLAPS] >= 0) { - - manual_control.flaps = limit_minus_one_to_one(_rc.chan[_rc.function[FLAPS]].scaled); - - if (isfinite(_parameters.rc_scale_flaps) && _parameters.rc_scale_flaps > 0.0f) { - manual_control.flaps *= _parameters.rc_scale_flaps; - } - } - - /* mode switch input */ - if (_rc.function[MODE] >= 0) { - manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled); - } - - /* assisted switch input */ - if (_rc.function[ASSISTED] >= 0) { - manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled); - } - - /* mission switch input */ - if (_rc.function[MISSION] >= 0) { - manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); - } - - /* return switch input */ - if (_rc.function[RETURN] >= 0) { - manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); - } - - /* offboard switch input */ - if (_rc.function[OFFBOARD_MODE] >= 0) { - manual_control.offboard_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); - } - - /* aux functions, only assign if valid mapping is present */ - if (_rc.function[AUX_1] >= 0) { - manual_control.aux1 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_1]].scaled); - } - - if (_rc.function[AUX_2] >= 0) { - manual_control.aux2 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_2]].scaled); - } - - if (_rc.function[AUX_3] >= 0) { - manual_control.aux3 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_3]].scaled); - } - - if (_rc.function[AUX_4] >= 0) { - manual_control.aux4 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_4]].scaled); - } - - if (_rc.function[AUX_5] >= 0) { - manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled); - } - - /* copy from mapped manual control to control group 3 */ - actuator_group_3.control[0] = manual_control.roll; - actuator_group_3.control[1] = manual_control.pitch; - actuator_group_3.control[2] = manual_control.yaw; - actuator_group_3.control[3] = manual_control.throttle; - actuator_group_3.control[4] = manual_control.flaps; - actuator_group_3.control[5] = manual_control.aux1; - actuator_group_3.control[6] = manual_control.aux2; - actuator_group_3.control[7] = manual_control.aux3; - - /* check if ready for publishing */ + /* publish rc_channels topic even if signal is invalid, for debug */ if (_rc_pub > 0) { orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); } else { - /* advertise the rc topic */ _rc_pub = orb_advertise(ORB_ID(rc_channels), &_rc); } - /* check if ready for publishing */ - if (_manual_control_pub > 0) { - orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control); + if (!signal_lost) { + struct manual_control_setpoint_s manual; + memset(&manual, 0 , sizeof(manual)); + + /* fill values in manual_control_setpoint topic only if signal is valid */ + manual.timestamp = rc_input.timestamp_last_signal; + + /* limit controls */ + manual.y = get_rc_value(ROLL, -1.0, 1.0); + manual.x = get_rc_value(PITCH, -1.0, 1.0); + manual.r = get_rc_value(YAW, -1.0, 1.0); + manual.z = get_rc_value(THROTTLE, 0.0, 1.0); + manual.flaps = get_rc_value(FLAPS, -1.0, 1.0); + manual.aux1 = get_rc_value(AUX_1, -1.0, 1.0); + manual.aux2 = get_rc_value(AUX_2, -1.0, 1.0); + manual.aux3 = get_rc_value(AUX_3, -1.0, 1.0); + manual.aux4 = get_rc_value(AUX_4, -1.0, 1.0); + manual.aux5 = get_rc_value(AUX_5, -1.0, 1.0); + + /* mode switches */ + manual.mode_switch = get_rc_sw3pos_position(MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv); + manual.posctl_switch = get_rc_sw2pos_position(POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv); + manual.return_switch = get_rc_sw2pos_position(RETURN, _parameters.rc_return_th, _parameters.rc_return_inv); + manual.loiter_switch = get_rc_sw2pos_position(LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv); + manual.offboard_switch = get_rc_sw2pos_position(OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv); + + /* publish manual_control_setpoint topic */ + if (_manual_control_pub > 0) { + orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual); - } else { - _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); - } + } else { + _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); + } - /* check if ready for publishing */ - if (_actuator_group_3_pub > 0) { - orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3); + /* copy from mapped manual control to control group 3 */ + struct actuator_controls_s actuator_group_3; + memset(&actuator_group_3, 0 , sizeof(actuator_group_3)); - } else { - _actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3); + actuator_group_3.timestamp = rc_input.timestamp_last_signal; + + actuator_group_3.control[0] = manual.y; + actuator_group_3.control[1] = manual.x; + actuator_group_3.control[2] = manual.r; + actuator_group_3.control[3] = manual.z; + actuator_group_3.control[4] = manual.flaps; + actuator_group_3.control[5] = manual.aux1; + actuator_group_3.control[6] = manual.aux2; + actuator_group_3.control[7] = manual.aux3; + + /* publish actuator_controls_3 topic */ + if (_actuator_group_3_pub > 0) { + orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3); + + } else { + _actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3); + } } } - } void @@ -1570,12 +1633,10 @@ Sensors::task_main() while (!_task_should_exit) { - /* wait for up to 100ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + /* wait for up to 50ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50); - /* timed out - periodic check for _task_should_exit, etc. */ - if (pret == 0) - continue; + /* if pret == 0 it timed out - periodic check for _task_should_exit, etc. */ /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { @@ -1605,8 +1666,9 @@ Sensors::task_main() diff_pres_poll(raw); /* Inform other processes that new data is available to copy */ - if (_publishing) + if (_publishing) { orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw); + } /* Look for new r/c input data */ rc_poll(); @@ -1614,7 +1676,7 @@ Sensors::task_main() perf_end(_loop_perf); } - printf("[sensors] exiting.\n"); + warnx("[sensors] exiting."); _sensors_task = -1; _exit(0); @@ -1629,7 +1691,7 @@ Sensors::start() _sensors_task = task_spawn_cmd("sensors_task", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 2048, + 2000, (main_t)&Sensors::task_main_trampoline, nullptr); @@ -1643,18 +1705,21 @@ Sensors::start() int sensors_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 1) { errx(1, "usage: sensors {start|stop|status}"); + } if (!strcmp(argv[1], "start")) { - if (sensors::g_sensors != nullptr) + if (sensors::g_sensors != nullptr) { errx(0, "already running"); + } sensors::g_sensors = new Sensors; - if (sensors::g_sensors == nullptr) + if (sensors::g_sensors == nullptr) { errx(1, "alloc failed"); + } if (OK != sensors::g_sensors->start()) { delete sensors::g_sensors; @@ -1666,8 +1731,9 @@ int sensors_main(int argc, char *argv[]) } if (!strcmp(argv[1], "stop")) { - if (sensors::g_sensors == nullptr) + if (sensors::g_sensors == nullptr) { errx(1, "not running"); + } delete sensors::g_sensors; sensors::g_sensors = nullptr; @@ -1686,4 +1752,3 @@ int sensors_main(int argc, char *argv[]) warnx("unrecognized command"); return 1; } - diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c index b05273c0d..bf3428a50 100644 --- a/src/modules/systemlib/mixer/mixer_load.c +++ b/src/modules/systemlib/mixer/mixer_load.c @@ -41,6 +41,7 @@ #include <string.h> #include <stdio.h> #include <ctype.h> +#include <systemlib/err.h> #include "mixer_load.h" diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 2d773fd25..7a499ca72 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -521,73 +521,15 @@ param_save_default(void) return ERROR; } - if (res == OK) { - res = param_export(fd, false); + res = param_export(fd, false); - if (res != OK) { - warnx("failed to write parameters to file: %s", filename); - } + if (res != OK) { + warnx("failed to write parameters to file: %s", filename); } close(fd); return res; - -#if 0 - const char *filename_tmp = malloc(strlen(filename) + 5); - sprintf(filename_tmp, "%s.tmp", filename); - - /* delete temp file if exist */ - res = unlink(filename_tmp); - - if (res != OK && errno == ENOENT) - res = OK; - - if (res != OK) - warn("failed to delete temp file: %s", filename_tmp); - - if (res == OK) { - /* write parameters to temp file */ - fd = open(filename_tmp, O_WRONLY | O_CREAT | O_EXCL); - - if (fd < 0) { - warn("failed to open temp file: %s", filename_tmp); - res = ERROR; - } - - if (res == OK) { - res = param_export(fd, false); - - if (res != OK) - warnx("failed to write parameters to file: %s", filename_tmp); - } - - close(fd); - } - - if (res == OK) { - /* delete parameters file */ - res = unlink(filename); - - if (res != OK && errno == ENOENT) - res = OK; - - if (res != OK) - warn("failed to delete parameters file: %s", filename); - } - - if (res == OK) { - /* rename temp file to parameters */ - res = rename(filename_tmp, filename); - - if (res != OK) - warn("failed to rename %s to %s", filename_tmp, filename); - } - - free(filename_tmp); - - return res; -#endif } /** diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 6a4e9392a..45f218a5b 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -53,7 +53,7 @@ #define SIGMA 0.000001f -__EXPORT void pid_init(PID_t *pid, uint8_t mode, float dt_min) +__EXPORT void pid_init(PID_t *pid, pid_mode_t mode, float dt_min) { pid->mode = mode; pid->dt_min = dt_min; diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c index 21e15ec56..c0c1a5cb4 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -140,7 +140,7 @@ int rc_calibration_check(int mavlink_fd) { /* sanity checks pass, enable channel */ if (count) { mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); - warnx(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); + warnx("ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); usleep(100000); } diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c index ec2bc3a9a..702e435ac 100644 --- a/src/modules/systemlib/system_params.c +++ b/src/modules/systemlib/system_params.c @@ -62,12 +62,23 @@ PARAM_DEFINE_INT32(SYS_AUTOSTART, 0); PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0); /** - * Set usage of IO board - * - * Can be used to use a standard startup script but with a FMU only set-up. Set to 0 to force the FMU only set-up. - * - * @min 0 - * @max 1 - * @group System - */ +* Set usage of IO board +* +* Can be used to use a standard startup script but with a FMU only set-up. Set to 0 to force the FMU only set-up. +* +* @min 0 +* @max 1 +* @group System +*/ PARAM_DEFINE_INT32(SYS_USE_IO, 1); + +/** +* Set restart type +* +* Set by px4io to indicate type of restart +* +* @min 0 +* @max 2 +* @group System +*/ +PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2); diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index fb24de8d1..90675bb2e 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -90,6 +90,9 @@ ORB_DEFINE(battery_status, struct battery_status_s); #include "topics/servorail_status.h" ORB_DEFINE(servorail_status, struct servorail_status_s); +#include "topics/system_power.h" +ORB_DEFINE(system_power, struct system_power_s); + #include "topics/vehicle_global_position.h" ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s); @@ -124,7 +127,7 @@ ORB_DEFINE(position_setpoint_triplet, struct position_setpoint_triplet_s); ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s); #include "topics/mission.h" -ORB_DEFINE(mission, struct mission_s); +ORB_DEFINE(offboard_mission, struct mission_s); ORB_DEFINE(onboard_mission, struct mission_s); #include "topics/mission_result.h" @@ -193,3 +196,6 @@ ORB_DEFINE(esc_status, struct esc_status_s); #include "topics/encoders.h" ORB_DEFINE(encoders, struct encoders_s); + +#include "topics/estimator_status.h" +ORB_DEFINE(estimator_status, struct estimator_status_report); diff --git a/src/modules/uORB/topics/airspeed.h b/src/modules/uORB/topics/airspeed.h index a3da3758f..d2ee754cd 100644 --- a/src/modules/uORB/topics/airspeed.h +++ b/src/modules/uORB/topics/airspeed.h @@ -52,9 +52,10 @@ * Airspeed */ struct airspeed_s { - uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ + uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ float indicated_airspeed_m_s; /**< indicated airspeed in meters per second, -1 if unknown */ - float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */ + float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */ + float air_temperature_celsius; /**< air temperature in degrees celsius, -1000 if unknown */ }; /** diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h index 3592c023c..01e14cda9 100644 --- a/src/modules/uORB/topics/differential_pressure.h +++ b/src/modules/uORB/topics/differential_pressure.h @@ -52,13 +52,14 @@ * Differential pressure. */ struct differential_pressure_s { - uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ - uint64_t error_count; + uint64_t timestamp; /**< Microseconds since system boot, needed to integrate */ + uint64_t error_count; /**< Number of errors detected by driver */ float differential_pressure_pa; /**< Differential pressure reading */ + float differential_pressure_raw_pa; /**< Raw differential pressure reading (may be negative) */ float differential_pressure_filtered_pa; /**< Low pass filtered differential pressure reading */ float max_differential_pressure_pa; /**< Maximum differential pressure reading */ - float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ - float temperature; /**< Temperature provided by sensor */ + float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ + float temperature; /**< Temperature provided by sensor, -1000.0f if unknown */ }; diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h index 11332d7a7..628824efa 100644 --- a/src/modules/uORB/topics/esc_status.h +++ b/src/modules/uORB/topics/esc_status.h @@ -60,7 +60,7 @@ enum ESC_VENDOR { ESC_VENDOR_GENERIC = 0, /**< generic ESC */ ESC_VENDOR_MIKROKOPTER, /**< Mikrokopter */ - ESC_VENDOR_GRAUPNER_HOTT /**< Graupner HoTT ESC */ + ESC_VENDOR_GRAUPNER_HOTT /**< Graupner HoTT ESC */ }; enum ESC_CONNECTION_TYPE { @@ -79,16 +79,15 @@ enum ESC_CONNECTION_TYPE { /** * Electronic speed controller status. */ -struct esc_status_s -{ +struct esc_status_s { /* use of a counter and timestamp recommended (but not necessary) */ uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - + uint8_t esc_count; /**< number of connected ESCs */ enum ESC_CONNECTION_TYPE esc_connectiontype; /**< how ESCs connected to the system */ - + struct { uint16_t esc_address; /**< Address of current ESC (in most cases 1-8 / must be set by driver) */ enum ESC_VENDOR esc_vendor; /**< Vendor of current ESC */ diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.h b/src/modules/uORB/topics/estimator_status.h index 500e3e197..5530cdb21 100644 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.h +++ b/src/modules/uORB/topics/estimator_status.h @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> - * + * 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 @@ -33,16 +31,50 @@ * ****************************************************************************/ -/* @file Fixed Wing Attitude Rate Control */ +/** + * @file estimator_status.h + * Definition of the estimator_status_report uORB topic. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +#ifndef ESTIMATOR_STATUS_H_ +#define ESTIMATOR_STATUS_H_ + +#include <stdint.h> +#include <stdbool.h> +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * Estimator status report. + * + * This is a generic status report struct which allows any of the onboard estimators + * to write the internal state to the system log. + * + */ +struct estimator_status_report { + + /* NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes - change with consideration only */ + + uint64_t timestamp; /**< Timestamp in microseconds since boot */ + float states[32]; /**< Internal filter states */ + float n_states; /**< Number of states effectively used */ + bool states_nan; /**< If set to true, one of the states is NaN */ + bool covariance_nan; /**< If set to true, the covariance matrix went NaN */ + bool kalman_gain_nan; /**< If set to true, the Kalman gain matrix went NaN */ -#ifndef FIXEDWING_ATT_CONTROL_RATE_H_ -#define FIXEDWING_ATT_CONTROL_RATE_H_ +}; -#include <uORB/topics/vehicle_rates_setpoint.h> -#include <uORB/topics/actuator_controls.h> +/** + * @} + */ -int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], - struct actuator_controls_s *actuators); +/* register this as object request broker structure */ +ORB_DECLARE(estimator_status); -#endif /* FIXEDWING_ATT_CONTROL_RATE_H_ */ +#endif diff --git a/src/modules/uORB/topics/filtered_bottom_flow.h b/src/modules/uORB/topics/filtered_bottom_flow.h index ab6de2613..c5d545542 100644 --- a/src/modules/uORB/topics/filtered_bottom_flow.h +++ b/src/modules/uORB/topics/filtered_bottom_flow.h @@ -53,8 +53,7 @@ /** * Filtered bottom flow in bodyframe. */ -struct filtered_bottom_flow_s -{ +struct filtered_bottom_flow_s { uint64_t timestamp; /**< time of this estimate, in microseconds since system start */ float sumx; /**< Integrated bodyframe x flow in meters */ diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index ab0c7a411..e4dd67fab 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -44,6 +44,16 @@ #include "../uORB.h" /** + * Switch position + */ +typedef enum { + SWITCH_POS_NONE = 0, /**< switch is not mapped */ + SWITCH_POS_ON, /**< switch activated (value = 1) */ + SWITCH_POS_MIDDLE, /**< middle position (value = 0) */ + SWITCH_POS_OFF /**< switch not activated (value = -1) */ +} switch_pos_t; + +/** * @addtogroup topics * @{ */ @@ -51,30 +61,43 @@ struct manual_control_setpoint_s { uint64_t timestamp; - float roll; /**< ailerons roll / roll rate input */ - float pitch; /**< elevator / pitch / pitch rate */ - float yaw; /**< rudder / yaw rate / yaw */ - float throttle; /**< throttle / collective thrust / altitude */ - - float mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ - float return_switch; /**< land 2 position switch (mandatory): land, no effect */ - float assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ - float mission_switch; /**< mission 2 position switch (optional): mission, loiter */ - float offboard_switch; /**< offboard switch (optional): offboard, onboard */ - /** - * Any of the channels below may not be available and be set to NaN + * Any of the channels may not be available and be set to NaN * to indicate that it does not contain valid data. + * The variable names follow the definition of the + * MANUAL_CONTROL mavlink message. + * The default range is from -1 to 1 (mavlink message -1000 to 1000) + * The range for the z variable is defined from 0 to 1. (The z field of + * the MANUAL_CONTROL mavlink message is defined from -1000 to 1000) */ + float x; /**< stick position in x direction -1..1 + in general corresponds to forward/back motion or pitch of vehicle, + in general a positive value means forward or negative pitch and + a negative value means backward or positive pitch */ + float y; /**< stick position in y direction -1..1 + in general corresponds to right/left motion or roll of vehicle, + in general a positive value means right or positive roll and + a negative value means left or negative roll */ + float z; /**< throttle stick position 0..1 + in general corresponds to up/down motion or thrust of vehicle, + in general the value corresponds to the demanded throttle by the user, + if the input is used for setting the setpoint of a vertical position + controller any value > 0.5 means up and any value < 0.5 means down */ + float r; /**< yaw stick/twist positon, -1..1 + in general corresponds to the righthand rotation around the vertical + (downwards) axis of the vehicle */ + float flaps; /**< flap position */ + float aux1; /**< default function: camera yaw / azimuth */ + float aux2; /**< default function: camera pitch / tilt */ + float aux3; /**< default function: camera trigger */ + float aux4; /**< default function: camera roll */ + float aux5; /**< default function: payload drop */ - float flaps; /**< flap position */ - - float aux1; /**< default function: camera yaw / azimuth */ - float aux2; /**< default function: camera pitch / tilt */ - float aux3; /**< default function: camera trigger */ - float aux4; /**< default function: camera roll */ - float aux5; /**< default function: payload drop */ - + switch_pos_t mode_switch; /**< main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO */ + switch_pos_t return_switch; /**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */ + switch_pos_t posctl_switch; /**< position control 2 position switch (optional): _ALTCTL_, POSCTL */ + switch_pos_t loiter_switch; /**< loiter 2 position switch (optional): _MISSION_, LOITER */ + switch_pos_t offboard_switch; /**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */ }; /**< manual control inputs */ /** diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 194e2ed0c..ef4bc1def 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -77,8 +77,7 @@ enum ORIGIN { * This is the position the MAV is heading towards. If it of type loiter, * the MAV is circling around it with the given loiter radius in meters. */ -struct mission_item_s -{ +struct mission_item_s { bool altitude_is_relative; /**< true if altitude is relative from start point */ double lat; /**< latitude in degrees */ double lon; /**< longitude in degrees */ @@ -106,7 +105,7 @@ struct mission_s */ /* register this as object request broker structure */ -ORB_DECLARE(mission); +ORB_DECLARE(offboard_mission); ORB_DECLARE(onboard_mission); #endif diff --git a/src/modules/uORB/topics/navigation_capabilities.h b/src/modules/uORB/topics/navigation_capabilities.h index 391756f99..7a5ae9891 100644 --- a/src/modules/uORB/topics/navigation_capabilities.h +++ b/src/modules/uORB/topics/navigation_capabilities.h @@ -52,12 +52,12 @@ * Airspeed */ struct navigation_capabilities_s { - float turn_distance; /**< the optimal distance to a waypoint to switch to the next */ + float turn_distance; /**< the optimal distance to a waypoint to switch to the next */ - /* Landing parameters: see fw_pos_control_l1/landingslope.h */ - float landing_horizontal_slope_displacement; - float landing_slope_angle_rad; - float landing_flare_length; + /* Landing parameters: see fw_pos_control_l1/landingslope.h */ + float landing_horizontal_slope_displacement; + float landing_slope_angle_rad; + float landing_flare_length; }; /** diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 7901b930a..68d3e494b 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -45,12 +45,11 @@ /** * Off-board control inputs. - * + * * Typically sent by a ground control station / joystick or by * some off-board controller via C or SIMULINK. */ -enum OFFBOARD_CONTROL_MODE -{ +enum OFFBOARD_CONTROL_MODE { OFFBOARD_CONTROL_MODE_DIRECT = 0, OFFBOARD_CONTROL_MODE_DIRECT_RATES = 1, OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE = 2, diff --git a/src/modules/uORB/topics/optical_flow.h b/src/modules/uORB/topics/optical_flow.h index 98f0e3fa2..0196ae86b 100644 --- a/src/modules/uORB/topics/optical_flow.h +++ b/src/modules/uORB/topics/optical_flow.h @@ -57,6 +57,7 @@ struct optical_flow_s { uint64_t timestamp; /**< in microseconds since system start */ + uint64_t flow_timestamp; /**< timestamp from flow sensor */ int16_t flow_raw_x; /**< flow in pixels in X direction, not rotation-compensated */ int16_t flow_raw_y; /**< flow in pixels in Y direction, not rotation-compensated */ float flow_comp_x_m; /**< speed over ground in meters, rotation-compensated */ diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index cf1ddfc38..34aaa30dd 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -54,24 +54,24 @@ enum SETPOINT_TYPE { - SETPOINT_TYPE_NORMAL = 0, /**< normal setpoint */ - SETPOINT_TYPE_LOITER, /**< loiter setpoint */ - SETPOINT_TYPE_TAKEOFF, /**< takeoff setpoint */ + SETPOINT_TYPE_NORMAL = 0, /**< normal setpoint */ + SETPOINT_TYPE_LOITER, /**< loiter setpoint */ + SETPOINT_TYPE_TAKEOFF, /**< takeoff setpoint */ SETPOINT_TYPE_LAND, /**< land setpoint, altitude must be ignored, vehicle must descend until landing */ SETPOINT_TYPE_IDLE, /**< do nothing, switch off motors or keep at idle speed (MC) */ }; struct position_setpoint_s { - bool valid; /**< true if setpoint is valid */ + bool valid; /**< true if setpoint is valid */ enum SETPOINT_TYPE type; /**< setpoint type to adjust behavior of position controller */ - 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 */ + 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 */ 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 pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ }; /** @@ -85,7 +85,7 @@ struct position_setpoint_triplet_s struct position_setpoint_s current; struct position_setpoint_s next; - nav_state_t nav_state; /**< navigation state */ + nav_state_t nav_state; /**< navigation state */ }; /** diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 086b2ef15..3fdf421be 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -52,29 +52,28 @@ */ #define RC_CHANNELS_MAPPED_MAX 15 -/** +/** * This defines the mapping of the RC functions. * The value assigned to the specific function corresponds to the entry of * the channel array chan[]. */ -enum RC_CHANNELS_FUNCTION -{ - THROTTLE = 0, - ROLL = 1, - PITCH = 2, - YAW = 3, - MODE = 4, - RETURN = 5, - ASSISTED = 6, - MISSION = 7, - OFFBOARD_MODE = 8, - FLAPS = 9, - AUX_1 = 10, - AUX_2 = 11, - AUX_3 = 12, - AUX_4 = 13, - AUX_5 = 14, - RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ +enum RC_CHANNELS_FUNCTION { + THROTTLE = 0, + ROLL = 1, + PITCH = 2, + YAW = 3, + MODE = 4, + RETURN = 5, + POSCTL = 6, + LOITER = 7, + OFFBOARD = 8, + FLAPS = 9, + AUX_1 = 10, + AUX_2 = 11, + AUX_3 = 12, + AUX_4 = 13, + AUX_5 = 14, + RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ }; /** @@ -85,16 +84,17 @@ enum RC_CHANNELS_FUNCTION struct rc_channels_s { uint64_t timestamp; /**< In microseconds since boot time. */ - uint64_t timestamp_last_valid; /**< timestamp of last valid RC signal. */ - struct { - float scaled; /**< Scaled to -1..1 (throttle: 0..1) */ - } chan[RC_CHANNELS_MAPPED_MAX]; - uint8_t chan_count; /**< number of valid channels */ + uint64_t timestamp_last_valid; /**< timestamp of last valid RC signal. */ + struct { + float scaled; /**< Scaled to -1..1 (throttle: 0..1) */ + } chan[RC_CHANNELS_MAPPED_MAX]; + uint8_t chan_count; /**< number of valid channels */ - /*String array to store the names of the functions*/ - char function_name[RC_CHANNELS_FUNCTION_MAX][20]; - int8_t function[RC_CHANNELS_FUNCTION_MAX]; - uint8_t rssi; /**< Overall receive signal strength */ + /*String array to store the names of the functions*/ + char function_name[RC_CHANNELS_FUNCTION_MAX][20]; + int8_t function[RC_CHANNELS_FUNCTION_MAX]; + uint8_t rssi; /**< Overall receive signal strength */ + bool signal_lost; /**< control signal lost, should be checked together with topic timeout */ }; /**< radio control channels. */ /** diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h index cc25a83c3..fa3367de9 100644 --- a/src/modules/uORB/topics/sensor_combined.h +++ b/src/modules/uORB/topics/sensor_combined.h @@ -98,7 +98,8 @@ struct sensor_combined_s { float baro_pres_mbar; /**< Barometric pressure, already temp. comp. */ float baro_alt_meter; /**< Altitude, already temp. comp. */ float baro_temp_celcius; /**< Temperature in degrees celsius */ - float adc_voltage_v[4]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */ + float adc_voltage_v[10]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */ + unsigned adc_mapping[10]; /**< Channel indices of each of these values */ float mcu_temp_celcius; /**< Internal temperature measurement of MCU */ uint64_t baro_timestamp; /**< Barometer timestamp */ diff --git a/src/modules/uORB/topics/subsystem_info.h b/src/modules/uORB/topics/subsystem_info.h index cfe0bf69e..d3a7ce10d 100644 --- a/src/modules/uORB/topics/subsystem_info.h +++ b/src/modules/uORB/topics/subsystem_info.h @@ -50,8 +50,7 @@ #include <stdbool.h> #include "../uORB.h" -enum SUBSYSTEM_TYPE -{ +enum SUBSYSTEM_TYPE { SUBSYSTEM_TYPE_GYRO = 1, SUBSYSTEM_TYPE_ACC = 2, SUBSYSTEM_TYPE_MAG = 4, diff --git a/src/modules/position_estimator_mc/position_estimator_mc_params.h b/src/modules/uORB/topics/system_power.h index c4c95f7b4..7763b6004 100755..100644 --- a/src/modules/position_estimator_mc/position_estimator_mc_params.h +++ b/src/modules/uORB/topics/system_power.h @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Damian Aregger <daregger@student.ethz.ch> - * Tobias Naegeli <naegelit@student.ethz.ch> - * Lorenz Meier <lm@inf.ethz.ch> + * Copyright (C) 2012-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 @@ -34,36 +31,41 @@ * ****************************************************************************/ -/* - * @file position_estimator_mc_params.h - * - * Parameters for Position Estimator +/** + * @file system_power.h + * + * Definition of the system_power voltage and power status uORB topic. */ -#include <systemlib/param/param.h> +#ifndef SYSTEM_POWER_H_ +#define SYSTEM_POWER_H_ -struct position_estimator_mc_params { - float addNoise; - float sigma; - float R; - int baro; /* consider barometer */ -}; +#include "../uORB.h" +#include <stdint.h> -struct position_estimator_mc_param_handles { - param_t addNoise; - param_t sigma; - param_t r; - param_t baro_param_handle; -}; +/** + * @addtogroup topics + * @{ + */ /** - * Initialize all parameter handles and values - * + * voltage and power supply status */ -int parameters_init(struct position_estimator_mc_param_handles *h); +struct system_power_s { + uint64_t timestamp; /**< microseconds since system boot */ + float voltage5V_v; /**< peripheral 5V rail voltage */ + uint8_t usb_connected:1; /**< USB is connected when 1 */ + uint8_t brick_valid:1; /**< brick power is good when 1 */ + uint8_t servo_valid:1; /**< servo power is good when 1 */ + uint8_t periph_5V_OC:1; /**< peripheral overcurrent when 1 */ + uint8_t hipower_5V_OC:1; /**< hi power peripheral overcurrent when 1 */ +}; /** - * Update all parameters - * + * @} */ -int parameters_update(const struct position_estimator_mc_param_handles *h, struct position_estimator_mc_params *p); + +/* register this as object request broker structure */ +ORB_DECLARE(system_power); + +#endif diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h index 5192d4d58..76693c46e 100644 --- a/src/modules/uORB/topics/telemetry_status.h +++ b/src/modules/uORB/topics/telemetry_status.h @@ -44,10 +44,10 @@ #include "../uORB.h" enum TELEMETRY_STATUS_RADIO_TYPE { - TELEMETRY_STATUS_RADIO_TYPE_GENERIC = 0, - TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO, - TELEMETRY_STATUS_RADIO_TYPE_UBIQUITY_BULLET, - TELEMETRY_STATUS_RADIO_TYPE_WIRE + TELEMETRY_STATUS_RADIO_TYPE_GENERIC = 0, + TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO, + TELEMETRY_STATUS_RADIO_TYPE_UBIQUITY_BULLET, + TELEMETRY_STATUS_RADIO_TYPE_WIRE }; /** @@ -57,14 +57,14 @@ enum TELEMETRY_STATUS_RADIO_TYPE { struct telemetry_status_s { uint64_t timestamp; - enum TELEMETRY_STATUS_RADIO_TYPE type; /**< type of the radio hardware */ - uint8_t rssi; /**< local signal strength */ - uint8_t remote_rssi; /**< remote signal strength */ - uint16_t rxerrors; /**< receive errors */ - uint16_t fixed; /**< count of error corrected packets */ - uint8_t noise; /**< background noise level */ - uint8_t remote_noise; /**< remote background noise level */ - uint8_t txbuf; /**< how full the tx buffer is as a percentage */ + enum TELEMETRY_STATUS_RADIO_TYPE type; /**< type of the radio hardware */ + uint8_t rssi; /**< local signal strength */ + uint8_t remote_rssi; /**< remote signal strength */ + uint16_t rxerrors; /**< receive errors */ + uint16_t fixed; /**< count of error corrected packets */ + uint8_t noise; /**< background noise level */ + uint8_t remote_noise; /**< remote background noise level */ + uint8_t txbuf; /**< how full the tx buffer is as a percentage */ }; /** diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h index 7596f944f..d526a8ff2 100644 --- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h +++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h @@ -52,8 +52,7 @@ /** * vehicle attitude setpoint. */ -struct vehicle_attitude_setpoint_s -{ +struct vehicle_attitude_setpoint_s { uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ float roll_body; /**< body angle in NED frame */ diff --git a/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h b/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h index fbfab09f3..fd1ade671 100644 --- a/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h +++ b/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h @@ -48,8 +48,7 @@ * @{ */ -struct vehicle_bodyframe_speed_setpoint_s -{ +struct vehicle_bodyframe_speed_setpoint_s { uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ float vx; /**< in m/s */ diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h index e6e4743c6..c21a29b13 100644 --- a/src/modules/uORB/topics/vehicle_command.h +++ b/src/modules/uORB/topics/vehicle_command.h @@ -51,43 +51,42 @@ * Should contain all commands from MAVLink's VEHICLE_CMD ENUM, * but can contain additional ones. */ -enum VEHICLE_CMD -{ - VEHICLE_CMD_NAV_WAYPOINT=16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */ - VEHICLE_CMD_NAV_LOITER_UNLIM=17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - VEHICLE_CMD_NAV_LOITER_TURNS=18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - VEHICLE_CMD_NAV_LOITER_TIME=19, /* Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - VEHICLE_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_NAV_LAND=21, /* Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| */ - VEHICLE_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */ - VEHICLE_CMD_NAV_ROI=80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ - VEHICLE_CMD_NAV_PATHPLANNING=81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ - VEHICLE_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| */ - VEHICLE_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_CONDITION_YAW=115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| */ - VEHICLE_CMD_CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_JUMP=177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ - VEHICLE_CMD_DO_SET_PARAMETER=180, /* Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_SET_RELAY=181, /* Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_REPEAT_RELAY=182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ - VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ - VEHICLE_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| */ - VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| */ - VEHICLE_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ - VEHICLE_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ - VEHICLE_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ - VEHICLE_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ - VEHICLE_CMD_ENUM_END=501, /* | */ +enum VEHICLE_CMD { + VEHICLE_CMD_NAV_WAYPOINT = 16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_NAV_LOITER_UNLIM = 17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_NAV_LOITER_TURNS = 18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_NAV_LOITER_TIME = 19, /* Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_NAV_RETURN_TO_LAUNCH = 20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_NAV_LAND = 21, /* Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_NAV_TAKEOFF = 22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_NAV_ROI = 80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ + VEHICLE_CMD_NAV_PATHPLANNING = 81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ + VEHICLE_CMD_NAV_LAST = 95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_CONDITION_DELAY = 112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_CONDITION_CHANGE_ALT = 113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| */ + VEHICLE_CMD_CONDITION_DISTANCE = 114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_CONDITION_YAW = 115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| */ + VEHICLE_CMD_CONDITION_LAST = 159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_SET_MODE = 176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_JUMP = 177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_CHANGE_SPEED = 178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_SET_HOME = 179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_DO_SET_PARAMETER = 180, /* Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_SET_RELAY = 181, /* Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_REPEAT_RELAY = 182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_SET_SERVO = 183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_REPEAT_SERVO = 184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_CONTROL_VIDEO = 200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_LAST = 240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_PREFLIGHT_CALIBRATION = 241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ + VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS = 242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ + VEHICLE_CMD_PREFLIGHT_STORAGE = 245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| */ + VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| */ + VEHICLE_CMD_OVERRIDE_GOTO = 252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ + VEHICLE_CMD_MISSION_START = 300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ + VEHICLE_CMD_COMPONENT_ARM_DISARM = 400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ + VEHICLE_CMD_START_RX_PAIR = 500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ + VEHICLE_CMD_ENUM_END = 501, /* | */ }; /** @@ -96,14 +95,13 @@ enum VEHICLE_CMD * Should contain all of MAVLink's VEHICLE_CMD_RESULT values * but can contain additional ones. */ -enum VEHICLE_CMD_RESULT -{ - VEHICLE_CMD_RESULT_ACCEPTED=0, /* Command ACCEPTED and EXECUTED | */ - VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED=1, /* Command TEMPORARY REJECTED/DENIED | */ - VEHICLE_CMD_RESULT_DENIED=2, /* Command PERMANENTLY DENIED | */ - VEHICLE_CMD_RESULT_UNSUPPORTED=3, /* Command UNKNOWN/UNSUPPORTED | */ - VEHICLE_CMD_RESULT_FAILED=4, /* Command executed, but failed | */ - VEHICLE_CMD_RESULT_ENUM_END=5, /* | */ +enum VEHICLE_CMD_RESULT { + VEHICLE_CMD_RESULT_ACCEPTED = 0, /* Command ACCEPTED and EXECUTED | */ + VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1, /* Command TEMPORARY REJECTED/DENIED | */ + VEHICLE_CMD_RESULT_DENIED = 2, /* Command PERMANENTLY DENIED | */ + VEHICLE_CMD_RESULT_UNSUPPORTED = 3, /* Command UNKNOWN/UNSUPPORTED | */ + VEHICLE_CMD_RESULT_FAILED = 4, /* Command executed, but failed | */ + VEHICLE_CMD_RESULT_ENUM_END = 5, /* | */ }; /** @@ -111,8 +109,7 @@ enum VEHICLE_CMD_RESULT * @{ */ -struct vehicle_command_s -{ +struct vehicle_command_s { float param1; /**< Parameter 1, as defined by MAVLink VEHICLE_CMD enum. */ float param2; /**< Parameter 2, as defined by MAVLink VEHICLE_CMD enum. */ float param3; /**< Parameter 3, as defined by MAVLink VEHICLE_CMD enum. */ diff --git a/src/modules/uORB/topics/vehicle_control_debug.h b/src/modules/uORB/topics/vehicle_control_debug.h index 6184284a4..2a45eaccc 100644 --- a/src/modules/uORB/topics/vehicle_control_debug.h +++ b/src/modules/uORB/topics/vehicle_control_debug.h @@ -47,8 +47,7 @@ * @addtogroup topics * @{ */ -struct vehicle_control_debug_s -{ +struct vehicle_control_debug_s { uint64_t timestamp; /**< in microseconds since system start */ float roll_p; /**< roll P control part */ @@ -77,9 +76,9 @@ struct vehicle_control_debug_s }; /**< vehicle_control_debug */ - /** - * @} - */ +/** +* @} +*/ /* register this as object request broker structure */ ORB_DECLARE(vehicle_control_debug); diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 5444c4ebf..49e2ba4b5 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -1,10 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> - * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch> - * @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> + * Copyright (c) 2012-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 @@ -38,8 +34,13 @@ /** * @file vehicle_control_mode.h * Definition of the vehicle_control_mode uORB topic. - * + * * All control apps should depend their actions based on the flags set here. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch> + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> */ #ifndef VEHICLE_CONTROL_MODE @@ -61,8 +62,7 @@ * Encodes the complete system state and is set by the commander app. */ -struct vehicle_control_mode_s -{ +struct vehicle_control_mode_s { uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ bool flag_armed; diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index ff9e98e1c..4897ca737 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> - * @author Lorenz Meier <lm@inf.ethz.ch> + * Copyright (c) 2012-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 @@ -37,6 +34,10 @@ /** * @file vehicle_global_position.h * Definition of the global fused WGS84 position uORB topic. + * + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> */ #ifndef VEHICLE_GLOBAL_POSITION_T_H_ @@ -59,13 +60,9 @@ * estimator, which will take more sources of information into account than just GPS, * e.g. control inputs of the vehicle in a Kalman-filter implementation. */ -struct vehicle_global_position_s -{ +struct vehicle_global_position_s { uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */ - bool global_valid; /**< true if position satisfies validity criteria of estimator */ - bool baro_valid; /**< true if baro_alt is valid (vel_d is also valid in this case) */ - uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ double lat; /**< Latitude in degrees */ double lon; /**< Longitude in degrees */ @@ -74,8 +71,8 @@ struct vehicle_global_position_s float vel_e; /**< Ground east velocity, m/s */ float vel_d; /**< Ground downside velocity, m/s */ float yaw; /**< Yaw in radians -PI..+PI. */ - - float baro_alt; /**< Barometric altitude (not raw baro but fused with accelerometer) */ + float eph; + float epv; }; /** diff --git a/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h b/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h index 73961cdfe..5dac877d0 100644 --- a/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h +++ b/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h @@ -47,8 +47,7 @@ * @{ */ -struct vehicle_global_velocity_setpoint_s -{ +struct vehicle_global_velocity_setpoint_s { float vx; /**< in m/s NED */ float vy; /**< in m/s NED */ float vz; /**< in m/s NED */ diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h index 1639a08c2..794c3f8bc 100644 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ b/src/modules/uORB/topics/vehicle_gps_position.h @@ -53,13 +53,12 @@ /** * GPS position in WGS84 coordinates. */ -struct vehicle_gps_position_s -{ +struct vehicle_gps_position_s { uint64_t timestamp_position; /**< Timestamp for position information */ int32_t lat; /**< Latitude in 1E-7 degrees */ int32_t lon; /**< Longitude in 1E-7 degrees */ int32_t alt; /**< Altitude in 1E-3 meters (millimeters) above MSL */ - + uint64_t timestamp_variance; float s_variance_m_s; /**< speed accuracy estimate m/s */ float p_variance_m; /**< position accuracy estimate m */ diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index d567f2e02..a15303ea4 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -1,7 +1,6 @@ /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +34,9 @@ /** * @file vehicle_local_position.h * Definition of the local fused NED position uORB topic. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> */ #ifndef TOPIC_VEHICLE_LOCAL_POSITION_H_ @@ -52,8 +54,7 @@ /** * Fused local position in NED. */ -struct vehicle_local_position_s -{ +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 */ @@ -73,8 +74,8 @@ struct vehicle_local_position_s 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 */ - int32_t ref_lat; /**< Reference point latitude in 1E7 degrees */ - int32_t ref_lon; /**< Reference point longitude in 1E7 degrees */ + 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 */ /* Distance to surface */ diff --git a/src/modules/uORB/topics/vehicle_local_position_setpoint.h b/src/modules/uORB/topics/vehicle_local_position_setpoint.h index d24d81e3a..8988a0330 100644 --- a/src/modules/uORB/topics/vehicle_local_position_setpoint.h +++ b/src/modules/uORB/topics/vehicle_local_position_setpoint.h @@ -49,8 +49,7 @@ * @{ */ -struct vehicle_local_position_setpoint_s -{ +struct vehicle_local_position_setpoint_s { float x; /**< in meters NED */ float y; /**< in meters NED */ float z; /**< in meters NED */ diff --git a/src/modules/uORB/topics/vehicle_rates_setpoint.h b/src/modules/uORB/topics/vehicle_rates_setpoint.h index 46e62c4b7..9f8b412a7 100644 --- a/src/modules/uORB/topics/vehicle_rates_setpoint.h +++ b/src/modules/uORB/topics/vehicle_rates_setpoint.h @@ -47,8 +47,7 @@ * @addtogroup topics * @{ */ -struct vehicle_rates_setpoint_s -{ +struct vehicle_rates_setpoint_s { uint64_t timestamp; /**< in microseconds since system start */ float roll; /**< body angular rates in NED frame */ @@ -58,9 +57,9 @@ struct vehicle_rates_setpoint_s }; /**< vehicle_rates_setpoint */ - /** - * @} - */ +/** +* @} +*/ /* register this as object request broker structure */ ORB_DECLARE(vehicle_rates_setpoint); diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 318f66a4f..8d58bb4b9 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -63,13 +63,15 @@ /* main state machine */ typedef enum { MAIN_STATE_MANUAL = 0, - MAIN_STATE_SEATBELT, - MAIN_STATE_EASY, + MAIN_STATE_ALTCTL, + MAIN_STATE_POSCTL, MAIN_STATE_AUTO, MAIN_STATE_OFFBOARD, MAIN_STATE_MAX } main_state_t; +// If you change the order, add or remove arming_state_t states make sure to update the arrays +// in state_machine_helper.cpp as well. typedef enum { ARMING_STATE_INIT = 0, ARMING_STATE_STANDBY, @@ -94,35 +96,6 @@ typedef enum { FAILSAFE_STATE_MAX } failsafe_state_t; -typedef enum { - MODE_SWITCH_MANUAL = 0, - MODE_SWITCH_ASSISTED, - MODE_SWITCH_AUTO -} mode_switch_pos_t; - -typedef enum { - ASSISTED_SWITCH_SEATBELT = 0, - ASSISTED_SWITCH_EASY -} assisted_switch_pos_t; - -typedef enum { - RETURN_SWITCH_NONE = 0, - RETURN_SWITCH_NORMAL, - RETURN_SWITCH_RETURN -} return_switch_pos_t; - -typedef enum { - MISSION_SWITCH_NONE = 0, - MISSION_SWITCH_LOITER, - MISSION_SWITCH_MISSION -} mission_switch_pos_t; - -typedef enum { - OFFBOARD_SWITCH_NONE = 0, - OFFBOARD_SWITCH_OFFBOARD, - OFFBOARD_SWITCH_ONBOARD -} offboard_switch_pos_t; - enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, @@ -138,31 +111,31 @@ enum VEHICLE_MODE_FLAG { * Should match 1:1 MAVLink's MAV_TYPE ENUM */ enum VEHICLE_TYPE { - VEHICLE_TYPE_GENERIC=0, /* Generic micro air vehicle. | */ - VEHICLE_TYPE_FIXED_WING=1, /* Fixed wing aircraft. | */ - VEHICLE_TYPE_QUADROTOR=2, /* Quadrotor | */ - VEHICLE_TYPE_COAXIAL=3, /* Coaxial helicopter | */ - VEHICLE_TYPE_HELICOPTER=4, /* Normal helicopter with tail rotor. | */ - VEHICLE_TYPE_ANTENNA_TRACKER=5, /* Ground installation | */ - VEHICLE_TYPE_GCS=6, /* Operator control unit / ground control station | */ - VEHICLE_TYPE_AIRSHIP=7, /* Airship, controlled | */ - VEHICLE_TYPE_FREE_BALLOON=8, /* Free balloon, uncontrolled | */ - VEHICLE_TYPE_ROCKET=9, /* Rocket | */ - VEHICLE_TYPE_GROUND_ROVER=10, /* Ground rover | */ - VEHICLE_TYPE_SURFACE_BOAT=11, /* Surface vessel, boat, ship | */ - VEHICLE_TYPE_SUBMARINE=12, /* Submarine | */ - VEHICLE_TYPE_HEXAROTOR=13, /* Hexarotor | */ - VEHICLE_TYPE_OCTOROTOR=14, /* Octorotor | */ - VEHICLE_TYPE_TRICOPTER=15, /* Octorotor | */ - VEHICLE_TYPE_FLAPPING_WING=16, /* Flapping wing | */ - VEHICLE_TYPE_KITE=17, /* Kite | */ - VEHICLE_TYPE_ENUM_END=18, /* | */ + VEHICLE_TYPE_GENERIC = 0, /* Generic micro air vehicle. | */ + VEHICLE_TYPE_FIXED_WING = 1, /* Fixed wing aircraft. | */ + VEHICLE_TYPE_QUADROTOR = 2, /* Quadrotor | */ + VEHICLE_TYPE_COAXIAL = 3, /* Coaxial helicopter | */ + VEHICLE_TYPE_HELICOPTER = 4, /* Normal helicopter with tail rotor. | */ + VEHICLE_TYPE_ANTENNA_TRACKER = 5, /* Ground installation | */ + VEHICLE_TYPE_GCS = 6, /* Operator control unit / ground control station | */ + VEHICLE_TYPE_AIRSHIP = 7, /* Airship, controlled | */ + VEHICLE_TYPE_FREE_BALLOON = 8, /* Free balloon, uncontrolled | */ + VEHICLE_TYPE_ROCKET = 9, /* Rocket | */ + VEHICLE_TYPE_GROUND_ROVER = 10, /* Ground rover | */ + VEHICLE_TYPE_SURFACE_BOAT = 11, /* Surface vessel, boat, ship | */ + VEHICLE_TYPE_SUBMARINE = 12, /* Submarine | */ + VEHICLE_TYPE_HEXAROTOR = 13, /* Hexarotor | */ + VEHICLE_TYPE_OCTOROTOR = 14, /* Octorotor | */ + VEHICLE_TYPE_TRICOPTER = 15, /* Octorotor | */ + VEHICLE_TYPE_FLAPPING_WING = 16, /* Flapping wing | */ + VEHICLE_TYPE_KITE = 17, /* Kite | */ + VEHICLE_TYPE_ENUM_END = 18, /* | */ }; enum VEHICLE_BATTERY_WARNING { - VEHICLE_BATTERY_WARNING_NONE = 0, /**< no battery low voltage warning active */ - VEHICLE_BATTERY_WARNING_LOW, /**< warning of low voltage */ - VEHICLE_BATTERY_WARNING_CRITICAL /**< alerting of critical voltage */ + VEHICLE_BATTERY_WARNING_NONE = 0, /**< no battery low voltage warning active */ + VEHICLE_BATTERY_WARNING_LOW, /**< warning of low voltage */ + VEHICLE_BATTERY_WARNING_CRITICAL /**< alerting of critical voltage */ }; /** @@ -175,8 +148,7 @@ enum VEHICLE_BATTERY_WARNING { * * Encodes the complete system state and is set by the commander app. */ -struct vehicle_status_s -{ +struct vehicle_status_s { /* use of a counter and timestamp recommended (but not necessary) */ uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ @@ -195,12 +167,6 @@ struct vehicle_status_s bool is_rotary_wing; - mode_switch_pos_t mode_switch; - return_switch_pos_t return_switch; - assisted_switch_pos_t assisted_switch; - mission_switch_pos_t mission_switch; - offboard_switch_pos_t offboard_switch; - bool condition_battery_voltage_valid; bool condition_system_in_air_restore; /**< true if we can restore in mid air */ bool condition_system_sensors_initialized; @@ -227,7 +193,7 @@ struct vehicle_status_s uint32_t onboard_control_sensors_present; uint32_t onboard_control_sensors_enabled; uint32_t onboard_control_sensors_health; - + float load; /**< processor load from 0 to 1 */ float battery_voltage; float battery_current; diff --git a/src/modules/uORB/topics/vehicle_vicon_position.h b/src/modules/uORB/topics/vehicle_vicon_position.h index 0822fa89a..e19a34a5d 100644 --- a/src/modules/uORB/topics/vehicle_vicon_position.h +++ b/src/modules/uORB/topics/vehicle_vicon_position.h @@ -52,8 +52,7 @@ /** * Fused local position in NED. */ -struct vehicle_vicon_position_s -{ +struct vehicle_vicon_position_s { uint64_t timestamp; /**< time of this estimate, in microseconds since system start */ bool valid; /**< true if position satisfies validity criteria of estimator */ diff --git a/src/modules/unit_test/unit_test.cpp b/src/modules/unit_test/unit_test.cpp index 64ee544a2..02d1af481 100644 --- a/src/modules/unit_test/unit_test.cpp +++ b/src/modules/unit_test/unit_test.cpp @@ -32,17 +32,10 @@ * ****************************************************************************/ -/** - * @file unit_test.cpp - * A unit test library. - * - */ - #include "unit_test.h" #include <systemlib/err.h> - UnitTest::UnitTest() { } @@ -51,15 +44,15 @@ UnitTest::~UnitTest() { } -void -UnitTest::print_results(const char* result) +void UnitTest::printResults(void) +{ + warnx(mu_tests_failed() ? "SOME TESTS FAILED" : "ALL TESTS PASSED"); + warnx(" Tests passed : %d", mu_tests_passed()); + warnx(" Tests failed : %d", mu_tests_failed()); + warnx(" Assertions : %d", mu_assertion()); +} + +void UnitTest::printAssert(const char* msg, const char* test, const char* file, int line) { - if (result != 0) { - warnx("Failed: %s:%d", mu_last_test(), mu_line()); - warnx("%s", result); - } else { - warnx("ALL TESTS PASSED"); - warnx(" Tests run : %d", mu_tests_run()); - warnx(" Assertion : %d", mu_assertion()); - } + warnx("Assertion failed: %s - %s (%s:%d)", msg, test, file, line); } diff --git a/src/modules/unit_test/unit_test.h b/src/modules/unit_test/unit_test.h index 3020734f4..32eb8c308 100644 --- a/src/modules/unit_test/unit_test.h +++ b/src/modules/unit_test/unit_test.h @@ -32,62 +32,55 @@ * ****************************************************************************/ -/** - * @file unit_test.h - * A unit test library based on MinUnit (http://www.jera.com/techinfo/jtns/jtn002.html). - * - */ - #ifndef UNIT_TEST_H_ -#define UNIT_TEST_ +#define UNIT_TEST_H_ #include <systemlib/err.h> - class __EXPORT UnitTest { public: -#define xstr(s) str(s) -#define str(s) #s #define INLINE_GLOBAL(type,func) inline type& func() { static type x; return x; } INLINE_GLOBAL(int, mu_tests_run) +INLINE_GLOBAL(int, mu_tests_failed) +INLINE_GLOBAL(int, mu_tests_passed) INLINE_GLOBAL(int, mu_assertion) INLINE_GLOBAL(int, mu_line) INLINE_GLOBAL(const char*, mu_last_test) -#define mu_assert(message, test) \ - do \ - { \ - if (!(test)) \ - return __FILE__ ":" xstr(__LINE__) " " message " (" #test ")"; \ - else \ - mu_assertion()++; \ - } while (0) - - -#define mu_run_test(test) \ -do \ -{ \ - const char *message; \ - mu_last_test() = #test; \ - mu_line() = __LINE__; \ - message = test(); \ - mu_tests_run()++; \ - if (message) \ - return message; \ -} while (0) - - -public: UnitTest(); virtual ~UnitTest(); - virtual const char* run_tests() = 0; - virtual void print_results(const char* result); -}; - + virtual void runTests(void) = 0; + void printResults(void); + + void printAssert(const char* msg, const char* test, const char* file, int line); + +#define ut_assert(message, test) \ + do { \ + if (!(test)) { \ + printAssert(message, #test, __FILE__, __LINE__); \ + return false; \ + } else { \ + mu_assertion()++; \ + } \ + } while (0) + +#define ut_run_test(test) \ + do { \ + warnx("RUNNING TEST: %s", #test); \ + mu_tests_run()++; \ + if (!test()) { \ + warnx("TEST FAILED: %s", #test); \ + mu_tests_failed()++; \ + } else { \ + warnx("TEST PASSED: %s", #test); \ + mu_tests_passed()++; \ + } \ + } while (0) +}; #endif /* UNIT_TEST_H_ */ |