diff options
Diffstat (limited to 'src/modules')
322 files changed, 67164 insertions, 0 deletions
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp new file mode 100644 index 000000000..97d7fdd75 --- /dev/null +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -0,0 +1,795 @@ +/**************************************************************************** + * + * 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> + +// 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), + // ekf matrices + F(9, 9), + G(9, 6), + P(9, 9), + P0(9, 9), + V(6, 6), + // attitude measurement ekf matrices + HAtt(4, 9), + RAtt(4, 4), + // position measurement ekf matrices + HPos(6, 9), + RPos(6, 6), + // attitude representations + C_nb(), + q(), + _accel_sub(-1), + _gyro_sub(-1), + _mag_sub(-1), + // 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)), + _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), + // 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; + + // initial state covariance matrix + P0 = Matrix::identity(9) * 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; + + // gyro, accel and mag subscriptions + _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); + _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); + _mag_sub = orb_subscribe(ORB_ID(sensor_mag)); + + struct accel_report accel; + orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel); + + struct mag_report mag; + orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag); + + // initialize rotation quaternion with a single raw sensor measurement + q = init(accel.x, accel.y, accel.z, mag.x, mag.y, mag.z); + + // initialize dcm + C_nb = Dcm(q); + + // 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 && + _sensors.accelerometer_counter > 10 && + _sensors.gyro_counter > 10 && + _sensors.magnetometer_counter > 10) { + if (correctAtt() == ret_ok) _attitudeInitCounter++; + + if (_attitudeInitCounter > 100) { + warnx("initialized EKF attitude\n"); + warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f\n", + 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); + _positionInitialized = true; + warnx("initialized EKF state with GPS\n"); + warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n", + double(vN), double(vE), double(vD), + lat, lon, 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; + + // position publication + _pos.timestamp = _pubTimeStamp; + _pos.time_gps_usec = _gps.timestamp_position; + _pos.valid = true; + _pos.lat = getLatDegE7(); + _pos.lon = getLonDegE7(); + _pos.alt = float(alt); + _pos.relative_alt = float(alt); // TODO, make relative + _pos.vx = vN; + _pos.vy = vE; + _pos.vz = vD; + _pos.hdg = psi; + + // 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(); + + 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) { + Vector3 w(_sensors.gyro_rad_s); + + // attitude + q = q + q.derivative(w) * dt; + + // renormalize quaternion if needed + if (fabsf(q.norm() - 1.0f) > 1e-4f) { + q = q.unit(); + } + + // C_nb update + C_nb = Dcm(q); + + // euler update + EulerAngles euler(C_nb); + phi = euler.getPhi(); + theta = euler.getTheta(); + psi = euler.getPsi(); + + // specific acceleration in nav frame + Vector3 accelB(_sensors.accelerometer_m_s2); + Vector3 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 predictioon equations + // for discrte time EKF + // http://en.wikipedia.org/wiki/Extended_Kalman_filter + P = P + (F * P + P * F.transpose() + G * V * G.transpose()) * 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::Dcm C_rp(math::EulerAngles(phi, theta, 0.0f));//C_nb.transpose(); + + // mag measurement + Vector3 magBody(_sensors.magnetometer_ga); + + // transform to earth frame + Vector3 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 + Vector3 zAccel(_sensors.accelerometer_m_s2); + float accelMag = zAccel.norm(); + zAccel = zAccel.unit(); + + // ignore accel correction when accel mag not close to g + Matrix 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 + Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -_g.get())).unit(); + + // calculate residual + Vector y(4); + y(0) = yMag; + y(1) = zAccel(0) - zAccelHat(0); + y(2) = zAccel(1) - zAccelHat(1); + y(3) = 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 S = HAtt * P * HAtt.transpose() + RAttAdjust; // residual covariance + Matrix K = P * HAtt.transpose() * S.inverse(); + Vector xCorrect = K * y; + + // check correciton is sane + for (size_t i = 0; i < xCorrect.getRows(); i++) { + float val = xCorrect(i); + + if (isnan(val) || isinf(val)) { + // abort correction and return + warnx("numerical failure in att correction\n"); + // 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.dot(S.inverse() * y); + + if (beta > _faultAtt.get()) { + warnx("fault in attitude: beta = %8.4f", (double)beta); + warnx("y:\n"); y.print(); + } + + // update quaternions from euler + // angle correction + q = Quaternion(EulerAngles(phi, theta, psi)); + + return ret_ok; +} + +int KalmanNav::correctPos() +{ + using namespace math; + + // residual + Vector y(6); + y(0) = _gps.vel_n_m_s - vN; + y(1) = _gps.vel_e_m_s - vE; + y(2) = double(_gps.lat) - lat * 1.0e7 * M_RAD_TO_DEG; + y(3) = double(_gps.lon) - lon * 1.0e7 * M_RAD_TO_DEG; + y(4) = double(_gps.alt) / 1.0e3 - alt; + y(5) = double(_sensors.baro_alt_meter) - alt; + + // compute correction + // http://en.wikipedia.org/wiki/Extended_Kalman_filter + Matrix S = HPos * P * HPos.transpose() + RPos; // residual covariance + Matrix K = P * HPos.transpose() * S.inverse(); + Vector xCorrect = K * y; + + // check correction is sane + for (size_t i = 0; i < xCorrect.getRows(); i++) { + float val = xCorrect(i); + + if (!isfinite(val)) { + // abort correction and return + warnx("numerical failure in gps correction\n"); + // 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 += double(xCorrect(ALT)); + + // update state covariance + // http://en.wikipedia.org/wiki/Extended_Kalman_filter + P = P - K * HPos * P; + + // fault detetcion + float beta = y.dot(S.inverse() * 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", + 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 new file mode 100644 index 000000000..49d0d157d --- /dev/null +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp @@ -0,0 +1,193 @@ +/**************************************************************************** + * + * 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 <controllib/block/UOrbSubscription.hpp> +#include <controllib/block/UOrbPublication.hpp> + +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_global_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 F; /**< Jacobian(f,x), where dx/dt = f(x,u) */ + math::Matrix G; /**< noise shaping matrix for gyro/accel */ + math::Matrix P; /**< state covariance matrix */ + math::Matrix P0; /**< initial state covariance matrix */ + math::Matrix V; /**< gyro/ accel noise matrix */ + math::Matrix HAtt; /**< attitude measurement matrix */ + math::Matrix RAtt; /**< attitude measurement noise matrix */ + math::Matrix HPos; /**< position measurement jacobian matrix */ + math::Matrix RPos; /**< position measurement noise matrix */ + // attitude + math::Dcm C_nb; /**< direction cosine matrix from body to nav frame */ + math::Quaternion q; /**< quaternion from body to nav frame */ + // subscriptions + control::UOrbSubscription<sensor_combined_s> _sensors; /**< sensors sub. */ + control::UOrbSubscription<vehicle_gps_position_s> _gps; /**< gps sub. */ + control::UOrbSubscription<parameter_update_s> _param_update; /**< parameter update sub. */ + // publications + control::UOrbPublication<vehicle_global_position_s> _pos; /**< position pub. */ + control::UOrbPublication<vehicle_attitude_s> _att; /**< attitude pub. */ + + int _accel_sub; /**< Accelerometer subscription */ + int _gyro_sub; /**< Gyroscope subscription */ + int _mag_sub; /**< Magnetometer subscription */ + + // 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 */ + float alt; /**< altitude, meters */ + // parameters + control::BlockParam<float> _vGyro; /**< gyro process noise */ + control::BlockParam<float> _vAccel; /**< accelerometer process noise */ + control::BlockParam<float> _rMag; /**< magnetometer measurement noise */ + control::BlockParam<float> _rGpsVel; /**< gps velocity measurement noise */ + control::BlockParam<float> _rGpsPos; /**< gps position measurement noise */ + control::BlockParam<float> _rGpsAlt; /**< gps altitude measurement noise */ + control::BlockParam<float> _rPressAlt; /**< press altitude measurement noise */ + control::BlockParam<float> _rAccel; /**< accelerometer measurement noise */ + control::BlockParam<float> _magDip; /**< magnetic inclination with level */ + control::BlockParam<float> _magDec; /**< magnetic declination, clockwise rotation */ + control::BlockParam<float> _g; /**< gravitational constant */ + control::BlockParam<float> _faultPos; /**< fault detection threshold for position */ + control::BlockParam<float> _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 new file mode 100644 index 000000000..4befdc879 --- /dev/null +++ b/src/modules/att_pos_estimator_ekf/kalman_main.cpp @@ -0,0 +1,156 @@ +/**************************************************************************** + * + * 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, + 4096, + 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"); + + } else { + warnx("not started\n"); + } + + exit(0); + } + + 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 new file mode 100644 index 000000000..8d4a40d95 --- /dev/null +++ b/src/modules/att_pos_estimator_ekf/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# 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 new file mode 100644 index 000000000..4af5edead --- /dev/null +++ b/src/modules/att_pos_estimator_ekf/params.c @@ -0,0 +1,49 @@ +/**************************************************************************** + * + * 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 new file mode 100755 index 000000000..d8b40ac3b --- /dev/null +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -0,0 +1,471 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: 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 attitude_estimator_ekf_main.c + * + * Extended Kalman Filter for Attitude Estimation. + */ + +#include <nuttx/config.h> +#include <unistd.h> +#include <stdlib.h> +#include <stdio.h> +#include <stdbool.h> +#include <poll.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/debug_key_value.h> +#include <uORB/topics/sensor_combined.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/parameter_update.h> +#include <drivers/drv_hrt.h> + +#include <systemlib/systemlib.h> +#include <systemlib/perf_counter.h> +#include <systemlib/err.h> + +#ifdef __cplusplus +extern "C" { +#endif +#include "codegen/attitudeKalmanfilter_initialize.h" +#include "codegen/attitudeKalmanfilter.h" +#include "attitude_estimator_ekf_params.h" +#ifdef __cplusplus +} +#endif + +extern "C" __EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]); + +static bool thread_should_exit = false; /**< Deamon exit flag */ +static bool thread_running = false; /**< Deamon status flag */ +static int attitude_estimator_ekf_task; /**< Handle of deamon task / thread */ + +/** + * Mainloop of attitude_estimator_ekf. + */ +int attitude_estimator_ekf_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); + + fprintf(stderr, "usage: attitude_estimator_ekf {start|stop|status} [-p <additional params>]\n\n"); + exit(1); +} + +/** + * The attitude_estimator_ekf 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 attitude_estimator_ekf_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + printf("attitude_estimator_ekf already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + attitude_estimator_ekf_task = task_spawn_cmd("attitude_estimator_ekf", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 14000, + attitude_estimator_ekf_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) { + printf("\tattitude_estimator_ekf app is running\n"); + + } else { + printf("\tattitude_estimator_ekf app not started\n"); + } + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +/* + * [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) + */ + +/* + * EKF Attitude Estimator main function. + * + * Estimates the attitude recursively once started. + * + * @param argc number of commandline arguments (plus command name) + * @param argv strings containing the arguments + */ +int attitude_estimator_ekf_thread_main(int argc, char *argv[]) +{ + +const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds + + float dt = 0.005f; +/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ + float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */ + float x_aposteriori_k[12]; /**< states */ + float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f, + }; /**< init: diagonal matrix with big values */ + + float x_aposteriori[12]; + float P_aposteriori[144]; + + /* output euler angles */ + float euler[3] = {0.0f, 0.0f, 0.0f}; + + float Rot_matrix[9] = {1.f, 0, 0, + 0, 1.f, 0, + 0, 0, 1.f + }; /**< init: identity matrix */ + + // print text + printf("Extended Kalman Filter Attitude Estimator initialized..\n\n"); + fflush(stdout); + + int overloadcounter = 19; + + /* Initialize filter */ + attitudeKalmanfilter_initialize(); + + /* store start time to guard against too slow update rates */ + uint64_t last_run = hrt_absolute_time(); + + struct sensor_combined_s raw; + memset(&raw, 0, sizeof(raw)); + struct vehicle_attitude_s att; + memset(&att, 0, sizeof(att)); + struct vehicle_status_s state; + memset(&state, 0, sizeof(state)); + + uint64_t last_data = 0; + uint64_t last_measurement = 0; + + /* subscribe to raw data */ + int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); + /* rate-limit raw data updates to 200Hz */ + orb_set_interval(sub_raw, 4); + + /* subscribe to param changes */ + int sub_params = orb_subscribe(ORB_ID(parameter_update)); + + /* subscribe to system state*/ + int sub_state = orb_subscribe(ORB_ID(vehicle_status)); + + /* advertise attitude */ + orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); + + + int loopcounter = 0; + int printcounter = 0; + + thread_running = true; + + /* advertise debug value */ + // struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; + // orb_advert_t pub_dbg = -1; + + float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f}; + // XXX write this out to perf regs + + /* keep track of sensor updates */ + uint32_t sensor_last_count[3] = {0, 0, 0}; + uint64_t sensor_last_timestamp[3] = {0, 0, 0}; + + struct attitude_estimator_ekf_params ekf_params; + + struct attitude_estimator_ekf_param_handles ekf_param_handles; + + /* initialize parameter handles */ + parameters_init(&ekf_param_handles); + + uint64_t start_time = hrt_absolute_time(); + bool initialized = false; + + float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; + unsigned offset_count = 0; + + /* register the perf counter */ + perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf"); + + /* Main loop*/ + while (!thread_should_exit) { + + struct pollfd fds[2]; + fds[0].fd = sub_raw; + fds[0].events = POLLIN; + fds[1].fd = sub_params; + fds[1].events = POLLIN; + int ret = poll(fds, 2, 1000); + + if (ret < 0) { + /* XXX this is seriously bad - should be an emergency */ + } else if (ret == 0) { + /* check if we're in HIL - not getting sensor data is fine then */ + orb_copy(ORB_ID(vehicle_status), sub_state, &state); + + if (!state.flag_hil_enabled) { + fprintf(stderr, + "[att ekf] WARNING: Not getting sensors - sensor app running?\n"); + } + + } else { + + /* only update parameters if they changed */ + if (fds[1].revents & POLLIN) { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), sub_params, &update); + + /* update parameters */ + parameters_update(&ekf_param_handles, &ekf_params); + } + + /* only run filter if sensor values changed */ + if (fds[0].revents & POLLIN) { + + /* get latest measurements */ + orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); + + if (!initialized) { + + gyro_offsets[0] += raw.gyro_rad_s[0]; + gyro_offsets[1] += raw.gyro_rad_s[1]; + gyro_offsets[2] += raw.gyro_rad_s[2]; + offset_count++; + + if (hrt_absolute_time() - start_time > 3000000LL) { + initialized = true; + gyro_offsets[0] /= offset_count; + gyro_offsets[1] /= offset_count; + gyro_offsets[2] /= offset_count; + } + + } else { + + perf_begin(ekf_loop_perf); + + /* Calculate data time difference in seconds */ + dt = (raw.timestamp - last_measurement) / 1000000.0f; + last_measurement = raw.timestamp; + uint8_t update_vect[3] = {0, 0, 0}; + + /* Fill in gyro measurements */ + if (sensor_last_count[0] != raw.gyro_counter) { + update_vect[0] = 1; + sensor_last_count[0] = raw.gyro_counter; + sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); + sensor_last_timestamp[0] = raw.timestamp; + } + + z_k[0] = raw.gyro_rad_s[0] - gyro_offsets[0]; + z_k[1] = raw.gyro_rad_s[1] - gyro_offsets[1]; + z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; + + /* update accelerometer measurements */ + if (sensor_last_count[1] != raw.accelerometer_counter) { + update_vect[1] = 1; + sensor_last_count[1] = raw.accelerometer_counter; + sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); + sensor_last_timestamp[1] = raw.timestamp; + } + + z_k[3] = raw.accelerometer_m_s2[0]; + z_k[4] = raw.accelerometer_m_s2[1]; + z_k[5] = raw.accelerometer_m_s2[2]; + + /* update magnetometer measurements */ + if (sensor_last_count[2] != raw.magnetometer_counter) { + update_vect[2] = 1; + sensor_last_count[2] = raw.magnetometer_counter; + sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); + sensor_last_timestamp[2] = raw.timestamp; + } + + z_k[6] = raw.magnetometer_ga[0]; + z_k[7] = raw.magnetometer_ga[1]; + z_k[8] = raw.magnetometer_ga[2]; + + uint64_t now = hrt_absolute_time(); + unsigned int time_elapsed = now - last_run; + last_run = now; + + if (time_elapsed > loop_interval_alarm) { + //TODO: add warning, cpu overload here + // if (overloadcounter == 20) { + // printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm); + // overloadcounter = 0; + // } + + overloadcounter++; + } + + static bool const_initialized = false; + + /* initialize with good values once we have a reasonable dt estimate */ + if (!const_initialized && dt < 0.05f && dt > 0.005f) { + dt = 0.005f; + parameters_update(&ekf_param_handles, &ekf_params); + + x_aposteriori_k[0] = z_k[0]; + x_aposteriori_k[1] = z_k[1]; + x_aposteriori_k[2] = z_k[2]; + x_aposteriori_k[3] = 0.0f; + x_aposteriori_k[4] = 0.0f; + x_aposteriori_k[5] = 0.0f; + x_aposteriori_k[6] = z_k[3]; + x_aposteriori_k[7] = z_k[4]; + x_aposteriori_k[8] = z_k[5]; + x_aposteriori_k[9] = z_k[6]; + x_aposteriori_k[10] = z_k[7]; + x_aposteriori_k[11] = z_k[8]; + + const_initialized = true; + } + + /* do not execute the filter if not initialized */ + if (!const_initialized) { + continue; + } + + uint64_t timing_start = hrt_absolute_time(); + + attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r, + euler, Rot_matrix, x_aposteriori, P_aposteriori); + + /* swap values for next iteration, check for fatal inputs */ + if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) { + memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k)); + memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k)); + + } else { + /* due to inputs or numerical failure the output is invalid, skip it */ + continue; + } + + if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data); + + last_data = raw.timestamp; + + /* send out */ + att.timestamp = raw.timestamp; + + // XXX Apply the same transformation to the rotation matrix + att.roll = euler[0] - ekf_params.roll_off; + att.pitch = euler[1] - ekf_params.pitch_off; + att.yaw = euler[2] - ekf_params.yaw_off; + + att.rollspeed = x_aposteriori[0]; + att.pitchspeed = x_aposteriori[1]; + att.yawspeed = x_aposteriori[2]; + att.rollacc = x_aposteriori[3]; + att.pitchacc = x_aposteriori[4]; + att.yawacc = x_aposteriori[5]; + + //att.yawspeed =z_k[2] ; + /* copy offsets */ + memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets)); + + /* copy rotation matrix */ + memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix)); + att.R_valid = true; + + if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { + // Broadcast + orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); + + } else { + warnx("NaN in roll/pitch/yaw estimate!"); + } + + perf_end(ekf_loop_perf); + } + } + } + + loopcounter++; + } + + thread_running = false; + + return 0; +} diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c new file mode 100755 index 000000000..7d3812abd --- /dev/null +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: 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 attitude_estimator_ekf_params.c + * + * Parameters for EKF filter + */ + +#include "attitude_estimator_ekf_params.h" + +/* Extended Kalman Filter covariances */ + +/* gyro process noise */ +PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q0, 1e-4f); +PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q1, 0.08f); +PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q2, 0.009f); +/* gyro offsets process noise */ +PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q3, 0.005f); +PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q4, 0.0f); + +/* gyro measurement noise */ +PARAM_DEFINE_FLOAT(EKF_ATT_V2_R0, 0.0008f); +PARAM_DEFINE_FLOAT(EKF_ATT_V2_R1, 0.8f); +PARAM_DEFINE_FLOAT(EKF_ATT_V2_R2, 1.0f); +/* accelerometer measurement noise */ +PARAM_DEFINE_FLOAT(EKF_ATT_V2_R3, 0.0f); + +/* offsets in roll, pitch and yaw of sensor plane and body */ +PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f); +PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f); +PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f); + +int parameters_init(struct attitude_estimator_ekf_param_handles *h) +{ + /* PID parameters */ + h->q0 = param_find("EKF_ATT_V2_Q0"); + h->q1 = param_find("EKF_ATT_V2_Q1"); + h->q2 = param_find("EKF_ATT_V2_Q2"); + h->q3 = param_find("EKF_ATT_V2_Q3"); + h->q4 = param_find("EKF_ATT_V2_Q4"); + + h->r0 = param_find("EKF_ATT_V2_R0"); + h->r1 = param_find("EKF_ATT_V2_R1"); + h->r2 = param_find("EKF_ATT_V2_R2"); + h->r3 = param_find("EKF_ATT_V2_R3"); + + h->roll_off = param_find("ATT_ROLL_OFFS"); + h->pitch_off = param_find("ATT_PITCH_OFFS"); + h->yaw_off = param_find("ATT_YAW_OFFS"); + + return OK; +} + +int parameters_update(const struct attitude_estimator_ekf_param_handles *h, struct attitude_estimator_ekf_params *p) +{ + param_get(h->q0, &(p->q[0])); + param_get(h->q1, &(p->q[1])); + param_get(h->q2, &(p->q[2])); + param_get(h->q3, &(p->q[3])); + param_get(h->q4, &(p->q[4])); + + param_get(h->r0, &(p->r[0])); + param_get(h->r1, &(p->r[1])); + param_get(h->r2, &(p->r[2])); + param_get(h->r3, &(p->r[3])); + + param_get(h->roll_off, &(p->roll_off)); + param_get(h->pitch_off, &(p->pitch_off)); + param_get(h->yaw_off, &(p->yaw_off)); + + return OK; +} diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h new file mode 100755 index 000000000..09817d58e --- /dev/null +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: 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 attitude_estimator_ekf_params.h + * + * Parameters for EKF filter + */ + +#include <systemlib/param/param.h> + +struct attitude_estimator_ekf_params { + float r[9]; + float q[12]; + float roll_off; + float pitch_off; + float yaw_off; +}; + +struct attitude_estimator_ekf_param_handles { + param_t r0, r1, r2, r3; + param_t q0, q1, q2, q3, q4; + param_t roll_off, pitch_off, yaw_off; +}; + +/** + * Initialize all parameter handles and values + * + */ +int parameters_init(struct attitude_estimator_ekf_param_handles *h); + +/** + * Update all parameters + * + */ +int parameters_update(const struct attitude_estimator_ekf_param_handles *h, struct attitude_estimator_ekf_params *p); diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c new file mode 100755 index 000000000..9e97ad11a --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c @@ -0,0 +1,1148 @@ +/*
+ * attitudeKalmanfilter.c
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "rdivide.h"
+#include "norm.h"
+#include "cross.h"
+#include "eye.h"
+#include "mrdivide.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+static real32_T rt_atan2f_snf(real32_T u0, real32_T u1);
+
+/* Function Definitions */
+static real32_T rt_atan2f_snf(real32_T u0, real32_T u1)
+{
+ real32_T y;
+ int32_T b_u0;
+ int32_T b_u1;
+ if (rtIsNaNF(u0) || rtIsNaNF(u1)) {
+ y = ((real32_T)rtNaN);
+ } else if (rtIsInfF(u0) && rtIsInfF(u1)) {
+ if (u0 > 0.0F) {
+ b_u0 = 1;
+ } else {
+ b_u0 = -1;
+ }
+
+ if (u1 > 0.0F) {
+ b_u1 = 1;
+ } else {
+ b_u1 = -1;
+ }
+
+ y = (real32_T)atan2((real32_T)b_u0, (real32_T)b_u1);
+ } else if (u1 == 0.0F) {
+ if (u0 > 0.0F) {
+ y = RT_PIF / 2.0F;
+ } else if (u0 < 0.0F) {
+ y = -(RT_PIF / 2.0F);
+ } else {
+ y = 0.0F;
+ }
+ } else {
+ y = (real32_T)atan2(u0, u1);
+ }
+
+ return y;
+}
+
+/*
+ * function [eulerAngles,Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter_wo(updateVect,dt,z,x_aposteriori_k,P_aposteriori_k,q,r)
+ */
+void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
+ real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T
+ P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T
+ eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T
+ P_aposteriori[144])
+{
+ real32_T wak[3];
+ real32_T O[9];
+ real_T dv0[9];
+ real32_T a[9];
+ int32_T i;
+ real32_T b_a[9];
+ real32_T x_n_b[3];
+ real32_T b_x_aposteriori_k[3];
+ real32_T z_n_b[3];
+ real32_T c_a[3];
+ real32_T d_a[3];
+ int32_T i0;
+ real32_T x_apriori[12];
+ real_T dv1[144];
+ real32_T A_lin[144];
+ static const int8_T iv0[36] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0,
+ 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
+
+ real32_T b_A_lin[144];
+ real32_T b_q[144];
+ real32_T c_A_lin[144];
+ real32_T d_A_lin[144];
+ real32_T e_A_lin[144];
+ int32_T i1;
+ real32_T P_apriori[144];
+ real32_T b_P_apriori[108];
+ static const int8_T iv1[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 };
+
+ real32_T K_k[108];
+ real32_T fv0[81];
+ static const int8_T iv2[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 };
+
+ real32_T b_r[81];
+ real32_T fv1[81];
+ real32_T f0;
+ real32_T c_P_apriori[36];
+ static const int8_T iv3[36] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
+
+ real32_T fv2[36];
+ static const int8_T iv4[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
+
+ real32_T c_r[9];
+ real32_T b_K_k[36];
+ real32_T d_P_apriori[72];
+ static const int8_T iv5[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 1, 0, 0, 0 };
+
+ real32_T c_K_k[72];
+ static const int8_T iv6[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
+ 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0 };
+
+ real32_T b_z[6];
+ static const int8_T iv7[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 1 };
+
+ static const int8_T iv8[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,
+ 0, 0, 0, 1 };
+
+ real32_T fv3[6];
+ real32_T c_z[6];
+
+ /* Extended Attitude Kalmanfilter */
+ /* */
+ /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
+ /* measurement vector z has the following entries [ax,ay,az||mx,my,mz||wmx,wmy,wmz]' */
+ /* knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */
+ /* */
+ /* [x_aposteriori,P_aposteriori] = AttKalman(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) */
+ /* */
+ /* Example.... */
+ /* */
+ /* $Author: Tobias Naegeli $ $Date: 2012 $ $Revision: 1 $ */
+ /* coder.varsize('udpIndVect', [9,1], [1,0]) */
+ /* udpIndVect=find(updVect); */
+ /* process and measurement noise covariance matrix */
+ /* Q = diag(q.^2*dt); */
+ /* observation matrix */
+ /* 'attitudeKalmanfilter:33' wx= x_aposteriori_k(1); */
+ /* 'attitudeKalmanfilter:34' wy= x_aposteriori_k(2); */
+ /* 'attitudeKalmanfilter:35' wz= x_aposteriori_k(3); */
+ /* 'attitudeKalmanfilter:37' wax= x_aposteriori_k(4); */
+ /* 'attitudeKalmanfilter:38' way= x_aposteriori_k(5); */
+ /* 'attitudeKalmanfilter:39' waz= x_aposteriori_k(6); */
+ /* 'attitudeKalmanfilter:41' zex= x_aposteriori_k(7); */
+ /* 'attitudeKalmanfilter:42' zey= x_aposteriori_k(8); */
+ /* 'attitudeKalmanfilter:43' zez= x_aposteriori_k(9); */
+ /* 'attitudeKalmanfilter:45' mux= x_aposteriori_k(10); */
+ /* 'attitudeKalmanfilter:46' muy= x_aposteriori_k(11); */
+ /* 'attitudeKalmanfilter:47' muz= x_aposteriori_k(12); */
+ /* % prediction section */
+ /* body angular accelerations */
+ /* 'attitudeKalmanfilter:51' wak =[wax;way;waz]; */
+ wak[0] = x_aposteriori_k[3];
+ wak[1] = x_aposteriori_k[4];
+ wak[2] = x_aposteriori_k[5];
+
+ /* body angular rates */
+ /* 'attitudeKalmanfilter:54' wk =[wx; wy; wz] + dt*wak; */
+ /* derivative of the prediction rotation matrix */
+ /* 'attitudeKalmanfilter:57' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */
+ O[0] = 0.0F;
+ O[1] = -x_aposteriori_k[2];
+ O[2] = x_aposteriori_k[1];
+ O[3] = x_aposteriori_k[2];
+ O[4] = 0.0F;
+ O[5] = -x_aposteriori_k[0];
+ O[6] = -x_aposteriori_k[1];
+ O[7] = x_aposteriori_k[0];
+ O[8] = 0.0F;
+
+ /* prediction of the earth z vector */
+ /* 'attitudeKalmanfilter:60' zek =(eye(3)+O*dt)*[zex;zey;zez]; */
+ eye(dv0);
+ for (i = 0; i < 9; i++) {
+ a[i] = (real32_T)dv0[i] + O[i] * dt;
+ }
+
+ /* prediction of the magnetic vector */
+ /* 'attitudeKalmanfilter:63' muk =(eye(3)+O*dt)*[mux;muy;muz]; */
+ eye(dv0);
+ for (i = 0; i < 9; i++) {
+ b_a[i] = (real32_T)dv0[i] + O[i] * dt;
+ }
+
+ /* 'attitudeKalmanfilter:65' EZ=[0,zez,-zey; */
+ /* 'attitudeKalmanfilter:66' -zez,0,zex; */
+ /* 'attitudeKalmanfilter:67' zey,-zex,0]'; */
+ /* 'attitudeKalmanfilter:68' MA=[0,muz,-muy; */
+ /* 'attitudeKalmanfilter:69' -muz,0,mux; */
+ /* 'attitudeKalmanfilter:70' zey,-mux,0]'; */
+ /* 'attitudeKalmanfilter:74' E=eye(3); */
+ /* 'attitudeKalmanfilter:76' Z=zeros(3); */
+ /* 'attitudeKalmanfilter:77' x_apriori=[wk;wak;zek;muk]; */
+ x_n_b[0] = x_aposteriori_k[0];
+ x_n_b[1] = x_aposteriori_k[1];
+ x_n_b[2] = x_aposteriori_k[2];
+ b_x_aposteriori_k[0] = x_aposteriori_k[6];
+ b_x_aposteriori_k[1] = x_aposteriori_k[7];
+ b_x_aposteriori_k[2] = x_aposteriori_k[8];
+ z_n_b[0] = x_aposteriori_k[9];
+ z_n_b[1] = x_aposteriori_k[10];
+ z_n_b[2] = x_aposteriori_k[11];
+ for (i = 0; i < 3; i++) {
+ c_a[i] = 0.0F;
+ for (i0 = 0; i0 < 3; i0++) {
+ c_a[i] += a[i + 3 * i0] * b_x_aposteriori_k[i0];
+ }
+
+ d_a[i] = 0.0F;
+ for (i0 = 0; i0 < 3; i0++) {
+ d_a[i] += b_a[i + 3 * i0] * z_n_b[i0];
+ }
+
+ x_apriori[i] = x_n_b[i] + dt * wak[i];
+ }
+
+ for (i = 0; i < 3; i++) {
+ x_apriori[i + 3] = wak[i];
+ }
+
+ for (i = 0; i < 3; i++) {
+ x_apriori[i + 6] = c_a[i];
+ }
+
+ for (i = 0; i < 3; i++) {
+ x_apriori[i + 9] = d_a[i];
+ }
+
+ /* 'attitudeKalmanfilter:81' A_lin=[ Z, E, Z, Z */
+ /* 'attitudeKalmanfilter:82' Z, Z, Z, Z */
+ /* 'attitudeKalmanfilter:83' EZ, Z, O, Z */
+ /* 'attitudeKalmanfilter:84' MA, Z, Z, O]; */
+ /* 'attitudeKalmanfilter:86' A_lin=eye(12)+A_lin*dt; */
+ b_eye(dv1);
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_lin[i0 + 12 * i] = (real32_T)iv0[i0 + 3 * i];
+ }
+
+ for (i0 = 0; i0 < 3; i0++) {
+ A_lin[(i0 + 12 * i) + 3] = 0.0F;
+ }
+ }
+
+ A_lin[6] = 0.0F;
+ A_lin[7] = x_aposteriori_k[8];
+ A_lin[8] = -x_aposteriori_k[7];
+ A_lin[18] = -x_aposteriori_k[8];
+ A_lin[19] = 0.0F;
+ A_lin[20] = x_aposteriori_k[6];
+ A_lin[30] = x_aposteriori_k[7];
+ A_lin[31] = -x_aposteriori_k[6];
+ A_lin[32] = 0.0F;
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_lin[(i0 + 12 * (i + 3)) + 6] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_lin[(i0 + 12 * (i + 6)) + 6] = O[i0 + 3 * i];
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_lin[(i0 + 12 * (i + 9)) + 6] = 0.0F;
+ }
+ }
+
+ A_lin[9] = 0.0F;
+ A_lin[10] = x_aposteriori_k[11];
+ A_lin[11] = -x_aposteriori_k[10];
+ A_lin[21] = -x_aposteriori_k[11];
+ A_lin[22] = 0.0F;
+ A_lin[23] = x_aposteriori_k[9];
+ A_lin[33] = x_aposteriori_k[7];
+ A_lin[34] = -x_aposteriori_k[9];
+ A_lin[35] = 0.0F;
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_lin[(i0 + 12 * (i + 3)) + 9] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_lin[(i0 + 12 * (i + 6)) + 9] = 0.0F;
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ A_lin[(i0 + 12 * (i + 9)) + 9] = O[i0 + 3 * i];
+ }
+ }
+
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ b_A_lin[i0 + 12 * i] = (real32_T)dv1[i0 + 12 * i] + A_lin[i0 + 12 * i] *
+ dt;
+ }
+ }
+
+ /* 'attitudeKalmanfilter:88' Qtemp=[ q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */
+ /* 'attitudeKalmanfilter:89' 0, q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */
+ /* 'attitudeKalmanfilter:90' 0, 0, q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0; */
+ /* 'attitudeKalmanfilter:91' 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0, 0, 0; */
+ /* 'attitudeKalmanfilter:92' 0, 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0, 0; */
+ /* 'attitudeKalmanfilter:93' 0, 0, 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0; */
+ /* 'attitudeKalmanfilter:94' 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0, 0, 0; */
+ /* 'attitudeKalmanfilter:95' 0, 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0, 0; */
+ /* 'attitudeKalmanfilter:96' 0, 0, 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0; */
+ /* 'attitudeKalmanfilter:97' 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4), 0, 0; */
+ /* 'attitudeKalmanfilter:98' 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4), 0; */
+ /* 'attitudeKalmanfilter:99' 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4)]; */
+ /* 'attitudeKalmanfilter:103' Q=A_lin*Qtemp*A_lin'; */
+ /* 'attitudeKalmanfilter:106' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */
+ b_q[0] = q[0];
+ b_q[12] = 0.0F;
+ b_q[24] = 0.0F;
+ b_q[36] = 0.0F;
+ b_q[48] = 0.0F;
+ b_q[60] = 0.0F;
+ b_q[72] = 0.0F;
+ b_q[84] = 0.0F;
+ b_q[96] = 0.0F;
+ b_q[108] = 0.0F;
+ b_q[120] = 0.0F;
+ b_q[132] = 0.0F;
+ b_q[1] = 0.0F;
+ b_q[13] = q[0];
+ b_q[25] = 0.0F;
+ b_q[37] = 0.0F;
+ b_q[49] = 0.0F;
+ b_q[61] = 0.0F;
+ b_q[73] = 0.0F;
+ b_q[85] = 0.0F;
+ b_q[97] = 0.0F;
+ b_q[109] = 0.0F;
+ b_q[121] = 0.0F;
+ b_q[133] = 0.0F;
+ b_q[2] = 0.0F;
+ b_q[14] = 0.0F;
+ b_q[26] = q[0];
+ b_q[38] = 0.0F;
+ b_q[50] = 0.0F;
+ b_q[62] = 0.0F;
+ b_q[74] = 0.0F;
+ b_q[86] = 0.0F;
+ b_q[98] = 0.0F;
+ b_q[110] = 0.0F;
+ b_q[122] = 0.0F;
+ b_q[134] = 0.0F;
+ b_q[3] = 0.0F;
+ b_q[15] = 0.0F;
+ b_q[27] = 0.0F;
+ b_q[39] = q[1];
+ b_q[51] = 0.0F;
+ b_q[63] = 0.0F;
+ b_q[75] = 0.0F;
+ b_q[87] = 0.0F;
+ b_q[99] = 0.0F;
+ b_q[111] = 0.0F;
+ b_q[123] = 0.0F;
+ b_q[135] = 0.0F;
+ b_q[4] = 0.0F;
+ b_q[16] = 0.0F;
+ b_q[28] = 0.0F;
+ b_q[40] = 0.0F;
+ b_q[52] = q[1];
+ b_q[64] = 0.0F;
+ b_q[76] = 0.0F;
+ b_q[88] = 0.0F;
+ b_q[100] = 0.0F;
+ b_q[112] = 0.0F;
+ b_q[124] = 0.0F;
+ b_q[136] = 0.0F;
+ b_q[5] = 0.0F;
+ b_q[17] = 0.0F;
+ b_q[29] = 0.0F;
+ b_q[41] = 0.0F;
+ b_q[53] = 0.0F;
+ b_q[65] = q[1];
+ b_q[77] = 0.0F;
+ b_q[89] = 0.0F;
+ b_q[101] = 0.0F;
+ b_q[113] = 0.0F;
+ b_q[125] = 0.0F;
+ b_q[137] = 0.0F;
+ b_q[6] = 0.0F;
+ b_q[18] = 0.0F;
+ b_q[30] = 0.0F;
+ b_q[42] = 0.0F;
+ b_q[54] = 0.0F;
+ b_q[66] = 0.0F;
+ b_q[78] = q[2];
+ b_q[90] = 0.0F;
+ b_q[102] = 0.0F;
+ b_q[114] = 0.0F;
+ b_q[126] = 0.0F;
+ b_q[138] = 0.0F;
+ b_q[7] = 0.0F;
+ b_q[19] = 0.0F;
+ b_q[31] = 0.0F;
+ b_q[43] = 0.0F;
+ b_q[55] = 0.0F;
+ b_q[67] = 0.0F;
+ b_q[79] = 0.0F;
+ b_q[91] = q[2];
+ b_q[103] = 0.0F;
+ b_q[115] = 0.0F;
+ b_q[127] = 0.0F;
+ b_q[139] = 0.0F;
+ b_q[8] = 0.0F;
+ b_q[20] = 0.0F;
+ b_q[32] = 0.0F;
+ b_q[44] = 0.0F;
+ b_q[56] = 0.0F;
+ b_q[68] = 0.0F;
+ b_q[80] = 0.0F;
+ b_q[92] = 0.0F;
+ b_q[104] = q[2];
+ b_q[116] = 0.0F;
+ b_q[128] = 0.0F;
+ b_q[140] = 0.0F;
+ b_q[9] = 0.0F;
+ b_q[21] = 0.0F;
+ b_q[33] = 0.0F;
+ b_q[45] = 0.0F;
+ b_q[57] = 0.0F;
+ b_q[69] = 0.0F;
+ b_q[81] = 0.0F;
+ b_q[93] = 0.0F;
+ b_q[105] = 0.0F;
+ b_q[117] = q[3];
+ b_q[129] = 0.0F;
+ b_q[141] = 0.0F;
+ b_q[10] = 0.0F;
+ b_q[22] = 0.0F;
+ b_q[34] = 0.0F;
+ b_q[46] = 0.0F;
+ b_q[58] = 0.0F;
+ b_q[70] = 0.0F;
+ b_q[82] = 0.0F;
+ b_q[94] = 0.0F;
+ b_q[106] = 0.0F;
+ b_q[118] = 0.0F;
+ b_q[130] = q[3];
+ b_q[142] = 0.0F;
+ b_q[11] = 0.0F;
+ b_q[23] = 0.0F;
+ b_q[35] = 0.0F;
+ b_q[47] = 0.0F;
+ b_q[59] = 0.0F;
+ b_q[71] = 0.0F;
+ b_q[83] = 0.0F;
+ b_q[95] = 0.0F;
+ b_q[107] = 0.0F;
+ b_q[119] = 0.0F;
+ b_q[131] = 0.0F;
+ b_q[143] = q[3];
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ A_lin[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_aposteriori_k[i1 + 12 *
+ i0];
+ }
+
+ c_A_lin[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ c_A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * b_q[i1 + 12 * i0];
+ }
+ }
+
+ for (i0 = 0; i0 < 12; i0++) {
+ d_A_lin[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ d_A_lin[i + 12 * i0] += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1];
+ }
+
+ e_A_lin[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ e_A_lin[i + 12 * i0] += c_A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1];
+ }
+ }
+ }
+
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ P_apriori[i0 + 12 * i] = d_A_lin[i0 + 12 * i] + e_A_lin[i0 + 12 * i];
+ }
+ }
+
+ /* % update */
+ /* 'attitudeKalmanfilter:110' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==1 */
+ if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 1)) {
+ /* 'attitudeKalmanfilter:111' if z(6)<4 || z(5)>15 */
+ if ((z[5] < 4.0F) || (z[4] > 15.0F)) {
+ /* 'attitudeKalmanfilter:112' r(2)=10000; */
+ r[1] = 10000.0F;
+ }
+
+ /* 'attitudeKalmanfilter:114' R=[r(1),0,0,0,0,0,0,0,0; */
+ /* 'attitudeKalmanfilter:115' 0,r(1),0,0,0,0,0,0,0; */
+ /* 'attitudeKalmanfilter:116' 0,0,r(1),0,0,0,0,0,0; */
+ /* 'attitudeKalmanfilter:117' 0,0,0,r(2),0,0,0,0,0; */
+ /* 'attitudeKalmanfilter:118' 0,0,0,0,r(2),0,0,0,0; */
+ /* 'attitudeKalmanfilter:119' 0,0,0,0,0,r(2),0,0,0; */
+ /* 'attitudeKalmanfilter:120' 0,0,0,0,0,0,r(3),0,0; */
+ /* 'attitudeKalmanfilter:121' 0,0,0,0,0,0,0,r(3),0; */
+ /* 'attitudeKalmanfilter:122' 0,0,0,0,0,0,0,0,r(3)]; */
+ /* observation matrix */
+ /* [zw;ze;zmk]; */
+ /* 'attitudeKalmanfilter:125' H_k=[ E, Z, Z, Z; */
+ /* 'attitudeKalmanfilter:126' Z, Z, E, Z; */
+ /* 'attitudeKalmanfilter:127' Z, Z, Z, E]; */
+ /* 'attitudeKalmanfilter:129' y_k=z(1:9)-H_k*x_apriori; */
+ /* 'attitudeKalmanfilter:132' S_k=H_k*P_apriori*H_k'+R; */
+ /* 'attitudeKalmanfilter:133' K_k=(P_apriori*H_k'/(S_k)); */
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 9; i0++) {
+ b_P_apriori[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ b_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)iv1[i1
+ + 12 * i0];
+ }
+ }
+ }
+
+ for (i = 0; i < 9; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ K_k[i + 9 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ K_k[i + 9 * i0] += (real32_T)iv2[i + 9 * i1] * P_apriori[i1 + 12 * i0];
+ }
+ }
+
+ for (i0 = 0; i0 < 9; i0++) {
+ fv0[i + 9 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ fv0[i + 9 * i0] += K_k[i + 9 * i1] * (real32_T)iv1[i1 + 12 * i0];
+ }
+ }
+ }
+
+ b_r[0] = r[0];
+ b_r[9] = 0.0F;
+ b_r[18] = 0.0F;
+ b_r[27] = 0.0F;
+ b_r[36] = 0.0F;
+ b_r[45] = 0.0F;
+ b_r[54] = 0.0F;
+ b_r[63] = 0.0F;
+ b_r[72] = 0.0F;
+ b_r[1] = 0.0F;
+ b_r[10] = r[0];
+ b_r[19] = 0.0F;
+ b_r[28] = 0.0F;
+ b_r[37] = 0.0F;
+ b_r[46] = 0.0F;
+ b_r[55] = 0.0F;
+ b_r[64] = 0.0F;
+ b_r[73] = 0.0F;
+ b_r[2] = 0.0F;
+ b_r[11] = 0.0F;
+ b_r[20] = r[0];
+ b_r[29] = 0.0F;
+ b_r[38] = 0.0F;
+ b_r[47] = 0.0F;
+ b_r[56] = 0.0F;
+ b_r[65] = 0.0F;
+ b_r[74] = 0.0F;
+ b_r[3] = 0.0F;
+ b_r[12] = 0.0F;
+ b_r[21] = 0.0F;
+ b_r[30] = r[1];
+ b_r[39] = 0.0F;
+ b_r[48] = 0.0F;
+ b_r[57] = 0.0F;
+ b_r[66] = 0.0F;
+ b_r[75] = 0.0F;
+ b_r[4] = 0.0F;
+ b_r[13] = 0.0F;
+ b_r[22] = 0.0F;
+ b_r[31] = 0.0F;
+ b_r[40] = r[1];
+ b_r[49] = 0.0F;
+ b_r[58] = 0.0F;
+ b_r[67] = 0.0F;
+ b_r[76] = 0.0F;
+ b_r[5] = 0.0F;
+ b_r[14] = 0.0F;
+ b_r[23] = 0.0F;
+ b_r[32] = 0.0F;
+ b_r[41] = 0.0F;
+ b_r[50] = r[1];
+ b_r[59] = 0.0F;
+ b_r[68] = 0.0F;
+ b_r[77] = 0.0F;
+ b_r[6] = 0.0F;
+ b_r[15] = 0.0F;
+ b_r[24] = 0.0F;
+ b_r[33] = 0.0F;
+ b_r[42] = 0.0F;
+ b_r[51] = 0.0F;
+ b_r[60] = r[2];
+ b_r[69] = 0.0F;
+ b_r[78] = 0.0F;
+ b_r[7] = 0.0F;
+ b_r[16] = 0.0F;
+ b_r[25] = 0.0F;
+ b_r[34] = 0.0F;
+ b_r[43] = 0.0F;
+ b_r[52] = 0.0F;
+ b_r[61] = 0.0F;
+ b_r[70] = r[2];
+ b_r[79] = 0.0F;
+ b_r[8] = 0.0F;
+ b_r[17] = 0.0F;
+ b_r[26] = 0.0F;
+ b_r[35] = 0.0F;
+ b_r[44] = 0.0F;
+ b_r[53] = 0.0F;
+ b_r[62] = 0.0F;
+ b_r[71] = 0.0F;
+ b_r[80] = r[2];
+ for (i = 0; i < 9; i++) {
+ for (i0 = 0; i0 < 9; i0++) {
+ fv1[i0 + 9 * i] = fv0[i0 + 9 * i] + b_r[i0 + 9 * i];
+ }
+ }
+
+ mrdivide(b_P_apriori, fv1, K_k);
+
+ /* 'attitudeKalmanfilter:136' x_aposteriori=x_apriori+K_k*y_k; */
+ for (i = 0; i < 9; i++) {
+ f0 = 0.0F;
+ for (i0 = 0; i0 < 12; i0++) {
+ f0 += (real32_T)iv2[i + 9 * i0] * x_apriori[i0];
+ }
+
+ O[i] = z[i] - f0;
+ }
+
+ for (i = 0; i < 12; i++) {
+ f0 = 0.0F;
+ for (i0 = 0; i0 < 9; i0++) {
+ f0 += K_k[i + 12 * i0] * O[i0];
+ }
+
+ x_aposteriori[i] = x_apriori[i] + f0;
+ }
+
+ /* 'attitudeKalmanfilter:137' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */
+ b_eye(dv1);
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ f0 = 0.0F;
+ for (i1 = 0; i1 < 9; i1++) {
+ f0 += K_k[i + 12 * i1] * (real32_T)iv2[i1 + 9 * i0];
+ }
+
+ b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
+ }
+ }
+
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ P_aposteriori[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + 12
+ * i0];
+ }
+ }
+ }
+ } else {
+ /* 'attitudeKalmanfilter:138' else */
+ /* 'attitudeKalmanfilter:139' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==0 */
+ if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 0)) {
+ /* 'attitudeKalmanfilter:141' R=[r(1),0,0; */
+ /* 'attitudeKalmanfilter:142' 0,r(1),0; */
+ /* 'attitudeKalmanfilter:143' 0,0,r(1)]; */
+ /* observation matrix */
+ /* 'attitudeKalmanfilter:146' H_k=[ E, Z, Z, Z]; */
+ /* 'attitudeKalmanfilter:148' y_k=z(1:3)-H_k(1:3,1:12)*x_apriori; */
+ /* 'attitudeKalmanfilter:150' S_k=H_k(1:3,1:12)*P_apriori*H_k(1:3,1:12)'+R(1:3,1:3); */
+ /* 'attitudeKalmanfilter:151' K_k=(P_apriori*H_k(1:3,1:12)'/(S_k)); */
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ c_P_apriori[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ c_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)
+ iv3[i1 + 12 * i0];
+ }
+ }
+ }
+
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ fv2[i + 3 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ fv2[i + 3 * i0] += (real32_T)iv4[i + 3 * i1] * P_apriori[i1 + 12 *
+ i0];
+ }
+ }
+
+ for (i0 = 0; i0 < 3; i0++) {
+ O[i + 3 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ O[i + 3 * i0] += fv2[i + 3 * i1] * (real32_T)iv3[i1 + 12 * i0];
+ }
+ }
+ }
+
+ c_r[0] = r[0];
+ c_r[3] = 0.0F;
+ c_r[6] = 0.0F;
+ c_r[1] = 0.0F;
+ c_r[4] = r[0];
+ c_r[7] = 0.0F;
+ c_r[2] = 0.0F;
+ c_r[5] = 0.0F;
+ c_r[8] = r[0];
+ for (i = 0; i < 3; i++) {
+ for (i0 = 0; i0 < 3; i0++) {
+ a[i0 + 3 * i] = O[i0 + 3 * i] + c_r[i0 + 3 * i];
+ }
+ }
+
+ b_mrdivide(c_P_apriori, a, b_K_k);
+
+ /* 'attitudeKalmanfilter:154' x_aposteriori=x_apriori+K_k*y_k; */
+ for (i = 0; i < 3; i++) {
+ f0 = 0.0F;
+ for (i0 = 0; i0 < 12; i0++) {
+ f0 += (real32_T)iv4[i + 3 * i0] * x_apriori[i0];
+ }
+
+ x_n_b[i] = z[i] - f0;
+ }
+
+ for (i = 0; i < 12; i++) {
+ f0 = 0.0F;
+ for (i0 = 0; i0 < 3; i0++) {
+ f0 += b_K_k[i + 12 * i0] * x_n_b[i0];
+ }
+
+ x_aposteriori[i] = x_apriori[i] + f0;
+ }
+
+ /* 'attitudeKalmanfilter:155' P_aposteriori=(eye(12)-K_k*H_k(1:3,1:12))*P_apriori; */
+ b_eye(dv1);
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ f0 = 0.0F;
+ for (i1 = 0; i1 < 3; i1++) {
+ f0 += b_K_k[i + 12 * i1] * (real32_T)iv4[i1 + 3 * i0];
+ }
+
+ b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
+ }
+ }
+
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ P_aposteriori[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 +
+ 12 * i0];
+ }
+ }
+ }
+ } else {
+ /* 'attitudeKalmanfilter:156' else */
+ /* 'attitudeKalmanfilter:157' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==0 */
+ if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 0))
+ {
+ /* 'attitudeKalmanfilter:158' if z(6)<4 || z(5)>15 */
+ if ((z[5] < 4.0F) || (z[4] > 15.0F)) {
+ /* 'attitudeKalmanfilter:159' r(2)=10000; */
+ r[1] = 10000.0F;
+ }
+
+ /* 'attitudeKalmanfilter:162' R=[r(1),0,0,0,0,0; */
+ /* 'attitudeKalmanfilter:163' 0,r(1),0,0,0,0; */
+ /* 'attitudeKalmanfilter:164' 0,0,r(1),0,0,0; */
+ /* 'attitudeKalmanfilter:165' 0,0,0,r(2),0,0; */
+ /* 'attitudeKalmanfilter:166' 0,0,0,0,r(2),0; */
+ /* 'attitudeKalmanfilter:167' 0,0,0,0,0,r(2)]; */
+ /* observation matrix */
+ /* 'attitudeKalmanfilter:170' H_k=[ E, Z, Z, Z; */
+ /* 'attitudeKalmanfilter:171' Z, Z, E, Z]; */
+ /* 'attitudeKalmanfilter:173' y_k=z(1:6)-H_k(1:6,1:12)*x_apriori; */
+ /* 'attitudeKalmanfilter:175' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */
+ /* 'attitudeKalmanfilter:176' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 6; i0++) {
+ d_P_apriori[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)
+ iv5[i1 + 12 * i0];
+ }
+ }
+ }
+
+ for (i = 0; i < 6; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ c_K_k[i + 6 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ c_K_k[i + 6 * i0] += (real32_T)iv6[i + 6 * i1] * P_apriori[i1 + 12
+ * i0];
+ }
+ }
+
+ for (i0 = 0; i0 < 6; i0++) {
+ fv2[i + 6 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv5[i1 + 12 * i0];
+ }
+ }
+ }
+
+ b_K_k[0] = r[0];
+ b_K_k[6] = 0.0F;
+ b_K_k[12] = 0.0F;
+ b_K_k[18] = 0.0F;
+ b_K_k[24] = 0.0F;
+ b_K_k[30] = 0.0F;
+ b_K_k[1] = 0.0F;
+ b_K_k[7] = r[0];
+ b_K_k[13] = 0.0F;
+ b_K_k[19] = 0.0F;
+ b_K_k[25] = 0.0F;
+ b_K_k[31] = 0.0F;
+ b_K_k[2] = 0.0F;
+ b_K_k[8] = 0.0F;
+ b_K_k[14] = r[0];
+ b_K_k[20] = 0.0F;
+ b_K_k[26] = 0.0F;
+ b_K_k[32] = 0.0F;
+ b_K_k[3] = 0.0F;
+ b_K_k[9] = 0.0F;
+ b_K_k[15] = 0.0F;
+ b_K_k[21] = r[1];
+ b_K_k[27] = 0.0F;
+ b_K_k[33] = 0.0F;
+ b_K_k[4] = 0.0F;
+ b_K_k[10] = 0.0F;
+ b_K_k[16] = 0.0F;
+ b_K_k[22] = 0.0F;
+ b_K_k[28] = r[1];
+ b_K_k[34] = 0.0F;
+ b_K_k[5] = 0.0F;
+ b_K_k[11] = 0.0F;
+ b_K_k[17] = 0.0F;
+ b_K_k[23] = 0.0F;
+ b_K_k[29] = 0.0F;
+ b_K_k[35] = r[1];
+ for (i = 0; i < 6; i++) {
+ for (i0 = 0; i0 < 6; i0++) {
+ c_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i];
+ }
+ }
+
+ c_mrdivide(d_P_apriori, c_P_apriori, c_K_k);
+
+ /* 'attitudeKalmanfilter:179' x_aposteriori=x_apriori+K_k*y_k; */
+ for (i = 0; i < 6; i++) {
+ f0 = 0.0F;
+ for (i0 = 0; i0 < 12; i0++) {
+ f0 += (real32_T)iv6[i + 6 * i0] * x_apriori[i0];
+ }
+
+ b_z[i] = z[i] - f0;
+ }
+
+ for (i = 0; i < 12; i++) {
+ f0 = 0.0F;
+ for (i0 = 0; i0 < 6; i0++) {
+ f0 += c_K_k[i + 12 * i0] * b_z[i0];
+ }
+
+ x_aposteriori[i] = x_apriori[i] + f0;
+ }
+
+ /* 'attitudeKalmanfilter:180' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */
+ b_eye(dv1);
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ f0 = 0.0F;
+ for (i1 = 0; i1 < 6; i1++) {
+ f0 += c_K_k[i + 12 * i1] * (real32_T)iv6[i1 + 6 * i0];
+ }
+
+ b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
+ }
+ }
+
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ P_aposteriori[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1
+ + 12 * i0];
+ }
+ }
+ }
+ } else {
+ /* 'attitudeKalmanfilter:181' else */
+ /* 'attitudeKalmanfilter:182' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==1 */
+ if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 1))
+ {
+ /* 'attitudeKalmanfilter:183' R=[r(1),0,0,0,0,0; */
+ /* 'attitudeKalmanfilter:184' 0,r(1),0,0,0,0; */
+ /* 'attitudeKalmanfilter:185' 0,0,r(1),0,0,0; */
+ /* 'attitudeKalmanfilter:186' 0,0,0,r(3),0,0; */
+ /* 'attitudeKalmanfilter:187' 0,0,0,0,r(3),0; */
+ /* 'attitudeKalmanfilter:188' 0,0,0,0,0,r(3)]; */
+ /* observation matrix */
+ /* 'attitudeKalmanfilter:191' H_k=[ E, Z, Z, Z; */
+ /* 'attitudeKalmanfilter:192' Z, Z, Z, E]; */
+ /* 'attitudeKalmanfilter:194' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apriori; */
+ /* 'attitudeKalmanfilter:196' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */
+ /* 'attitudeKalmanfilter:197' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 6; i0++) {
+ d_P_apriori[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)
+ iv7[i1 + 12 * i0];
+ }
+ }
+ }
+
+ for (i = 0; i < 6; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ c_K_k[i + 6 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ c_K_k[i + 6 * i0] += (real32_T)iv8[i + 6 * i1] * P_apriori[i1 +
+ 12 * i0];
+ }
+ }
+
+ for (i0 = 0; i0 < 6; i0++) {
+ fv2[i + 6 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv7[i1 + 12 *
+ i0];
+ }
+ }
+ }
+
+ b_K_k[0] = r[0];
+ b_K_k[6] = 0.0F;
+ b_K_k[12] = 0.0F;
+ b_K_k[18] = 0.0F;
+ b_K_k[24] = 0.0F;
+ b_K_k[30] = 0.0F;
+ b_K_k[1] = 0.0F;
+ b_K_k[7] = r[0];
+ b_K_k[13] = 0.0F;
+ b_K_k[19] = 0.0F;
+ b_K_k[25] = 0.0F;
+ b_K_k[31] = 0.0F;
+ b_K_k[2] = 0.0F;
+ b_K_k[8] = 0.0F;
+ b_K_k[14] = r[0];
+ b_K_k[20] = 0.0F;
+ b_K_k[26] = 0.0F;
+ b_K_k[32] = 0.0F;
+ b_K_k[3] = 0.0F;
+ b_K_k[9] = 0.0F;
+ b_K_k[15] = 0.0F;
+ b_K_k[21] = r[2];
+ b_K_k[27] = 0.0F;
+ b_K_k[33] = 0.0F;
+ b_K_k[4] = 0.0F;
+ b_K_k[10] = 0.0F;
+ b_K_k[16] = 0.0F;
+ b_K_k[22] = 0.0F;
+ b_K_k[28] = r[2];
+ b_K_k[34] = 0.0F;
+ b_K_k[5] = 0.0F;
+ b_K_k[11] = 0.0F;
+ b_K_k[17] = 0.0F;
+ b_K_k[23] = 0.0F;
+ b_K_k[29] = 0.0F;
+ b_K_k[35] = r[2];
+ for (i = 0; i < 6; i++) {
+ for (i0 = 0; i0 < 6; i0++) {
+ c_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i];
+ }
+ }
+
+ c_mrdivide(d_P_apriori, c_P_apriori, c_K_k);
+
+ /* 'attitudeKalmanfilter:200' x_aposteriori=x_apriori+K_k*y_k; */
+ for (i = 0; i < 3; i++) {
+ b_z[i] = z[i];
+ }
+
+ for (i = 0; i < 3; i++) {
+ b_z[i + 3] = z[i + 6];
+ }
+
+ for (i = 0; i < 6; i++) {
+ fv3[i] = 0.0F;
+ for (i0 = 0; i0 < 12; i0++) {
+ fv3[i] += (real32_T)iv8[i + 6 * i0] * x_apriori[i0];
+ }
+
+ c_z[i] = b_z[i] - fv3[i];
+ }
+
+ for (i = 0; i < 12; i++) {
+ f0 = 0.0F;
+ for (i0 = 0; i0 < 6; i0++) {
+ f0 += c_K_k[i + 12 * i0] * c_z[i0];
+ }
+
+ x_aposteriori[i] = x_apriori[i] + f0;
+ }
+
+ /* 'attitudeKalmanfilter:201' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */
+ b_eye(dv1);
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ f0 = 0.0F;
+ for (i1 = 0; i1 < 6; i1++) {
+ f0 += c_K_k[i + 12 * i1] * (real32_T)iv8[i1 + 6 * i0];
+ }
+
+ b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
+ }
+ }
+
+ for (i = 0; i < 12; i++) {
+ for (i0 = 0; i0 < 12; i0++) {
+ P_aposteriori[i + 12 * i0] = 0.0F;
+ for (i1 = 0; i1 < 12; i1++) {
+ P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] *
+ P_apriori[i1 + 12 * i0];
+ }
+ }
+ }
+ } else {
+ /* 'attitudeKalmanfilter:202' else */
+ /* 'attitudeKalmanfilter:203' x_aposteriori=x_apriori; */
+ for (i = 0; i < 12; i++) {
+ x_aposteriori[i] = x_apriori[i];
+ }
+
+ /* 'attitudeKalmanfilter:204' P_aposteriori=P_apriori; */
+ memcpy(&P_aposteriori[0], &P_apriori[0], 144U * sizeof(real32_T));
+ }
+ }
+ }
+ }
+
+ /* % euler anglels extraction */
+ /* 'attitudeKalmanfilter:213' z_n_b = -x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */
+ for (i = 0; i < 3; i++) {
+ x_n_b[i] = -x_aposteriori[i + 6];
+ }
+
+ rdivide(x_n_b, norm(*(real32_T (*)[3])&x_aposteriori[6]), z_n_b);
+
+ /* 'attitudeKalmanfilter:214' m_n_b = x_aposteriori(10:12)./norm(x_aposteriori(10:12)); */
+ rdivide(*(real32_T (*)[3])&x_aposteriori[9], norm(*(real32_T (*)[3])&
+ x_aposteriori[9]), wak);
+
+ /* 'attitudeKalmanfilter:216' y_n_b=cross(z_n_b,m_n_b); */
+ for (i = 0; i < 3; i++) {
+ x_n_b[i] = wak[i];
+ }
+
+ cross(z_n_b, x_n_b, wak);
+
+ /* 'attitudeKalmanfilter:217' y_n_b=y_n_b./norm(y_n_b); */
+ for (i = 0; i < 3; i++) {
+ x_n_b[i] = wak[i];
+ }
+
+ rdivide(x_n_b, norm(wak), wak);
+
+ /* 'attitudeKalmanfilter:219' x_n_b=(cross(y_n_b,z_n_b)); */
+ cross(wak, z_n_b, x_n_b);
+
+ /* 'attitudeKalmanfilter:220' x_n_b=x_n_b./norm(x_n_b); */
+ for (i = 0; i < 3; i++) {
+ b_x_aposteriori_k[i] = x_n_b[i];
+ }
+
+ rdivide(b_x_aposteriori_k, norm(x_n_b), x_n_b);
+
+ /* 'attitudeKalmanfilter:226' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */
+ for (i = 0; i < 3; i++) {
+ Rot_matrix[i] = x_n_b[i];
+ Rot_matrix[3 + i] = wak[i];
+ Rot_matrix[6 + i] = z_n_b[i];
+ }
+
+ /* 'attitudeKalmanfilter:230' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */
+ /* 'attitudeKalmanfilter:231' theta=-asin(Rot_matrix(1,3)); */
+ /* 'attitudeKalmanfilter:232' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */
+ /* 'attitudeKalmanfilter:233' eulerAngles=[phi;theta;psi]; */
+ eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]);
+ eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]);
+ eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]);
+}
+
+/* End of code generation (attitudeKalmanfilter.c) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h new file mode 100755 index 000000000..afa63c1a9 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h @@ -0,0 +1,34 @@ +/*
+ * attitudeKalmanfilter.h
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+#ifndef __ATTITUDEKALMANFILTER_H__
+#define __ATTITUDEKALMANFILTER_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include "rt_defines.h"
+#include "rt_nonfinite.h"
+
+#include "rtwtypes.h"
+#include "attitudeKalmanfilter_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]);
+#endif
+/* End of code generation (attitudeKalmanfilter.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c new file mode 100755 index 000000000..7d620d7fa --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c @@ -0,0 +1,31 @@ +/*
+ * attitudeKalmanfilter_initialize.c
+ *
+ * Code generation for function 'attitudeKalmanfilter_initialize'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "attitudeKalmanfilter_initialize.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+void attitudeKalmanfilter_initialize(void)
+{
+ rt_InitInfAndNaN(8U);
+}
+
+/* End of code generation (attitudeKalmanfilter_initialize.c) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h new file mode 100755 index 000000000..8b599be66 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h @@ -0,0 +1,34 @@ +/*
+ * attitudeKalmanfilter_initialize.h
+ *
+ * Code generation for function 'attitudeKalmanfilter_initialize'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+#ifndef __ATTITUDEKALMANFILTER_INITIALIZE_H__
+#define __ATTITUDEKALMANFILTER_INITIALIZE_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include "rt_defines.h"
+#include "rt_nonfinite.h"
+
+#include "rtwtypes.h"
+#include "attitudeKalmanfilter_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void attitudeKalmanfilter_initialize(void);
+#endif
+/* End of code generation (attitudeKalmanfilter_initialize.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c new file mode 100755 index 000000000..7f9727419 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c @@ -0,0 +1,31 @@ +/*
+ * attitudeKalmanfilter_terminate.c
+ *
+ * Code generation for function 'attitudeKalmanfilter_terminate'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "attitudeKalmanfilter_terminate.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+void attitudeKalmanfilter_terminate(void)
+{
+ /* (no terminate code required) */
+}
+
+/* End of code generation (attitudeKalmanfilter_terminate.c) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h new file mode 100755 index 000000000..da84a5024 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h @@ -0,0 +1,34 @@ +/*
+ * attitudeKalmanfilter_terminate.h
+ *
+ * Code generation for function 'attitudeKalmanfilter_terminate'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+#ifndef __ATTITUDEKALMANFILTER_TERMINATE_H__
+#define __ATTITUDEKALMANFILTER_TERMINATE_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include "rt_defines.h"
+#include "rt_nonfinite.h"
+
+#include "rtwtypes.h"
+#include "attitudeKalmanfilter_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void attitudeKalmanfilter_terminate(void);
+#endif
+/* End of code generation (attitudeKalmanfilter_terminate.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h new file mode 100755 index 000000000..30fd1e75e --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h @@ -0,0 +1,16 @@ +/*
+ * attitudeKalmanfilter_types.h
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+#ifndef __ATTITUDEKALMANFILTER_TYPES_H__
+#define __ATTITUDEKALMANFILTER_TYPES_H__
+
+/* Type Definitions */
+
+#endif
+/* End of code generation (attitudeKalmanfilter_types.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/cross.c b/src/modules/attitude_estimator_ekf/codegen/cross.c new file mode 100755 index 000000000..84ada9002 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/cross.c @@ -0,0 +1,37 @@ +/*
+ * cross.c
+ *
+ * Code generation for function 'cross'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "cross.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+
+/*
+ *
+ */
+void cross(const real32_T a[3], const real32_T b[3], real32_T c[3])
+{
+ c[0] = a[1] * b[2] - a[2] * b[1];
+ c[1] = a[2] * b[0] - a[0] * b[2];
+ c[2] = a[0] * b[1] - a[1] * b[0];
+}
+
+/* End of code generation (cross.c) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/cross.h b/src/modules/attitude_estimator_ekf/codegen/cross.h new file mode 100755 index 000000000..e727f0684 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/cross.h @@ -0,0 +1,34 @@ +/*
+ * cross.h
+ *
+ * Code generation for function 'cross'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+#ifndef __CROSS_H__
+#define __CROSS_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include "rt_defines.h"
+#include "rt_nonfinite.h"
+
+#include "rtwtypes.h"
+#include "attitudeKalmanfilter_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void cross(const real32_T a[3], const real32_T b[3], real32_T c[3]);
+#endif
+/* End of code generation (cross.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/eye.c b/src/modules/attitude_estimator_ekf/codegen/eye.c new file mode 100755 index 000000000..b89ab58ef --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/eye.c @@ -0,0 +1,51 @@ +/*
+ * eye.c
+ *
+ * Code generation for function 'eye'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "eye.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+
+/*
+ *
+ */
+void b_eye(real_T I[144])
+{
+ int32_T i;
+ memset(&I[0], 0, 144U * sizeof(real_T));
+ for (i = 0; i < 12; i++) {
+ I[i + 12 * i] = 1.0;
+ }
+}
+
+/*
+ *
+ */
+void eye(real_T I[9])
+{
+ int32_T i;
+ memset(&I[0], 0, 9U * sizeof(real_T));
+ for (i = 0; i < 3; i++) {
+ I[i + 3 * i] = 1.0;
+ }
+}
+
+/* End of code generation (eye.c) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/eye.h b/src/modules/attitude_estimator_ekf/codegen/eye.h new file mode 100755 index 000000000..94fbe7671 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/eye.h @@ -0,0 +1,35 @@ +/*
+ * eye.h
+ *
+ * Code generation for function 'eye'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+#ifndef __EYE_H__
+#define __EYE_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include "rt_defines.h"
+#include "rt_nonfinite.h"
+
+#include "rtwtypes.h"
+#include "attitudeKalmanfilter_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void b_eye(real_T I[144]);
+extern void eye(real_T I[9]);
+#endif
+/* End of code generation (eye.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/mrdivide.c b/src/modules/attitude_estimator_ekf/codegen/mrdivide.c new file mode 100755 index 000000000..a810f22e4 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/mrdivide.c @@ -0,0 +1,357 @@ +/*
+ * mrdivide.c
+ *
+ * Code generation for function 'mrdivide'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "mrdivide.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+
+/*
+ *
+ */
+void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36])
+{
+ real32_T b_A[9];
+ int32_T rtemp;
+ int32_T k;
+ real32_T b_B[36];
+ int32_T r1;
+ int32_T r2;
+ int32_T r3;
+ real32_T maxval;
+ real32_T a21;
+ real32_T Y[36];
+ for (rtemp = 0; rtemp < 3; rtemp++) {
+ for (k = 0; k < 3; k++) {
+ b_A[k + 3 * rtemp] = B[rtemp + 3 * k];
+ }
+ }
+
+ for (rtemp = 0; rtemp < 12; rtemp++) {
+ for (k = 0; k < 3; k++) {
+ b_B[k + 3 * rtemp] = A[rtemp + 12 * k];
+ }
+ }
+
+ r1 = 0;
+ r2 = 1;
+ r3 = 2;
+ maxval = (real32_T)fabs(b_A[0]);
+ a21 = (real32_T)fabs(b_A[1]);
+ if (a21 > maxval) {
+ maxval = a21;
+ r1 = 1;
+ r2 = 0;
+ }
+
+ if ((real32_T)fabs(b_A[2]) > maxval) {
+ r1 = 2;
+ r2 = 1;
+ r3 = 0;
+ }
+
+ b_A[r2] /= b_A[r1];
+ b_A[r3] /= b_A[r1];
+ b_A[3 + r2] -= b_A[r2] * b_A[3 + r1];
+ b_A[3 + r3] -= b_A[r3] * b_A[3 + r1];
+ b_A[6 + r2] -= b_A[r2] * b_A[6 + r1];
+ b_A[6 + r3] -= b_A[r3] * b_A[6 + r1];
+ if ((real32_T)fabs(b_A[3 + r3]) > (real32_T)fabs(b_A[3 + r2])) {
+ rtemp = r2;
+ r2 = r3;
+ r3 = rtemp;
+ }
+
+ b_A[3 + r3] /= b_A[3 + r2];
+ b_A[6 + r3] -= b_A[3 + r3] * b_A[6 + r2];
+ for (k = 0; k < 12; k++) {
+ Y[3 * k] = b_B[r1 + 3 * k];
+ Y[1 + 3 * k] = b_B[r2 + 3 * k] - Y[3 * k] * b_A[r2];
+ Y[2 + 3 * k] = (b_B[r3 + 3 * k] - Y[3 * k] * b_A[r3]) - Y[1 + 3 * k] * b_A[3
+ + r3];
+ Y[2 + 3 * k] /= b_A[6 + r3];
+ Y[3 * k] -= Y[2 + 3 * k] * b_A[6 + r1];
+ Y[1 + 3 * k] -= Y[2 + 3 * k] * b_A[6 + r2];
+ Y[1 + 3 * k] /= b_A[3 + r2];
+ Y[3 * k] -= Y[1 + 3 * k] * b_A[3 + r1];
+ Y[3 * k] /= b_A[r1];
+ }
+
+ for (rtemp = 0; rtemp < 3; rtemp++) {
+ for (k = 0; k < 12; k++) {
+ y[k + 12 * rtemp] = Y[rtemp + 3 * k];
+ }
+ }
+}
+
+/*
+ *
+ */
+void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72])
+{
+ real32_T b_A[36];
+ int8_T ipiv[6];
+ int32_T i3;
+ int32_T iy;
+ int32_T j;
+ int32_T c;
+ int32_T ix;
+ real32_T temp;
+ int32_T k;
+ real32_T s;
+ int32_T jy;
+ int32_T ijA;
+ real32_T Y[72];
+ for (i3 = 0; i3 < 6; i3++) {
+ for (iy = 0; iy < 6; iy++) {
+ b_A[iy + 6 * i3] = B[i3 + 6 * iy];
+ }
+
+ ipiv[i3] = (int8_T)(1 + i3);
+ }
+
+ for (j = 0; j < 5; j++) {
+ c = j * 7;
+ iy = 0;
+ ix = c;
+ temp = (real32_T)fabs(b_A[c]);
+ for (k = 2; k <= 6 - j; k++) {
+ ix++;
+ s = (real32_T)fabs(b_A[ix]);
+ if (s > temp) {
+ iy = k - 1;
+ temp = s;
+ }
+ }
+
+ if (b_A[c + iy] != 0.0F) {
+ if (iy != 0) {
+ ipiv[j] = (int8_T)((j + iy) + 1);
+ ix = j;
+ iy += j;
+ for (k = 0; k < 6; k++) {
+ temp = b_A[ix];
+ b_A[ix] = b_A[iy];
+ b_A[iy] = temp;
+ ix += 6;
+ iy += 6;
+ }
+ }
+
+ i3 = (c - j) + 6;
+ for (jy = c + 1; jy + 1 <= i3; jy++) {
+ b_A[jy] /= b_A[c];
+ }
+ }
+
+ iy = c;
+ jy = c + 6;
+ for (k = 1; k <= 5 - j; k++) {
+ temp = b_A[jy];
+ if (b_A[jy] != 0.0F) {
+ ix = c + 1;
+ i3 = (iy - j) + 12;
+ for (ijA = 7 + iy; ijA + 1 <= i3; ijA++) {
+ b_A[ijA] += b_A[ix] * -temp;
+ ix++;
+ }
+ }
+
+ jy += 6;
+ iy += 6;
+ }
+ }
+
+ for (i3 = 0; i3 < 12; i3++) {
+ for (iy = 0; iy < 6; iy++) {
+ Y[iy + 6 * i3] = A[i3 + 12 * iy];
+ }
+ }
+
+ for (jy = 0; jy < 6; jy++) {
+ if (ipiv[jy] != jy + 1) {
+ for (j = 0; j < 12; j++) {
+ temp = Y[jy + 6 * j];
+ Y[jy + 6 * j] = Y[(ipiv[jy] + 6 * j) - 1];
+ Y[(ipiv[jy] + 6 * j) - 1] = temp;
+ }
+ }
+ }
+
+ for (j = 0; j < 12; j++) {
+ c = 6 * j;
+ for (k = 0; k < 6; k++) {
+ iy = 6 * k;
+ if (Y[k + c] != 0.0F) {
+ for (jy = k + 2; jy < 7; jy++) {
+ Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1];
+ }
+ }
+ }
+ }
+
+ for (j = 0; j < 12; j++) {
+ c = 6 * j;
+ for (k = 5; k > -1; k += -1) {
+ iy = 6 * k;
+ if (Y[k + c] != 0.0F) {
+ Y[k + c] /= b_A[k + iy];
+ for (jy = 0; jy + 1 <= k; jy++) {
+ Y[jy + c] -= Y[k + c] * b_A[jy + iy];
+ }
+ }
+ }
+ }
+
+ for (i3 = 0; i3 < 6; i3++) {
+ for (iy = 0; iy < 12; iy++) {
+ y[iy + 12 * i3] = Y[i3 + 6 * iy];
+ }
+ }
+}
+
+/*
+ *
+ */
+void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108])
+{
+ real32_T b_A[81];
+ int8_T ipiv[9];
+ int32_T i2;
+ int32_T iy;
+ int32_T j;
+ int32_T c;
+ int32_T ix;
+ real32_T temp;
+ int32_T k;
+ real32_T s;
+ int32_T jy;
+ int32_T ijA;
+ real32_T Y[108];
+ for (i2 = 0; i2 < 9; i2++) {
+ for (iy = 0; iy < 9; iy++) {
+ b_A[iy + 9 * i2] = B[i2 + 9 * iy];
+ }
+
+ ipiv[i2] = (int8_T)(1 + i2);
+ }
+
+ for (j = 0; j < 8; j++) {
+ c = j * 10;
+ iy = 0;
+ ix = c;
+ temp = (real32_T)fabs(b_A[c]);
+ for (k = 2; k <= 9 - j; k++) {
+ ix++;
+ s = (real32_T)fabs(b_A[ix]);
+ if (s > temp) {
+ iy = k - 1;
+ temp = s;
+ }
+ }
+
+ if (b_A[c + iy] != 0.0F) {
+ if (iy != 0) {
+ ipiv[j] = (int8_T)((j + iy) + 1);
+ ix = j;
+ iy += j;
+ for (k = 0; k < 9; k++) {
+ temp = b_A[ix];
+ b_A[ix] = b_A[iy];
+ b_A[iy] = temp;
+ ix += 9;
+ iy += 9;
+ }
+ }
+
+ i2 = (c - j) + 9;
+ for (jy = c + 1; jy + 1 <= i2; jy++) {
+ b_A[jy] /= b_A[c];
+ }
+ }
+
+ iy = c;
+ jy = c + 9;
+ for (k = 1; k <= 8 - j; k++) {
+ temp = b_A[jy];
+ if (b_A[jy] != 0.0F) {
+ ix = c + 1;
+ i2 = (iy - j) + 18;
+ for (ijA = 10 + iy; ijA + 1 <= i2; ijA++) {
+ b_A[ijA] += b_A[ix] * -temp;
+ ix++;
+ }
+ }
+
+ jy += 9;
+ iy += 9;
+ }
+ }
+
+ for (i2 = 0; i2 < 12; i2++) {
+ for (iy = 0; iy < 9; iy++) {
+ Y[iy + 9 * i2] = A[i2 + 12 * iy];
+ }
+ }
+
+ for (jy = 0; jy < 9; jy++) {
+ if (ipiv[jy] != jy + 1) {
+ for (j = 0; j < 12; j++) {
+ temp = Y[jy + 9 * j];
+ Y[jy + 9 * j] = Y[(ipiv[jy] + 9 * j) - 1];
+ Y[(ipiv[jy] + 9 * j) - 1] = temp;
+ }
+ }
+ }
+
+ for (j = 0; j < 12; j++) {
+ c = 9 * j;
+ for (k = 0; k < 9; k++) {
+ iy = 9 * k;
+ if (Y[k + c] != 0.0F) {
+ for (jy = k + 2; jy < 10; jy++) {
+ Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1];
+ }
+ }
+ }
+ }
+
+ for (j = 0; j < 12; j++) {
+ c = 9 * j;
+ for (k = 8; k > -1; k += -1) {
+ iy = 9 * k;
+ if (Y[k + c] != 0.0F) {
+ Y[k + c] /= b_A[k + iy];
+ for (jy = 0; jy + 1 <= k; jy++) {
+ Y[jy + c] -= Y[k + c] * b_A[jy + iy];
+ }
+ }
+ }
+ }
+
+ for (i2 = 0; i2 < 9; i2++) {
+ for (iy = 0; iy < 12; iy++) {
+ y[iy + 12 * i2] = Y[i2 + 9 * iy];
+ }
+ }
+}
+
+/* End of code generation (mrdivide.c) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/mrdivide.h b/src/modules/attitude_estimator_ekf/codegen/mrdivide.h new file mode 100755 index 000000000..2d3b0d51f --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/mrdivide.h @@ -0,0 +1,36 @@ +/*
+ * mrdivide.h
+ *
+ * Code generation for function 'mrdivide'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+#ifndef __MRDIVIDE_H__
+#define __MRDIVIDE_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include "rt_defines.h"
+#include "rt_nonfinite.h"
+
+#include "rtwtypes.h"
+#include "attitudeKalmanfilter_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36]);
+extern void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72]);
+extern void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]);
+#endif
+/* End of code generation (mrdivide.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/norm.c b/src/modules/attitude_estimator_ekf/codegen/norm.c new file mode 100755 index 000000000..0c418cc7b --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/norm.c @@ -0,0 +1,54 @@ +/*
+ * norm.c
+ *
+ * Code generation for function 'norm'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "norm.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+
+/*
+ *
+ */
+real32_T norm(const real32_T x[3])
+{
+ real32_T y;
+ real32_T scale;
+ int32_T k;
+ real32_T absxk;
+ real32_T t;
+ y = 0.0F;
+ scale = 1.17549435E-38F;
+ for (k = 0; k < 3; k++) {
+ absxk = (real32_T)fabs(x[k]);
+ if (absxk > scale) {
+ t = scale / absxk;
+ y = 1.0F + y * t * t;
+ scale = absxk;
+ } else {
+ t = absxk / scale;
+ y += t * t;
+ }
+ }
+
+ return scale * (real32_T)sqrt(y);
+}
+
+/* End of code generation (norm.c) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/norm.h b/src/modules/attitude_estimator_ekf/codegen/norm.h new file mode 100755 index 000000000..60cf77b57 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/norm.h @@ -0,0 +1,34 @@ +/*
+ * norm.h
+ *
+ * Code generation for function 'norm'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+#ifndef __NORM_H__
+#define __NORM_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include "rt_defines.h"
+#include "rt_nonfinite.h"
+
+#include "rtwtypes.h"
+#include "attitudeKalmanfilter_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern real32_T norm(const real32_T x[3]);
+#endif
+/* End of code generation (norm.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/rdivide.c b/src/modules/attitude_estimator_ekf/codegen/rdivide.c new file mode 100755 index 000000000..d035dae5e --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rdivide.c @@ -0,0 +1,38 @@ +/*
+ * rdivide.c
+ *
+ * Code generation for function 'rdivide'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "rdivide.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+
+/*
+ *
+ */
+void rdivide(const real32_T x[3], real32_T y, real32_T z[3])
+{
+ int32_T i;
+ for (i = 0; i < 3; i++) {
+ z[i] = x[i] / y;
+ }
+}
+
+/* End of code generation (rdivide.c) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/rdivide.h b/src/modules/attitude_estimator_ekf/codegen/rdivide.h new file mode 100755 index 000000000..4bbebebe2 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rdivide.h @@ -0,0 +1,34 @@ +/*
+ * rdivide.h
+ *
+ * Code generation for function 'rdivide'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+#ifndef __RDIVIDE_H__
+#define __RDIVIDE_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include "rt_defines.h"
+#include "rt_nonfinite.h"
+
+#include "rtwtypes.h"
+#include "attitudeKalmanfilter_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void rdivide(const real32_T x[3], real32_T y, real32_T z[3]);
+#endif
+/* End of code generation (rdivide.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c new file mode 100755 index 000000000..34164d104 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c @@ -0,0 +1,139 @@ +/*
+ * rtGetInf.c
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Sat Jan 19 15:25: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/attitude_estimator_ekf/codegen/rtGetInf.h b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h new file mode 100755 index 000000000..145373cd0 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h @@ -0,0 +1,23 @@ +/*
+ * rtGetInf.h
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Sat Jan 19 15:25: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/attitude_estimator_ekf/codegen/rtGetNaN.c b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c new file mode 100755 index 000000000..d84ca9573 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c @@ -0,0 +1,96 @@ +/*
+ * rtGetNaN.c
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Sat Jan 19 15:25: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/attitude_estimator_ekf/codegen/rtGetNaN.h b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h new file mode 100755 index 000000000..65fdaa96f --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h @@ -0,0 +1,21 @@ +/*
+ * rtGetNaN.h
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Sat Jan 19 15:25: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/attitude_estimator_ekf/codegen/rt_defines.h b/src/modules/attitude_estimator_ekf/codegen/rt_defines.h new file mode 100755 index 000000000..356498363 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rt_defines.h @@ -0,0 +1,24 @@ +/*
+ * rt_defines.h
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+#ifndef __RT_DEFINES_H__
+#define __RT_DEFINES_H__
+
+#include <stdlib.h>
+
+#define RT_PI 3.14159265358979323846
+#define RT_PIF 3.1415927F
+#define RT_LN_10 2.30258509299404568402
+#define RT_LN_10F 2.3025851F
+#define RT_LOG10E 0.43429448190325182765
+#define RT_LOG10EF 0.43429449F
+#define RT_E 2.7182818284590452354
+#define RT_EF 2.7182817F
+#endif
+/* End of code generation (rt_defines.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c new file mode 100755 index 000000000..303d1d9d2 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c @@ -0,0 +1,87 @@ +/*
+ * rt_nonfinite.c
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Sat Jan 19 15:25: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/attitude_estimator_ekf/codegen/rt_nonfinite.h b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h new file mode 100755 index 000000000..bd56b30d9 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h @@ -0,0 +1,53 @@ +/*
+ * rt_nonfinite.h
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Sat Jan 19 15:25: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/attitude_estimator_ekf/codegen/rtwtypes.h b/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h new file mode 100755 index 000000000..9a5c96267 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h @@ -0,0 +1,159 @@ +/*
+ * rtwtypes.h
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Sat Jan 19 15:25: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: Zero
+ * Shift right on a signed integer as arithmetic shift: on
+ *=======================================================================*/
+
+/*=======================================================================*
+ * Fixed width word size data types: *
+ * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers *
+ * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers *
+ * real32_T, real64_T - 32 and 64 bit floating point numbers *
+ *=======================================================================*/
+
+typedef signed char int8_T;
+typedef unsigned char uint8_T;
+typedef short int16_T;
+typedef unsigned short uint16_T;
+typedef int int32_T;
+typedef unsigned int uint32_T;
+typedef float real32_T;
+typedef double real64_T;
+
+/*===========================================================================*
+ * Generic type definitions: 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/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk new file mode 100644 index 000000000..d98647f99 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/module.mk @@ -0,0 +1,52 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Attitude estimator (Extended Kalman Filter) +# + +MODULE_COMMAND = attitude_estimator_ekf + +SRCS = attitude_estimator_ekf_main.cpp \ + attitude_estimator_ekf_params.c \ + codegen/eye.c \ + codegen/attitudeKalmanfilter.c \ + codegen/mrdivide.c \ + codegen/rdivide.c \ + codegen/attitudeKalmanfilter_initialize.c \ + codegen/attitudeKalmanfilter_terminate.c \ + codegen/rt_nonfinite.c \ + codegen/rtGetInf.c \ + codegen/rtGetNaN.c \ + codegen/norm.c \ + codegen/cross.c diff --git a/src/modules/attitude_estimator_so3_comp/README b/src/modules/attitude_estimator_so3_comp/README new file mode 100644 index 000000000..79c50a531 --- /dev/null +++ b/src/modules/attitude_estimator_so3_comp/README @@ -0,0 +1,5 @@ +Synopsis + + nsh> attitude_estimator_so3_comp start -d /dev/ttyS1 -b 115200 + +Option -d is for debugging packet. See code for detailed packet structure. diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp new file mode 100755 index 000000000..3ca50fb39 --- /dev/null +++ b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp @@ -0,0 +1,833 @@ +/* + * Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr> + * + * @file attitude_estimator_so3_comp_main.c + * + * 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. + * + * Theory of nonlinear complementary filters on the SO(3) is based on [1]. + * Quaternion realization of [1] is based on [2]. + * Optmized quaternion update code is based on Sebastian Madgwick's implementation. + * + * References + * [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008 + * [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008 + */ + +#include <nuttx/config.h> +#include <unistd.h> +#include <stdlib.h> +#include <string.h> +#include <stdio.h> +#include <stdbool.h> +#include <poll.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/debug_key_value.h> +#include <uORB/topics/sensor_combined.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/parameter_update.h> +#include <drivers/drv_hrt.h> + +#include <systemlib/systemlib.h> +#include <systemlib/perf_counter.h> +#include <systemlib/err.h> + +#ifdef __cplusplus +extern "C" { +#endif +#include "attitude_estimator_so3_comp_params.h" +#ifdef __cplusplus +} +#endif + +extern "C" __EXPORT int attitude_estimator_so3_comp_main(int argc, char *argv[]); + +static bool thread_should_exit = false; /**< Deamon exit flag */ +static bool thread_running = false; /**< Deamon status flag */ +static int attitude_estimator_so3_comp_task; /**< Handle of deamon task / thread */ +static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; /** quaternion of sensor frame relative to auxiliary frame */ +static float dq0 = 0.0f, dq1 = 0.0f, dq2 = 0.0f, dq3 = 0.0f; /** quaternion of sensor frame relative to auxiliary frame */ +static float gyro_bias[3] = {0.0f, 0.0f, 0.0f}; /** bias estimation */ +static bool bFilterInit = false; + +//! Auxiliary variables to reduce number of repeated operations +static float q0q0, q0q1, q0q2, q0q3; +static float q1q1, q1q2, q1q3; +static float q2q2, q2q3; +static float q3q3; + +//! Serial packet related +static int uart; +static int baudrate; + +/** + * Mainloop of attitude_estimator_so3_comp. + */ +int attitude_estimator_so3_comp_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); + + fprintf(stderr, "usage: attitude_estimator_so3_comp {start|stop|status} [-d <devicename>] [-b <baud rate>]\n" + "-d and -b options are for separate visualization with raw data (quaternion packet) transfer\n" + "ex) attitude_estimator_so3_comp start -d /dev/ttyS1 -b 115200\n"); + exit(1); +} + +/** + * The attitude_estimator_so3_comp 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 attitude_estimator_so3_comp_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + printf("attitude_estimator_so3_comp already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + attitude_estimator_so3_comp_task = task_spawn_cmd("attitude_estimator_so3_comp", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 12400, + attitude_estimator_so3_comp_thread_main, + (const char **)argv); + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + + while(thread_running){ + usleep(200000); + printf("."); + } + printf("terminated."); + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + printf("\tattitude_estimator_so3_comp app is running\n"); + + } else { + printf("\tattitude_estimator_so3_comp app not started\n"); + } + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +//--------------------------------------------------------------------------------------------------- +// Fast inverse square-root +// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root +float invSqrt(float number) { + volatile long i; + volatile float x, y; + volatile const float f = 1.5F; + + x = number * 0.5F; + y = number; + i = * (( long * ) &y); + i = 0x5f375a86 - ( i >> 1 ); + y = * (( float * ) &i); + y = y * ( f - ( x * y * y ) ); + return y; +} + +//! Using accelerometer, sense the gravity vector. +//! Using magnetometer, sense yaw. +void NonlinearSO3AHRSinit(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); + + q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading; + q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading; + q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading; + q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; + + // auxillary variables to reduce number of repeated operations, for 1st pass + q0q0 = q0 * q0; + q0q1 = q0 * q1; + q0q2 = q0 * q2; + q0q3 = q0 * q3; + q1q1 = q1 * q1; + q1q2 = q1 * q2; + q1q3 = q1 * q3; + q2q2 = q2 * q2; + q2q3 = q2 * q3; + q3q3 = q3 * q3; +} + +void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt) { + float recipNorm; + float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f; + + //! Make filter converge to initial solution faster + //! This function assumes you are in static position. + //! WARNING : in case air reboot, this can cause problem. But this is very + //! unlikely happen. + if(bFilterInit == false) + { + NonlinearSO3AHRSinit(ax,ay,az,mx,my,mz); + bFilterInit = true; + } + + //! If magnetometer measurement is available, use it. + if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { + float hx, hy, hz, bx, bz; + float halfwx, halfwy, halfwz; + + // Normalise magnetometer measurement + // Will sqrt work better? PX4 system is powerful enough? + recipNorm = invSqrt(mx * mx + my * my + mz * mz); + mx *= recipNorm; + my *= recipNorm; + mz *= recipNorm; + + // Reference direction of Earth's magnetic field + hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2)); + hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1)); + hz = 2 * mx * (q1q3 - q0q2) + 2 * my * (q2q3 + q0q1) + 2 * mz * (0.5 - q1q1 - q2q2); + bx = sqrt(hx * hx + hy * hy); + bz = hz; + + // Estimated direction of magnetic field + halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2); + halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3); + halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2); + + // Error is sum of cross product between estimated direction and measured direction of field vectors + halfex += (my * halfwz - mz * halfwy); + halfey += (mz * halfwx - mx * halfwz); + halfez += (mx * halfwy - my * halfwx); + } + + // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) + if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { + float halfvx, halfvy, halfvz; + + // Normalise accelerometer measurement + recipNorm = invSqrt(ax * ax + ay * ay + az * az); + ax *= recipNorm; + ay *= recipNorm; + az *= recipNorm; + + // Estimated direction of gravity and magnetic field + halfvx = q1q3 - q0q2; + halfvy = q0q1 + q2q3; + halfvz = q0q0 - 0.5f + q3q3; + + // Error is sum of cross product between estimated direction and measured direction of field vectors + halfex += ay * halfvz - az * halfvy; + halfey += az * halfvx - ax * halfvz; + halfez += ax * halfvy - ay * halfvx; + } + + // Apply feedback only when valid data has been gathered from the accelerometer or magnetometer + if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) { + // Compute and apply integral feedback if enabled + if(twoKi > 0.0f) { + gyro_bias[0] += twoKi * halfex * dt; // integral error scaled by Ki + gyro_bias[1] += twoKi * halfey * dt; + gyro_bias[2] += twoKi * halfez * dt; + gx += gyro_bias[0]; // apply integral feedback + gy += gyro_bias[1]; + gz += gyro_bias[2]; + } + else { + gyro_bias[0] = 0.0f; // prevent integral windup + gyro_bias[1] = 0.0f; + gyro_bias[2] = 0.0f; + } + + // Apply proportional feedback + gx += twoKp * halfex; + gy += twoKp * halfey; + gz += twoKp * halfez; + } + + //! Integrate rate of change of quaternion +#if 0 + gx *= (0.5f * dt); // pre-multiply common factors + gy *= (0.5f * dt); + gz *= (0.5f * dt); +#endif + + // Time derivative of quaternion. q_dot = 0.5*q\otimes omega. + //! q_k = q_{k-1} + dt*\dot{q} + //! \dot{q} = 0.5*q \otimes P(\omega) + dq0 = 0.5f*(-q1 * gx - q2 * gy - q3 * gz); + dq1 = 0.5f*(q0 * gx + q2 * gz - q3 * gy); + dq2 = 0.5f*(q0 * gy - q1 * gz + q3 * gx); + dq3 = 0.5f*(q0 * gz + q1 * gy - q2 * gx); + + q0 += dt*dq0; + q1 += dt*dq1; + q2 += dt*dq2; + q3 += dt*dq3; + + // Normalise quaternion + recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + q0 *= recipNorm; + q1 *= recipNorm; + q2 *= recipNorm; + q3 *= recipNorm; + + // Auxiliary variables to avoid repeated arithmetic + q0q0 = q0 * q0; + q0q1 = q0 * q1; + q0q2 = q0 * q2; + q0q3 = q0 * q3; + q1q1 = q1 * q1; + q1q2 = q1 * q2; + q1q3 = q1 * q3; + q2q2 = q2 * q2; + q2q3 = q2 * q3; + q3q3 = q3 * q3; +} + +void send_uart_byte(char c) +{ + write(uart,&c,1); +} + +void send_uart_bytes(uint8_t *data, int length) +{ + write(uart,data,(size_t)(sizeof(uint8_t)*length)); +} + +void send_uart_float(float f) { + uint8_t * b = (uint8_t *) &f; + + //! Assume float is 4-bytes + for(int i=0; i<4; i++) { + + uint8_t b1 = (b[i] >> 4) & 0x0f; + uint8_t b2 = (b[i] & 0x0f); + + uint8_t c1 = (b1 < 10) ? ('0' + b1) : 'A' + b1 - 10; + uint8_t c2 = (b2 < 10) ? ('0' + b2) : 'A' + b2 - 10; + + send_uart_bytes(&c1,1); + send_uart_bytes(&c2,1); + } +} + +void send_uart_float_arr(float *arr, int length) +{ + for(int i=0;i<length;++i) + { + send_uart_float(arr[i]); + send_uart_byte(','); + } +} + +int open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb) +{ + int speed; + + switch (baud) { + case 0: speed = B0; break; + case 50: speed = B50; break; + case 75: speed = B75; break; + case 110: speed = B110; break; + case 134: speed = B134; break; + case 150: speed = B150; break; + case 200: speed = B200; break; + case 300: speed = B300; break; + case 600: speed = B600; break; + case 1200: speed = B1200; break; + case 1800: speed = B1800; break; + case 2400: speed = B2400; break; + case 4800: speed = B4800; break; + case 9600: speed = B9600; break; + case 19200: speed = B19200; break; + case 38400: speed = B38400; break; + case 57600: speed = B57600; break; + case 115200: speed = B115200; break; + case 230400: speed = B230400; break; + case 460800: speed = B460800; break; + case 921600: speed = B921600; break; + default: + printf("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud); + return -EINVAL; + } + + printf("[so3_comp_filt] UART is %s, baudrate is %d\n", uart_name, baud); + uart = open(uart_name, O_RDWR | O_NOCTTY); + + /* Try to set baud rate */ + struct termios uart_config; + int termios_state; + *is_usb = false; + + /* make some wild guesses including that USB serial is indicated by either /dev/ttyACM0 or /dev/console */ + if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/console") != OK) { + /* Back up the original uart configuration to restore it after exit */ + if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { + printf("ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state); + close(uart); + return -1; + } + + /* Fill the struct for the new configuration */ + tcgetattr(uart, &uart_config); + + /* Clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; + + /* Set baud rate */ + if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { + printf("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); + close(uart); + return -1; + } + + + if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { + printf("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); + close(uart); + return -1; + } + + } else { + *is_usb = true; + } + + return uart; +} + +/* + * [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) + */ + +/* + * EKF Attitude Estimator main function. + * + * Estimates the attitude recursively once started. + * + * @param argc number of commandline arguments (plus command name) + * @param argv strings containing the arguments + */ +int attitude_estimator_so3_comp_thread_main(int argc, char *argv[]) +{ + +const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds + + //! Serial debug related + int ch; + struct termios uart_config_original; + bool usb_uart; + bool debug_mode = false; + char *device_name = "/dev/ttyS2"; + baudrate = 115200; + + //! Time constant + float dt = 0.005f; + + /* output euler angles */ + float euler[3] = {0.0f, 0.0f, 0.0f}; + + float Rot_matrix[9] = {1.f, 0, 0, + 0, 1.f, 0, + 0, 0, 1.f + }; /**< init: identity matrix */ + + float acc[3] = {0.0f, 0.0f, 0.0f}; + float gyro[3] = {0.0f, 0.0f, 0.0f}; + float mag[3] = {0.0f, 0.0f, 0.0f}; + + /* work around some stupidity in task_create's argv handling */ + argc -= 2; + argv += 2; + + //! -d <device_name>, default : /dev/ttyS2 + //! -b <baud_rate>, default : 115200 + while ((ch = getopt(argc,argv,"d:b:")) != EOF){ + switch(ch){ + case 'b': + baudrate = strtoul(optarg, NULL, 10); + if(baudrate == 0) + printf("invalid baud rate '%s'",optarg); + break; + case 'd': + device_name = optarg; + debug_mode = true; + break; + default: + usage("invalid argument"); + } + } + + if(debug_mode){ + printf("Opening debugging port for 3D visualization\n"); + uart = open_uart(baudrate, device_name, &uart_config_original, &usb_uart); + if (uart < 0) + printf("could not open %s", device_name); + else + printf("Open port success\n"); + } + + // print text + printf("Nonlinear SO3 Attitude Estimator initialized..\n\n"); + fflush(stdout); + + int overloadcounter = 19; + + /* store start time to guard against too slow update rates */ + uint64_t last_run = hrt_absolute_time(); + + struct sensor_combined_s raw; + memset(&raw, 0, sizeof(raw)); + + //! Initialize attitude vehicle uORB message. + struct vehicle_attitude_s att; + memset(&att, 0, sizeof(att)); + + struct vehicle_status_s state; + memset(&state, 0, sizeof(state)); + + uint64_t last_data = 0; + uint64_t last_measurement = 0; + + /* subscribe to raw data */ + int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); + /* rate-limit raw data updates to 200Hz */ + orb_set_interval(sub_raw, 4); + + /* subscribe to param changes */ + int sub_params = orb_subscribe(ORB_ID(parameter_update)); + + /* subscribe to system state*/ + int sub_state = orb_subscribe(ORB_ID(vehicle_status)); + + /* advertise attitude */ + orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); + + int loopcounter = 0; + int printcounter = 0; + + thread_running = true; + + /* advertise debug value */ + // struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; + // orb_advert_t pub_dbg = -1; + + float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f}; + // XXX write this out to perf regs + + /* keep track of sensor updates */ + uint32_t sensor_last_count[3] = {0, 0, 0}; + uint64_t sensor_last_timestamp[3] = {0, 0, 0}; + + struct attitude_estimator_so3_comp_params so3_comp_params; + struct attitude_estimator_so3_comp_param_handles so3_comp_param_handles; + + /* initialize parameter handles */ + parameters_init(&so3_comp_param_handles); + + uint64_t start_time = hrt_absolute_time(); + bool initialized = false; + + float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; + unsigned offset_count = 0; + + /* register the perf counter */ + perf_counter_t so3_comp_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_so3_comp"); + + /* Main loop*/ + while (!thread_should_exit) { + + struct pollfd fds[2]; + fds[0].fd = sub_raw; + fds[0].events = POLLIN; + fds[1].fd = sub_params; + fds[1].events = POLLIN; + int ret = poll(fds, 2, 1000); + + if (ret < 0) { + /* XXX this is seriously bad - should be an emergency */ + } else if (ret == 0) { + /* check if we're in HIL - not getting sensor data is fine then */ + orb_copy(ORB_ID(vehicle_status), sub_state, &state); + + if (!state.flag_hil_enabled) { + fprintf(stderr, + "[att so3_comp] WARNING: Not getting sensors - sensor app running?\n"); + } + + } else { + + /* only update parameters if they changed */ + if (fds[1].revents & POLLIN) { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), sub_params, &update); + + /* update parameters */ + parameters_update(&so3_comp_param_handles, &so3_comp_params); + } + + /* only run filter if sensor values changed */ + if (fds[0].revents & POLLIN) { + + /* get latest measurements */ + orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); + + if (!initialized) { + + gyro_offsets[0] += raw.gyro_rad_s[0]; + gyro_offsets[1] += raw.gyro_rad_s[1]; + gyro_offsets[2] += raw.gyro_rad_s[2]; + offset_count++; + + if (hrt_absolute_time() - start_time > 3000000LL) { + initialized = true; + gyro_offsets[0] /= offset_count; + gyro_offsets[1] /= offset_count; + gyro_offsets[2] /= offset_count; + } + + } else { + + perf_begin(so3_comp_loop_perf); + + /* Calculate data time difference in seconds */ + dt = (raw.timestamp - last_measurement) / 1000000.0f; + last_measurement = raw.timestamp; + uint8_t update_vect[3] = {0, 0, 0}; + + /* Fill in gyro measurements */ + if (sensor_last_count[0] != raw.gyro_counter) { + update_vect[0] = 1; + sensor_last_count[0] = raw.gyro_counter; + sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); + sensor_last_timestamp[0] = raw.timestamp; + } + + gyro[0] = raw.gyro_rad_s[0] - gyro_offsets[0]; + gyro[1] = raw.gyro_rad_s[1] - gyro_offsets[1]; + gyro[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; + + /* update accelerometer measurements */ + if (sensor_last_count[1] != raw.accelerometer_counter) { + update_vect[1] = 1; + sensor_last_count[1] = raw.accelerometer_counter; + sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); + sensor_last_timestamp[1] = raw.timestamp; + } + + acc[0] = raw.accelerometer_m_s2[0]; + acc[1] = raw.accelerometer_m_s2[1]; + acc[2] = raw.accelerometer_m_s2[2]; + + /* update magnetometer measurements */ + if (sensor_last_count[2] != raw.magnetometer_counter) { + update_vect[2] = 1; + sensor_last_count[2] = raw.magnetometer_counter; + sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); + sensor_last_timestamp[2] = raw.timestamp; + } + + mag[0] = raw.magnetometer_ga[0]; + mag[1] = raw.magnetometer_ga[1]; + mag[2] = raw.magnetometer_ga[2]; + + uint64_t now = hrt_absolute_time(); + unsigned int time_elapsed = now - last_run; + last_run = now; + + if (time_elapsed > loop_interval_alarm) { + //TODO: add warning, cpu overload here + // if (overloadcounter == 20) { + // printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm); + // overloadcounter = 0; + // } + + overloadcounter++; + } + + static bool const_initialized = false; + + /* initialize with good values once we have a reasonable dt estimate */ + if (!const_initialized && dt < 0.05f && dt > 0.005f) { + dt = 0.005f; + parameters_update(&so3_comp_param_handles, &so3_comp_params); + const_initialized = true; + } + + /* do not execute the filter if not initialized */ + if (!const_initialized) { + continue; + } + + uint64_t timing_start = hrt_absolute_time(); + + // NOTE : Accelerometer is reversed. + // Because proper mount of PX4 will give you a reversed accelerometer readings. + NonlinearSO3AHRSupdate(gyro[0],gyro[1],gyro[2],-acc[0],-acc[1],-acc[2],mag[0],mag[1],mag[2],so3_comp_params.Kp,so3_comp_params.Ki, dt); + + // Convert q->R. + Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11 + Rot_matrix[1] = 2.0 * (q1*q2 + q0*q3); // 12 + Rot_matrix[2] = 2.0 * (q1*q3 - q0*q2); // 13 + Rot_matrix[3] = 2.0 * (q1*q2 - q0*q3); // 21 + Rot_matrix[4] = q0q0 - q1q1 + q2q2 - q3q3;// 22 + Rot_matrix[5] = 2.0 * (q2*q3 + q0*q1); // 23 + Rot_matrix[6] = 2.0 * (q1*q3 + q0*q2); // 31 + Rot_matrix[7] = 2.0 * (q2*q3 - q0*q1); // 32 + Rot_matrix[8] = q0q0 - q1q1 - q2q2 + q3q3;// 33 + + //1-2-3 Representation. + //Equation (290) + //Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors, James Diebel. + // Existing PX4 EKF code was generated by MATLAB which uses coloum major order matrix. + euler[0] = atan2f(Rot_matrix[5], Rot_matrix[8]); //! Roll + euler[1] = -asinf(Rot_matrix[2]); //! Pitch + euler[2] = atan2f(Rot_matrix[1],Rot_matrix[0]); //! Yaw + + /* swap values for next iteration, check for fatal inputs */ + if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) { + /* Do something */ + } else { + /* due to inputs or numerical failure the output is invalid, skip it */ + continue; + } + + if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator so3_comp] sensor data missed! (%llu)\n", raw.timestamp - last_data); + + last_data = raw.timestamp; + + /* send out */ + att.timestamp = raw.timestamp; + + // XXX Apply the same transformation to the rotation matrix + att.roll = euler[0] - so3_comp_params.roll_off; + att.pitch = euler[1] - so3_comp_params.pitch_off; + att.yaw = euler[2] - so3_comp_params.yaw_off; + + //! Euler angle rate. But it needs to be investigated again. + /* + att.rollspeed = 2.0f*(-q1*dq0 + q0*dq1 - q3*dq2 + q2*dq3); + att.pitchspeed = 2.0f*(-q2*dq0 + q3*dq1 + q0*dq2 - q1*dq3); + att.yawspeed = 2.0f*(-q3*dq0 -q2*dq1 + q1*dq2 + q0*dq3); + */ + att.rollspeed = gyro[0]; + att.pitchspeed = gyro[1]; + att.yawspeed = gyro[2]; + + att.rollacc = 0; + att.pitchacc = 0; + att.yawacc = 0; + + //! Quaternion + att.q[0] = q0; + att.q[1] = q1; + att.q[2] = q2; + att.q[3] = q3; + att.q_valid = true; + + /* TODO: Bias estimation required */ + memcpy(&att.rate_offsets, &(gyro_bias), sizeof(att.rate_offsets)); + + /* copy rotation matrix */ + memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix)); + att.R_valid = true; + + if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { + // Broadcast + orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); + + } else { + warnx("NaN in roll/pitch/yaw estimate!"); + } + + perf_end(so3_comp_loop_perf); + + //! This will print out debug packet to visualization software + if(debug_mode) + { + float quat[4]; + quat[0] = q0; + quat[1] = q1; + quat[2] = q2; + quat[3] = q3; + send_uart_float_arr(quat,4); + send_uart_byte('\n'); + } + } + } + } + + loopcounter++; + }// while + + thread_running = false; + + /* Reset the UART flags to original state */ + if (!usb_uart) + tcsetattr(uart, TCSANOW, &uart_config_original); + + return 0; +} diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c new file mode 100755 index 000000000..f962515df --- /dev/null +++ b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c @@ -0,0 +1,63 @@ +/* + * Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr> + * + * @file attitude_estimator_so3_comp_params.c + * + * 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. + * + * Theory of nonlinear complementary filters on the SO(3) is based on [1]. + * Quaternion realization of [1] is based on [2]. + * Optmized quaternion update code is based on Sebastian Madgwick's implementation. + * + * References + * [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008 + * [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008 + */ + +#include "attitude_estimator_so3_comp_params.h" + +/* This is filter gain for nonlinear SO3 complementary filter */ +/* NOTE : How to tune the gain? First of all, stick with this default gain. And let the quad in stable place. + Log the steady state reponse of filter. If it is too slow, increase SO3_COMP_KP. + If you are flying from ground to high altitude in short amount of time, please increase SO3_COMP_KI which + will compensate gyro bias which depends on temperature and vibration of your vehicle */ +PARAM_DEFINE_FLOAT(SO3_COMP_KP, 1.0f); //! This parameter will give you about 15 seconds convergence time. + //! You can set this gain higher if you want more fast response. + //! But note that higher gain will give you also higher overshoot. +PARAM_DEFINE_FLOAT(SO3_COMP_KI, 0.05f); //! This gain will incorporate slow time-varying bias (e.g., temperature change) + //! This gain is depend on your vehicle status. + +/* offsets in roll, pitch and yaw of sensor plane and body */ +PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f); +PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f); +PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f); + +int parameters_init(struct attitude_estimator_so3_comp_param_handles *h) +{ + /* Filter gain parameters */ + h->Kp = param_find("SO3_COMP_KP"); + h->Ki = param_find("SO3_COMP_KI"); + + /* Attitude offset (WARNING: Do not change if you do not know what exactly this variable wil lchange) */ + h->roll_off = param_find("ATT_ROLL_OFFS"); + h->pitch_off = param_find("ATT_PITCH_OFFS"); + h->yaw_off = param_find("ATT_YAW_OFFS"); + + return OK; +} + +int parameters_update(const struct attitude_estimator_so3_comp_param_handles *h, struct attitude_estimator_so3_comp_params *p) +{ + /* Update filter gain */ + param_get(h->Kp, &(p->Kp)); + param_get(h->Ki, &(p->Ki)); + + /* Update attitude offset */ + param_get(h->roll_off, &(p->roll_off)); + param_get(h->pitch_off, &(p->pitch_off)); + param_get(h->yaw_off, &(p->yaw_off)); + + return OK; +} diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.h b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.h new file mode 100755 index 000000000..f00695630 --- /dev/null +++ b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.h @@ -0,0 +1,44 @@ +/* + * Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr> + * + * @file attitude_estimator_so3_comp_params.h + * + * 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. + * + * Theory of nonlinear complementary filters on the SO(3) is based on [1]. + * Quaternion realization of [1] is based on [2]. + * Optmized quaternion update code is based on Sebastian Madgwick's implementation. + * + * References + * [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008 + * [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008 + */ + +#include <systemlib/param/param.h> + +struct attitude_estimator_so3_comp_params { + float Kp; + float Ki; + float roll_off; + float pitch_off; + float yaw_off; +}; + +struct attitude_estimator_so3_comp_param_handles { + param_t Kp, Ki; + param_t roll_off, pitch_off, yaw_off; +}; + +/** + * Initialize all parameter handles and values + * + */ +int parameters_init(struct attitude_estimator_so3_comp_param_handles *h); + +/** + * Update all parameters + * + */ +int parameters_update(const struct attitude_estimator_so3_comp_param_handles *h, struct attitude_estimator_so3_comp_params *p); diff --git a/src/modules/attitude_estimator_so3_comp/module.mk b/src/modules/attitude_estimator_so3_comp/module.mk new file mode 100644 index 000000000..92f43d920 --- /dev/null +++ b/src/modules/attitude_estimator_so3_comp/module.mk @@ -0,0 +1,8 @@ +# +# Attitude estimator (Nonlinear SO3 complementary Filter) +# + +MODULE_COMMAND = attitude_estimator_so3_comp + +SRCS = attitude_estimator_so3_comp_main.cpp \ + attitude_estimator_so3_comp_params.c diff --git a/src/modules/commander/accelerometer_calibration.c b/src/modules/commander/accelerometer_calibration.c new file mode 100644 index 000000000..48a36ac26 --- /dev/null +++ b/src/modules/commander/accelerometer_calibration.c @@ -0,0 +1,448 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * 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 + * 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 accelerometer_calibration.c + * + * Implementation of accelerometer calibration. + * + * Transform acceleration vector to true orientation, scale and offset + * + * ===== Model ===== + * accel_corr = accel_T * (accel_raw - accel_offs) + * + * accel_corr[3] - fully corrected acceleration vector in body frame + * accel_T[3][3] - accelerometers transform matrix, rotation and scaling transform + * accel_raw[3] - raw acceleration vector + * accel_offs[3] - acceleration offset vector + * + * ===== Calibration ===== + * + * Reference vectors + * accel_corr_ref[6][3] = [ g 0 0 ] // nose up + * | -g 0 0 | // nose down + * | 0 g 0 | // left side down + * | 0 -g 0 | // right side down + * | 0 0 g | // on back + * [ 0 0 -g ] // level + * accel_raw_ref[6][3] + * + * accel_corr_ref[i] = accel_T * (accel_raw_ref[i] - accel_offs), i = 0...5 + * + * 6 reference vectors * 3 axes = 18 equations + * 9 (accel_T) + 3 (accel_offs) = 12 unknown constants + * + * Find accel_offs + * + * accel_offs[i] = (accel_raw_ref[i*2][i] + accel_raw_ref[i*2+1][i]) / 2 + * + * Find accel_T + * + * 9 unknown constants + * need 9 equations -> use 3 of 6 measurements -> 3 * 3 = 9 equations + * + * accel_corr_ref[i*2] = accel_T * (accel_raw_ref[i*2] - accel_offs), i = 0...2 + * + * Solve separate system for each row of accel_T: + * + * accel_corr_ref[j*2][i] = accel_T[i] * (accel_raw_ref[j*2] - accel_offs), j = 0...2 + * + * A * x = b + * + * x = [ accel_T[0][i] ] + * | accel_T[1][i] | + * [ accel_T[2][i] ] + * + * b = [ accel_corr_ref[0][i] ] // One measurement per axis is enough + * | accel_corr_ref[2][i] | + * [ accel_corr_ref[4][i] ] + * + * a[i][j] = accel_raw_ref[i][j] - accel_offs[j], i = 0;2;4, j = 0...2 + * + * Matrix A is common for all three systems: + * A = [ a[0][0] a[0][1] a[0][2] ] + * | a[2][0] a[2][1] a[2][2] | + * [ a[4][0] a[4][1] a[4][2] ] + * + * x = A^-1 * b + * + * accel_T = A^-1 * g + * g = 9.80665 + * + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#include "accelerometer_calibration.h" + +#include <poll.h> +#include <drivers/drv_hrt.h> +#include <uORB/topics/sensor_combined.h> +#include <drivers/drv_accel.h> +#include <systemlib/conversions.h> +#include <mavlink/mavlink_log.h> + +void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd); +int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]); +int detect_orientation(int mavlink_fd, int sub_sensor_combined); +int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num); +int mat_invert3(float src[3][3], float dst[3][3]); +int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g); + +void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd) { + /* announce change */ + mavlink_log_info(mavlink_fd, "accel calibration started"); + /* set to accel calibration mode */ + status->flag_preflight_accel_calibration = true; + state_machine_publish(status_pub, status, mavlink_fd); + + /* measure and calculate offsets & scales */ + float accel_offs[3]; + float accel_scale[3]; + int res = do_accel_calibration_mesurements(mavlink_fd, accel_offs, accel_scale); + + if (res == OK) { + /* measurements complete successfully, set parameters */ + if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs[0])) + || param_set(param_find("SENS_ACC_YOFF"), &(accel_offs[1])) + || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs[2])) + || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale[0])) + || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale[1])) + || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale[2]))) { + mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed"); + } + + int fd = open(ACCEL_DEVICE_PATH, 0); + struct accel_scale ascale = { + accel_offs[0], + accel_scale[0], + accel_offs[1], + accel_scale[1], + accel_offs[2], + accel_scale[2], + }; + + if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) + warn("WARNING: failed to set scale / offsets for accel"); + + close(fd); + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + } + + mavlink_log_info(mavlink_fd, "accel calibration done"); + tune_confirm(); + sleep(2); + tune_confirm(); + sleep(2); + /* third beep by cal end routine */ + } else { + /* measurements error */ + mavlink_log_info(mavlink_fd, "accel calibration aborted"); + tune_error(); + sleep(2); + } + + /* exit accel calibration mode */ + status->flag_preflight_accel_calibration = false; + state_machine_publish(status_pub, status, mavlink_fd); +} + +int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) { + const int samples_num = 2500; + float accel_ref[6][3]; + bool data_collected[6] = { false, false, false, false, false, false }; + const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" }; + + /* reset existing calibration */ + int fd = open(ACCEL_DEVICE_PATH, 0); + struct accel_scale ascale_null = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + int ioctl_res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null); + close(fd); + + if (OK != ioctl_res) { + warn("ERROR: failed to set scale / offsets for accel"); + return ERROR; + } + + int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); + while (true) { + bool done = true; + char str[80]; + int str_ptr; + str_ptr = sprintf(str, "keep vehicle still:"); + for (int i = 0; i < 6; i++) { + if (!data_collected[i]) { + str_ptr += sprintf(&(str[str_ptr]), " %s", orientation_strs[i]); + done = false; + } + } + if (done) + break; + mavlink_log_info(mavlink_fd, str); + + int orient = detect_orientation(mavlink_fd, sensor_combined_sub); + if (orient < 0) + return ERROR; + + sprintf(str, "meas started: %s", orientation_strs[orient]); + mavlink_log_info(mavlink_fd, str); + read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); + str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], accel_ref[orient][0], accel_ref[orient][1], accel_ref[orient][2]); + mavlink_log_info(mavlink_fd, str); + data_collected[orient] = true; + tune_confirm(); + } + close(sensor_combined_sub); + + /* calculate offsets and rotation+scale matrix */ + float accel_T[3][3]; + int res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); + if (res != 0) { + mavlink_log_info(mavlink_fd, "ERROR: calibration values calc error"); + return ERROR; + } + + /* convert accel transform matrix to scales, + * rotation part of transform matrix is not used by now + */ + for (int i = 0; i < 3; i++) { + accel_scale[i] = accel_T[i][i]; + } + + return OK; +} + +/* + * Wait for vehicle become still and detect it's orientation. + * + * @return 0..5 according to orientation when vehicle is still and ready for measurements, + * ERROR if vehicle is not still after 30s or orientation error is more than 5m/s^2 + */ +int detect_orientation(int mavlink_fd, int sub_sensor_combined) { + struct sensor_combined_s sensor; + /* exponential moving average of accel */ + float accel_ema[3] = { 0.0f, 0.0f, 0.0f }; + /* max-hold dispersion of accel */ + float accel_disp[3] = { 0.0f, 0.0f, 0.0f }; + /* EMA time constant in seconds*/ + float ema_len = 0.2f; + /* set "still" threshold to 0.1 m/s^2 */ + float still_thr2 = pow(0.1f, 2); + /* set accel error threshold to 5m/s^2 */ + float accel_err_thr = 5.0f; + /* still time required in us */ + int64_t still_time = 2000000; + struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; + + hrt_abstime t_start = hrt_absolute_time(); + /* set timeout to 30s */ + hrt_abstime timeout = 30000000; + hrt_abstime t_timeout = t_start + timeout; + hrt_abstime t = t_start; + hrt_abstime t_prev = t_start; + hrt_abstime t_still = 0; + while (true) { + /* wait blocking for new data */ + int poll_ret = poll(fds, 1, 1000); + if (poll_ret) { + orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &sensor); + t = hrt_absolute_time(); + float dt = (t - t_prev) / 1000000.0f; + t_prev = t; + float w = dt / ema_len; + for (int i = 0; i < 3; i++) { + accel_ema[i] = accel_ema[i] * (1.0f - w) + sensor.accelerometer_m_s2[i] * w; + float d = (float) sensor.accelerometer_m_s2[i] - accel_ema[i]; + d = d * d; + accel_disp[i] = accel_disp[i] * (1.0f - w); + if (d > accel_disp[i]) + accel_disp[i] = d; + } + /* still detector with hysteresis */ + if ( accel_disp[0] < still_thr2 && + accel_disp[1] < still_thr2 && + accel_disp[2] < still_thr2 ) { + /* is still now */ + if (t_still == 0) { + /* first time */ + mavlink_log_info(mavlink_fd, "still..."); + t_still = t; + t_timeout = t + timeout; + } else { + /* still since t_still */ + if ((int64_t) t - (int64_t) t_still > still_time) { + /* vehicle is still, exit from the loop to detection of its orientation */ + break; + } + } + } else if ( accel_disp[0] > still_thr2 * 2.0f || + accel_disp[1] > still_thr2 * 2.0f || + accel_disp[2] > still_thr2 * 2.0f) { + /* not still, reset still start time */ + if (t_still != 0) { + mavlink_log_info(mavlink_fd, "moving..."); + t_still = 0; + } + } + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "ERROR: poll failure"); + return -3; + } + if (t > t_timeout) { + mavlink_log_info(mavlink_fd, "ERROR: timeout"); + return -1; + } + } + + if ( fabs(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr && + fabs(accel_ema[1]) < accel_err_thr && + fabs(accel_ema[2]) < accel_err_thr ) + return 0; // [ g, 0, 0 ] + if ( fabs(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr && + fabs(accel_ema[1]) < accel_err_thr && + fabs(accel_ema[2]) < accel_err_thr ) + return 1; // [ -g, 0, 0 ] + if ( fabs(accel_ema[0]) < accel_err_thr && + fabs(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr && + fabs(accel_ema[2]) < accel_err_thr ) + return 2; // [ 0, g, 0 ] + if ( fabs(accel_ema[0]) < accel_err_thr && + fabs(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr && + fabs(accel_ema[2]) < accel_err_thr ) + return 3; // [ 0, -g, 0 ] + if ( fabs(accel_ema[0]) < accel_err_thr && + fabs(accel_ema[1]) < accel_err_thr && + fabs(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr ) + return 4; // [ 0, 0, g ] + if ( fabs(accel_ema[0]) < accel_err_thr && + fabs(accel_ema[1]) < accel_err_thr && + fabs(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr ) + return 5; // [ 0, 0, -g ] + + mavlink_log_info(mavlink_fd, "ERROR: invalid orientation"); + + return -2; // Can't detect orientation +} + +/* + * Read specified number of accelerometer samples, calculate average and dispersion. + */ +int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num) { + struct pollfd fds[1] = { { .fd = sensor_combined_sub, .events = POLLIN } }; + int count = 0; + float accel_sum[3] = { 0.0f, 0.0f, 0.0f }; + + while (count < samples_num) { + int poll_ret = poll(fds, 1, 1000); + if (poll_ret == 1) { + struct sensor_combined_s sensor; + orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); + for (int i = 0; i < 3; i++) + accel_sum[i] += sensor.accelerometer_m_s2[i]; + count++; + } else { + return ERROR; + } + } + + for (int i = 0; i < 3; i++) { + accel_avg[i] = accel_sum[i] / count; + } + + return OK; +} + +int mat_invert3(float src[3][3], float dst[3][3]) { + float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) - + 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.0) + 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; + dst[2][0] = (src[1][0] * src[2][1] - src[1][1] * src[2][0]) / det; + dst[0][1] = (src[0][2] * src[2][1] - src[0][1] * src[2][2]) / det; + dst[1][1] = (src[0][0] * src[2][2] - src[0][2] * src[2][0]) / det; + dst[2][1] = (src[0][1] * src[2][0] - src[0][0] * src[2][1]) / det; + dst[0][2] = (src[0][1] * src[1][2] - src[0][2] * src[1][1]) / det; + dst[1][2] = (src[0][2] * src[1][0] - src[0][0] * src[1][2]) / det; + dst[2][2] = (src[0][0] * src[1][1] - src[0][1] * src[1][0]) / det; + + return OK; +} + +int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g) { + /* calculate offsets */ + for (int i = 0; i < 3; i++) { + accel_offs[i] = (accel_ref[i * 2][i] + accel_ref[i * 2 + 1][i]) / 2; + } + + /* fill matrix A for linear equations system*/ + float mat_A[3][3]; + memset(mat_A, 0, sizeof(mat_A)); + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + float a = accel_ref[i * 2][j] - accel_offs[j]; + mat_A[i][j] = a; + } + } + + /* calculate inverse matrix for A */ + float mat_A_inv[3][3]; + if (mat_invert3(mat_A, mat_A_inv) != OK) + return ERROR; + + /* copy results to accel_T */ + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + /* simplify matrices mult because b has only one non-zero element == g at index i */ + accel_T[j][i] = mat_A_inv[j][i] * g; + } + } + + return OK; +} diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h new file mode 100644 index 000000000..f93a867ba --- /dev/null +++ b/src/modules/commander/accelerometer_calibration.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * 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 + * 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 accelerometer_calibration.h + * + * Definition of accelerometer calibration. + * + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#ifndef ACCELEROMETER_CALIBRATION_H_ +#define ACCELEROMETER_CALIBRATION_H_ + +#include <stdint.h> +#include <uORB/topics/vehicle_status.h> + +void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd); + +#endif /* ACCELEROMETER_CALIBRATION_H_ */ diff --git a/src/modules/commander/calibration_routines.c b/src/modules/commander/calibration_routines.c new file mode 100644 index 000000000..a26938637 --- /dev/null +++ b/src/modules/commander/calibration_routines.c @@ -0,0 +1,219 @@ +/**************************************************************************** + * + * 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 calibration_routines.c + * Calibration routines implementations. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +#include <math.h> + +#include "calibration_routines.h" + + +int sphere_fit_least_squares(const float x[], const float y[], const float z[], + unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius) +{ + + float x_sumplain = 0.0f; + float x_sumsq = 0.0f; + float x_sumcube = 0.0f; + + float y_sumplain = 0.0f; + float y_sumsq = 0.0f; + float y_sumcube = 0.0f; + + float z_sumplain = 0.0f; + float z_sumsq = 0.0f; + float z_sumcube = 0.0f; + + float xy_sum = 0.0f; + float xz_sum = 0.0f; + float yz_sum = 0.0f; + + float x2y_sum = 0.0f; + float x2z_sum = 0.0f; + float y2x_sum = 0.0f; + float y2z_sum = 0.0f; + float z2x_sum = 0.0f; + float z2y_sum = 0.0f; + + for (unsigned int i = 0; i < size; i++) { + + float x2 = x[i] * x[i]; + float y2 = y[i] * y[i]; + float z2 = z[i] * z[i]; + + x_sumplain += x[i]; + x_sumsq += x2; + x_sumcube += x2 * x[i]; + + y_sumplain += y[i]; + y_sumsq += y2; + y_sumcube += y2 * y[i]; + + z_sumplain += z[i]; + z_sumsq += z2; + z_sumcube += z2 * z[i]; + + xy_sum += x[i] * y[i]; + xz_sum += x[i] * z[i]; + yz_sum += y[i] * z[i]; + + x2y_sum += x2 * y[i]; + x2z_sum += x2 * z[i]; + + y2x_sum += y2 * x[i]; + y2z_sum += y2 * z[i]; + + z2x_sum += z2 * x[i]; + z2y_sum += z2 * y[i]; + } + + // + //Least Squares Fit a sphere A,B,C with radius squared Rsq to 3D data + // + // P is a structure that has been computed with the data earlier. + // P.npoints is the number of elements; the length of X,Y,Z are identical. + // P's members are logically named. + // + // X[n] is the x component of point n + // Y[n] is the y component of point n + // Z[n] is the z component of point n + // + // A is the x coordiante of the sphere + // B is the y coordiante of the sphere + // C is the z coordiante of the sphere + // Rsq is the radius squared of the sphere. + // + //This method should converge; maybe 5-100 iterations or more. + // + float x_sum = x_sumplain / size; //sum( X[n] ) + float x_sum2 = x_sumsq / size; //sum( X[n]^2 ) + float x_sum3 = x_sumcube / size; //sum( X[n]^3 ) + float y_sum = y_sumplain / size; //sum( Y[n] ) + float y_sum2 = y_sumsq / size; //sum( Y[n]^2 ) + float y_sum3 = y_sumcube / size; //sum( Y[n]^3 ) + float z_sum = z_sumplain / size; //sum( Z[n] ) + float z_sum2 = z_sumsq / size; //sum( Z[n]^2 ) + float z_sum3 = z_sumcube / size; //sum( Z[n]^3 ) + + float XY = xy_sum / size; //sum( X[n] * Y[n] ) + float XZ = xz_sum / size; //sum( X[n] * Z[n] ) + float YZ = yz_sum / size; //sum( Y[n] * Z[n] ) + float X2Y = x2y_sum / size; //sum( X[n]^2 * Y[n] ) + float X2Z = x2z_sum / size; //sum( X[n]^2 * Z[n] ) + float Y2X = y2x_sum / size; //sum( Y[n]^2 * X[n] ) + float Y2Z = y2z_sum / size; //sum( Y[n]^2 * Z[n] ) + float Z2X = z2x_sum / size; //sum( Z[n]^2 * X[n] ) + float Z2Y = z2y_sum / size; //sum( Z[n]^2 * Y[n] ) + + //Reduction of multiplications + float F0 = x_sum2 + y_sum2 + z_sum2; + float F1 = 0.5f * F0; + float F2 = -8.0f * (x_sum3 + Y2X + Z2X); + float F3 = -8.0f * (X2Y + y_sum3 + Z2Y); + float F4 = -8.0f * (X2Z + Y2Z + z_sum3); + + //Set initial conditions: + float A = x_sum; + float B = y_sum; + float C = z_sum; + + //First iteration computation: + float A2 = A * A; + float B2 = B * B; + float C2 = C * C; + float QS = A2 + B2 + C2; + float QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum); + + //Set initial conditions: + float Rsq = F0 + QB + QS; + + //First iteration computation: + float Q0 = 0.5f * (QS - Rsq); + float Q1 = F1 + Q0; + float Q2 = 8.0f * (QS - Rsq + QB + F0); + float aA, aB, aC, nA, nB, nC, dA, dB, dC; + + //Iterate N times, ignore stop condition. + int n = 0; + + while (n < max_iterations) { + n++; + + //Compute denominator: + aA = Q2 + 16.0f * (A2 - 2.0f * A * x_sum + x_sum2); + aB = Q2 + 16.0f * (B2 - 2.0f * B * y_sum + y_sum2); + aC = Q2 + 16.0f * (C2 - 2.0f * C * z_sum + z_sum2); + aA = (aA == 0.0f) ? 1.0f : aA; + aB = (aB == 0.0f) ? 1.0f : aB; + aC = (aC == 0.0f) ? 1.0f : aC; + + //Compute next iteration + nA = A - ((F2 + 16.0f * (B * XY + C * XZ + x_sum * (-A2 - Q0) + A * (x_sum2 + Q1 - C * z_sum - B * y_sum))) / aA); + nB = B - ((F3 + 16.0f * (A * XY + C * YZ + y_sum * (-B2 - Q0) + B * (y_sum2 + Q1 - A * x_sum - C * z_sum))) / aB); + nC = C - ((F4 + 16.0f * (A * XZ + B * YZ + z_sum * (-C2 - Q0) + C * (z_sum2 + Q1 - A * x_sum - B * y_sum))) / aC); + + //Check for stop condition + dA = (nA - A); + dB = (nB - B); + dC = (nC - C); + + if ((dA * dA + dB * dB + dC * dC) <= delta) { break; } + + //Compute next iteration's values + A = nA; + B = nB; + C = nC; + A2 = A * A; + B2 = B * B; + C2 = C * C; + QS = A2 + B2 + C2; + QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum); + Rsq = F0 + QB + QS; + Q0 = 0.5f * (QS - Rsq); + Q1 = F1 + Q0; + Q2 = 8.0f * (QS - Rsq + QB + F0); + } + + *sphere_x = A; + *sphere_y = B; + *sphere_z = C; + *sphere_radius = sqrtf(Rsq); + + return 0; +} diff --git a/src/modules/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h new file mode 100644 index 000000000..e3e7fbafd --- /dev/null +++ b/src/modules/commander/calibration_routines.h @@ -0,0 +1,61 @@ +/**************************************************************************** + * + * 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 calibration_routines.h + * Calibration routines definitions. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +/** + * Least-squares fit of a sphere to a set of points. + * + * Fits a sphere to a set of points on the sphere surface. + * + * @param x point coordinates on the X axis + * @param y point coordinates on the Y axis + * @param z point coordinates on the Z axis + * @param size number of points + * @param max_iterations abort if maximum number of iterations have been reached. If unsure, set to 100. + * @param delta abort if error is below delta. If unsure, set to 0 to run max_iterations times. + * @param sphere_x coordinate of the sphere center on the X axis + * @param sphere_y coordinate of the sphere center on the Y axis + * @param sphere_z coordinate of the sphere center on the Z axis + * @param sphere_radius sphere radius + * + * @return 0 on success, 1 on failure + */ +int sphere_fit_least_squares(const float x[], const float y[], const float z[], + unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius);
\ No newline at end of file diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c new file mode 100644 index 000000000..67f053e22 --- /dev/null +++ b/src/modules/commander/commander.c @@ -0,0 +1,2078 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch> + * Lorenz Meier <lm@inf.ethz.ch> + * Thomas Gubler <thomasgubler@student.ethz.ch> + * 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 + * 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 commander.c + * Main system state machine implementation. + * + * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * + */ + +#include "commander.h" + +#include <nuttx/config.h> +#include <pthread.h> +#include <stdio.h> +#include <stdlib.h> +#include <stdbool.h> +#include <string.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> +#include <sys/prctl.h> +#include <string.h> +#include <drivers/drv_led.h> +#include <drivers/drv_hrt.h> +#include <drivers/drv_tone_alarm.h> +#include "state_machine_helper.h" +#include "systemlib/systemlib.h" +#include <math.h> +#include <poll.h> +#include <uORB/uORB.h> +#include <uORB/topics/sensor_combined.h> +#include <uORB/topics/battery_status.h> +#include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/offboard_control_setpoint.h> +#include <uORB/topics/home_position.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_command.h> +#include <uORB/topics/subsystem_info.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/parameter_update.h> +#include <uORB/topics/differential_pressure.h> +#include <mavlink/mavlink_log.h> + +#include <systemlib/param/param.h> +#include <systemlib/systemlib.h> +#include <systemlib/err.h> + +/* XXX MOVE CALIBRATION TO SENSORS APP THREAD */ +#include <drivers/drv_accel.h> +#include <drivers/drv_gyro.h> +#include <drivers/drv_mag.h> +#include <drivers/drv_baro.h> + +#include "calibration_routines.h" +#include "accelerometer_calibration.h" + +PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */ +//PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0); /**< Go into high-level failsafe after 0 ms */ +PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); +PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); +PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); + +#include <systemlib/cpuload.h> +extern struct system_load_s system_load; + +/* Decouple update interval and hysteris counters, all depends on intervals */ +#define COMMANDER_MONITORING_INTERVAL 50000 +#define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f)) +#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) +#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) + +#define STICK_ON_OFF_LIMIT 0.75f +#define STICK_THRUST_RANGE 1.0f +#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 +#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) + +#define GPS_FIX_TYPE_2D 2 +#define GPS_FIX_TYPE_3D 3 +#define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000 +#define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) + +/* File descriptors */ +static int leds; +static int buzzer; +static int mavlink_fd; +static bool commander_initialized = false; +static struct vehicle_status_s current_status; /**< Main state machine */ +static orb_advert_t stat_pub; + +// static uint16_t nofix_counter = 0; +// static uint16_t gotfix_counter = 0; + +static unsigned int failsafe_lowlevel_timeout_ms; + +static bool thread_should_exit = false; /**< daemon exit flag */ +static bool thread_running = false; /**< daemon status flag */ +static int daemon_task; /**< Handle of daemon task / thread */ + +/* pthread loops */ +static void *orb_receive_loop(void *arg); + +__EXPORT int commander_main(int argc, char *argv[]); + +/** + * Mainloop of commander. + */ +int commander_thread_main(int argc, char *argv[]); + +static int buzzer_init(void); +static void buzzer_deinit(void); +static int led_init(void); +static void led_deinit(void); +static int led_toggle(int led); +static int led_on(int led); +static int led_off(int led); +static void do_gyro_calibration(int status_pub, struct vehicle_status_s *status); +static void do_mag_calibration(int status_pub, struct vehicle_status_s *status); +static void do_rc_calibration(int status_pub, struct vehicle_status_s *status); +static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd); + +int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state); + + + +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +/** + * Sort calibration values. + * + * Sorts the calibration values with bubble sort. + * + * @param a The array to sort + * @param n The number of entries in the array + */ +// static void cal_bsort(float a[], int n); + +static int buzzer_init() +{ + buzzer = open("/dev/tone_alarm", O_WRONLY); + + if (buzzer < 0) { + warnx("Buzzer: open fail\n"); + return ERROR; + } + + return 0; +} + +static void buzzer_deinit() +{ + close(buzzer); +} + + +static int led_init() +{ + leds = open(LED_DEVICE_PATH, 0); + + if (leds < 0) { + warnx("LED: open fail\n"); + return ERROR; + } + + if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) { + warnx("LED: ioctl fail\n"); + return ERROR; + } + + return 0; +} + +static void led_deinit() +{ + close(leds); +} + +static int led_toggle(int led) +{ + static int last_blue = LED_ON; + static int last_amber = LED_ON; + + if (led == LED_BLUE) last_blue = (last_blue == LED_ON) ? LED_OFF : LED_ON; + + if (led == LED_AMBER) last_amber = (last_amber == LED_ON) ? LED_OFF : LED_ON; + + return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led); +} + +static int led_on(int led) +{ + return ioctl(leds, LED_ON, led); +} + +static int led_off(int led) +{ + return ioctl(leds, LED_OFF, led); +} + +enum AUDIO_PATTERN { + AUDIO_PATTERN_ERROR = 2, + AUDIO_PATTERN_NOTIFY_POSITIVE = 3, + AUDIO_PATTERN_NOTIFY_NEUTRAL = 4, + AUDIO_PATTERN_NOTIFY_NEGATIVE = 5, + AUDIO_PATTERN_NOTIFY_CHARGE = 6 +}; + +int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state) +{ + + /* Trigger alarm if going into any error state */ + if (((new_state == SYSTEM_STATE_GROUND_ERROR) && (old_state != SYSTEM_STATE_GROUND_ERROR)) || + ((new_state == SYSTEM_STATE_MISSION_ABORT) && (old_state != SYSTEM_STATE_MISSION_ABORT))) { + ioctl(buzzer, TONE_SET_ALARM, 0); + ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_ERROR); + } + + /* Trigger neutral on arming / disarming */ + if (((new_state == SYSTEM_STATE_GROUND_READY) && (old_state != SYSTEM_STATE_GROUND_READY))) { + ioctl(buzzer, TONE_SET_ALARM, 0); + ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_NOTIFY_NEUTRAL); + } + + /* Trigger Tetris on being bored */ + + return 0; +} + +void tune_confirm(void) +{ + ioctl(buzzer, TONE_SET_ALARM, 3); +} + +void tune_error(void) +{ + ioctl(buzzer, TONE_SET_ALARM, 4); +} + +void do_rc_calibration(int status_pub, struct vehicle_status_s *status) +{ + if (current_status.offboard_control_signal_lost) { + mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal."); + return; + } + + int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint)); + struct manual_control_setpoint_s sp; + orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); + + /* set parameters */ + + float p = sp.roll; + param_set(param_find("TRIM_ROLL"), &p); + p = sp.pitch; + param_set(param_find("TRIM_PITCH"), &p); + p = sp.yaw; + param_set(param_find("TRIM_YAW"), &p); + + /* store to permanent storage */ + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); + } + + mavlink_log_info(mavlink_fd, "trim calibration done"); +} + +void do_mag_calibration(int status_pub, struct vehicle_status_s *status) +{ + + /* set to mag calibration mode */ + status->flag_preflight_mag_calibration = true; + state_machine_publish(status_pub, status, mavlink_fd); + + int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); + struct mag_report mag; + + /* 45 seconds */ + uint64_t calibration_interval = 45 * 1000 * 1000; + + /* maximum 2000 values */ + const unsigned int calibration_maxcount = 500; + unsigned int calibration_counter = 0; + + /* limit update rate to get equally spaced measurements over time (in ms) */ + orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); + + // XXX old cal + // * FLT_MIN is not the most negative float number, + // * but the smallest number by magnitude float can + // * represent. Use -FLT_MAX to initialize the most + // * negative number + + // float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX}; + // float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX}; + + int fd = open(MAG_DEVICE_PATH, O_RDONLY); + + /* erase old calibration */ + struct mag_scale mscale_null = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { + warn("WARNING: failed to set scale / offsets for mag"); + mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag"); + } + + /* calibrate range */ + if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) { + warnx("failed to calibrate scale"); + } + + close(fd); + + /* calibrate offsets */ + + // uint64_t calibration_start = hrt_absolute_time(); + + uint64_t axis_deadline = hrt_absolute_time(); + uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; + + const char axislabels[3] = { 'X', 'Y', 'Z'}; + int axis_index = -1; + + float *x = (float *)malloc(sizeof(float) * calibration_maxcount); + float *y = (float *)malloc(sizeof(float) * calibration_maxcount); + float *z = (float *)malloc(sizeof(float) * calibration_maxcount); + + if (x == NULL || y == NULL || z == NULL) { + warnx("mag cal failed: out of memory"); + mavlink_log_info(mavlink_fd, "mag cal failed: out of memory"); + warnx("x:%p y:%p z:%p\n", x, y, z); + return; + } + + tune_confirm(); + sleep(2); + tune_confirm(); + + while (hrt_absolute_time() < calibration_deadline && + calibration_counter < calibration_maxcount) { + + /* wait blocking for new data */ + struct pollfd fds[1] = { { .fd = sub_mag, .events = POLLIN } }; + + /* user guidance */ + if (hrt_absolute_time() >= axis_deadline && + axis_index < 3) { + + axis_index++; + + char buf[50]; + sprintf(buf, "Please rotate around %c", axislabels[axis_index]); + mavlink_log_info(mavlink_fd, buf); + tune_confirm(); + + axis_deadline += calibration_interval / 3; + } + + if (!(axis_index < 3)) { + break; + } + + // int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time(); + + // if ((axis_left / 1000) == 0 && axis_left > 0) { + // char buf[50]; + // sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]); + // mavlink_log_info(mavlink_fd, buf); + // } + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); + + x[calibration_counter] = mag.x; + y[calibration_counter] = mag.y; + z[calibration_counter] = mag.z; + + /* get min/max values */ + + // if (mag.x < mag_min[0]) { + // mag_min[0] = mag.x; + // } + // else if (mag.x > mag_max[0]) { + // mag_max[0] = mag.x; + // } + + // if (raw.magnetometer_ga[1] < mag_min[1]) { + // mag_min[1] = raw.magnetometer_ga[1]; + // } + // else if (raw.magnetometer_ga[1] > mag_max[1]) { + // mag_max[1] = raw.magnetometer_ga[1]; + // } + + // if (raw.magnetometer_ga[2] < mag_min[2]) { + // mag_min[2] = raw.magnetometer_ga[2]; + // } + // else if (raw.magnetometer_ga[2] > mag_max[2]) { + // mag_max[2] = raw.magnetometer_ga[2]; + // } + + calibration_counter++; + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "mag cal canceled (timed out)"); + break; + } + } + + float sphere_x; + float sphere_y; + float sphere_z; + float sphere_radius; + + sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); + + free(x); + free(y); + free(z); + + if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) { + + fd = open(MAG_DEVICE_PATH, 0); + + struct mag_scale mscale; + + if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale)) + warn("WARNING: failed to get scale / offsets for mag"); + + mscale.x_offset = sphere_x; + mscale.y_offset = sphere_y; + mscale.z_offset = sphere_z; + + if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) + warn("WARNING: failed to set scale / offsets for mag"); + + close(fd); + + /* announce and set new offset */ + + if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) { + warnx("Setting X mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) { + warnx("Setting Y mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) { + warnx("Setting Z mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) { + warnx("Setting X mag scale failed!\n"); + } + + if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) { + warnx("Setting Y mag scale failed!\n"); + } + + if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) { + warnx("Setting Z mag scale failed!\n"); + } + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + mavlink_log_info(mavlink_fd, "FAILED storing calibration"); + } + + warnx("\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n", + (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale, + (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius); + + char buf[52]; + sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset, + (double)mscale.y_offset, (double)mscale.z_offset); + mavlink_log_info(mavlink_fd, buf); + + sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale, + (double)mscale.y_scale, (double)mscale.z_scale); + mavlink_log_info(mavlink_fd, buf); + + mavlink_log_info(mavlink_fd, "mag calibration done"); + + tune_confirm(); + sleep(2); + tune_confirm(); + sleep(2); + /* third beep by cal end routine */ + + } else { + mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)"); + } + + /* disable calibration mode */ + status->flag_preflight_mag_calibration = false; + state_machine_publish(status_pub, status, mavlink_fd); + + close(sub_mag); +} + +void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) +{ + /* set to gyro calibration mode */ + status->flag_preflight_gyro_calibration = true; + state_machine_publish(status_pub, status, mavlink_fd); + + const int calibration_count = 5000; + + int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); + struct sensor_combined_s raw; + + int calibration_counter = 0; + float gyro_offset[3] = {0.0f, 0.0f, 0.0f}; + + /* set offsets to zero */ + int fd = open(GYRO_DEVICE_PATH, 0); + struct gyro_scale gscale_null = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null)) + warn("WARNING: failed to set scale / offsets for gyro"); + + close(fd); + + while (calibration_counter < calibration_count) { + + /* wait blocking for new data */ + struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); + gyro_offset[0] += raw.gyro_rad_s[0]; + gyro_offset[1] += raw.gyro_rad_s[1]; + gyro_offset[2] += raw.gyro_rad_s[2]; + calibration_counter++; + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); + return; + } + } + + gyro_offset[0] = gyro_offset[0] / calibration_count; + gyro_offset[1] = gyro_offset[1] / calibration_count; + gyro_offset[2] = gyro_offset[2] / calibration_count; + + /* exit gyro calibration mode */ + status->flag_preflight_gyro_calibration = false; + state_machine_publish(status_pub, status, mavlink_fd); + + if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { + + if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0])) + || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1])) + || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) { + mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!"); + } + + /* set offsets to actual value */ + fd = open(GYRO_DEVICE_PATH, 0); + struct gyro_scale gscale = { + gyro_offset[0], + 1.0f, + gyro_offset[1], + 1.0f, + gyro_offset[2], + 1.0f, + }; + + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) + warn("WARNING: failed to set scale / offsets for gyro"); + + close(fd); + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + } + + // char buf[50]; + // sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); + // mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "gyro calibration done"); + + tune_confirm(); + sleep(2); + tune_confirm(); + sleep(2); + /* third beep by cal end routine */ + + } else { + mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)"); + } + + close(sub_sensor_combined); +} + +void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status) +{ + /* announce change */ + + mavlink_log_info(mavlink_fd, "keep it still"); + /* set to accel calibration mode */ + status->flag_preflight_airspeed_calibration = true; + state_machine_publish(status_pub, status, mavlink_fd); + + const int calibration_count = 2500; + + int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); + struct differential_pressure_s diff_pres; + + int calibration_counter = 0; + float diff_pres_offset = 0.0f; + + while (calibration_counter < calibration_count) { + + /* wait blocking for new data */ + struct pollfd fds[1] = { { .fd = diff_pres_sub, .events = POLLIN } }; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); + diff_pres_offset += diff_pres.differential_pressure_pa; + calibration_counter++; + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); + return; + } + } + + diff_pres_offset = diff_pres_offset / calibration_count; + + if (isfinite(diff_pres_offset)) { + + if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { + mavlink_log_critical(mavlink_fd, "Setting offs failed!"); + } + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + } + + //char buf[50]; + //sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); + //mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "airspeed calibration done"); + + tune_confirm(); + sleep(2); + tune_confirm(); + sleep(2); + /* third beep by cal end routine */ + + } else { + mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); + } + + /* exit airspeed calibration mode */ + status->flag_preflight_airspeed_calibration = false; + state_machine_publish(status_pub, status, mavlink_fd); + + close(diff_pres_sub); +} + + + +void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd) +{ + /* result of the command */ + uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; + + /* announce command handling */ + tune_confirm(); + + + /* supported command handling start */ + + /* request to set different system mode */ + switch (cmd->command) { + case VEHICLE_CMD_DO_SET_MODE: { + if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, (uint8_t)cmd->param1)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } + break; + + case VEHICLE_CMD_COMPONENT_ARM_DISARM: { + /* request to arm */ + if ((int)cmd->param1 == 1) { + if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + + /* request to disarm */ + + } else if ((int)cmd->param1 == 0) { + if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } + } + break; + + /* request for an autopilot reboot */ + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { + if ((int)cmd->param1 == 1) { + if (OK == do_state_update(status_pub, current_vehicle_status, mavlink_fd, SYSTEM_STATE_REBOOT)) { + /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + /* system may return here */ + result = VEHICLE_CMD_RESULT_DENIED; + } + } + } + break; + +// /* request to land */ +// case VEHICLE_CMD_NAV_LAND: +// { +// //TODO: add check if landing possible +// //TODO: add landing maneuver +// +// if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) { +// result = VEHICLE_CMD_RESULT_ACCEPTED; +// } } +// break; +// +// /* request to takeoff */ +// case VEHICLE_CMD_NAV_TAKEOFF: +// { +// //TODO: add check if takeoff possible +// //TODO: add takeoff maneuver +// +// if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) { +// result = VEHICLE_CMD_RESULT_ACCEPTED; +// } +// } +// break; +// + /* preflight calibration */ + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { + bool handled = false; + + /* gyro calibration */ + if ((int)(cmd->param1) == 1) { + /* transition to calibration state */ + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); + + if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + mavlink_log_info(mavlink_fd, "starting gyro cal"); + tune_confirm(); + do_gyro_calibration(status_pub, ¤t_status); + mavlink_log_info(mavlink_fd, "finished gyro cal"); + tune_confirm(); + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + mavlink_log_critical(mavlink_fd, "REJECTING gyro cal"); + result = VEHICLE_CMD_RESULT_DENIED; + } + + handled = true; + } + + /* magnetometer calibration */ + if ((int)(cmd->param2) == 1) { + /* transition to calibration state */ + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); + + if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + mavlink_log_info(mavlink_fd, "starting mag cal"); + tune_confirm(); + do_mag_calibration(status_pub, ¤t_status); + mavlink_log_info(mavlink_fd, "finished mag cal"); + tune_confirm(); + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + mavlink_log_critical(mavlink_fd, "REJECTING mag cal"); + result = VEHICLE_CMD_RESULT_DENIED; + } + + handled = true; + } + + /* zero-altitude pressure calibration */ + if ((int)(cmd->param3) == 1) { + /* transition to calibration state */ + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); + + if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented"); + tune_confirm(); + + } else { + mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration"); + result = VEHICLE_CMD_RESULT_DENIED; + } + + handled = true; + } + + /* trim calibration */ + if ((int)(cmd->param4) == 1) { + /* transition to calibration state */ + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); + + if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + mavlink_log_info(mavlink_fd, "starting trim cal"); + tune_confirm(); + do_rc_calibration(status_pub, ¤t_status); + mavlink_log_info(mavlink_fd, "finished trim cal"); + tune_confirm(); + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + mavlink_log_critical(mavlink_fd, "REJECTING trim cal"); + result = VEHICLE_CMD_RESULT_DENIED; + } + + handled = true; + } + + /* accel calibration */ + if ((int)(cmd->param5) == 1) { + /* transition to calibration state */ + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); + + if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + mavlink_log_info(mavlink_fd, "CMD starting accel cal"); + tune_confirm(); + do_accel_calibration(status_pub, ¤t_status, mavlink_fd); + tune_confirm(); + mavlink_log_info(mavlink_fd, "CMD finished accel cal"); + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + mavlink_log_critical(mavlink_fd, "REJECTING accel cal"); + result = VEHICLE_CMD_RESULT_DENIED; + } + + handled = true; + } + + /* airspeed calibration */ + if ((int)(cmd->param6) == 1) { //xxx: this is not defined by the mavlink protocol + /* transition to calibration state */ + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); + + if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + mavlink_log_info(mavlink_fd, "CMD starting airspeed cal"); + tune_confirm(); + do_airspeed_calibration(status_pub, ¤t_status); + tune_confirm(); + mavlink_log_info(mavlink_fd, "CMD finished airspeed cal"); + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + mavlink_log_critical(mavlink_fd, "REJECTING airspeed cal"); + result = VEHICLE_CMD_RESULT_DENIED; + } + + handled = true; + } + + /* none found */ + if (!handled) { + //warnx("refusing unsupported calibration request\n"); + mavlink_log_critical(mavlink_fd, "CMD refusing unsup. calib. request"); + result = VEHICLE_CMD_RESULT_UNSUPPORTED; + } + } + break; + + case VEHICLE_CMD_PREFLIGHT_STORAGE: { + if (current_status.flag_system_armed && + ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) { + /* do not perform expensive memory tasks on multirotors in flight */ + // XXX this is over-safe, as soon as cmd is in low prio thread this can be allowed + mavlink_log_info(mavlink_fd, "REJECTING save cmd while multicopter armed"); + + } else { + + // XXX move this to LOW PRIO THREAD of commander app + /* Read all parameters from EEPROM to RAM */ + + if (((int)(cmd->param1)) == 0) { + + /* read all parameters from EEPROM to RAM */ + int read_ret = param_load_default(); + + if (read_ret == OK) { + //warnx("[mavlink pm] Loaded EEPROM params in RAM\n"); + mavlink_log_info(mavlink_fd, "OK loading params from"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else if (read_ret == 1) { + mavlink_log_info(mavlink_fd, "OK no changes in"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + if (read_ret < -1) { + mavlink_log_info(mavlink_fd, "ERR loading params from"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + + } else { + mavlink_log_info(mavlink_fd, "ERR no param file named"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + } + + result = VEHICLE_CMD_RESULT_FAILED; + } + + } else if (((int)(cmd->param1)) == 1) { + + /* write all parameters from RAM to EEPROM */ + int write_ret = param_save_default(); + + if (write_ret == OK) { + mavlink_log_info(mavlink_fd, "OK saved param file"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + if (write_ret < -1) { + mavlink_log_info(mavlink_fd, "ERR params file does not exit:"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + + } else { + mavlink_log_info(mavlink_fd, "ERR writing params to"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + } + + result = VEHICLE_CMD_RESULT_FAILED; + } + + } else { + mavlink_log_info(mavlink_fd, "[pm] refusing unsupp. STOR request"); + result = VEHICLE_CMD_RESULT_UNSUPPORTED; + } + } + } + break; + + default: { + mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command"); + result = VEHICLE_CMD_RESULT_UNSUPPORTED; + /* announce command rejection */ + ioctl(buzzer, TONE_SET_ALARM, 4); + } + break; + } + + /* supported command handling stop */ + if (result == VEHICLE_CMD_RESULT_FAILED || + result == VEHICLE_CMD_RESULT_DENIED || + result == VEHICLE_CMD_RESULT_UNSUPPORTED) { + ioctl(buzzer, TONE_SET_ALARM, 5); + + } else if (result == VEHICLE_CMD_RESULT_ACCEPTED) { + tune_confirm(); + } + + /* send any requested ACKs */ + if (cmd->confirmation > 0) { + /* send acknowledge command */ + // XXX TODO + } + +} + +static void *orb_receive_loop(void *arg) //handles status information coming from subsystems (present, enabled, health), these values do not indicate the quality (variance) of the signal +{ + /* Set thread name */ + prctl(PR_SET_NAME, "commander orb rcv", getpid()); + + /* Subscribe to command topic */ + int subsys_sub = orb_subscribe(ORB_ID(subsystem_info)); + struct subsystem_info_s info; + + struct vehicle_status_s *vstatus = (struct vehicle_status_s *)arg; + + while (!thread_should_exit) { + struct pollfd fds[1] = { { .fd = subsys_sub, .events = POLLIN } }; + + if (poll(fds, 1, 5000) == 0) { + /* timeout, but this is no problem, silently ignore */ + } else { + /* got command */ + orb_copy(ORB_ID(subsystem_info), subsys_sub, &info); + + warnx("Subsys changed: %d\n", (int)info.subsystem_type); + + /* mark / unmark as present */ + if (info.present) { + vstatus->onboard_control_sensors_present |= info.subsystem_type; + + } else { + vstatus->onboard_control_sensors_present &= ~info.subsystem_type; + } + + /* mark / unmark as enabled */ + if (info.enabled) { + vstatus->onboard_control_sensors_enabled |= info.subsystem_type; + + } else { + vstatus->onboard_control_sensors_enabled &= ~info.subsystem_type; + } + + /* mark / unmark as ok */ + if (info.ok) { + vstatus->onboard_control_sensors_health |= info.subsystem_type; + + } else { + vstatus->onboard_control_sensors_health &= ~info.subsystem_type; + } + } + } + + close(subsys_sub); + + return NULL; +} + +/* + * Provides a coarse estimate of remaining battery power. + * + * The estimate is very basic and based on decharging voltage curves. + * + * @return the estimated remaining capacity in 0..1 + */ +float battery_remaining_estimate_voltage(float voltage); + +PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f); +PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f); +PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3); + +float battery_remaining_estimate_voltage(float voltage) +{ + float ret = 0; + static param_t bat_volt_empty; + static param_t bat_volt_full; + static param_t bat_n_cells; + static bool initialized = false; + static unsigned int counter = 0; + static float ncells = 3; + // XXX change cells to int (and param to INT32) + + if (!initialized) { + bat_volt_empty = param_find("BAT_V_EMPTY"); + bat_volt_full = param_find("BAT_V_FULL"); + bat_n_cells = param_find("BAT_N_CELLS"); + initialized = true; + } + + static float chemistry_voltage_empty = 3.2f; + static float chemistry_voltage_full = 4.05f; + + if (counter % 100 == 0) { + param_get(bat_volt_empty, &chemistry_voltage_empty); + param_get(bat_volt_full, &chemistry_voltage_full); + param_get(bat_n_cells, &ncells); + } + + counter++; + + ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty)); + + /* limit to sane values */ + ret = (ret < 0) ? 0 : ret; + ret = (ret > 1) ? 1 : ret; + return ret; +} + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + + fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n"); + exit(1); +} + +/** + * The daemon 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 commander_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("commander already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + daemon_task = task_spawn_cmd("commander", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 40, + 3000, + commander_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("\tcommander is running\n"); + + } else { + warnx("\tcommander not started\n"); + } + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +int commander_thread_main(int argc, char *argv[]) +{ + /* not yet initialized */ + commander_initialized = false; + bool home_position_set = false; + + /* set parameters */ + failsafe_lowlevel_timeout_ms = 0; + param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms); + + param_t _param_sys_type = param_find("MAV_TYPE"); + param_t _param_system_id = param_find("MAV_SYS_ID"); + param_t _param_component_id = param_find("MAV_COMP_ID"); + + /* welcome user */ + warnx("I am in command now!\n"); + + /* pthreads for command and subsystem info handling */ + // pthread_t command_handling_thread; + pthread_t subsystem_info_thread; + + /* initialize */ + if (led_init() != 0) { + warnx("ERROR: Failed to initialize leds\n"); + } + + if (buzzer_init() != 0) { + warnx("ERROR: Failed to initialize buzzer\n"); + } + + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + if (mavlink_fd < 0) { + warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.\n"); + } + + /* make sure we are in preflight state */ + memset(¤t_status, 0, sizeof(current_status)); + current_status.state_machine = SYSTEM_STATE_PREFLIGHT; + current_status.flag_system_armed = false; + /* neither manual nor offboard control commands have been received */ + current_status.offboard_control_signal_found_once = false; + current_status.rc_signal_found_once = false; + /* mark all signals lost as long as they haven't been found */ + current_status.rc_signal_lost = true; + current_status.offboard_control_signal_lost = true; + /* allow manual override initially */ + current_status.flag_external_manual_override_ok = true; + /* flag position info as bad, do not allow auto mode */ + current_status.flag_vector_flight_mode_ok = false; + /* set battery warning flag */ + current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; + + /* advertise to ORB */ + stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); + /* publish current state machine */ + state_machine_publish(stat_pub, ¤t_status, mavlink_fd); + + /* home position */ + orb_advert_t home_pub = -1; + struct home_position_s home; + memset(&home, 0, sizeof(home)); + + if (stat_pub < 0) { + warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n"); + warnx("exiting."); + exit(ERROR); + } + + mavlink_log_info(mavlink_fd, "system is running"); + + /* create pthreads */ + pthread_attr_t subsystem_info_attr; + pthread_attr_init(&subsystem_info_attr); + pthread_attr_setstacksize(&subsystem_info_attr, 2048); + pthread_create(&subsystem_info_thread, &subsystem_info_attr, orb_receive_loop, ¤t_status); + + /* Start monitoring loop */ + uint16_t counter = 0; + uint8_t flight_env; + + /* Initialize to 0.0V */ + float battery_voltage = 0.0f; + bool battery_voltage_valid = false; + bool low_battery_voltage_actions_done = false; + bool critical_battery_voltage_actions_done = false; + uint8_t low_voltage_counter = 0; + uint16_t critical_voltage_counter = 0; + int16_t mode_switch_rc_value; + float bat_remain = 1.0f; + + uint16_t stick_off_counter = 0; + uint16_t stick_on_counter = 0; + + /* Subscribe to manual control data */ + int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + struct manual_control_setpoint_s sp_man; + memset(&sp_man, 0, sizeof(sp_man)); + + /* Subscribe to offboard control data */ + int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); + struct offboard_control_setpoint_s sp_offboard; + memset(&sp_offboard, 0, sizeof(sp_offboard)); + + 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)); + uint64_t last_global_position_time = 0; + + int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + struct vehicle_local_position_s local_position; + memset(&local_position, 0, sizeof(local_position)); + uint64_t last_local_position_time = 0; + + /* + * The home position is set based on GPS only, to prevent a dependency between + * position estimator and commander. RAW GPS is more than good enough for a + * non-flying vehicle. + */ + int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + struct vehicle_gps_position_s gps_position; + memset(&gps_position, 0, sizeof(gps_position)); + + int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + struct sensor_combined_s sensors; + memset(&sensors, 0, sizeof(sensors)); + + int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); + struct differential_pressure_s diff_pres; + memset(&diff_pres, 0, sizeof(diff_pres)); + uint64_t last_diff_pres_time = 0; + + /* Subscribe to command topic */ + int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); + struct vehicle_command_s cmd; + memset(&cmd, 0, sizeof(cmd)); + + /* Subscribe to parameters changed topic */ + int param_changed_sub = orb_subscribe(ORB_ID(parameter_update)); + struct parameter_update_s param_changed; + memset(¶m_changed, 0, sizeof(param_changed)); + + /* subscribe to battery topic */ + int battery_sub = orb_subscribe(ORB_ID(battery_status)); + struct battery_status_s battery; + memset(&battery, 0, sizeof(battery)); + battery.voltage_v = 0.0f; + + // uint8_t vehicle_state_previous = current_status.state_machine; + float voltage_previous = 0.0f; + + uint64_t last_idle_time = 0; + + /* now initialized */ + commander_initialized = true; + thread_running = true; + + uint64_t start_time = hrt_absolute_time(); + uint64_t failsave_ll_start_time = 0; + + bool state_changed = true; + bool param_init_forced = true; + + while (!thread_should_exit) { + + /* Get current values */ + bool new_data; + orb_check(sp_man_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); + } + + orb_check(sp_offboard_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); + } + + orb_check(sensor_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); + } + + orb_check(diff_pres_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); + last_diff_pres_time = diff_pres.timestamp; + } + + orb_check(cmd_sub, &new_data); + + if (new_data) { + /* got command */ + orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); + + /* handle it */ + handle_command(stat_pub, ¤t_status, &cmd); + } + + /* update parameters */ + orb_check(param_changed_sub, &new_data); + + if (new_data || param_init_forced) { + param_init_forced = false; + /* parameters changed */ + orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); + + + /* update parameters */ + if (!current_status.flag_system_armed) { + if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { + warnx("failed setting new system type"); + } + + /* disable manual override for all systems that rely on electronic stabilization */ + if (current_status.system_type == VEHICLE_TYPE_QUADROTOR || + current_status.system_type == VEHICLE_TYPE_HEXAROTOR || + current_status.system_type == VEHICLE_TYPE_OCTOROTOR) { + current_status.flag_external_manual_override_ok = false; + + } else { + current_status.flag_external_manual_override_ok = true; + } + + /* check and update system / component ID */ + param_get(_param_system_id, &(current_status.system_id)); + param_get(_param_component_id, &(current_status.component_id)); + + } + } + + /* update global position estimate */ + orb_check(global_position_sub, &new_data); + + if (new_data) { + /* position changed */ + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); + last_global_position_time = global_position.timestamp; + } + + /* update local position estimate */ + orb_check(local_position_sub, &new_data); + + if (new_data) { + /* position changed */ + orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); + last_local_position_time = local_position.timestamp; + } + + /* update battery status */ + orb_check(battery_sub, &new_data); + if (new_data) { + orb_copy(ORB_ID(battery_status), battery_sub, &battery); + battery_voltage = battery.voltage_v; + battery_voltage_valid = true; + + /* + * Only update battery voltage estimate if system has + * been running for two and a half seconds. + */ + if (hrt_absolute_time() - start_time > 2500000) { + bat_remain = battery_remaining_estimate_voltage(battery_voltage); + } + } + + /* Slow but important 8 Hz checks */ + if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) { + /* toggle activity (blue) led at 1 Hz in standby, 10 Hz in armed mode */ + if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY || + current_status.state_machine == SYSTEM_STATE_AUTO || + current_status.state_machine == SYSTEM_STATE_MANUAL)) { + /* armed, solid */ + led_on(LED_AMBER); + + } else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + /* not armed */ + led_toggle(LED_AMBER); + } + + if (hrt_absolute_time() - gps_position.timestamp_position < 2000000) { + + /* toggle GPS (blue) led at 1 Hz if GPS present but no lock, make is solid once locked */ + if ((hrt_absolute_time() - gps_position.timestamp_position < 2000000) + && (gps_position.fix_type == GPS_FIX_TYPE_3D)) { + /* GPS lock */ + led_on(LED_BLUE); + + } else if ((counter + 4) % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + /* no GPS lock, but GPS module is aquiring lock */ + led_toggle(LED_BLUE); + } + + } else { + /* no GPS info, don't light the blue led */ + led_off(LED_BLUE); + } + + /* toggle GPS led at 5 Hz in HIL mode */ + if (current_status.flag_hil_enabled) { + /* hil enabled */ + led_toggle(LED_BLUE); + + } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { + /* toggle arming (red) at 5 Hz on low battery or error */ + led_toggle(LED_AMBER); + + } else { + // /* Constant error indication in standby mode without GPS */ + // if (!current_status.gps_valid) { + // led_on(LED_AMBER); + + // } else { + // led_off(LED_AMBER); + // } + } + + 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) + current_status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle + + last_idle_time = system_load.tasks[0].total_runtime; + } + } + + // // XXX Export patterns and threshold to parameters + /* Trigger audio event for low battery */ + if (bat_remain < 0.1f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 4) == 0)) { + /* For less than 10%, start be really annoying at 5 Hz */ + ioctl(buzzer, TONE_SET_ALARM, 0); + ioctl(buzzer, TONE_SET_ALARM, 3); + + } else if (bat_remain < 0.1f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 4) == 2)) { + ioctl(buzzer, TONE_SET_ALARM, 0); + + } else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 0)) { + /* For less than 20%, start be slightly annoying at 1 Hz */ + ioctl(buzzer, TONE_SET_ALARM, 0); + tune_confirm(); + + } else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 2)) { + ioctl(buzzer, TONE_SET_ALARM, 0); + } + + /* Check battery voltage */ + /* write to sys_status */ + if (battery_voltage_valid) { + current_status.voltage_battery = battery_voltage; + + } else { + current_status.voltage_battery = 0.0f; + } + + /* if battery voltage is getting lower, warn using buzzer, etc. */ + if (battery_voltage_valid && (bat_remain < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS + + if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { + low_battery_voltage_actions_done = true; + mavlink_log_critical(mavlink_fd, "[cmd] WARNING! LOW BATTERY!"); + current_status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING; + } + + low_voltage_counter++; + } + + /* Critical, this is rather an emergency, kill signal to sdlog and change state machine */ + else if (battery_voltage_valid && (bat_remain < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { + if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { + critical_battery_voltage_actions_done = true; + mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!"); + current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; + state_machine_emergency(stat_pub, ¤t_status, mavlink_fd); + } + + critical_voltage_counter++; + + } else { + low_voltage_counter = 0; + critical_voltage_counter = 0; + } + + /* End battery voltage check */ + + + /* + * Check for valid position information. + * + * If the system has a valid position source from an onboard + * position estimator, it is safe to operate it autonomously. + * The flag_vector_flight_mode_ok flag indicates that a minimum + * set of position measurements is available. + */ + + /* store current state to reason later about a state change */ + bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; + bool global_pos_valid = current_status.flag_global_position_valid; + bool local_pos_valid = current_status.flag_local_position_valid; + bool airspeed_valid = current_status.flag_airspeed_valid; + + /* check for global or local position updates, set a timeout of 2s */ + if (hrt_absolute_time() - last_global_position_time < 2000000) { + current_status.flag_global_position_valid = true; + // XXX check for controller status and home position as well + + } else { + current_status.flag_global_position_valid = false; + } + + if (hrt_absolute_time() - last_local_position_time < 2000000) { + current_status.flag_local_position_valid = true; + // XXX check for controller status and home position as well + + } else { + current_status.flag_local_position_valid = false; + } + + /* Check for valid airspeed/differential pressure measurements */ + if (hrt_absolute_time() - last_diff_pres_time < 2000000) { + current_status.flag_airspeed_valid = true; + + } else { + current_status.flag_airspeed_valid = false; + } + + /* + * Consolidate global position and local position valid flags + * for vector flight mode. + */ + if (current_status.flag_local_position_valid || + current_status.flag_global_position_valid) { + current_status.flag_vector_flight_mode_ok = true; + + } else { + current_status.flag_vector_flight_mode_ok = false; + } + + /* consolidate state change, flag as changed if required */ + if (vector_flight_mode_ok != current_status.flag_vector_flight_mode_ok || + global_pos_valid != current_status.flag_global_position_valid || + local_pos_valid != current_status.flag_local_position_valid || + airspeed_valid != current_status.flag_airspeed_valid) { + state_changed = true; + } + + /* + * Mark the position of the first position lock as return to launch (RTL) + * position. The MAV will return here on command or emergency. + * + * Conditions: + * + * 1) The system aquired position lock just now + * 2) The system has not aquired position lock before + * 3) The system is not armed (on the ground) + */ + if (!current_status.flag_valid_launch_position && + !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok && + !current_status.flag_system_armed) { + /* first time a valid position, store it and emit it */ + + // XXX implement storage and publication of RTL position + current_status.flag_valid_launch_position = true; + current_status.flag_auto_flight_mode_ok = true; + state_changed = true; + } + + if (orb_check(ORB_ID(vehicle_gps_position), &new_data)) { + + orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); + + /* check for first, long-term and valid GPS lock -> set home position */ + float hdop_m = gps_position.eph_m; + float vdop_m = gps_position.epv_m; + + /* check if gps fix is ok */ + // 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. + */ + + if (gps_position.fix_type == GPS_FIX_TYPE_3D + && (hdop_m < hdop_threshold_m) + && (vdop_m < vdop_threshold_m) + && !home_position_set + && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) + && !current_status.flag_system_armed) { + warnx("setting home position"); + + /* copy position data to uORB home message, store it locally as well */ + home.lat = gps_position.lat; + home.lon = gps_position.lon; + home.alt = gps_position.alt; + + home.eph_m = gps_position.eph_m; + home.epv_m = gps_position.epv_m; + + home.s_variance_m_s = gps_position.s_variance_m_s; + home.p_variance_m = gps_position.p_variance_m; + + /* 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 */ + home_position_set = true; + tune_confirm(); + } + } + + /* ignore RC signals if in offboard control mode */ + if (!current_status.offboard_control_signal_found_once && sp_man.timestamp != 0) { + /* Start RC state check */ + + if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { + + // /* + // * Check if manual control modes have to be switched + // */ + // if (!isfinite(sp_man.manual_mode_switch)) { + // warnx("man mode sw not finite\n"); + + // /* this switch is not properly mapped, set default */ + // if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { + + // /* assuming a rotary wing, fall back to SAS */ + // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; + // current_status.flag_control_attitude_enabled = true; + // current_status.flag_control_rates_enabled = true; + // } else { + + // /* assuming a fixed wing, fall back to direct pass-through */ + // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; + // current_status.flag_control_attitude_enabled = false; + // current_status.flag_control_rates_enabled = false; + // } + + // } else if (sp_man.manual_mode_switch < -STICK_ON_OFF_LIMIT) { + + // /* bottom stick position, set direct mode for vehicles supporting it */ + // if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { + + // /* assuming a rotary wing, fall back to SAS */ + // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; + // current_status.flag_control_attitude_enabled = true; + // current_status.flag_control_rates_enabled = true; + // } else { + + // /* assuming a fixed wing, set to direct pass-through as requested */ + // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; + // current_status.flag_control_attitude_enabled = false; + // current_status.flag_control_rates_enabled = false; + // } + + // } else if (sp_man.manual_mode_switch > STICK_ON_OFF_LIMIT) { + + // /* top stick position, set SAS for all vehicle types */ + // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; + // current_status.flag_control_attitude_enabled = true; + // current_status.flag_control_rates_enabled = true; + + // } else { + // /* center stick position, set rate control */ + // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_RATES; + // current_status.flag_control_attitude_enabled = false; + // current_status.flag_control_rates_enabled = true; + // } + + // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); + + /* + * Check if manual stability control modes have to be switched + */ + if (!isfinite(sp_man.manual_sas_switch)) { + + /* this switch is not properly mapped, set default */ + current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; + + } else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) { + + /* bottom stick position, set altitude hold */ + current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE; + + } else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) { + + /* top stick position */ + current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_SIMPLE; + + } else { + /* center stick position, set default */ + current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; + } + + /* + * Check if left stick is in lower left position --> switch to standby state. + * Do this only for multirotors, not for fixed wing aircraft. + */ + if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) + ) && + ((sp_man.yaw < -STICK_ON_OFF_LIMIT)) && + (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { + if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { + update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); + stick_on_counter = 0; + + } else { + stick_off_counter++; + stick_on_counter = 0; + } + } + + /* check if left stick is in lower right position --> arm */ + if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) { + if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { + update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); + stick_on_counter = 0; + + } else { + stick_on_counter++; + stick_off_counter = 0; + } + } + + /* check manual override switch - switch to manual or auto mode */ + if (sp_man.manual_override_switch > STICK_ON_OFF_LIMIT) { + /* enable manual override */ + update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); + + } else if (sp_man.manual_override_switch < -STICK_ON_OFF_LIMIT) { + // /* check auto mode switch for correct mode */ + // if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) { + // /* enable guided mode */ + // update_state_machine_mode_guided(stat_pub, ¤t_status, mavlink_fd); + + // } else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) { + // XXX hardcode to auto for now + update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); + + // } + + } else { + /* center stick position, set SAS for all vehicle types */ + update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); + } + + /* handle the case where RC signal was regained */ + if (!current_status.rc_signal_found_once) { + current_status.rc_signal_found_once = true; + mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME."); + + } else { + if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); + } + + current_status.rc_signal_cutting_off = false; + current_status.rc_signal_lost = false; + current_status.rc_signal_lost_interval = 0; + + } else { + static uint64_t last_print_time = 0; + + /* print error message for first RC glitch and then every 5 s / 5000 ms) */ + if (!current_status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) { + /* only complain if the offboard control is NOT active */ + current_status.rc_signal_cutting_off = true; + mavlink_log_critical(mavlink_fd, "CRITICAL - NO REMOTE SIGNAL!"); + last_print_time = hrt_absolute_time(); + } + + /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ + current_status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp; + + /* if the RC signal is gone for a full second, consider it lost */ + if (current_status.rc_signal_lost_interval > 1000000) { + current_status.rc_signal_lost = true; + current_status.failsave_lowlevel = true; + state_changed = true; + } + + // if (hrt_absolute_time() - current_status.failsave_ll_start_time > failsafe_lowlevel_timeout_ms*1000) { + // publish_armed_status(¤t_status); + // } + } + } + + + + + /* End mode switch */ + + /* END RC state check */ + + + /* State machine update for offboard control */ + if (!current_status.rc_signal_found_once && sp_offboard.timestamp != 0) { + if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) { + + /* decide about attitude control flag, enable in att/pos/vel */ + bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || + sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || + sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); + + /* decide about rate control flag, enable it always XXX (for now) */ + bool rates_ctrl_enabled = true; + + /* set up control mode */ + if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) { + current_status.flag_control_attitude_enabled = attitude_ctrl_enabled; + state_changed = true; + } + + if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) { + current_status.flag_control_rates_enabled = rates_ctrl_enabled; + state_changed = true; + } + + /* handle the case where offboard control signal was regained */ + if (!current_status.offboard_control_signal_found_once) { + current_status.offboard_control_signal_found_once = true; + /* enable offboard control, disable manual input */ + current_status.flag_control_manual_enabled = false; + current_status.flag_control_offboard_enabled = true; + state_changed = true; + tune_confirm(); + + mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST"); + + } else { + if (current_status.offboard_control_signal_lost) { + mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL"); + state_changed = true; + tune_confirm(); + } + } + + current_status.offboard_control_signal_weak = false; + current_status.offboard_control_signal_lost = false; + current_status.offboard_control_signal_lost_interval = 0; + + /* arm / disarm on request */ + if (sp_offboard.armed && !current_status.flag_system_armed) { + update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); + /* switch to stabilized mode = takeoff */ + update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); + + } else if (!sp_offboard.armed && current_status.flag_system_armed) { + update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); + } + + } else { + static uint64_t last_print_time = 0; + + /* print error message for first RC glitch and then every 5 s / 5000 ms) */ + if (!current_status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) { + current_status.offboard_control_signal_weak = true; + mavlink_log_critical(mavlink_fd, "CRIT:NO OFFBOARD CONTROL!"); + last_print_time = hrt_absolute_time(); + } + + /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ + current_status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp; + + /* if the signal is gone for 0.1 seconds, consider it lost */ + if (current_status.offboard_control_signal_lost_interval > 100000) { + current_status.offboard_control_signal_lost = true; + current_status.failsave_lowlevel_start_time = hrt_absolute_time(); + tune_confirm(); + + /* kill motors after timeout */ + if (hrt_absolute_time() - current_status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) { + current_status.failsave_lowlevel = true; + state_changed = true; + } + } + } + } + + + current_status.counter++; + current_status.timestamp = hrt_absolute_time(); + + + /* If full run came back clean, transition to standby */ + if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT && + current_status.flag_preflight_gyro_calibration == false && + current_status.flag_preflight_mag_calibration == false && + current_status.flag_preflight_accel_calibration == false) { + /* All ok, no calibration going on, go to standby */ + do_state_update(stat_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + } + + /* publish at least with 1 Hz */ + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) { + publish_armed_status(¤t_status); + orb_publish(ORB_ID(vehicle_status), stat_pub, ¤t_status); + state_changed = false; + } + + /* Store old modes to detect and act on state transitions */ + voltage_previous = current_status.voltage_battery; + + fflush(stdout); + counter++; + usleep(COMMANDER_MONITORING_INTERVAL); + } + + /* wait for threads to complete */ + // pthread_join(command_handling_thread, NULL); + pthread_join(subsystem_info_thread, NULL); + + /* close fds */ + led_deinit(); + buzzer_deinit(); + close(sp_man_sub); + close(sp_offboard_sub); + close(global_position_sub); + close(sensor_sub); + close(cmd_sub); + + warnx("exiting..\n"); + fflush(stdout); + + thread_running = false; + + return 0; +} diff --git a/src/modules/commander/commander.h b/src/modules/commander/commander.h new file mode 100644 index 000000000..04b4e72ab --- /dev/null +++ b/src/modules/commander/commander.h @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch> + * Lorenz Meier <lm@inf.ethz.ch> + * Thomas Gubler <thomasgubler@student.ethz.ch> + * 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 + * 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 commander.h + * Main system state machine definition. + * + * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * + */ + +#ifndef COMMANDER_H_ +#define COMMANDER_H_ + +#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f +#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f + +void tune_confirm(void); +void tune_error(void); + +#endif /* COMMANDER_H_ */ diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk new file mode 100644 index 000000000..fe44e955a --- /dev/null +++ b/src/modules/commander/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Main system state machine +# + +MODULE_COMMAND = commander +SRCS = commander.c \ + state_machine_helper.c \ + calibration_routines.c \ + accelerometer_calibration.c + diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c new file mode 100644 index 000000000..ab728c7bb --- /dev/null +++ b/src/modules/commander/state_machine_helper.c @@ -0,0 +1,757 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler <thomasgubler@student.ethz.ch> + * 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 + * 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 state_machine_helper.c + * State machine helper functions implementations + */ + +#include <stdio.h> +#include <unistd.h> + +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/actuator_controls.h> +#include <systemlib/systemlib.h> +#include <drivers/drv_hrt.h> +#include <mavlink/mavlink_log.h> + +#include "state_machine_helper.h" + +static const char *system_state_txt[] = { + "SYSTEM_STATE_PREFLIGHT", + "SYSTEM_STATE_STANDBY", + "SYSTEM_STATE_GROUND_READY", + "SYSTEM_STATE_MANUAL", + "SYSTEM_STATE_STABILIZED", + "SYSTEM_STATE_AUTO", + "SYSTEM_STATE_MISSION_ABORT", + "SYSTEM_STATE_EMCY_LANDING", + "SYSTEM_STATE_EMCY_CUTOFF", + "SYSTEM_STATE_GROUND_ERROR", + "SYSTEM_STATE_REBOOT", + +}; + +/** + * Transition from one state to another + */ +int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state) +{ + int invalid_state = false; + int ret = ERROR; + + commander_state_machine_t old_state = current_status->state_machine; + + switch (new_state) { + case SYSTEM_STATE_MISSION_ABORT: { + /* Indoor or outdoor */ + // if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { + ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING); + + // } else { + // ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF); + // } + } + break; + + case SYSTEM_STATE_EMCY_LANDING: + /* Tell the controller to land */ + + /* set system flags according to state */ + current_status->flag_system_armed = true; + + warnx("EMERGENCY LANDING!\n"); + mavlink_log_critical(mavlink_fd, "EMERGENCY LANDING!"); + break; + + case SYSTEM_STATE_EMCY_CUTOFF: + /* Tell the controller to cutoff the motors (thrust = 0) */ + + /* set system flags according to state */ + current_status->flag_system_armed = false; + + warnx("EMERGENCY MOTOR CUTOFF!\n"); + mavlink_log_critical(mavlink_fd, "EMERGENCY MOTOR CUTOFF!"); + break; + + case SYSTEM_STATE_GROUND_ERROR: + + /* set system flags according to state */ + + /* prevent actuators from arming */ + current_status->flag_system_armed = false; + + warnx("GROUND ERROR, locking down propulsion system\n"); + mavlink_log_critical(mavlink_fd, "GROUND ERROR, locking down system"); + break; + + case SYSTEM_STATE_PREFLIGHT: + if (current_status->state_machine == SYSTEM_STATE_STANDBY + || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { + /* set system flags according to state */ + current_status->flag_system_armed = false; + mavlink_log_critical(mavlink_fd, "Switched to PREFLIGHT state"); + + } else { + invalid_state = true; + mavlink_log_critical(mavlink_fd, "REFUSED to switch to PREFLIGHT state"); + } + + break; + + case SYSTEM_STATE_REBOOT: + if (current_status->state_machine == SYSTEM_STATE_STANDBY + || current_status->state_machine == SYSTEM_STATE_PREFLIGHT + || current_status->flag_hil_enabled) { + invalid_state = false; + /* set system flags according to state */ + current_status->flag_system_armed = false; + mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); + usleep(500000); + up_systemreset(); + /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ + + } else { + invalid_state = true; + mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT"); + } + + break; + + case SYSTEM_STATE_STANDBY: + /* set system flags according to state */ + + /* standby enforces disarmed */ + current_status->flag_system_armed = false; + + mavlink_log_critical(mavlink_fd, "Switched to STANDBY state"); + break; + + case SYSTEM_STATE_GROUND_READY: + + /* set system flags according to state */ + + /* ground ready has motors / actuators armed */ + current_status->flag_system_armed = true; + + mavlink_log_critical(mavlink_fd, "Switched to GROUND READY state"); + break; + + case SYSTEM_STATE_AUTO: + + /* set system flags according to state */ + + /* auto is airborne and in auto mode, motors armed */ + current_status->flag_system_armed = true; + + mavlink_log_critical(mavlink_fd, "Switched to FLYING / AUTO mode"); + break; + + case SYSTEM_STATE_STABILIZED: + + /* set system flags according to state */ + current_status->flag_system_armed = true; + + mavlink_log_critical(mavlink_fd, "Switched to FLYING / STABILIZED mode"); + break; + + case SYSTEM_STATE_MANUAL: + + /* set system flags according to state */ + current_status->flag_system_armed = true; + + mavlink_log_critical(mavlink_fd, "Switched to FLYING / MANUAL mode"); + break; + + default: + invalid_state = true; + break; + } + + if (invalid_state == false || old_state != new_state) { + current_status->state_machine = new_state; + state_machine_publish(status_pub, current_status, mavlink_fd); + publish_armed_status(current_status); + ret = OK; + } + + if (invalid_state) { + mavlink_log_critical(mavlink_fd, "REJECTING invalid state transition"); + ret = ERROR; + } + + return ret; +} + +void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + /* publish the new state */ + current_status->counter++; + current_status->timestamp = hrt_absolute_time(); + + /* assemble state vector based on flag values */ + if (current_status->flag_control_rates_enabled) { + current_status->onboard_control_sensors_present |= 0x400; + + } else { + current_status->onboard_control_sensors_present &= ~0x400; + } + + current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; + current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; + current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; + current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; + + current_status->onboard_control_sensors_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 0; + current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; + current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; + current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; + current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; + + orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + printf("[cmd] new state: %s\n", system_state_txt[current_status->state_machine]); +} + +void publish_armed_status(const struct vehicle_status_s *current_status) +{ + struct actuator_armed_s armed; + armed.armed = current_status->flag_system_armed; + + /* XXX allow arming by external components on multicopters only if not yet armed by RC */ + /* XXX allow arming only if core sensors are ok */ + armed.ready_to_arm = true; + + /* lock down actuators if required, only in HIL */ + armed.lockdown = (current_status->flag_hil_enabled) ? true : false; + orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); + orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); +} + + +/* + * Private functions, update the state machine + */ +void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + warnx("EMERGENCY HANDLER\n"); + /* Depending on the current state go to one of the error states */ + + if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) { + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR); + + } else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) { + + // DO NOT abort mission + //do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT); + + } else { + warnx("Unknown system state: #%d\n", current_status->state_machine); + } +} + +void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) //do not call state_machine_emergency_always_critical if we are in manual mode for these errors +{ + if (current_status->state_machine != SYSTEM_STATE_MANUAL) { //if we are in manual: user can react to errors themself + state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd); + + } else { + //global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL); + } + +} + + + +// /* +// * Wrapper functions (to be used in the commander), all functions assume lock on current_status +// */ + +// /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR +// * +// * START SUBSYSTEM/EMERGENCY FUNCTIONS +// * */ + +// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// current_status->onboard_control_sensors_present |= 1 << *subsystem_type; +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); +// } + +// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type); +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + +// /* if a subsystem was removed something went completely wrong */ + +// switch (*subsystem_type) { +// case SUBSYSTEM_TYPE_GYRO: +// //global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_ACC: +// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_MAG: +// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_GPS: +// { +// uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]; + +// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { +// //global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency(status_pub, current_status); +// } +// } +// break; + +// default: +// break; +// } + +// } + +// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type; +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); +// } + +// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type); +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + +// /* if a subsystem was disabled something went completely wrong */ + +// switch (*subsystem_type) { +// case SUBSYSTEM_TYPE_GYRO: +// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_ACC: +// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_MAG: +// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_GPS: +// { +// uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]); + +// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { +// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency(status_pub, current_status); +// } +// } +// break; + +// default: +// break; +// } + +// } + + +// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// current_status->onboard_control_sensors_health |= 1 << *subsystem_type; +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + +// switch (*subsystem_type) { +// case SUBSYSTEM_TYPE_GYRO: +// //TODO state machine change (recovering) +// break; + +// case SUBSYSTEM_TYPE_ACC: +// //TODO state machine change +// break; + +// case SUBSYSTEM_TYPE_MAG: +// //TODO state machine change +// break; + +// case SUBSYSTEM_TYPE_GPS: +// //TODO state machine change +// break; + +// default: +// break; +// } + + +// } + + +// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// bool previosly_healthy = (bool)(current_status->onboard_control_sensors_health & 1 << *subsystem_type); +// current_status->onboard_control_sensors_health &= ~(1 << *subsystem_type); +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + +// /* if we received unhealthy message more than *_HEALTH_COUNTER_LIMIT, switch to error state */ + +// switch (*subsystem_type) { +// case SUBSYSTEM_TYPE_GYRO: +// //global_data_send_mavlink_statustext_message_out("Commander: gyro unhealthy", MAV_SEVERITY_CRITICAL); + +// if (previosly_healthy) //only throw emergency if previously healthy +// state_machine_emergency_always_critical(status_pub, current_status); + +// break; + +// case SUBSYSTEM_TYPE_ACC: +// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer unhealthy", MAV_SEVERITY_CRITICAL); + +// if (previosly_healthy) //only throw emergency if previously healthy +// state_machine_emergency_always_critical(status_pub, current_status); + +// break; + +// case SUBSYSTEM_TYPE_MAG: +// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer unhealthy", MAV_SEVERITY_CRITICAL); + +// if (previosly_healthy) //only throw emergency if previously healthy +// state_machine_emergency_always_critical(status_pub, current_status); + +// break; + +// case SUBSYSTEM_TYPE_GPS: +// // //TODO: remove this block +// // break; +// // /////////////////// +// //global_data_send_mavlink_statustext_message_out("Commander: GPS unhealthy", MAV_SEVERITY_CRITICAL); + +// // printf("previosly_healthy = %u\n", previosly_healthy); +// if (previosly_healthy) //only throw emergency if previously healthy +// state_machine_emergency(status_pub, current_status); + +// break; + +// default: +// break; +// } + +// } + + +/* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ + + +void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + /* Depending on the current state switch state */ + if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); + } +} + +void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + /* Depending on the current state switch state */ + if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_AUTO) { + state_machine_emergency(status_pub, current_status, mavlink_fd); + } +} + +void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + if (current_status->state_machine == SYSTEM_STATE_STANDBY) { + printf("[cmd] arming\n"); + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); + } +} + +void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { + printf("[cmd] going standby\n"); + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); + + } else if (current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { + printf("[cmd] MISSION ABORT!\n"); + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); + } +} + +void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + int old_mode = current_status->flight_mode; + current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; + + current_status->flag_control_manual_enabled = true; + + /* set behaviour based on airframe */ + if ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status->system_type == VEHICLE_TYPE_OCTOROTOR)) { + + /* assuming a rotary wing, set to SAS */ + current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; + current_status->flag_control_attitude_enabled = true; + current_status->flag_control_rates_enabled = true; + + } else { + + /* assuming a fixed wing, set to direct pass-through */ + current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; + current_status->flag_control_attitude_enabled = false; + current_status->flag_control_rates_enabled = false; + } + + if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); + + if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { + printf("[cmd] manual mode\n"); + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); + } +} + +void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { + int old_mode = current_status->flight_mode; + int old_manual_control_mode = current_status->manual_control_mode; + current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; + current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; + current_status->flag_control_attitude_enabled = true; + current_status->flag_control_rates_enabled = true; + current_status->flag_control_manual_enabled = true; + + if (old_mode != current_status->flight_mode || + old_manual_control_mode != current_status->manual_control_mode) { + printf("[cmd] att stabilized mode\n"); + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); + state_machine_publish(status_pub, current_status, mavlink_fd); + } + + } +} + +void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + if (!current_status->flag_vector_flight_mode_ok) { + mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. GUIDED MODE"); + tune_error(); + return; + } + + if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { + printf("[cmd] position guided mode\n"); + int old_mode = current_status->flight_mode; + current_status->flight_mode = VEHICLE_FLIGHT_MODE_STAB; + current_status->flag_control_manual_enabled = false; + current_status->flag_control_attitude_enabled = true; + current_status->flag_control_rates_enabled = true; + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED); + + if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); + + } +} + +void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + if (!current_status->flag_vector_flight_mode_ok) { + mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. AUTO MODE"); + return; + } + + if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) { + printf("[cmd] auto mode\n"); + int old_mode = current_status->flight_mode; + current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO; + current_status->flag_control_manual_enabled = false; + current_status->flag_control_attitude_enabled = true; + current_status->flag_control_rates_enabled = true; + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO); + + if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); + } +} + + +uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) +{ + uint8_t ret = 1; + + /* Switch on HIL if in standby and not already in HIL mode */ + if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED) + && !current_status->flag_hil_enabled) { + if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) { + /* Enable HIL on request */ + current_status->flag_hil_enabled = true; + ret = OK; + state_machine_publish(status_pub, current_status, mavlink_fd); + publish_armed_status(current_status); + printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); + + } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && + current_status->flag_system_armed) { + + mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!") + + } else { + + mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.") + } + } + + /* switch manual / auto */ + if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { + update_state_machine_mode_auto(status_pub, current_status, mavlink_fd); + + } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) { + update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd); + + } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { + update_state_machine_mode_guided(status_pub, current_status, mavlink_fd); + + } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { + update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); + } + + /* vehicle is disarmed, mode requests arming */ + if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { + /* only arm in standby state */ + // XXX REMOVE + if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); + ret = OK; + printf("[cmd] arming due to command request\n"); + } + } + + /* vehicle is armed, mode requests disarming */ + if (current_status->flag_system_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { + /* only disarm in ground ready */ + if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); + ret = OK; + printf("[cmd] disarming due to command request\n"); + } + } + + /* NEVER actually switch off HIL without reboot */ + if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { + warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n"); + mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL"); + ret = ERROR; + } + + return ret; +} + +uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode) //TODO: add more checks to avoid state switching in critical situations +{ + commander_state_machine_t current_system_state = current_status->state_machine; + + uint8_t ret = 1; + + switch (custom_mode) { + case SYSTEM_STATE_GROUND_READY: + break; + + case SYSTEM_STATE_STANDBY: + break; + + case SYSTEM_STATE_REBOOT: + printf("try to reboot\n"); + + if (current_system_state == SYSTEM_STATE_STANDBY + || current_system_state == SYSTEM_STATE_PREFLIGHT + || current_status->flag_hil_enabled) { + printf("system will reboot\n"); + mavlink_log_critical(mavlink_fd, "Rebooting.."); + usleep(200000); + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_REBOOT); + ret = 0; + } + + break; + + case SYSTEM_STATE_AUTO: + printf("try to switch to auto/takeoff\n"); + + if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_MANUAL) { + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO); + printf("state: auto\n"); + ret = 0; + } + + break; + + case SYSTEM_STATE_MANUAL: + printf("try to switch to manual\n"); + + if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_AUTO) { + do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); + printf("state: manual\n"); + ret = 0; + } + + break; + + default: + break; + } + + return ret; +} + diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h new file mode 100644 index 000000000..2f2ccc729 --- /dev/null +++ b/src/modules/commander/state_machine_helper.h @@ -0,0 +1,209 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler <thomasgubler@student.ethz.ch> + * 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 + * 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 state_machine_helper.h + * State machine helper functions definitions + */ + +#ifndef STATE_MACHINE_HELPER_H_ +#define STATE_MACHINE_HELPER_H_ + +#define GPS_NOFIX_COUNTER_LIMIT 4 //need GPS_NOFIX_COUNTER_LIMIT gps packets with a bad fix to call an error (if outdoor) +#define GPS_GOTFIX_COUNTER_REQUIRED 4 //need GPS_GOTFIX_COUNTER_REQUIRED gps packets with a good fix to obtain position lock + +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_status.h> + +/** + * Switch to new state with no checking. + * + * do_state_update: this is the functions that all other functions have to call in order to update the state. + * the function does not question the state change, this must be done before + * The function performs actions that are connected with the new state (buzzer, reboot, ...) + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + * + * @return 0 (macro OK) or 1 on error (macro ERROR) + */ +int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state); + +/* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR */ +// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); +// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); + +// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); +// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); + +// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); +// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); + + +/** + * Handle state machine if got position fix + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + +/** + * Handle state machine if position fix lost + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + +/** + * Handle state machine if user wants to arm + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + +/** + * Handle state machine if user wants to disarm + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + +/** + * Handle state machine if mode switch is manual + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + +/** + * Handle state machine if mode switch is stabilized + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + +/** + * Handle state machine if mode switch is guided + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + +/** + * Handle state machine if mode switch is auto + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + +/** + * Publish current state information + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + + +/* + * Functions that handle incoming requests to change the state machine or a parameter (probably from the mavlink app). + * If the request is obeyed the functions return 0 + * + */ + +/** + * Handles *incoming request* to switch to a specific state, if state change is successful returns 0 + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode); + +/** + * Handles *incoming request* to switch to a specific custom state, if state change is successful returns 0 + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode); + +/** + * Always switches to critical mode under any circumstances. + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + +/** + * Switches to emergency if required. + * + * @param status_pub file descriptor for state update topic publication + * @param current_status pointer to the current state machine to operate on + * @param mavlink_fd file descriptor for MAVLink statustext messages + */ +void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + +/** + * Publish the armed state depending on the current system state + * + * @param current_status the current system status + */ +void publish_armed_status(const struct vehicle_status_s *current_status); + + + +#endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/modules/controllib/block/Block.cpp b/src/modules/controllib/block/Block.cpp new file mode 100644 index 000000000..5994d2315 --- /dev/null +++ b/src/modules/controllib/block/Block.cpp @@ -0,0 +1,210 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Block.cpp + * + * Controller library code + */ + +#include <math.h> +#include <string.h> +#include <stdio.h> + +#include "Block.hpp" +#include "BlockParam.hpp" +#include "UOrbSubscription.hpp" +#include "UOrbPublication.hpp" + +namespace control +{ + +Block::Block(SuperBlock *parent, const char *name) : + _name(name), + _parent(parent), + _dt(0), + _subscriptions(), + _params() +{ + if (getParent() != NULL) { + getParent()->getChildren().add(this); + } +} + +void Block::getName(char *buf, size_t n) +{ + if (getParent() == NULL) { + strncpy(buf, _name, n); + + } else { + char parentName[blockNameLengthMax]; + getParent()->getName(parentName, n); + + if (!strcmp(_name, "")) { + strncpy(buf, parentName, blockNameLengthMax); + + } else { + snprintf(buf, blockNameLengthMax, "%s_%s", parentName, _name); + } + } +} + +void Block::updateParams() +{ + BlockParamBase *param = getParams().getHead(); + int count = 0; + + while (param != NULL) { + if (count++ > maxParamsPerBlock) { + char name[blockNameLengthMax]; + getName(name, blockNameLengthMax); + printf("exceeded max params for block: %s\n", name); + break; + } + + //printf("updating param: %s\n", param->getName()); + param->update(); + param = param->getSibling(); + } +} + +void Block::updateSubscriptions() +{ + UOrbSubscriptionBase *sub = getSubscriptions().getHead(); + int count = 0; + + while (sub != NULL) { + if (count++ > maxSubscriptionsPerBlock) { + char name[blockNameLengthMax]; + getName(name, blockNameLengthMax); + printf("exceeded max subscriptions for block: %s\n", name); + break; + } + + sub->update(); + sub = sub->getSibling(); + } +} + +void Block::updatePublications() +{ + UOrbPublicationBase *pub = getPublications().getHead(); + int count = 0; + + while (pub != NULL) { + if (count++ > maxPublicationsPerBlock) { + char name[blockNameLengthMax]; + getName(name, blockNameLengthMax); + printf("exceeded max publications for block: %s\n", name); + break; + } + + pub->update(); + pub = pub->getSibling(); + } +} + +void SuperBlock::setDt(float dt) +{ + Block::setDt(dt); + Block *child = getChildren().getHead(); + int count = 0; + + while (child != NULL) { + if (count++ > maxChildrenPerBlock) { + char name[40]; + getName(name, 40); + printf("exceeded max children for block: %s\n", name); + break; + } + + child->setDt(dt); + child = child->getSibling(); + } +} + +void SuperBlock::updateChildParams() +{ + Block *child = getChildren().getHead(); + int count = 0; + + while (child != NULL) { + if (count++ > maxChildrenPerBlock) { + char name[40]; + getName(name, 40); + printf("exceeded max children for block: %s\n", name); + break; + } + + child->updateParams(); + child = child->getSibling(); + } +} + +void SuperBlock::updateChildSubscriptions() +{ + Block *child = getChildren().getHead(); + int count = 0; + + while (child != NULL) { + if (count++ > maxChildrenPerBlock) { + char name[40]; + getName(name, 40); + printf("exceeded max children for block: %s\n", name); + break; + } + + child->updateSubscriptions(); + child = child->getSibling(); + } +} + +void SuperBlock::updateChildPublications() +{ + Block *child = getChildren().getHead(); + int count = 0; + + while (child != NULL) { + if (count++ > maxChildrenPerBlock) { + char name[40]; + getName(name, 40); + printf("exceeded max children for block: %s\n", name); + break; + } + + child->updatePublications(); + child = child->getSibling(); + } +} + +} // namespace control diff --git a/src/modules/controllib/block/Block.hpp b/src/modules/controllib/block/Block.hpp new file mode 100644 index 000000000..258701f27 --- /dev/null +++ b/src/modules/controllib/block/Block.hpp @@ -0,0 +1,131 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Block.h + * + * Controller library code + */ + +#pragma once + +#include <stdint.h> +#include <inttypes.h> + +#include "List.hpp" + +namespace control +{ + +static const uint16_t maxChildrenPerBlock = 100; +static const uint16_t maxParamsPerBlock = 100; +static const uint16_t maxSubscriptionsPerBlock = 100; +static const uint16_t maxPublicationsPerBlock = 100; +static const uint8_t blockNameLengthMax = 80; + +// forward declaration +class BlockParamBase; +class UOrbSubscriptionBase; +class UOrbPublicationBase; +class SuperBlock; + +/** + */ +class __EXPORT Block : + public ListNode<Block *> +{ +public: + friend class BlockParamBase; +// methods + Block(SuperBlock *parent, const char *name); + void getName(char *name, size_t n); + virtual ~Block() {}; + virtual void updateParams(); + virtual void updateSubscriptions(); + virtual void updatePublications(); + virtual void setDt(float dt) { _dt = dt; } +// accessors + float getDt() { return _dt; } +protected: +// accessors + SuperBlock *getParent() { return _parent; } + List<UOrbSubscriptionBase *> & getSubscriptions() { return _subscriptions; } + List<UOrbPublicationBase *> & getPublications() { return _publications; } + List<BlockParamBase *> & getParams() { return _params; } +// attributes + const char *_name; + SuperBlock *_parent; + float _dt; + List<UOrbSubscriptionBase *> _subscriptions; + List<UOrbPublicationBase *> _publications; + List<BlockParamBase *> _params; +}; + +class __EXPORT SuperBlock : + public Block +{ +public: + friend class Block; +// methods + SuperBlock(SuperBlock *parent, const char *name) : + Block(parent, name), + _children() { + } + virtual ~SuperBlock() {}; + virtual void setDt(float dt); + virtual void updateParams() { + Block::updateParams(); + + if (getChildren().getHead() != NULL) updateChildParams(); + } + virtual void updateSubscriptions() { + Block::updateSubscriptions(); + + if (getChildren().getHead() != NULL) updateChildSubscriptions(); + } + virtual void updatePublications() { + Block::updatePublications(); + + if (getChildren().getHead() != NULL) updateChildPublications(); + } +protected: +// methods + List<Block *> & getChildren() { return _children; } + void updateChildParams(); + void updateChildSubscriptions(); + void updateChildPublications(); +// attributes + List<Block *> _children; +}; + +} // namespace control diff --git a/src/modules/controllib/block/BlockParam.cpp b/src/modules/controllib/block/BlockParam.cpp new file mode 100644 index 000000000..fd12e365d --- /dev/null +++ b/src/modules/controllib/block/BlockParam.cpp @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Blockparam.cpp + * + * Controller library code + */ + +#include <math.h> +#include <stdio.h> +#include <string.h> + +#include "BlockParam.hpp" + +namespace control +{ + +BlockParamBase::BlockParamBase(Block *parent, const char *name, bool parent_prefix) : + _handle(PARAM_INVALID) +{ + char fullname[blockNameLengthMax]; + + if (parent == NULL) { + strncpy(fullname, name, blockNameLengthMax); + + } else { + char parentName[blockNameLengthMax]; + parent->getName(parentName, blockNameLengthMax); + + if (!strcmp(name, "")) { + strncpy(fullname, parentName, blockNameLengthMax); + + } else if (parent_prefix) { + snprintf(fullname, blockNameLengthMax, "%s_%s", parentName, name); + } else { + strncpy(fullname, name, blockNameLengthMax); + } + + parent->getParams().add(this); + } + + _handle = param_find(fullname); + + if (_handle == PARAM_INVALID) + printf("error finding param: %s\n", fullname); +}; + +} // namespace control diff --git a/src/modules/controllib/block/BlockParam.hpp b/src/modules/controllib/block/BlockParam.hpp new file mode 100644 index 000000000..58a9bfc0d --- /dev/null +++ b/src/modules/controllib/block/BlockParam.hpp @@ -0,0 +1,90 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file BlockParam.h + * + * Controller library code + */ + +#pragma once + +#include <systemlib/param/param.h> + +#include "Block.hpp" +#include "List.hpp" + +namespace control +{ + +/** + * A base class for block params that enables traversing linked list. + */ +class __EXPORT BlockParamBase : public ListNode<BlockParamBase *> +{ +public: + /** + * Instantiate a block param base. + * + * @param parent_prefix Set to true to include the parent name in the parameter name + */ + BlockParamBase(Block *parent, const char *name, bool parent_prefix=true); + virtual ~BlockParamBase() {}; + virtual void update() = 0; + const char *getName() { return param_name(_handle); } +protected: + param_t _handle; +}; + +/** + * Parameters that are tied to blocks for updating and nameing. + */ +template<class T> +class __EXPORT BlockParam : public BlockParamBase +{ +public: + BlockParam(Block *block, const char *name, bool parent_prefix=true) : + BlockParamBase(block, name, parent_prefix), + _val() { + update(); + } + T get() { return _val; } + void set(T val) { _val = val; } + void update() { + if (_handle != PARAM_INVALID) param_get(_handle, &_val); + } +protected: + T _val; +}; + +} // namespace control diff --git a/src/modules/controllib/block/List.hpp b/src/modules/controllib/block/List.hpp new file mode 100644 index 000000000..96b0b94d1 --- /dev/null +++ b/src/modules/controllib/block/List.hpp @@ -0,0 +1,71 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Node.h + * + * A node of a linked list. + */ + +#pragma once + +template<class T> +class __EXPORT ListNode +{ +public: + ListNode() : _sibling(NULL) { + } + void setSibling(T sibling) { _sibling = sibling; } + T getSibling() { return _sibling; } + T get() { + return _sibling; + } +protected: + T _sibling; +}; + +template<class T> +class __EXPORT List +{ +public: + List() : _head() { + } + void add(T newNode) { + newNode->setSibling(getHead()); + setHead(newNode); + } + T getHead() { return _head; } +private: + void setHead(T &head) { _head = head; } + T _head; +}; diff --git a/src/modules/controllib/block/UOrbPublication.cpp b/src/modules/controllib/block/UOrbPublication.cpp new file mode 100644 index 000000000..f69b39d90 --- /dev/null +++ b/src/modules/controllib/block/UOrbPublication.cpp @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file UOrbPublication.cpp + * + */ + +#include "UOrbPublication.hpp" diff --git a/src/modules/controllib/block/UOrbPublication.hpp b/src/modules/controllib/block/UOrbPublication.hpp new file mode 100644 index 000000000..0a8ae2ff7 --- /dev/null +++ b/src/modules/controllib/block/UOrbPublication.hpp @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file UOrbPublication.h + * + */ + +#pragma once + +#include <uORB/uORB.h> +#include "Block.hpp" +#include "List.hpp" + + +namespace control +{ + +class Block; + +/** + * Base publication warapper class, used in list traversal + * of various publications. + */ +class __EXPORT UOrbPublicationBase : public ListNode<control::UOrbPublicationBase *> +{ +public: + + UOrbPublicationBase( + List<UOrbPublicationBase *> * list, + const struct orb_metadata *meta) : + _meta(meta), + _handle(-1) { + if (list != NULL) list->add(this); + } + void update() { + if (_handle > 0) { + orb_publish(getMeta(), getHandle(), getDataVoidPtr()); + } else { + setHandle(orb_advertise(getMeta(), getDataVoidPtr())); + } + } + virtual void *getDataVoidPtr() = 0; + virtual ~UOrbPublicationBase() { + orb_unsubscribe(getHandle()); + } + const struct orb_metadata *getMeta() { return _meta; } + int getHandle() { return _handle; } +protected: + void setHandle(orb_advert_t handle) { _handle = handle; } + const struct orb_metadata *_meta; + orb_advert_t _handle; +}; + +/** + * UOrb Publication wrapper class + */ +template<class T> +class UOrbPublication : + public T, // this must be first! + public UOrbPublicationBase +{ +public: + /** + * Constructor + * + * @param list A list interface for adding to list during construction + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + */ + UOrbPublication( + List<UOrbPublicationBase *> * list, + const struct orb_metadata *meta) : + T(), // initialize data structure to zero + UOrbPublicationBase(list, meta) { + } + virtual ~UOrbPublication() {} + /* + * XXX + * This function gets the T struct, assuming + * the struct is the first base class, this + * should use dynamic cast, but doesn't + * seem to be available + */ + void *getDataVoidPtr() { return (void *)(T *)(this); } +}; + +} // namespace control diff --git a/src/modules/controllib/block/UOrbSubscription.cpp b/src/modules/controllib/block/UOrbSubscription.cpp new file mode 100644 index 000000000..022cadd24 --- /dev/null +++ b/src/modules/controllib/block/UOrbSubscription.cpp @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file UOrbSubscription.cpp + * + */ + +#include "UOrbSubscription.hpp" + +namespace control +{ + +bool __EXPORT UOrbSubscriptionBase::updated() +{ + bool isUpdated = false; + orb_check(_handle, &isUpdated); + return isUpdated; +} + +} // namespace control diff --git a/src/modules/controllib/block/UOrbSubscription.hpp b/src/modules/controllib/block/UOrbSubscription.hpp new file mode 100644 index 000000000..22cc2e114 --- /dev/null +++ b/src/modules/controllib/block/UOrbSubscription.hpp @@ -0,0 +1,137 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file UOrbSubscription.h + * + */ + +#pragma once + +#include <uORB/uORB.h> +#include "Block.hpp" +#include "List.hpp" + + +namespace control +{ + +class Block; + +/** + * Base subscription warapper class, used in list traversal + * of various subscriptions. + */ +class __EXPORT UOrbSubscriptionBase : + public ListNode<control::UOrbSubscriptionBase *> +{ +public: +// methods + + /** + * Constructor + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + */ + UOrbSubscriptionBase( + List<UOrbSubscriptionBase *> * list, + const struct orb_metadata *meta) : + _meta(meta), + _handle() { + if (list != NULL) list->add(this); + } + bool updated(); + void update() { + if (updated()) { + orb_copy(_meta, _handle, getDataVoidPtr()); + } + } + virtual void *getDataVoidPtr() = 0; + virtual ~UOrbSubscriptionBase() { + orb_unsubscribe(_handle); + } +// accessors + const struct orb_metadata *getMeta() { return _meta; } + int getHandle() { return _handle; } +protected: +// accessors + void setHandle(int handle) { _handle = handle; } +// attributes + const struct orb_metadata *_meta; + int _handle; +}; + +/** + * UOrb Subscription wrapper class + */ +template<class T> +class __EXPORT UOrbSubscription : + public T, // this must be first! + public UOrbSubscriptionBase +{ +public: + /** + * Constructor + * + * @param list A list interface for adding to list during construction + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @param interval The minimum interval in milliseconds between updates + */ + UOrbSubscription( + List<UOrbSubscriptionBase *> * list, + const struct orb_metadata *meta, unsigned interval) : + T(), // initialize data structure to zero + UOrbSubscriptionBase(list, meta) { + setHandle(orb_subscribe(getMeta())); + orb_set_interval(getHandle(), interval); + } + + /** + * Deconstructor + */ + virtual ~UOrbSubscription() {} + + /* + * XXX + * This function gets the T struct, assuming + * the struct is the first base class, this + * should use dynamic cast, but doesn't + * seem to be available + */ + void *getDataVoidPtr() { return (void *)(T *)(this); } + T getData() { return T(*this); } +}; + +} // namespace control diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp new file mode 100644 index 000000000..c6c374300 --- /dev/null +++ b/src/modules/controllib/blocks.cpp @@ -0,0 +1,486 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file blocks.cpp + * + * Controller library code + */ + +#include <math.h> +#include <stdio.h> + +#include "blocks.hpp" + +namespace control +{ + +int basicBlocksTest() +{ + blockLimitTest(); + blockLimitSymTest(); + blockLowPassTest(); + blockHighPassTest(); + blockIntegralTest(); + blockIntegralTrapTest(); + blockDerivativeTest(); + blockPTest(); + blockPITest(); + blockPDTest(); + blockPIDTest(); + blockOutputTest(); + blockRandUniformTest(); + blockRandGaussTest(); + return 0; +} + +float BlockLimit::update(float input) +{ + if (input > getMax()) { + input = _max.get(); + + } else if (input < getMin()) { + input = getMin(); + } + + return input; +} + +int blockLimitTest() +{ + printf("Test BlockLimit\t\t\t: "); + BlockLimit limit(NULL, "TEST"); + // initial state + ASSERT(equal(1.0f, limit.getMax())); + ASSERT(equal(-1.0f, limit.getMin())); + ASSERT(equal(0.0f, limit.getDt())); + // update + ASSERT(equal(-1.0f, limit.update(-2.0f))); + ASSERT(equal(1.0f, limit.update(2.0f))); + ASSERT(equal(0.0f, limit.update(0.0f))); + printf("PASS\n"); + return 0; +} + +float BlockLimitSym::update(float input) +{ + if (input > getMax()) { + input = _max.get(); + + } else if (input < -getMax()) { + input = -getMax(); + } + + return input; +} + +int blockLimitSymTest() +{ + printf("Test BlockLimitSym\t\t: "); + BlockLimitSym limit(NULL, "TEST"); + // initial state + ASSERT(equal(1.0f, limit.getMax())); + ASSERT(equal(0.0f, limit.getDt())); + // update + ASSERT(equal(-1.0f, limit.update(-2.0f))); + ASSERT(equal(1.0f, limit.update(2.0f))); + ASSERT(equal(0.0f, limit.update(0.0f))); + printf("PASS\n"); + return 0; +} + +float BlockLowPass::update(float input) +{ + float b = 2 * float(M_PI) * getFCut() * getDt(); + float a = b / (1 + b); + setState(a * input + (1 - a)*getState()); + return getState(); +} + +int blockLowPassTest() +{ + printf("Test BlockLowPass\t\t: "); + BlockLowPass lowPass(NULL, "TEST_LP"); + // test initial state + ASSERT(equal(10.0f, lowPass.getFCut())); + ASSERT(equal(0.0f, lowPass.getState())); + ASSERT(equal(0.0f, lowPass.getDt())); + // set dt + lowPass.setDt(0.1f); + ASSERT(equal(0.1f, lowPass.getDt())); + // set state + lowPass.setState(1.0f); + ASSERT(equal(1.0f, lowPass.getState())); + // test update + ASSERT(equal(1.8626974f, lowPass.update(2.0f))); + + // test end condition + for (int i = 0; i < 100; i++) { + lowPass.update(2.0f); + } + + ASSERT(equal(2.0f, lowPass.getState())); + ASSERT(equal(2.0f, lowPass.update(2.0f))); + printf("PASS\n"); + return 0; +}; + +float BlockHighPass::update(float input) +{ + float b = 2 * float(M_PI) * getFCut() * getDt(); + float a = 1 / (1 + b); + setY(a * (getY() + input - getU())); + setU(input); + return getY(); +} + +int blockHighPassTest() +{ + printf("Test BlockHighPass\t\t: "); + BlockHighPass highPass(NULL, "TEST_HP"); + // test initial state + ASSERT(equal(10.0f, highPass.getFCut())); + ASSERT(equal(0.0f, highPass.getU())); + ASSERT(equal(0.0f, highPass.getY())); + ASSERT(equal(0.0f, highPass.getDt())); + // set dt + highPass.setDt(0.1f); + ASSERT(equal(0.1f, highPass.getDt())); + // set state + highPass.setU(1.0f); + ASSERT(equal(1.0f, highPass.getU())); + highPass.setY(1.0f); + ASSERT(equal(1.0f, highPass.getY())); + // test update + ASSERT(equal(0.2746051f, highPass.update(2.0f))); + + // test end condition + for (int i = 0; i < 100; i++) { + highPass.update(2.0f); + } + + ASSERT(equal(0.0f, highPass.getY())); + ASSERT(equal(0.0f, highPass.update(2.0f))); + printf("PASS\n"); + return 0; +} + +float BlockIntegral::update(float input) +{ + // trapezoidal integration + setY(_limit.update(getY() + input * getDt())); + return getY(); +} + +int blockIntegralTest() +{ + printf("Test BlockIntegral\t\t: "); + BlockIntegral integral(NULL, "TEST_I"); + // test initial state + ASSERT(equal(1.0f, integral.getMax())); + ASSERT(equal(0.0f, integral.getDt())); + // set dt + integral.setDt(0.1f); + ASSERT(equal(0.1f, integral.getDt())); + // set Y + integral.setY(0.9f); + ASSERT(equal(0.9f, integral.getY())); + + // test exceed max + for (int i = 0; i < 100; i++) { + integral.update(1.0f); + } + + ASSERT(equal(1.0f, integral.update(1.0f))); + // test exceed min + integral.setY(-0.9f); + ASSERT(equal(-0.9f, integral.getY())); + + for (int i = 0; i < 100; i++) { + integral.update(-1.0f); + } + + ASSERT(equal(-1.0f, integral.update(-1.0f))); + // test update + integral.setY(0.1f); + ASSERT(equal(0.2f, integral.update(1.0))); + ASSERT(equal(0.2f, integral.getY())); + printf("PASS\n"); + return 0; +} + +float BlockIntegralTrap::update(float input) +{ + // trapezoidal integration + setY(_limit.update(getY() + + (getU() + input) / 2.0f * getDt())); + setU(input); + return getY(); +} + +int blockIntegralTrapTest() +{ + printf("Test BlockIntegralTrap\t\t: "); + BlockIntegralTrap integral(NULL, "TEST_I"); + // test initial state + ASSERT(equal(1.0f, integral.getMax())); + ASSERT(equal(0.0f, integral.getDt())); + // set dt + integral.setDt(0.1f); + ASSERT(equal(0.1f, integral.getDt())); + // set U + integral.setU(1.0f); + ASSERT(equal(1.0f, integral.getU())); + // set Y + integral.setY(0.9f); + ASSERT(equal(0.9f, integral.getY())); + + // test exceed max + for (int i = 0; i < 100; i++) { + integral.update(1.0f); + } + + ASSERT(equal(1.0f, integral.update(1.0f))); + // test exceed min + integral.setU(-1.0f); + integral.setY(-0.9f); + ASSERT(equal(-0.9f, integral.getY())); + + for (int i = 0; i < 100; i++) { + integral.update(-1.0f); + } + + ASSERT(equal(-1.0f, integral.update(-1.0f))); + // test update + integral.setU(2.0f); + integral.setY(0.1f); + ASSERT(equal(0.25f, integral.update(1.0))); + ASSERT(equal(0.25f, integral.getY())); + ASSERT(equal(1.0f, integral.getU())); + printf("PASS\n"); + return 0; +} + +float BlockDerivative::update(float input) +{ + float output = _lowPass.update((input - getU()) / getDt()); + setU(input); + return output; +} + +int blockDerivativeTest() +{ + printf("Test BlockDerivative\t\t: "); + BlockDerivative derivative(NULL, "TEST_D"); + // test initial state + ASSERT(equal(0.0f, derivative.getU())); + ASSERT(equal(10.0f, derivative.getLP())); + // set dt + derivative.setDt(0.1f); + ASSERT(equal(0.1f, derivative.getDt())); + // set U + derivative.setU(1.0f); + ASSERT(equal(1.0f, derivative.getU())); + // test update + ASSERT(equal(8.6269744f, derivative.update(2.0f))); + ASSERT(equal(2.0f, derivative.getU())); + printf("PASS\n"); + return 0; +} + +int blockPTest() +{ + printf("Test BlockP\t\t\t: "); + BlockP blockP(NULL, "TEST_P"); + // test initial state + ASSERT(equal(0.2f, blockP.getKP())); + ASSERT(equal(0.0f, blockP.getDt())); + // set dt + blockP.setDt(0.1f); + ASSERT(equal(0.1f, blockP.getDt())); + // test update + ASSERT(equal(0.4f, blockP.update(2.0f))); + printf("PASS\n"); + return 0; +} + +int blockPITest() +{ + printf("Test BlockPI\t\t\t: "); + BlockPI blockPI(NULL, "TEST"); + // test initial state + ASSERT(equal(0.2f, blockPI.getKP())); + ASSERT(equal(0.1f, blockPI.getKI())); + ASSERT(equal(0.0f, blockPI.getDt())); + ASSERT(equal(1.0f, blockPI.getIntegral().getMax())); + // set dt + blockPI.setDt(0.1f); + ASSERT(equal(0.1f, blockPI.getDt())); + // set integral state + blockPI.getIntegral().setY(0.1f); + ASSERT(equal(0.1f, blockPI.getIntegral().getY())); + // test update + // 0.2*2 + 0.1*(2*0.1 + 0.1) = 0.43 + ASSERT(equal(0.43f, blockPI.update(2.0f))); + printf("PASS\n"); + return 0; +} + +int blockPDTest() +{ + printf("Test BlockPD\t\t\t: "); + BlockPD blockPD(NULL, "TEST"); + // test initial state + ASSERT(equal(0.2f, blockPD.getKP())); + ASSERT(equal(0.01f, blockPD.getKD())); + ASSERT(equal(0.0f, blockPD.getDt())); + ASSERT(equal(10.0f, blockPD.getDerivative().getLP())); + // set dt + blockPD.setDt(0.1f); + ASSERT(equal(0.1f, blockPD.getDt())); + // set derivative state + blockPD.getDerivative().setU(1.0f); + ASSERT(equal(1.0f, blockPD.getDerivative().getU())); + // test update + // 0.2*2 + 0.1*(0.1*8.626...) = 0.486269744 + ASSERT(equal(0.486269744f, blockPD.update(2.0f))); + printf("PASS\n"); + return 0; +} + +int blockPIDTest() +{ + printf("Test BlockPID\t\t\t: "); + BlockPID blockPID(NULL, "TEST"); + // test initial state + ASSERT(equal(0.2f, blockPID.getKP())); + ASSERT(equal(0.1f, blockPID.getKI())); + ASSERT(equal(0.01f, blockPID.getKD())); + ASSERT(equal(0.0f, blockPID.getDt())); + ASSERT(equal(10.0f, blockPID.getDerivative().getLP())); + ASSERT(equal(1.0f, blockPID.getIntegral().getMax())); + // set dt + blockPID.setDt(0.1f); + ASSERT(equal(0.1f, blockPID.getDt())); + // set derivative state + blockPID.getDerivative().setU(1.0f); + ASSERT(equal(1.0f, blockPID.getDerivative().getU())); + // set integral state + blockPID.getIntegral().setY(0.1f); + ASSERT(equal(0.1f, blockPID.getIntegral().getY())); + // test update + // 0.2*2 + 0.1*(2*0.1 + 0.1) + 0.1*(0.1*8.626...) = 0.5162697 + ASSERT(equal(0.5162697f, blockPID.update(2.0f))); + printf("PASS\n"); + return 0; +} + +int blockOutputTest() +{ + printf("Test BlockOutput\t\t: "); + BlockOutput blockOutput(NULL, "TEST"); + // test initial state + ASSERT(equal(0.0f, blockOutput.getDt())); + ASSERT(equal(0.5f, blockOutput.get())); + ASSERT(equal(-1.0f, blockOutput.getMin())); + ASSERT(equal(1.0f, blockOutput.getMax())); + // test update below min + blockOutput.update(-2.0f); + ASSERT(equal(-1.0f, blockOutput.get())); + // test update above max + blockOutput.update(2.0f); + ASSERT(equal(1.0f, blockOutput.get())); + // test trim + blockOutput.update(0.0f); + ASSERT(equal(0.5f, blockOutput.get())); + printf("PASS\n"); + return 0; +} + +int blockRandUniformTest() +{ + srand(1234); + printf("Test BlockRandUniform\t\t: "); + BlockRandUniform blockRandUniform(NULL, "TEST"); + // test initial state + ASSERT(equal(0.0f, blockRandUniform.getDt())); + ASSERT(equal(-1.0f, blockRandUniform.getMin())); + ASSERT(equal(1.0f, blockRandUniform.getMax())); + // test update + int n = 10000; + float mean = blockRandUniform.update(); + + // recursive mean algorithm from Knuth + for (int i = 2; i < n + 1; i++) { + float val = blockRandUniform.update(); + mean += (val - mean) / i; + ASSERT(val <= blockRandUniform.getMax()); + ASSERT(val >= blockRandUniform.getMin()); + } + + ASSERT(equal(mean, (blockRandUniform.getMin() + + blockRandUniform.getMax()) / 2, 1e-1)); + printf("PASS\n"); + return 0; +} + +int blockRandGaussTest() +{ + srand(1234); + printf("Test BlockRandGauss\t\t: "); + BlockRandGauss blockRandGauss(NULL, "TEST"); + // test initial state + ASSERT(equal(0.0f, blockRandGauss.getDt())); + ASSERT(equal(1.0f, blockRandGauss.getMean())); + ASSERT(equal(2.0f, blockRandGauss.getStdDev())); + // test update + int n = 10000; + float mean = blockRandGauss.update(); + float sum = 0; + + // recursive mean, stdev algorithm from Knuth + for (int i = 2; i < n + 1; i++) { + float val = blockRandGauss.update(); + float newMean = mean + (val - mean) / i; + sum += (val - mean) * (val - newMean); + mean = newMean; + } + + float stdDev = sqrt(sum / (n - 1)); + ASSERT(equal(mean, blockRandGauss.getMean(), 1e-1)); + ASSERT(equal(stdDev, blockRandGauss.getStdDev(), 1e-1)); + printf("PASS\n"); + return 0; +} + +} // namespace control diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp new file mode 100644 index 000000000..7a785d12e --- /dev/null +++ b/src/modules/controllib/blocks.hpp @@ -0,0 +1,494 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file blocks.h + * + * Controller library code + */ + +#pragma once + +#include <assert.h> +#include <time.h> +#include <stdlib.h> +#include <mathlib/math/test/test.hpp> + +#include "block/Block.hpp" +#include "block/BlockParam.hpp" + +namespace control +{ + +int __EXPORT basicBlocksTest(); + +/** + * A limiter/ saturation. + * The output of update is the input, bounded + * by min/max. + */ +class __EXPORT BlockLimit : public Block +{ +public: +// methods + BlockLimit(SuperBlock *parent, const char *name) : + Block(parent, name), + _min(this, "MIN"), + _max(this, "MAX") + {}; + virtual ~BlockLimit() {}; + float update(float input); +// accessors + float getMin() { return _min.get(); } + float getMax() { return _max.get(); } +protected: +// attributes + BlockParam<float> _min; + BlockParam<float> _max; +}; + +int __EXPORT blockLimitTest(); + +/** + * A symmetric limiter/ saturation. + * Same as limiter but with only a max, is used for + * upper limit of +max, and lower limit of -max + */ +class __EXPORT BlockLimitSym : public Block +{ +public: +// methods + BlockLimitSym(SuperBlock *parent, const char *name) : + Block(parent, name), + _max(this, "MAX") + {}; + virtual ~BlockLimitSym() {}; + float update(float input); +// accessors + float getMax() { return _max.get(); } +protected: +// attributes + BlockParam<float> _max; +}; + +int __EXPORT blockLimitSymTest(); + +/** + * A low pass filter as described here: + * http://en.wikipedia.org/wiki/Low-pass_filter. + */ +class __EXPORT BlockLowPass : public Block +{ +public: +// methods + BlockLowPass(SuperBlock *parent, const char *name) : + Block(parent, name), + _state(0), + _fCut(this, "") // only one parameter, no need to name + {}; + virtual ~BlockLowPass() {}; + float update(float input); +// accessors + float getState() { return _state; } + float getFCut() { return _fCut.get(); } + void setState(float state) { _state = state; } +protected: +// attributes + float _state; + BlockParam<float> _fCut; +}; + +int __EXPORT blockLowPassTest(); + +/** + * A high pass filter as described here: + * http://en.wikipedia.org/wiki/High-pass_filter. + */ +class __EXPORT BlockHighPass : public Block +{ +public: +// methods + BlockHighPass(SuperBlock *parent, const char *name) : + Block(parent, name), + _u(0), + _y(0), + _fCut(this, "") // only one parameter, no need to name + {}; + virtual ~BlockHighPass() {}; + float update(float input); +// accessors + float getU() {return _u;} + float getY() {return _y;} + float getFCut() {return _fCut.get();} + void setU(float u) {_u = u;} + void setY(float y) {_y = y;} +protected: +// attributes + float _u; /**< previous input */ + float _y; /**< previous output */ + BlockParam<float> _fCut; /**< cut-off frequency, Hz */ +}; + +int __EXPORT blockHighPassTest(); + +/** + * A rectangular integrator. + * A limiter is built into the class to bound the + * integral's internal state. This is important + * for windup protection. + * @see Limit + */ +class __EXPORT BlockIntegral: public SuperBlock +{ +public: +// methods + BlockIntegral(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + _y(0), + _limit(this, "") {}; + virtual ~BlockIntegral() {}; + float update(float input); +// accessors + float getY() {return _y;} + float getMax() {return _limit.getMax();} + void setY(float y) {_y = y;} +protected: +// attributes + float _y; /**< previous output */ + BlockLimitSym _limit; /**< limiter */ +}; + +int __EXPORT blockIntegralTest(); + +/** + * A trapezoidal integrator. + * http://en.wikipedia.org/wiki/Trapezoidal_rule + * A limiter is built into the class to bound the + * integral's internal state. This is important + * for windup protection. + * @see Limit + */ +class __EXPORT BlockIntegralTrap : public SuperBlock +{ +public: +// methods + BlockIntegralTrap(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + _u(0), + _y(0), + _limit(this, "") {}; + virtual ~BlockIntegralTrap() {}; + float update(float input); +// accessors + float getU() {return _u;} + float getY() {return _y;} + float getMax() {return _limit.getMax();} + void setU(float u) {_u = u;} + void setY(float y) {_y = y;} +protected: +// attributes + float _u; /**< previous input */ + float _y; /**< previous output */ + BlockLimitSym _limit; /**< limiter */ +}; + +int __EXPORT blockIntegralTrapTest(); + +/** + * A simple derivative approximation. + * This uses the previous and current input. + * This has a built in low pass filter. + * @see LowPass + */ +class __EXPORT BlockDerivative : public SuperBlock +{ +public: +// methods + BlockDerivative(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + _u(0), + _lowPass(this, "LP") + {}; + virtual ~BlockDerivative() {}; + float update(float input); +// accessors + void setU(float u) {_u = u;} + float getU() {return _u;} + float getLP() {return _lowPass.getFCut();} +protected: +// attributes + float _u; /**< previous input */ + BlockLowPass _lowPass; /**< low pass filter */ +}; + +int __EXPORT blockDerivativeTest(); + +/** + * A proportional controller. + * @link http://en.wikipedia.org/wiki/PID_controller + */ +class __EXPORT BlockP: public Block +{ +public: +// methods + BlockP(SuperBlock *parent, const char *name) : + Block(parent, name), + _kP(this, "") // only one param, no need to name + {}; + virtual ~BlockP() {}; + float update(float input) { + return getKP() * input; + } +// accessors + float getKP() { return _kP.get(); } +protected: + BlockParam<float> _kP; +}; + +int __EXPORT blockPTest(); + +/** + * A proportional-integral controller. + * @link http://en.wikipedia.org/wiki/PID_controller + */ +class __EXPORT BlockPI: public SuperBlock +{ +public: +// methods + BlockPI(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + _integral(this, "I"), + _kP(this, "P"), + _kI(this, "I") + {}; + virtual ~BlockPI() {}; + float update(float input) { + return getKP() * input + + getKI() * getIntegral().update(input); + } +// accessors + float getKP() { return _kP.get(); } + float getKI() { return _kI.get(); } + BlockIntegral &getIntegral() { return _integral; } +private: + BlockIntegral _integral; + BlockParam<float> _kP; + BlockParam<float> _kI; +}; + +int __EXPORT blockPITest(); + +/** + * A proportional-derivative controller. + * @link http://en.wikipedia.org/wiki/PID_controller + */ +class __EXPORT BlockPD: public SuperBlock +{ +public: +// methods + BlockPD(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + _derivative(this, "D"), + _kP(this, "P"), + _kD(this, "D") + {}; + virtual ~BlockPD() {}; + float update(float input) { + return getKP() * input + + getKD() * getDerivative().update(input); + } +// accessors + float getKP() { return _kP.get(); } + float getKD() { return _kD.get(); } + BlockDerivative &getDerivative() { return _derivative; } +private: + BlockDerivative _derivative; + BlockParam<float> _kP; + BlockParam<float> _kD; +}; + +int __EXPORT blockPDTest(); + +/** + * A proportional-integral-derivative controller. + * @link http://en.wikipedia.org/wiki/PID_controller + */ +class __EXPORT BlockPID: public SuperBlock +{ +public: +// methods + BlockPID(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + _integral(this, "I"), + _derivative(this, "D"), + _kP(this, "P"), + _kI(this, "I"), + _kD(this, "D") + {}; + virtual ~BlockPID() {}; + float update(float input) { + return getKP() * input + + getKI() * getIntegral().update(input) + + getKD() * getDerivative().update(input); + } +// accessors + float getKP() { return _kP.get(); } + float getKI() { return _kI.get(); } + float getKD() { return _kD.get(); } + BlockIntegral &getIntegral() { return _integral; } + BlockDerivative &getDerivative() { return _derivative; } +private: +// attributes + BlockIntegral _integral; + BlockDerivative _derivative; + BlockParam<float> _kP; + BlockParam<float> _kI; + BlockParam<float> _kD; +}; + +int __EXPORT blockPIDTest(); + +/** + * An output trim/ saturation block + */ +class __EXPORT BlockOutput: public SuperBlock +{ +public: +// methods + BlockOutput(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + _trim(this, "TRIM"), + _limit(this, ""), + _val(0) { + update(0); + }; + virtual ~BlockOutput() {}; + void update(float input) { + _val = _limit.update(input + getTrim()); + } +// accessors + float getMin() { return _limit.getMin(); } + float getMax() { return _limit.getMax(); } + float getTrim() { return _trim.get(); } + float get() { return _val; } +private: +// attributes + BlockParam<float> _trim; + BlockLimit _limit; + float _val; +}; + +int __EXPORT blockOutputTest(); + +/** + * A uniform random number generator + */ +class __EXPORT BlockRandUniform: public Block +{ +public: +// methods + BlockRandUniform(SuperBlock *parent, + const char *name) : + Block(parent, name), + _min(this, "MIN"), + _max(this, "MAX") { + // seed should be initialized somewhere + // in main program for all calls to rand + // XXX currently in nuttx if you seed to 0, rand breaks + }; + virtual ~BlockRandUniform() {}; + float update() { + static float rand_max = MAX_RAND; + float rand_val = rand(); + float bounds = getMax() - getMin(); + return getMin() + (rand_val * bounds) / rand_max; + } +// accessors + float getMin() { return _min.get(); } + float getMax() { return _max.get(); } +private: +// attributes + BlockParam<float> _min; + BlockParam<float> _max; +}; + +int __EXPORT blockRandUniformTest(); + +class __EXPORT BlockRandGauss: public Block +{ +public: +// methods + BlockRandGauss(SuperBlock *parent, + const char *name) : + Block(parent, name), + _mean(this, "MEAN"), + _stdDev(this, "DEV") { + // seed should be initialized somewhere + // in main program for all calls to rand + // XXX currently in nuttx if you seed to 0, rand breaks + }; + virtual ~BlockRandGauss() {}; + float update() { + static float V1, V2, S; + static int phase = 0; + float X; + + if (phase == 0) { + do { + float U1 = (float)rand() / MAX_RAND; + float U2 = (float)rand() / MAX_RAND; + V1 = 2 * U1 - 1; + V2 = 2 * U2 - 1; + S = V1 * V1 + V2 * V2; + } while (S >= 1 || fabsf(S) < 1e-8f); + + X = V1 * float(sqrt(-2 * float(log(S)) / S)); + + } else + X = V2 * float(sqrt(-2 * float(log(S)) / S)); + + phase = 1 - phase; + return X * getStdDev() + getMean(); + } +// accessors + float getMean() { return _mean.get(); } + float getStdDev() { return _stdDev.get(); } +private: +// attributes + BlockParam<float> _mean; + BlockParam<float> _stdDev; +}; + +int __EXPORT blockRandGaussTest(); + +} // namespace control diff --git a/src/modules/controllib/fixedwing.cpp b/src/modules/controllib/fixedwing.cpp new file mode 100644 index 000000000..77b2ac806 --- /dev/null +++ b/src/modules/controllib/fixedwing.cpp @@ -0,0 +1,379 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file fixedwing.cpp + * + * Controller library code + */ + +#include "fixedwing.hpp" + +namespace control +{ + +namespace fixedwing +{ + +BlockYawDamper::BlockYawDamper(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + _rLowPass(this, "R_LP"), + _rWashout(this, "R_HP"), + _r2Rdr(this, "R2RDR"), + _rudder(0) +{ +} + +BlockYawDamper::~BlockYawDamper() {}; + +void BlockYawDamper::update(float rCmd, float r, float outputScale) +{ + _rudder = outputScale*_r2Rdr.update(rCmd - + _rWashout.update(_rLowPass.update(r))); +} + +BlockStabilization::BlockStabilization(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + _yawDamper(this, ""), + _pLowPass(this, "P_LP"), + _qLowPass(this, "Q_LP"), + _p2Ail(this, "P2AIL"), + _q2Elv(this, "Q2ELV"), + _aileron(0), + _elevator(0) +{ +} + +BlockStabilization::~BlockStabilization() {}; + +void BlockStabilization::update(float pCmd, float qCmd, float rCmd, + float p, float q, float r, float outputScale) +{ + _aileron = outputScale*_p2Ail.update( + pCmd - _pLowPass.update(p)); + _elevator = outputScale*_q2Elv.update( + qCmd - _qLowPass.update(q)); + _yawDamper.update(rCmd, r, outputScale); +} + +BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + _xtYawLimit(this, "XT2YAW"), + _xt2Yaw(this, "XT2YAW"), + _psiCmd(0) +{ +} + +BlockWaypointGuidance::~BlockWaypointGuidance() {}; + +void BlockWaypointGuidance::update(vehicle_global_position_s &pos, + vehicle_attitude_s &att, + vehicle_global_position_setpoint_s &posCmd, + vehicle_global_position_setpoint_s &lastPosCmd) +{ + + // heading to waypoint + float psiTrack = get_bearing_to_next_waypoint( + (double)pos.lat / (double)1e7d, + (double)pos.lon / (double)1e7d, + (double)posCmd.lat / (double)1e7d, + (double)posCmd.lon / (double)1e7d); + + // cross track + struct crosstrack_error_s xtrackError; + get_distance_to_line(&xtrackError, + (double)pos.lat / (double)1e7d, + (double)pos.lon / (double)1e7d, + (double)lastPosCmd.lat / (double)1e7d, + (double)lastPosCmd.lon / (double)1e7d, + (double)posCmd.lat / (double)1e7d, + (double)posCmd.lon / (double)1e7d); + + _psiCmd = _wrap_2pi(psiTrack - + _xtYawLimit.update(_xt2Yaw.update(xtrackError.distance))); +} + +BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + // subscriptions + _att(&getSubscriptions(), ORB_ID(vehicle_attitude), 20), + _attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20), + _ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20), + _pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20), + _posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_set_triplet), 20), + _manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20), + _status(&getSubscriptions(), ORB_ID(vehicle_status), 20), + _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz + // publications + _actuators(&getPublications(), ORB_ID(actuator_controls_0)) +{ +} + +BlockUorbEnabledAutopilot::~BlockUorbEnabledAutopilot() {}; + +BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name) : + BlockUorbEnabledAutopilot(parent, name), + _stabilization(this, ""), // no name needed, already unique + + // heading hold + _psi2Phi(this, "PSI2PHI"), + _phi2P(this, "PHI2P"), + _phiLimit(this, "PHI_LIM"), + + // velocity hold + _v2Theta(this, "V2THE"), + _theta2Q(this, "THE2Q"), + _theLimit(this, "THE"), + _vLimit(this, "V"), + + // altitude/climb rate hold + _h2Thr(this, "H2THR"), + _cr2Thr(this, "CR2THR"), + + // guidance block + _guide(this, ""), + + _trimAil(this, "TRIM_ROLL", false), /* general roll trim (full name: TRIM_ROLL) */ + _trimElv(this, "TRIM_PITCH", false), /* general pitch trim */ + _trimRdr(this, "TRIM_YAW", false), /* general yaw trim */ + _trimThr(this, "TRIM_THR"), /* FWB_ specific throttle trim (full name: FWB_TRIM_THR) */ + _trimV(this, "TRIM_V"), /* FWB_ specific trim velocity (full name : FWB_TRIM_V) */ + + _vCmd(this, "V_CMD"), + _crMax(this, "CR_MAX"), + _attPoll(), + _lastPosCmd(), + _timeStamp(0) +{ + _attPoll.fd = _att.getHandle(); + _attPoll.events = POLLIN; +} + +void BlockMultiModeBacksideAutopilot::update() +{ + // wait for a sensor update, check for exit condition every 100 ms + if (poll(&_attPoll, 1, 100) < 0) return; // poll error + + uint64_t newTimeStamp = hrt_absolute_time(); + float dt = (newTimeStamp - _timeStamp) / 1.0e6f; + _timeStamp = newTimeStamp; + + // check for sane values of dt + // to prevent large control responses + if (dt > 1.0f || dt < 0) return; + + // set dt for all child blocks + setDt(dt); + + // store old position command before update if new command sent + if (_posCmd.updated()) { + _lastPosCmd = _posCmd.getData(); + } + + // check for new updates + if (_param_update.updated()) updateParams(); + + // get new information from subscriptions + updateSubscriptions(); + + // default all output to zero unless handled by mode + for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++) + _actuators.control[i] = 0.0f; + + // only update guidance in auto mode + if (_status.state_machine == SYSTEM_STATE_AUTO) { + // update guidance + _guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current); + } + + // XXX handle STABILIZED (loiter on spot) as well + // once the system switches from manual or auto to stabilized + // the setpoint should update to loitering around this position + + // handle autopilot modes + if (_status.state_machine == SYSTEM_STATE_AUTO || + _status.state_machine == SYSTEM_STATE_STABILIZED) { + + // update guidance + _guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current); + + // calculate velocity, XXX should be airspeed, but using ground speed for now + // for the purpose of control we will limit the velocity feedback between + // the min/max velocity + float v = _vLimit.update(sqrtf( + _pos.vx * _pos.vx + + _pos.vy * _pos.vy + + _pos.vz * _pos.vz)); + + // limit velocity command between min/max velocity + float vCmd = _vLimit.update(_vCmd.get()); + + // altitude hold + float dThrottle = _h2Thr.update(_posCmd.current.altitude - _pos.alt); + + // heading hold + float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw); + float phiCmd = _phiLimit.update(_psi2Phi.update(psiError)); + float pCmd = _phi2P.update(phiCmd - _att.roll); + + // velocity hold + // negative sign because nose over to increase speed + float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v)); + float qCmd = _theta2Q.update(thetaCmd - _att.pitch); + + // yaw rate cmd + float rCmd = 0; + + // stabilization + float velocityRatio = _trimV.get()/v; + float outputScale = velocityRatio*velocityRatio; + // this term scales the output based on the dynamic pressure change from trim + _stabilization.update(pCmd, qCmd, rCmd, + _att.rollspeed, _att.pitchspeed, _att.yawspeed, + outputScale); + + // output + _actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get(); + _actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get(); + _actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get(); + _actuators.control[CH_THR] = dThrottle + _trimThr.get(); + + // XXX limit throttle to manual setting (safety) for now. + // If it turns out to be confusing, it can be removed later once + // a first binary release can be targeted. + // This is not a hack, but a design choice. + + /* do not limit in HIL */ + if (!_status.flag_hil_enabled) { + /* limit to value of manual throttle */ + _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? + _actuators.control[CH_THR] : _manual.throttle; + } + + } else if (_status.state_machine == SYSTEM_STATE_MANUAL) { + + if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { + _actuators.control[CH_AIL] = _manual.roll; + _actuators.control[CH_ELV] = _manual.pitch; + _actuators.control[CH_RDR] = _manual.yaw; + _actuators.control[CH_THR] = _manual.throttle; + + } else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { + + // calculate velocity, XXX should be airspeed, but using ground speed for now + // for the purpose of control we will limit the velocity feedback between + // the min/max velocity + float v = _vLimit.update(sqrtf( + _pos.vx * _pos.vx + + _pos.vy * _pos.vy + + _pos.vz * _pos.vz)); + + // pitch channel -> rate of climb + // TODO, might want to put a gain on this, otherwise commanding + // from +1 -> -1 m/s for rate of climb + //float dThrottle = _cr2Thr.update( + //_crMax.get()*_manual.pitch - _pos.vz); + + // roll channel -> bank angle + float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax()); + float pCmd = _phi2P.update(phiCmd - _att.roll); + + // throttle channel -> velocity + // negative sign because nose over to increase speed + float vCmd = _vLimit.update(_manual.throttle * + (_vLimit.getMax() - _vLimit.getMin()) + + _vLimit.getMin()); + float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v)); + float qCmd = _theta2Q.update(thetaCmd - _att.pitch); + + // yaw rate cmd + float rCmd = 0; + + // stabilization + _stabilization.update(pCmd, qCmd, rCmd, + _att.rollspeed, _att.pitchspeed, _att.yawspeed); + + // output + _actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get(); + _actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get(); + _actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get(); + + // currently using manual throttle + // XXX if you enable this watch out, vz might be very noisy + //_actuators.control[CH_THR] = dThrottle + _trimThr.get(); + _actuators.control[CH_THR] = _manual.throttle; + + // XXX limit throttle to manual setting (safety) for now. + // If it turns out to be confusing, it can be removed later once + // a first binary release can be targeted. + // This is not a hack, but a design choice. + + /* do not limit in HIL */ + if (!_status.flag_hil_enabled) { + /* limit to value of manual throttle */ + _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? + _actuators.control[CH_THR] : _manual.throttle; + } + } + + // body rates controller, disabled for now + else if (0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) { + + _stabilization.update(_manual.roll, _manual.pitch, _manual.yaw, + _att.rollspeed, _att.pitchspeed, _att.yawspeed); + + _actuators.control[CH_AIL] = _stabilization.getAileron(); + _actuators.control[CH_ELV] = _stabilization.getElevator(); + _actuators.control[CH_RDR] = _stabilization.getRudder(); + _actuators.control[CH_THR] = _manual.throttle; + } + } + + // update all publications + updatePublications(); +} + +BlockMultiModeBacksideAutopilot::~BlockMultiModeBacksideAutopilot() +{ + // send one last publication when destroyed, setting + // all output to zero + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) + _actuators.control[i] = 0.0f; + + updatePublications(); +} + +} // namespace fixedwing + +} // namespace control + diff --git a/src/modules/controllib/fixedwing.hpp b/src/modules/controllib/fixedwing.hpp new file mode 100644 index 000000000..e4028c40d --- /dev/null +++ b/src/modules/controllib/fixedwing.hpp @@ -0,0 +1,344 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file fixedwing.h + * + * Controller library code + */ + +#pragma once + +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_global_position_set_triplet.h> +#include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/parameter_update.h> + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <math.h> + +#include <drivers/drv_hrt.h> +#include <poll.h> + +#include "blocks.hpp" +#include "block/UOrbSubscription.hpp" +#include "block/UOrbPublication.hpp" + +extern "C" { +#include <systemlib/geo/geo.h> +} + +namespace control +{ + +namespace fixedwing +{ + +/** + * BlockYawDamper + * + * This block has more explations to help new developers + * add their own blocks. It includes a limited explanation + * of some C++ basics. + * + * Block: The generic class describing a typical block as you + * would expect in Simulink or ScicosLab. A block can have + * parameters. It cannot have other blocks. + * + * SuperBlock: A superblock is a block that can have other + * blocks. It has methods that manage the blocks beneath it. + * + * BlockYawDamper inherits from SuperBlock publically, this + * means that any public function in SuperBlock are public within + * BlockYawDamper and may be called from outside the + * class methods. Any protected function within block + * are private to the class and may not be called from + * outside this class. Protected should be preferred + * where possible to public as it is important to + * limit access to the bare minimum to prevent + * accidental errors. + */ +class __EXPORT BlockYawDamper : public SuperBlock +{ +private: + /** + * Declaring other blocks used by this block + * + * In this section we declare all child blocks that + * this block is composed of. They are private + * members so only this block has direct access to + * them. + */ + BlockLowPass _rLowPass; + BlockHighPass _rWashout; + BlockP _r2Rdr; + + /** + * Declaring output values and accessors + * + * If we have any output values for the block we + * declare them here. Output can be directly returned + * through the update function, but outputs should be + * declared here if the information will likely be requested + * again, or if there are multiple outputs. + * + * You should only be able to set outputs from this block, + * so the outputs are declared in the private section. + * A get accessor is provided + * in the public section for other blocks to get the + * value of the output. + */ + float _rudder; +public: + /** + * BlockYawDamper Constructor + * + * The job of the constructor is to initialize all + * parameter in this block and initialize all child + * blocks. Note also, that the output values must be + * initialized here. The order of the members in the + * member initialization list should follow the + * order in which they are declared within the class. + * See the private member declarations above. + * + * Block Construction + * + * All blocks are constructed with their parent block + * and their name. This allows parameters within the + * block to construct a fully qualified name from + * concatenating the two. If the name provided to the + * block is "", then the block will use the parent + * name as it's name. This is useful in cases where + * you have a block that has parameters "MIN", "MAX", + * such as BlockLimit and you do not want an extra name + * to qualify them since the parent block has no "MIN", + * "MAX" parameters. + * + * Block Parameter Construction + * + * Block parameters are named constants, they are + * constructed using: + * BlockParam::BlockParam(Block * parent, const char * name) + * This funciton takes both a parent block and a name. + * The constructore then uses the parent name and the name of + * the paramter to ask the px4 param library if it has any + * parameters with this name. If it does, a handle to the + * parameter is retrieved. + * + * Block/ BlockParam Naming + * + * When desigining new blocks, the naming of the parameters and + * blocks determines the fully qualified name of the parameters + * within the ground station, so it is important to choose + * short, easily understandable names. Again, when a name of + * "" is passed, the parent block name is used as the value to + * prepend to paramters names. + */ + BlockYawDamper(SuperBlock *parent, const char *name); + /** + * Block deconstructor + * + * It is always a good idea to declare a virtual + * deconstructor so that upon calling delete from + * a class derived from this, all of the + * deconstructors all called, the derived class first, and + * then the base class + */ + virtual ~BlockYawDamper(); + + /** + * Block update function + * + * The job of the update function is to compute the output + * values for the block. In a simple block with one output, + * the output may be returned directly. If the output is + * required frequenly by other processses, it might be a + * good idea to declare a member to store the temporary + * variable. + */ + void update(float rCmd, float r, float outputScale = 1.0); + + /** + * Rudder output value accessor + * + * This is a public accessor function, which means that the + * private value _rudder is returned to anyone calling + * BlockYawDamper::getRudder(). Note thate a setRudder() is + * not provided, this is because the updateParams() call + * for a block provides the mechanism for updating the + * paramter. + */ + float getRudder() { return _rudder; } +}; + +/** + * Stability augmentation system. + * Aircraft Control and Simulation, Stevens and Lewis, pg. 292, 299 + */ +class __EXPORT BlockStabilization : public SuperBlock +{ +private: + BlockYawDamper _yawDamper; + BlockLowPass _pLowPass; + BlockLowPass _qLowPass; + BlockP _p2Ail; + BlockP _q2Elv; + float _aileron; + float _elevator; +public: + BlockStabilization(SuperBlock *parent, const char *name); + virtual ~BlockStabilization(); + void update(float pCmd, float qCmd, float rCmd, + float p, float q, float r, + float outputScale = 1.0); + float getAileron() { return _aileron; } + float getElevator() { return _elevator; } + float getRudder() { return _yawDamper.getRudder(); } +}; + +/** + * Frontside/ Backside Control Systems + * + * Frontside : + * velocity error -> throttle + * altitude error -> elevator + * + * Backside : + * velocity error -> elevator + * altitude error -> throttle + * + * Backside control systems are more resilient at + * slow speeds on the back-side of the power + * required curve/ landing etc. Less performance + * than frontside at high speeds. + */ + +/** + * Waypoint Guidance block + */ +class __EXPORT BlockWaypointGuidance : public SuperBlock +{ +private: + BlockLimitSym _xtYawLimit; + BlockP _xt2Yaw; + float _psiCmd; +public: + BlockWaypointGuidance(SuperBlock *parent, const char *name); + virtual ~BlockWaypointGuidance(); + void update(vehicle_global_position_s &pos, + vehicle_attitude_s &att, + vehicle_global_position_setpoint_s &posCmd, + vehicle_global_position_setpoint_s &lastPosCmd); + float getPsiCmd() { return _psiCmd; } +}; + +/** + * UorbEnabledAutopilot + */ +class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock +{ +protected: + // subscriptions + UOrbSubscription<vehicle_attitude_s> _att; + UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd; + UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd; + UOrbSubscription<vehicle_global_position_s> _pos; + UOrbSubscription<vehicle_global_position_set_triplet_s> _posCmd; + UOrbSubscription<manual_control_setpoint_s> _manual; + UOrbSubscription<vehicle_status_s> _status; + UOrbSubscription<parameter_update_s> _param_update; + // publications + UOrbPublication<actuator_controls_s> _actuators; +public: + BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name); + virtual ~BlockUorbEnabledAutopilot(); +}; + +/** + * Multi-mode Autopilot + */ +class __EXPORT BlockMultiModeBacksideAutopilot : public BlockUorbEnabledAutopilot +{ +private: + // stabilization + BlockStabilization _stabilization; + + // heading hold + BlockP _psi2Phi; + BlockP _phi2P; + BlockLimitSym _phiLimit; + + // velocity hold + BlockPID _v2Theta; + BlockPID _theta2Q; + BlockLimit _theLimit; + BlockLimit _vLimit; + + // altitude/ climb rate hold + BlockPID _h2Thr; + BlockPID _cr2Thr; + + // guidance + BlockWaypointGuidance _guide; + + // block params + BlockParam<float> _trimAil; + BlockParam<float> _trimElv; + BlockParam<float> _trimRdr; + BlockParam<float> _trimThr; + BlockParam<float> _trimV; + BlockParam<float> _vCmd; + BlockParam<float> _crMax; + + struct pollfd _attPoll; + vehicle_global_position_set_triplet_s _lastPosCmd; + enum {CH_AIL, CH_ELV, CH_RDR, CH_THR}; + uint64_t _timeStamp; +public: + BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name); + void update(); + virtual ~BlockMultiModeBacksideAutopilot(); +}; + + +} // namespace fixedwing + +} // namespace control + diff --git a/src/modules/controllib/module.mk b/src/modules/controllib/module.mk new file mode 100644 index 000000000..13d1069c7 --- /dev/null +++ b/src/modules/controllib/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Control library +# +SRCS = test_params.c \ + block/Block.cpp \ + block/BlockParam.cpp \ + block/UOrbPublication.cpp \ + block/UOrbSubscription.cpp \ + blocks.cpp \ + fixedwing.cpp diff --git a/src/modules/controllib/test_params.c b/src/modules/controllib/test_params.c new file mode 100644 index 000000000..7c609cad3 --- /dev/null +++ b/src/modules/controllib/test_params.c @@ -0,0 +1,22 @@ +#include <systemlib/param/param.h> +// WARNING: +// do not changes these unless +// you want to recompute the +// answers for all of the unit tests + +PARAM_DEFINE_FLOAT(TEST_MIN, -1.0f); +PARAM_DEFINE_FLOAT(TEST_MAX, 1.0f); +PARAM_DEFINE_FLOAT(TEST_TRIM, 0.5f); +PARAM_DEFINE_FLOAT(TEST_HP, 10.0f); +PARAM_DEFINE_FLOAT(TEST_LP, 10.0f); + +PARAM_DEFINE_FLOAT(TEST_P, 0.2f); + +PARAM_DEFINE_FLOAT(TEST_I, 0.1f); +PARAM_DEFINE_FLOAT(TEST_I_MAX, 1.0f); + +PARAM_DEFINE_FLOAT(TEST_D, 0.01f); +PARAM_DEFINE_FLOAT(TEST_D_LP, 10.0f); + +PARAM_DEFINE_FLOAT(TEST_MEAN, 1.0f); +PARAM_DEFINE_FLOAT(TEST_DEV, 2.0f); diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c new file mode 100644 index 000000000..2aeca3a98 --- /dev/null +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c @@ -0,0 +1,169 @@ +/**************************************************************************** + * + * 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_att.h b/src/modules/fixedwing_att_control/fixedwing_att_control_att.h new file mode 100644 index 000000000..600e35b89 --- /dev/null +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_att.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * 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 Fixed Wing Attitude Control */ + +#ifndef FIXEDWING_ATT_CONTROL_ATT_H_ +#define FIXEDWING_ATT_CONTROL_ATT_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> + +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); + +#endif /* FIXEDWING_ATT_CONTROL_ATT_H_ */ diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c b/src/modules/fixedwing_att_control/fixedwing_att_control_main.c new file mode 100644 index 000000000..6c9c137bb --- /dev/null +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_main.c @@ -0,0 +1,370 @@ +/**************************************************************************** + * + * 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_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_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 vstatus_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_status), vstatus_sub, &vstatus); + + gyro[0] = att.rollspeed; + gyro[1] = att.pitchspeed; + gyro[2] = att.yawspeed; + + /* control */ + + if (vstatus.state_machine == SYSTEM_STATE_AUTO || + vstatus.state_machine == SYSTEM_STATE_STABILIZED) { + /* 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; + + } else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) { + if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { + + /* 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(); + + /* 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; + + /* pass through flaps */ + if (isfinite(manual_sp.flaps)) { + actuators.control[4] = manual_sp.flaps; + + } else { + actuators.control[4] = 0.0f; + } + + } else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { + /* 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; + } + } + } + + /* 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 new file mode 100644 index 000000000..cdab39edc --- /dev/null +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c @@ -0,0 +1,211 @@ +/**************************************************************************** + * + * 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_att_control/fixedwing_att_control_rate.h b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.h new file mode 100644 index 000000000..500e3e197 --- /dev/null +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.h @@ -0,0 +1,48 @@ +/**************************************************************************** + * + * 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 Fixed Wing Attitude Rate Control */ + +#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); + +#endif /* FIXEDWING_ATT_CONTROL_RATE_H_ */ diff --git a/src/modules/fixedwing_att_control/module.mk b/src/modules/fixedwing_att_control/module.mk new file mode 100644 index 000000000..fd1a8724a --- /dev/null +++ b/src/modules/fixedwing_att_control/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# 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 Attitude Control application +# + +MODULE_COMMAND = fixedwing_att_control + +SRCS = fixedwing_att_control_main.c \ + fixedwing_att_control_att.c \ + fixedwing_att_control_rate.c diff --git a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp new file mode 100644 index 000000000..4803a526e --- /dev/null +++ b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp @@ -0,0 +1,172 @@ +/**************************************************************************** + * + * 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 fixedwing_backside_main.cpp + * @author James Goppert + * + * Fixedwing backside controller using control library + */ + +#include <nuttx/config.h> +#include <unistd.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <systemlib/systemlib.h> +#include <controllib/fixedwing.hpp> +#include <systemlib/param/param.h> +#include <systemlib/err.h> +#include <drivers/drv_hrt.h> +#include <math.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 */ + +/** + * Deamon management function. + */ +extern "C" __EXPORT int fixedwing_backside_main(int argc, char *argv[]); + +/** + * Mainloop of deamon. + */ +int control_demo_thread_main(int argc, char *argv[]); + +/** + * Test function + */ +void test(); + +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + + fprintf(stderr, "usage: fixedwing_backside {start|stop|status} [-p <additional params>]\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_backside_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; + + deamon_task = task_spawn_cmd("fixedwing_backside", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 10, + 5120, + control_demo_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + + if (!strcmp(argv[1], "test")) { + test(); + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("is running"); + + } else { + warnx("not started"); + } + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +int control_demo_thread_main(int argc, char *argv[]) +{ + + warnx("starting"); + + using namespace control; + + fixedwing::BlockMultiModeBacksideAutopilot autopilot(NULL, "FWB"); + + thread_running = true; + + while (!thread_should_exit) { + autopilot.update(); + } + + warnx("exiting."); + + thread_running = false; + + return 0; +} + +void test() +{ + warnx("beginning control lib test"); + control::basicBlocksTest(); +} diff --git a/src/modules/fixedwing_backside/module.mk b/src/modules/fixedwing_backside/module.mk new file mode 100644 index 000000000..ec958d7cb --- /dev/null +++ b/src/modules/fixedwing_backside/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# 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 backside controller +# + +MODULE_COMMAND = fixedwing_backside + +SRCS = fixedwing_backside_main.cpp \ + params.c diff --git a/src/modules/fixedwing_backside/params.c b/src/modules/fixedwing_backside/params.c new file mode 100644 index 000000000..db30af416 --- /dev/null +++ b/src/modules/fixedwing_backside/params.c @@ -0,0 +1,72 @@ +#include <systemlib/param/param.h> + +// currently tuned for easystar from arkhangar in HIL +//https://github.com/arktools/arkhangar + +// 16 is max name length + +// gyro low pass filter +PARAM_DEFINE_FLOAT(FWB_P_LP, 300.0f); // roll rate low pass cut freq +PARAM_DEFINE_FLOAT(FWB_Q_LP, 300.0f); // pitch rate low pass cut freq +PARAM_DEFINE_FLOAT(FWB_R_LP, 300.0f); // yaw rate low pass cut freq + +// yaw washout +PARAM_DEFINE_FLOAT(FWB_R_HP, 1.0f); // yaw rate high pass + +// stabilization mode +PARAM_DEFINE_FLOAT(FWB_P2AIL, 0.3f); // roll rate 2 aileron +PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.1f); // pitch rate 2 elevator +PARAM_DEFINE_FLOAT(FWB_R2RDR, 0.1f); // yaw rate 2 rudder + +// psi -> phi -> p +PARAM_DEFINE_FLOAT(FWB_PSI2PHI, 0.5f); // heading 2 roll +PARAM_DEFINE_FLOAT(FWB_PHI2P, 1.0f); // roll to roll rate +PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 0.3f); // roll limit, 28 deg + +// velocity -> theta +PARAM_DEFINE_FLOAT(FWB_V2THE_P, 1.0f); // velocity to pitch angle PID, prop gain +PARAM_DEFINE_FLOAT(FWB_V2THE_I, 0.0f); // integral gain +PARAM_DEFINE_FLOAT(FWB_V2THE_D, 0.0f); // derivative gain +PARAM_DEFINE_FLOAT(FWB_V2THE_D_LP, 0.0f); // derivative low-pass +PARAM_DEFINE_FLOAT(FWB_V2THE_I_MAX, 0.0f); // integrator wind up guard +PARAM_DEFINE_FLOAT(FWB_THE_MIN, -0.5f); // the max commanded pitch angle +PARAM_DEFINE_FLOAT(FWB_THE_MAX, 0.5f); // the min commanded pitch angle + + +// theta -> q +PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 1.0f); // pitch angle to pitch-rate PID +PARAM_DEFINE_FLOAT(FWB_THE2Q_I, 0.0f); +PARAM_DEFINE_FLOAT(FWB_THE2Q_D, 0.0f); +PARAM_DEFINE_FLOAT(FWB_THE2Q_D_LP, 0.0f); +PARAM_DEFINE_FLOAT(FWB_THE2Q_I_MAX, 0.0f); + +// h -> thr +PARAM_DEFINE_FLOAT(FWB_H2THR_P, 0.01f); // altitude to throttle PID +PARAM_DEFINE_FLOAT(FWB_H2THR_I, 0.0f); +PARAM_DEFINE_FLOAT(FWB_H2THR_D, 0.0f); +PARAM_DEFINE_FLOAT(FWB_H2THR_D_LP, 0.0f); +PARAM_DEFINE_FLOAT(FWB_H2THR_I_MAX, 0.0f); + +// crosstrack +PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.57f); // cross-track to yaw angle limit 90 deg +PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.005f); // cross-track to yaw angle gain + +// speed command +PARAM_DEFINE_FLOAT(FWB_V_MIN, 10.0f); // minimum commanded velocity +PARAM_DEFINE_FLOAT(FWB_V_CMD, 12.0f); // commanded velocity +PARAM_DEFINE_FLOAT(FWB_V_MAX, 16.0f); // maximum commanded velocity + +// rate of climb +// this is what rate of climb is commanded (in m/s) +// when the pitch stick is fully defelcted in simple mode +PARAM_DEFINE_FLOAT(FWB_CR_MAX, 1.0f); + +// climb rate -> thr +PARAM_DEFINE_FLOAT(FWB_CR2THR_P, 0.01f); // rate of climb to throttle PID +PARAM_DEFINE_FLOAT(FWB_CR2THR_I, 0.0f); +PARAM_DEFINE_FLOAT(FWB_CR2THR_D, 0.0f); +PARAM_DEFINE_FLOAT(FWB_CR2THR_D_LP, 0.0f); +PARAM_DEFINE_FLOAT(FWB_CR2THR_I_MAX, 0.0f); + +PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.8f); // trim throttle (0,1) +PARAM_DEFINE_FLOAT(FWB_TRIM_V, 12.0f); // trim velocity, m/s diff --git a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c new file mode 100644 index 000000000..73df3fb9e --- /dev/null +++ b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c @@ -0,0 +1,479 @@ +/**************************************************************************** + * + * 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((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d, + (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 new file mode 100644 index 000000000..b976377e9 --- /dev/null +++ b/src/modules/fixedwing_pos_control/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# 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/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c new file mode 100644 index 000000000..8b4c0cb30 --- /dev/null +++ b/src/modules/gpio_led/gpio_led.c @@ -0,0 +1,285 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * 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 + * 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 gpio_led.c + * + * Status LED via GPIO driver. + * + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <fcntl.h> +#include <stdbool.h> +#include <nuttx/wqueue.h> +#include <nuttx/clock.h> +#include <systemlib/systemlib.h> +#include <systemlib/err.h> +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_status.h> +#include <poll.h> +#include <drivers/drv_gpio.h> + +#define PX4IO_RELAY1 (1<<0) +#define PX4IO_RELAY2 (1<<1) +#define PX4IO_ACC1 (1<<2) +#define PX4IO_ACC2 (1<<3) + +struct gpio_led_s { + struct work_s work; + int gpio_fd; + bool use_io; + int pin; + struct vehicle_status_s status; + int vehicle_status_sub; + bool led_state; + int counter; +}; + +static struct gpio_led_s gpio_led_data; +static bool gpio_led_started = false; + +__EXPORT int gpio_led_main(int argc, char *argv[]); + +void gpio_led_start(FAR void *arg); + +void gpio_led_cycle(FAR void *arg); + +int gpio_led_main(int argc, char *argv[]) +{ + if (argc < 2) { + errx(1, "usage: gpio_led {start|stop} [-p <1|2|a1|a2|r1|r2>]\n" + "\t-p\tUse pin:\n" + "\t\t1\tPX4FMU GPIO_EXT1 (default)\n" + "\t\t2\tPX4FMU GPIO_EXT2\n" + "\t\ta1\tPX4IO ACC1\n" + "\t\ta2\tPX4IO ACC2\n" + "\t\tr1\tPX4IO RELAY1\n" + "\t\tr2\tPX4IO RELAY2"); + + } else { + + if (!strcmp(argv[1], "start")) { + if (gpio_led_started) { + errx(1, "already running"); + } + + bool use_io = false; + int pin = GPIO_EXT_1; + + if (argc > 2) { + if (!strcmp(argv[2], "-p")) { + if (!strcmp(argv[3], "1")) { + use_io = false; + pin = GPIO_EXT_1; + + } else if (!strcmp(argv[3], "2")) { + use_io = false; + pin = GPIO_EXT_2; + + } else if (!strcmp(argv[3], "a1")) { + use_io = true; + pin = PX4IO_ACC1; + + } else if (!strcmp(argv[3], "a2")) { + use_io = true; + pin = PX4IO_ACC2; + + } else if (!strcmp(argv[3], "r1")) { + use_io = true; + pin = PX4IO_RELAY1; + + } else if (!strcmp(argv[3], "r2")) { + use_io = true; + pin = PX4IO_RELAY2; + + } else { + errx(1, "unsupported pin: %s", argv[3]); + } + } + } + + memset(&gpio_led_data, 0, sizeof(gpio_led_data)); + gpio_led_data.use_io = use_io; + gpio_led_data.pin = pin; + int ret = work_queue(LPWORK, &gpio_led_data.work, gpio_led_start, &gpio_led_data, 0); + + if (ret != 0) { + errx(1, "failed to queue work: %d", ret); + + } else { + gpio_led_started = true; + char pin_name[24]; + + if (use_io) { + if (pin & (PX4IO_ACC1 | PX4IO_ACC2)) { + sprintf(pin_name, "PX4IO ACC%i", (pin >> 3)); + + } else { + sprintf(pin_name, "PX4IO RELAY%i", pin); + } + + } else { + sprintf(pin_name, "PX4FMU GPIO_EXT%i", pin); + + } + + warnx("start, using pin: %s", pin_name); + } + + exit(0); + + + } else if (!strcmp(argv[1], "stop")) { + if (gpio_led_started) { + gpio_led_started = false; + warnx("stop"); + + } else { + errx(1, "not running"); + } + + } else { + errx(1, "unrecognized command '%s', only supporting 'start' or 'stop'", argv[1]); + } + } +} + +void gpio_led_start(FAR void *arg) +{ + FAR struct gpio_led_s *priv = (FAR struct gpio_led_s *)arg; + + char *gpio_dev; + + if (priv->use_io) { + gpio_dev = "/dev/px4io"; + + } else { + gpio_dev = "/dev/px4fmu"; + } + + /* open GPIO device */ + priv->gpio_fd = open(gpio_dev, 0); + + if (priv->gpio_fd < 0) { + // TODO find way to print errors + //printf("gpio_led: GPIO device \"%s\" open fail\n", gpio_dev); + gpio_led_started = false; + return; + } + + /* configure GPIO pin */ + ioctl(priv->gpio_fd, GPIO_SET_OUTPUT, priv->pin); + + /* subscribe to vehicle status topic */ + memset(&priv->status, 0, sizeof(priv->status)); + priv->vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + + /* add worker to queue */ + int ret = work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, 0); + + if (ret != 0) { + // TODO find way to print errors + //printf("gpio_led: failed to queue work: %d\n", ret); + gpio_led_started = false; + return; + } +} + +void gpio_led_cycle(FAR void *arg) +{ + FAR struct gpio_led_s *priv = (FAR struct gpio_led_s *)arg; + + /* check for status updates*/ + bool status_updated; + orb_check(priv->vehicle_status_sub, &status_updated); + + if (status_updated) + orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status); + + /* select pattern for current status */ + int pattern = 0; + + if (priv->status.flag_system_armed) { + if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { + pattern = 0x3f; // ****** solid (armed) + + } else { + pattern = 0x2A; // *_*_*_ fast blink (armed, battery warning) + } + + } else { + if (priv->status.state_machine == SYSTEM_STATE_PREFLIGHT) { + pattern = 0x00; // ______ off (disarmed, preflight check) + + } else if (priv->status.state_machine == SYSTEM_STATE_STANDBY && + priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { + pattern = 0x38; // ***___ slow blink (disarmed, ready) + + } else { + pattern = 0x28; // *_*___ slow double blink (disarmed, not good to arm) + } + } + + /* blink pattern */ + bool led_state_new = (pattern & (1 << priv->counter)) != 0; + + if (led_state_new != priv->led_state) { + priv->led_state = led_state_new; + + if (led_state_new) { + ioctl(priv->gpio_fd, GPIO_SET, priv->pin); + + } else { + ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin); + } + } + + priv->counter++; + + if (priv->counter > 5) + priv->counter = 0; + + /* repeat cycle at 5 Hz */ + if (gpio_led_started) { + work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, USEC2TICK(200000)); + + } else { + /* switch off LED on stop */ + ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin); + } +} diff --git a/src/modules/gpio_led/module.mk b/src/modules/gpio_led/module.mk new file mode 100644 index 000000000..3b8553489 --- /dev/null +++ b/src/modules/gpio_led/module.mk @@ -0,0 +1,39 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Status LED via GPIO driver +# + +MODULE_COMMAND = gpio_led +SRCS = gpio_led.c diff --git a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h b/src/modules/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h new file mode 100644 index 000000000..8f39acd9d --- /dev/null +++ b/src/modules/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h @@ -0,0 +1,264 @@ +/**************************************************************************//**
+ * @file ARMCM3.h
+ * @brief CMSIS Core Peripheral Access Layer Header File for
+ * ARMCM3 Device Series
+ * @version V1.07
+ * @date 30. January 2012
+ *
+ * @note
+ * Copyright (C) 2012 ARM Limited. All rights reserved.
+ *
+ * @par
+ * ARM Limited (ARM) is supplying this software for use with Cortex-M
+ * processor based microcontrollers. This file can be freely distributed
+ * within development tools that are supporting such ARM based processors.
+ *
+ * @par
+ * THIS SOFTWARE IS PROVIDED "AS IS". NO WARRANTIES, WHETHER EXPRESS, IMPLIED
+ * OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE.
+ * ARM SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL, OR
+ * CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.
+ *
+ ******************************************************************************/
+
+#ifndef ARMCM3_H
+#define ARMCM3_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/* ------------------------- Interrupt Number Definition ------------------------ */
+
+typedef enum IRQn
+{
+/* ------------------- Cortex-M3 Processor Exceptions Numbers ------------------- */
+ NonMaskableInt_IRQn = -14, /*!< 2 Non Maskable Interrupt */
+ HardFault_IRQn = -13, /*!< 3 HardFault Interrupt */
+ MemoryManagement_IRQn = -12, /*!< 4 Memory Management Interrupt */
+ BusFault_IRQn = -11, /*!< 5 Bus Fault Interrupt */
+ UsageFault_IRQn = -10, /*!< 6 Usage Fault Interrupt */
+ SVCall_IRQn = -5, /*!< 11 SV Call Interrupt */
+ DebugMonitor_IRQn = -4, /*!< 12 Debug Monitor Interrupt */
+ PendSV_IRQn = -2, /*!< 14 Pend SV Interrupt */
+ SysTick_IRQn = -1, /*!< 15 System Tick Interrupt */
+
+/* ---------------------- ARMCM3 Specific Interrupt Numbers --------------------- */
+ WDT_IRQn = 0, /*!< Watchdog Timer Interrupt */
+ RTC_IRQn = 1, /*!< Real Time Clock Interrupt */
+ TIM0_IRQn = 2, /*!< Timer0 / Timer1 Interrupt */
+ TIM2_IRQn = 3, /*!< Timer2 / Timer3 Interrupt */
+ MCIA_IRQn = 4, /*!< MCIa Interrupt */
+ MCIB_IRQn = 5, /*!< MCIb Interrupt */
+ UART0_IRQn = 6, /*!< UART0 Interrupt */
+ UART1_IRQn = 7, /*!< UART1 Interrupt */
+ UART2_IRQn = 8, /*!< UART2 Interrupt */
+ UART4_IRQn = 9, /*!< UART4 Interrupt */
+ AACI_IRQn = 10, /*!< AACI / AC97 Interrupt */
+ CLCD_IRQn = 11, /*!< CLCD Combined Interrupt */
+ ENET_IRQn = 12, /*!< Ethernet Interrupt */
+ USBDC_IRQn = 13, /*!< USB Device Interrupt */
+ USBHC_IRQn = 14, /*!< USB Host Controller Interrupt */
+ CHLCD_IRQn = 15, /*!< Character LCD Interrupt */
+ FLEXRAY_IRQn = 16, /*!< Flexray Interrupt */
+ CAN_IRQn = 17, /*!< CAN Interrupt */
+ LIN_IRQn = 18, /*!< LIN Interrupt */
+ I2C_IRQn = 19, /*!< I2C ADC/DAC Interrupt */
+ CPU_CLCD_IRQn = 28, /*!< CPU CLCD Combined Interrupt */
+ UART3_IRQn = 30, /*!< UART3 Interrupt */
+ SPI_IRQn = 31, /*!< SPI Touchscreen Interrupt */
+} IRQn_Type;
+
+
+/* ================================================================================ */
+/* ================ Processor and Core Peripheral Section ================ */
+/* ================================================================================ */
+
+/* -------- Configuration of the Cortex-M4 Processor and Core Peripherals ------- */
+#define __CM3_REV 0x0201 /*!< Core revision r2p1 */
+#define __MPU_PRESENT 1 /*!< MPU present or not */
+#define __NVIC_PRIO_BITS 3 /*!< Number of Bits used for Priority Levels */
+#define __Vendor_SysTickConfig 0 /*!< Set to 1 if different SysTick Config is used */
+
+#include <core_cm3.h> /* Processor and core peripherals */
+/* NuttX */
+//#include "system_ARMCM3.h" /* System Header */
+
+
+/* ================================================================================ */
+/* ================ Device Specific Peripheral Section ================ */
+/* ================================================================================ */
+
+/* ------------------- Start of section using anonymous unions ------------------ */
+#if defined(__CC_ARM)
+ #pragma push
+ #pragma anon_unions
+#elif defined(__ICCARM__)
+ #pragma language=extended
+#elif defined(__GNUC__)
+ /* anonymous unions are enabled by default */
+#elif defined(__TMS470__)
+/* anonymous unions are enabled by default */
+#elif defined(__TASKING__)
+ #pragma warning 586
+#else
+ #warning Not supported compiler type
+#endif
+
+
+
+/* ================================================================================ */
+/* ================ CPU FPGA System (CPU_SYS) ================ */
+/* ================================================================================ */
+typedef struct
+{
+ __I uint32_t ID; /* Offset: 0x000 (R/ ) Board and FPGA Identifier */
+ __IO uint32_t MEMCFG; /* Offset: 0x004 (R/W) Remap and Alias Memory Control */
+ __I uint32_t SW; /* Offset: 0x008 (R/ ) Switch States */
+ __IO uint32_t LED; /* Offset: 0x00C (R/W) LED Output States */
+ __I uint32_t TS; /* Offset: 0x010 (R/ ) Touchscreen Register */
+ __IO uint32_t CTRL1; /* Offset: 0x014 (R/W) Misc Control Functions */
+ uint32_t RESERVED0[2];
+ __IO uint32_t CLKCFG; /* Offset: 0x020 (R/W) System Clock Configuration */
+ __IO uint32_t WSCFG; /* Offset: 0x024 (R/W) Flash Waitstate Configuration */
+ __IO uint32_t CPUCFG; /* Offset: 0x028 (R/W) Processor Configuration */
+ uint32_t RESERVED1[3];
+ __IO uint32_t BASE; /* Offset: 0x038 (R/W) ROM Table base Address */
+ __IO uint32_t ID2; /* Offset: 0x03C (R/W) Secondary Identification Register */
+} ARM_CPU_SYS_TypeDef;
+
+
+/* ================================================================================ */
+/* ================ DUT FPGA System (DUT_SYS) ================ */
+/* ================================================================================ */
+typedef struct
+{
+ __I uint32_t ID; /* Offset: 0x000 (R/ ) Board and FPGA Identifier */
+ __IO uint32_t PERCFG; /* Offset: 0x004 (R/W) Peripheral Control Signals */
+ __I uint32_t SW; /* Offset: 0x008 (R/ ) Switch States */
+ __IO uint32_t LED; /* Offset: 0x00C (R/W) LED Output States */
+ __IO uint32_t SEG7; /* Offset: 0x010 (R/W) 7-segment LED Output States */
+ __I uint32_t CNT25MHz; /* Offset: 0x014 (R/ ) Freerunning counter incrementing at 25MHz */
+ __I uint32_t CNT100Hz; /* Offset: 0x018 (R/ ) Freerunning counter incrementing at 100Hz */
+} ARM_DUT_SYS_TypeDef;
+
+
+/* ================================================================================ */
+/* ================ Timer (TIM) ================ */
+/* ================================================================================ */
+typedef struct
+{
+ __IO uint32_t Timer1Load; /* Offset: 0x000 (R/W) Timer 1 Load */
+ __I uint32_t Timer1Value; /* Offset: 0x004 (R/ ) Timer 1 Counter Current Value */
+ __IO uint32_t Timer1Control; /* Offset: 0x008 (R/W) Timer 1 Control */
+ __O uint32_t Timer1IntClr; /* Offset: 0x00C ( /W) Timer 1 Interrupt Clear */
+ __I uint32_t Timer1RIS; /* Offset: 0x010 (R/ ) Timer 1 Raw Interrupt Status */
+ __I uint32_t Timer1MIS; /* Offset: 0x014 (R/ ) Timer 1 Masked Interrupt Status */
+ __IO uint32_t Timer1BGLoad; /* Offset: 0x018 (R/W) Background Load Register */
+ uint32_t RESERVED0[1];
+ __IO uint32_t Timer2Load; /* Offset: 0x020 (R/W) Timer 2 Load */
+ __I uint32_t Timer2Value; /* Offset: 0x024 (R/ ) Timer 2 Counter Current Value */
+ __IO uint32_t Timer2Control; /* Offset: 0x028 (R/W) Timer 2 Control */
+ __O uint32_t Timer2IntClr; /* Offset: 0x02C ( /W) Timer 2 Interrupt Clear */
+ __I uint32_t Timer2RIS; /* Offset: 0x030 (R/ ) Timer 2 Raw Interrupt Status */
+ __I uint32_t Timer2MIS; /* Offset: 0x034 (R/ ) Timer 2 Masked Interrupt Status */
+ __IO uint32_t Timer2BGLoad; /* Offset: 0x038 (R/W) Background Load Register */
+} ARM_TIM_TypeDef;
+
+
+/* ================================================================================ */
+/* ============== Universal Asyncronous Receiver / Transmitter (UART) ============= */
+/* ================================================================================ */
+typedef struct
+{
+ __IO uint32_t DR; /* Offset: 0x000 (R/W) Data */
+ union {
+ __I uint32_t RSR; /* Offset: 0x000 (R/ ) Receive Status */
+ __O uint32_t ECR; /* Offset: 0x000 ( /W) Error Clear */
+ };
+ uint32_t RESERVED0[4];
+ __IO uint32_t FR; /* Offset: 0x018 (R/W) Flags */
+ uint32_t RESERVED1[1];
+ __IO uint32_t ILPR; /* Offset: 0x020 (R/W) IrDA Low-power Counter */
+ __IO uint32_t IBRD; /* Offset: 0x024 (R/W) Interger Baud Rate */
+ __IO uint32_t FBRD; /* Offset: 0x028 (R/W) Fractional Baud Rate */
+ __IO uint32_t LCR_H; /* Offset: 0x02C (R/W) Line Control */
+ __IO uint32_t CR; /* Offset: 0x030 (R/W) Control */
+ __IO uint32_t IFLS; /* Offset: 0x034 (R/W) Interrupt FIFO Level Select */
+ __IO uint32_t IMSC; /* Offset: 0x038 (R/W) Interrupt Mask Set / Clear */
+ __IO uint32_t RIS; /* Offset: 0x03C (R/W) Raw Interrupt Status */
+ __IO uint32_t MIS; /* Offset: 0x040 (R/W) Masked Interrupt Status */
+ __O uint32_t ICR; /* Offset: 0x044 ( /W) Interrupt Clear */
+ __IO uint32_t DMACR; /* Offset: 0x048 (R/W) DMA Control */
+} ARM_UART_TypeDef;
+
+
+/* -------------------- End of section using anonymous unions ------------------- */
+#if defined(__CC_ARM)
+ #pragma pop
+#elif defined(__ICCARM__)
+ /* leave anonymous unions enabled */
+#elif defined(__GNUC__)
+ /* anonymous unions are enabled by default */
+#elif defined(__TMS470__)
+ /* anonymous unions are enabled by default */
+#elif defined(__TASKING__)
+ #pragma warning restore
+#else
+ #warning Not supported compiler type
+#endif
+
+
+
+
+/* ================================================================================ */
+/* ================ Peripheral memory map ================ */
+/* ================================================================================ */
+/* -------------------------- CPU FPGA memory map ------------------------------- */
+#define ARM_FLASH_BASE (0x00000000UL)
+#define ARM_RAM_BASE (0x20000000UL)
+#define ARM_RAM_FPGA_BASE (0x1EFF0000UL)
+#define ARM_CPU_CFG_BASE (0xDFFF0000UL)
+
+#define ARM_CPU_SYS_BASE (ARM_CPU_CFG_BASE + 0x00000)
+#define ARM_UART3_BASE (ARM_CPU_CFG_BASE + 0x05000)
+
+/* -------------------------- DUT FPGA memory map ------------------------------- */
+#define ARM_APB_BASE (0x40000000UL)
+#define ARM_AHB_BASE (0x4FF00000UL)
+#define ARM_DMC_BASE (0x60000000UL)
+#define ARM_SMC_BASE (0xA0000000UL)
+
+#define ARM_TIM0_BASE (ARM_APB_BASE + 0x02000)
+#define ARM_TIM2_BASE (ARM_APB_BASE + 0x03000)
+#define ARM_DUT_SYS_BASE (ARM_APB_BASE + 0x04000)
+#define ARM_UART0_BASE (ARM_APB_BASE + 0x06000)
+#define ARM_UART1_BASE (ARM_APB_BASE + 0x07000)
+#define ARM_UART2_BASE (ARM_APB_BASE + 0x08000)
+#define ARM_UART4_BASE (ARM_APB_BASE + 0x09000)
+
+
+/* ================================================================================ */
+/* ================ Peripheral declaration ================ */
+/* ================================================================================ */
+/* -------------------------- CPU FPGA Peripherals ------------------------------ */
+#define ARM_CPU_SYS ((ARM_CPU_SYS_TypeDef *) ARM_CPU_SYS_BASE)
+#define ARM_UART3 (( ARM_UART_TypeDef *) ARM_UART3_BASE)
+
+/* -------------------------- DUT FPGA Peripherals ------------------------------ */
+#define ARM_DUT_SYS ((ARM_DUT_SYS_TypeDef *) ARM_DUT_SYS_BASE)
+#define ARM_TIM0 (( ARM_TIM_TypeDef *) ARM_TIM0_BASE)
+#define ARM_TIM2 (( ARM_TIM_TypeDef *) ARM_TIM2_BASE)
+#define ARM_UART0 (( ARM_UART_TypeDef *) ARM_UART0_BASE)
+#define ARM_UART1 (( ARM_UART_TypeDef *) ARM_UART1_BASE)
+#define ARM_UART2 (( ARM_UART_TypeDef *) ARM_UART2_BASE)
+#define ARM_UART4 (( ARM_UART_TypeDef *) ARM_UART4_BASE)
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* ARMCM3_H */
diff --git a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h b/src/modules/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h new file mode 100644 index 000000000..181b7e433 --- /dev/null +++ b/src/modules/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h @@ -0,0 +1,265 @@ +/**************************************************************************//**
+ * @file ARMCM4.h
+ * @brief CMSIS Core Peripheral Access Layer Header File for
+ * ARMCM4 Device Series
+ * @version V1.07
+ * @date 30. January 2012
+ *
+ * @note
+ * Copyright (C) 2012 ARM Limited. All rights reserved.
+ *
+ * @par
+ * ARM Limited (ARM) is supplying this software for use with Cortex-M
+ * processor based microcontrollers. This file can be freely distributed
+ * within development tools that are supporting such ARM based processors.
+ *
+ * @par
+ * THIS SOFTWARE IS PROVIDED "AS IS". NO WARRANTIES, WHETHER EXPRESS, IMPLIED
+ * OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE.
+ * ARM SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL, OR
+ * CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.
+ *
+ ******************************************************************************/
+
+#ifndef ARMCM4_H
+#define ARMCM4_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/* ------------------------- Interrupt Number Definition ------------------------ */
+
+typedef enum IRQn
+{
+/* ------------------- Cortex-M4 Processor Exceptions Numbers ------------------- */
+ NonMaskableInt_IRQn = -14, /*!< 2 Non Maskable Interrupt */
+ HardFault_IRQn = -13, /*!< 3 HardFault Interrupt */
+ MemoryManagement_IRQn = -12, /*!< 4 Memory Management Interrupt */
+ BusFault_IRQn = -11, /*!< 5 Bus Fault Interrupt */
+ UsageFault_IRQn = -10, /*!< 6 Usage Fault Interrupt */
+ SVCall_IRQn = -5, /*!< 11 SV Call Interrupt */
+ DebugMonitor_IRQn = -4, /*!< 12 Debug Monitor Interrupt */
+ PendSV_IRQn = -2, /*!< 14 Pend SV Interrupt */
+ SysTick_IRQn = -1, /*!< 15 System Tick Interrupt */
+
+/* ---------------------- ARMCM4 Specific Interrupt Numbers --------------------- */
+ WDT_IRQn = 0, /*!< Watchdog Timer Interrupt */
+ RTC_IRQn = 1, /*!< Real Time Clock Interrupt */
+ TIM0_IRQn = 2, /*!< Timer0 / Timer1 Interrupt */
+ TIM2_IRQn = 3, /*!< Timer2 / Timer3 Interrupt */
+ MCIA_IRQn = 4, /*!< MCIa Interrupt */
+ MCIB_IRQn = 5, /*!< MCIb Interrupt */
+ UART0_IRQn = 6, /*!< UART0 Interrupt */
+ UART1_IRQn = 7, /*!< UART1 Interrupt */
+ UART2_IRQn = 8, /*!< UART2 Interrupt */
+ UART4_IRQn = 9, /*!< UART4 Interrupt */
+ AACI_IRQn = 10, /*!< AACI / AC97 Interrupt */
+ CLCD_IRQn = 11, /*!< CLCD Combined Interrupt */
+ ENET_IRQn = 12, /*!< Ethernet Interrupt */
+ USBDC_IRQn = 13, /*!< USB Device Interrupt */
+ USBHC_IRQn = 14, /*!< USB Host Controller Interrupt */
+ CHLCD_IRQn = 15, /*!< Character LCD Interrupt */
+ FLEXRAY_IRQn = 16, /*!< Flexray Interrupt */
+ CAN_IRQn = 17, /*!< CAN Interrupt */
+ LIN_IRQn = 18, /*!< LIN Interrupt */
+ I2C_IRQn = 19, /*!< I2C ADC/DAC Interrupt */
+ CPU_CLCD_IRQn = 28, /*!< CPU CLCD Combined Interrupt */
+ UART3_IRQn = 30, /*!< UART3 Interrupt */
+ SPI_IRQn = 31, /*!< SPI Touchscreen Interrupt */
+} IRQn_Type;
+
+
+/* ================================================================================ */
+/* ================ Processor and Core Peripheral Section ================ */
+/* ================================================================================ */
+
+/* -------- Configuration of the Cortex-M4 Processor and Core Peripherals ------- */
+#define __CM4_REV 0x0001 /*!< Core revision r0p1 */
+#define __MPU_PRESENT 1 /*!< MPU present or not */
+#define __NVIC_PRIO_BITS 3 /*!< Number of Bits used for Priority Levels */
+#define __Vendor_SysTickConfig 0 /*!< Set to 1 if different SysTick Config is used */
+#define __FPU_PRESENT 1 /*!< FPU present or not */
+
+#include <core_cm4.h> /* Processor and core peripherals */
+/* NuttX */
+//#include "system_ARMCM4.h" /* System Header */
+
+
+/* ================================================================================ */
+/* ================ Device Specific Peripheral Section ================ */
+/* ================================================================================ */
+
+/* ------------------- Start of section using anonymous unions ------------------ */
+#if defined(__CC_ARM)
+ #pragma push
+ #pragma anon_unions
+#elif defined(__ICCARM__)
+ #pragma language=extended
+#elif defined(__GNUC__)
+ /* anonymous unions are enabled by default */
+#elif defined(__TMS470__)
+/* anonymous unions are enabled by default */
+#elif defined(__TASKING__)
+ #pragma warning 586
+#else
+ #warning Not supported compiler type
+#endif
+
+
+
+/* ================================================================================ */
+/* ================ CPU FPGA System (CPU_SYS) ================ */
+/* ================================================================================ */
+typedef struct
+{
+ __I uint32_t ID; /* Offset: 0x000 (R/ ) Board and FPGA Identifier */
+ __IO uint32_t MEMCFG; /* Offset: 0x004 (R/W) Remap and Alias Memory Control */
+ __I uint32_t SW; /* Offset: 0x008 (R/ ) Switch States */
+ __IO uint32_t LED; /* Offset: 0x00C (R/W) LED Output States */
+ __I uint32_t TS; /* Offset: 0x010 (R/ ) Touchscreen Register */
+ __IO uint32_t CTRL1; /* Offset: 0x014 (R/W) Misc Control Functions */
+ uint32_t RESERVED0[2];
+ __IO uint32_t CLKCFG; /* Offset: 0x020 (R/W) System Clock Configuration */
+ __IO uint32_t WSCFG; /* Offset: 0x024 (R/W) Flash Waitstate Configuration */
+ __IO uint32_t CPUCFG; /* Offset: 0x028 (R/W) Processor Configuration */
+ uint32_t RESERVED1[3];
+ __IO uint32_t BASE; /* Offset: 0x038 (R/W) ROM Table base Address */
+ __IO uint32_t ID2; /* Offset: 0x03C (R/W) Secondary Identification Register */
+} ARM_CPU_SYS_TypeDef;
+
+
+/* ================================================================================ */
+/* ================ DUT FPGA System (DUT_SYS) ================ */
+/* ================================================================================ */
+typedef struct
+{
+ __I uint32_t ID; /* Offset: 0x000 (R/ ) Board and FPGA Identifier */
+ __IO uint32_t PERCFG; /* Offset: 0x004 (R/W) Peripheral Control Signals */
+ __I uint32_t SW; /* Offset: 0x008 (R/ ) Switch States */
+ __IO uint32_t LED; /* Offset: 0x00C (R/W) LED Output States */
+ __IO uint32_t SEG7; /* Offset: 0x010 (R/W) 7-segment LED Output States */
+ __I uint32_t CNT25MHz; /* Offset: 0x014 (R/ ) Freerunning counter incrementing at 25MHz */
+ __I uint32_t CNT100Hz; /* Offset: 0x018 (R/ ) Freerunning counter incrementing at 100Hz */
+} ARM_DUT_SYS_TypeDef;
+
+
+/* ================================================================================ */
+/* ================ Timer (TIM) ================ */
+/* ================================================================================ */
+typedef struct
+{
+ __IO uint32_t Timer1Load; /* Offset: 0x000 (R/W) Timer 1 Load */
+ __I uint32_t Timer1Value; /* Offset: 0x004 (R/ ) Timer 1 Counter Current Value */
+ __IO uint32_t Timer1Control; /* Offset: 0x008 (R/W) Timer 1 Control */
+ __O uint32_t Timer1IntClr; /* Offset: 0x00C ( /W) Timer 1 Interrupt Clear */
+ __I uint32_t Timer1RIS; /* Offset: 0x010 (R/ ) Timer 1 Raw Interrupt Status */
+ __I uint32_t Timer1MIS; /* Offset: 0x014 (R/ ) Timer 1 Masked Interrupt Status */
+ __IO uint32_t Timer1BGLoad; /* Offset: 0x018 (R/W) Background Load Register */
+ uint32_t RESERVED0[1];
+ __IO uint32_t Timer2Load; /* Offset: 0x020 (R/W) Timer 2 Load */
+ __I uint32_t Timer2Value; /* Offset: 0x024 (R/ ) Timer 2 Counter Current Value */
+ __IO uint32_t Timer2Control; /* Offset: 0x028 (R/W) Timer 2 Control */
+ __O uint32_t Timer2IntClr; /* Offset: 0x02C ( /W) Timer 2 Interrupt Clear */
+ __I uint32_t Timer2RIS; /* Offset: 0x030 (R/ ) Timer 2 Raw Interrupt Status */
+ __I uint32_t Timer2MIS; /* Offset: 0x034 (R/ ) Timer 2 Masked Interrupt Status */
+ __IO uint32_t Timer2BGLoad; /* Offset: 0x038 (R/W) Background Load Register */
+} ARM_TIM_TypeDef;
+
+
+/* ================================================================================ */
+/* ============== Universal Asyncronous Receiver / Transmitter (UART) ============= */
+/* ================================================================================ */
+typedef struct
+{
+ __IO uint32_t DR; /* Offset: 0x000 (R/W) Data */
+ union {
+ __I uint32_t RSR; /* Offset: 0x000 (R/ ) Receive Status */
+ __O uint32_t ECR; /* Offset: 0x000 ( /W) Error Clear */
+ };
+ uint32_t RESERVED0[4];
+ __IO uint32_t FR; /* Offset: 0x018 (R/W) Flags */
+ uint32_t RESERVED1[1];
+ __IO uint32_t ILPR; /* Offset: 0x020 (R/W) IrDA Low-power Counter */
+ __IO uint32_t IBRD; /* Offset: 0x024 (R/W) Interger Baud Rate */
+ __IO uint32_t FBRD; /* Offset: 0x028 (R/W) Fractional Baud Rate */
+ __IO uint32_t LCR_H; /* Offset: 0x02C (R/W) Line Control */
+ __IO uint32_t CR; /* Offset: 0x030 (R/W) Control */
+ __IO uint32_t IFLS; /* Offset: 0x034 (R/W) Interrupt FIFO Level Select */
+ __IO uint32_t IMSC; /* Offset: 0x038 (R/W) Interrupt Mask Set / Clear */
+ __IO uint32_t RIS; /* Offset: 0x03C (R/W) Raw Interrupt Status */
+ __IO uint32_t MIS; /* Offset: 0x040 (R/W) Masked Interrupt Status */
+ __O uint32_t ICR; /* Offset: 0x044 ( /W) Interrupt Clear */
+ __IO uint32_t DMACR; /* Offset: 0x048 (R/W) DMA Control */
+} ARM_UART_TypeDef;
+
+
+/* -------------------- End of section using anonymous unions ------------------- */
+#if defined(__CC_ARM)
+ #pragma pop
+#elif defined(__ICCARM__)
+ /* leave anonymous unions enabled */
+#elif defined(__GNUC__)
+ /* anonymous unions are enabled by default */
+#elif defined(__TMS470__)
+ /* anonymous unions are enabled by default */
+#elif defined(__TASKING__)
+ #pragma warning restore
+#else
+ #warning Not supported compiler type
+#endif
+
+
+
+
+/* ================================================================================ */
+/* ================ Peripheral memory map ================ */
+/* ================================================================================ */
+/* -------------------------- CPU FPGA memory map ------------------------------- */
+#define ARM_FLASH_BASE (0x00000000UL)
+#define ARM_RAM_BASE (0x20000000UL)
+#define ARM_RAM_FPGA_BASE (0x1EFF0000UL)
+#define ARM_CPU_CFG_BASE (0xDFFF0000UL)
+
+#define ARM_CPU_SYS_BASE (ARM_CPU_CFG_BASE + 0x00000)
+#define ARM_UART3_BASE (ARM_CPU_CFG_BASE + 0x05000)
+
+/* -------------------------- DUT FPGA memory map ------------------------------- */
+#define ARM_APB_BASE (0x40000000UL)
+#define ARM_AHB_BASE (0x4FF00000UL)
+#define ARM_DMC_BASE (0x60000000UL)
+#define ARM_SMC_BASE (0xA0000000UL)
+
+#define ARM_TIM0_BASE (ARM_APB_BASE + 0x02000)
+#define ARM_TIM2_BASE (ARM_APB_BASE + 0x03000)
+#define ARM_DUT_SYS_BASE (ARM_APB_BASE + 0x04000)
+#define ARM_UART0_BASE (ARM_APB_BASE + 0x06000)
+#define ARM_UART1_BASE (ARM_APB_BASE + 0x07000)
+#define ARM_UART2_BASE (ARM_APB_BASE + 0x08000)
+#define ARM_UART4_BASE (ARM_APB_BASE + 0x09000)
+
+
+/* ================================================================================ */
+/* ================ Peripheral declaration ================ */
+/* ================================================================================ */
+/* -------------------------- CPU FPGA Peripherals ------------------------------ */
+#define ARM_CPU_SYS ((ARM_CPU_SYS_TypeDef *) ARM_CPU_SYS_BASE)
+#define ARM_UART3 (( ARM_UART_TypeDef *) ARM_UART3_BASE)
+
+/* -------------------------- DUT FPGA Peripherals ------------------------------ */
+#define ARM_DUT_SYS ((ARM_DUT_SYS_TypeDef *) ARM_DUT_SYS_BASE)
+#define ARM_TIM0 (( ARM_TIM_TypeDef *) ARM_TIM0_BASE)
+#define ARM_TIM2 (( ARM_TIM_TypeDef *) ARM_TIM2_BASE)
+#define ARM_UART0 (( ARM_UART_TypeDef *) ARM_UART0_BASE)
+#define ARM_UART1 (( ARM_UART_TypeDef *) ARM_UART1_BASE)
+#define ARM_UART2 (( ARM_UART_TypeDef *) ARM_UART2_BASE)
+#define ARM_UART4 (( ARM_UART_TypeDef *) ARM_UART4_BASE)
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* ARMCM4_H */
diff --git a/src/modules/mathlib/CMSIS/Include/arm_common_tables.h b/src/modules/mathlib/CMSIS/Include/arm_common_tables.h new file mode 100644 index 000000000..9c37ab4e5 --- /dev/null +++ b/src/modules/mathlib/CMSIS/Include/arm_common_tables.h @@ -0,0 +1,93 @@ +/* ----------------------------------------------------------------------
+* Copyright (C) 2010-2013 ARM Limited. All rights reserved.
+*
+* $Date: 17. January 2013
+* $Revision: V1.4.1
+*
+* Project: CMSIS DSP Library
+* Title: arm_common_tables.h
+*
+* Description: This file has extern declaration for common tables like Bitreverse, reciprocal etc which are used across different functions
+*
+* Target Processor: Cortex-M4/Cortex-M3
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+* - Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* - 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.
+* - Neither the name of ARM LIMITED 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.
+* -------------------------------------------------------------------- */
+
+#ifndef _ARM_COMMON_TABLES_H
+#define _ARM_COMMON_TABLES_H
+
+#include "arm_math.h"
+
+extern const uint16_t armBitRevTable[1024];
+extern const q15_t armRecipTableQ15[64];
+extern const q31_t armRecipTableQ31[64];
+extern const q31_t realCoefAQ31[1024];
+extern const q31_t realCoefBQ31[1024];
+extern const float32_t twiddleCoef_16[32];
+extern const float32_t twiddleCoef_32[64];
+extern const float32_t twiddleCoef_64[128];
+extern const float32_t twiddleCoef_128[256];
+extern const float32_t twiddleCoef_256[512];
+extern const float32_t twiddleCoef_512[1024];
+extern const float32_t twiddleCoef_1024[2048];
+extern const float32_t twiddleCoef_2048[4096];
+extern const float32_t twiddleCoef_4096[8192];
+#define twiddleCoef twiddleCoef_4096
+extern const q31_t twiddleCoefQ31[6144];
+extern const q15_t twiddleCoefQ15[6144];
+extern const float32_t twiddleCoef_rfft_32[32];
+extern const float32_t twiddleCoef_rfft_64[64];
+extern const float32_t twiddleCoef_rfft_128[128];
+extern const float32_t twiddleCoef_rfft_256[256];
+extern const float32_t twiddleCoef_rfft_512[512];
+extern const float32_t twiddleCoef_rfft_1024[1024];
+extern const float32_t twiddleCoef_rfft_2048[2048];
+extern const float32_t twiddleCoef_rfft_4096[4096];
+
+
+#define ARMBITREVINDEXTABLE__16_TABLE_LENGTH ((uint16_t)20 )
+#define ARMBITREVINDEXTABLE__32_TABLE_LENGTH ((uint16_t)48 )
+#define ARMBITREVINDEXTABLE__64_TABLE_LENGTH ((uint16_t)56 )
+#define ARMBITREVINDEXTABLE_128_TABLE_LENGTH ((uint16_t)208 )
+#define ARMBITREVINDEXTABLE_256_TABLE_LENGTH ((uint16_t)440 )
+#define ARMBITREVINDEXTABLE_512_TABLE_LENGTH ((uint16_t)448 )
+#define ARMBITREVINDEXTABLE1024_TABLE_LENGTH ((uint16_t)1800)
+#define ARMBITREVINDEXTABLE2048_TABLE_LENGTH ((uint16_t)3808)
+#define ARMBITREVINDEXTABLE4096_TABLE_LENGTH ((uint16_t)4032)
+
+extern const uint16_t armBitRevIndexTable16[ARMBITREVINDEXTABLE__16_TABLE_LENGTH];
+extern const uint16_t armBitRevIndexTable32[ARMBITREVINDEXTABLE__32_TABLE_LENGTH];
+extern const uint16_t armBitRevIndexTable64[ARMBITREVINDEXTABLE__64_TABLE_LENGTH];
+extern const uint16_t armBitRevIndexTable128[ARMBITREVINDEXTABLE_128_TABLE_LENGTH];
+extern const uint16_t armBitRevIndexTable256[ARMBITREVINDEXTABLE_256_TABLE_LENGTH];
+extern const uint16_t armBitRevIndexTable512[ARMBITREVINDEXTABLE_512_TABLE_LENGTH];
+extern const uint16_t armBitRevIndexTable1024[ARMBITREVINDEXTABLE1024_TABLE_LENGTH];
+extern const uint16_t armBitRevIndexTable2048[ARMBITREVINDEXTABLE2048_TABLE_LENGTH];
+extern const uint16_t armBitRevIndexTable4096[ARMBITREVINDEXTABLE4096_TABLE_LENGTH];
+
+#endif /* ARM_COMMON_TABLES_H */
diff --git a/src/modules/mathlib/CMSIS/Include/arm_const_structs.h b/src/modules/mathlib/CMSIS/Include/arm_const_structs.h new file mode 100644 index 000000000..406f737dc --- /dev/null +++ b/src/modules/mathlib/CMSIS/Include/arm_const_structs.h @@ -0,0 +1,85 @@ +/* ----------------------------------------------------------------------
+* Copyright (C) 2010-2013 ARM Limited. All rights reserved.
+*
+* $Date: 17. January 2013
+* $Revision: V1.4.1
+*
+* Project: CMSIS DSP Library
+* Title: arm_const_structs.h
+*
+* Description: This file has constant structs that are initialized for
+* user convenience. For example, some can be given as
+* arguments to the arm_cfft_f32() function.
+*
+* Target Processor: Cortex-M4/Cortex-M3
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+* - Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* - 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.
+* - Neither the name of ARM LIMITED 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.
+* -------------------------------------------------------------------- */
+
+#ifndef _ARM_CONST_STRUCTS_H
+#define _ARM_CONST_STRUCTS_H
+
+#include "arm_math.h"
+#include "arm_common_tables.h"
+
+ const arm_cfft_instance_f32 arm_cfft_sR_f32_len16 = {
+ 16, twiddleCoef_16, armBitRevIndexTable16, ARMBITREVINDEXTABLE__16_TABLE_LENGTH
+ };
+
+ const arm_cfft_instance_f32 arm_cfft_sR_f32_len32 = {
+ 32, twiddleCoef_32, armBitRevIndexTable32, ARMBITREVINDEXTABLE__32_TABLE_LENGTH
+ };
+
+ const arm_cfft_instance_f32 arm_cfft_sR_f32_len64 = {
+ 64, twiddleCoef_64, armBitRevIndexTable64, ARMBITREVINDEXTABLE__64_TABLE_LENGTH
+ };
+
+ const arm_cfft_instance_f32 arm_cfft_sR_f32_len128 = {
+ 128, twiddleCoef_128, armBitRevIndexTable128, ARMBITREVINDEXTABLE_128_TABLE_LENGTH
+ };
+
+ const arm_cfft_instance_f32 arm_cfft_sR_f32_len256 = {
+ 256, twiddleCoef_256, armBitRevIndexTable256, ARMBITREVINDEXTABLE_256_TABLE_LENGTH
+ };
+
+ const arm_cfft_instance_f32 arm_cfft_sR_f32_len512 = {
+ 512, twiddleCoef_512, armBitRevIndexTable512, ARMBITREVINDEXTABLE_512_TABLE_LENGTH
+ };
+
+ const arm_cfft_instance_f32 arm_cfft_sR_f32_len1024 = {
+ 1024, twiddleCoef_1024, armBitRevIndexTable1024, ARMBITREVINDEXTABLE1024_TABLE_LENGTH
+ };
+
+ const arm_cfft_instance_f32 arm_cfft_sR_f32_len2048 = {
+ 2048, twiddleCoef_2048, armBitRevIndexTable2048, ARMBITREVINDEXTABLE2048_TABLE_LENGTH
+ };
+
+ const arm_cfft_instance_f32 arm_cfft_sR_f32_len4096 = {
+ 4096, twiddleCoef_4096, armBitRevIndexTable4096, ARMBITREVINDEXTABLE4096_TABLE_LENGTH
+ };
+
+#endif
diff --git a/src/modules/mathlib/CMSIS/Include/arm_math.h b/src/modules/mathlib/CMSIS/Include/arm_math.h new file mode 100644 index 000000000..6f66f9ee3 --- /dev/null +++ b/src/modules/mathlib/CMSIS/Include/arm_math.h @@ -0,0 +1,7318 @@ +/* ----------------------------------------------------------------------
+* Copyright (C) 2010-2013 ARM Limited. All rights reserved.
+*
+* $Date: 17. January 2013
+* $Revision: V1.4.1
+*
+* Project: CMSIS DSP Library
+* Title: arm_math.h
+*
+* Description: Public header file for CMSIS DSP Library
+*
+* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+* - Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* - 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.
+* - Neither the name of ARM LIMITED 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.
+ * -------------------------------------------------------------------- */
+
+/**
+ \mainpage CMSIS DSP Software Library
+ *
+ * <b>Introduction</b>
+ *
+ * This user manual describes the CMSIS DSP software library,
+ * a suite of common signal processing functions for use on Cortex-M processor based devices.
+ *
+ * The library is divided into a number of functions each covering a specific category:
+ * - Basic math functions
+ * - Fast math functions
+ * - Complex math functions
+ * - Filters
+ * - Matrix functions
+ * - Transforms
+ * - Motor control functions
+ * - Statistical functions
+ * - Support functions
+ * - Interpolation functions
+ *
+ * The library has separate functions for operating on 8-bit integers, 16-bit integers,
+ * 32-bit integer and 32-bit floating-point values.
+ *
+ * <b>Using the Library</b>
+ *
+ * The library installer contains prebuilt versions of the libraries in the <code>Lib</code> folder.
+ * - arm_cortexM4lf_math.lib (Little endian and Floating Point Unit on Cortex-M4)
+ * - arm_cortexM4bf_math.lib (Big endian and Floating Point Unit on Cortex-M4)
+ * - arm_cortexM4l_math.lib (Little endian on Cortex-M4)
+ * - arm_cortexM4b_math.lib (Big endian on Cortex-M4)
+ * - arm_cortexM3l_math.lib (Little endian on Cortex-M3)
+ * - arm_cortexM3b_math.lib (Big endian on Cortex-M3)
+ * - arm_cortexM0l_math.lib (Little endian on Cortex-M0)
+ * - arm_cortexM0b_math.lib (Big endian on Cortex-M3)
+ *
+ * The library functions are declared in the public file <code>arm_math.h</code> which is placed in the <code>Include</code> folder.
+ * Simply include this file and link the appropriate library in the application and begin calling the library functions. The Library supports single
+ * public header file <code> arm_math.h</code> for Cortex-M4/M3/M0 with little endian and big endian. Same header file will be used for floating point unit(FPU) variants.
+ * Define the appropriate pre processor MACRO ARM_MATH_CM4 or ARM_MATH_CM3 or
+ * ARM_MATH_CM0 or ARM_MATH_CM0PLUS depending on the target processor in the application.
+ *
+ * <b>Examples</b>
+ *
+ * The library ships with a number of examples which demonstrate how to use the library functions.
+ *
+ * <b>Toolchain Support</b>
+ *
+ * The library has been developed and tested with MDK-ARM version 4.60.
+ * The library is being tested in GCC and IAR toolchains and updates on this activity will be made available shortly.
+ *
+ * <b>Building the Library</b>
+ *
+ * The library installer contains project files to re build libraries on MDK Tool chain in the <code>CMSIS\\DSP_Lib\\Source\\ARM</code> folder.
+ * - arm_cortexM0b_math.uvproj
+ * - arm_cortexM0l_math.uvproj
+ * - arm_cortexM3b_math.uvproj
+ * - arm_cortexM3l_math.uvproj
+ * - arm_cortexM4b_math.uvproj
+ * - arm_cortexM4l_math.uvproj
+ * - arm_cortexM4bf_math.uvproj
+ * - arm_cortexM4lf_math.uvproj
+ *
+ *
+ * The project can be built by opening the appropriate project in MDK-ARM 4.60 chain and defining the optional pre processor MACROs detailed above.
+ *
+ * <b>Pre-processor Macros</b>
+ *
+ * Each library project have differant pre-processor macros.
+ *
+ * - UNALIGNED_SUPPORT_DISABLE:
+ *
+ * Define macro UNALIGNED_SUPPORT_DISABLE, If the silicon does not support unaligned memory access
+ *
+ * - ARM_MATH_BIG_ENDIAN:
+ *
+ * Define macro ARM_MATH_BIG_ENDIAN to build the library for big endian targets. By default library builds for little endian targets.
+ *
+ * - ARM_MATH_MATRIX_CHECK:
+ *
+ * Define macro ARM_MATH_MATRIX_CHECK for checking on the input and output sizes of matrices
+ *
+ * - ARM_MATH_ROUNDING:
+ *
+ * Define macro ARM_MATH_ROUNDING for rounding on support functions
+ *
+ * - ARM_MATH_CMx:
+ *
+ * Define macro ARM_MATH_CM4 for building the library on Cortex-M4 target, ARM_MATH_CM3 for building library on Cortex-M3 target
+ * and ARM_MATH_CM0 for building library on cortex-M0 target, ARM_MATH_CM0PLUS for building library on cortex-M0+ target.
+ *
+ * - __FPU_PRESENT:
+ *
+ * Initialize macro __FPU_PRESENT = 1 when building on FPU supported Targets. Enable this macro for M4bf and M4lf libraries
+ *
+ * <b>Copyright Notice</b>
+ *
+ * Copyright (C) 2010-2013 ARM Limited. All rights reserved.
+ */
+
+
+/**
+ * @defgroup groupMath Basic Math Functions
+ */
+
+/**
+ * @defgroup groupFastMath Fast Math Functions
+ * This set of functions provides a fast approximation to sine, cosine, and square root.
+ * As compared to most of the other functions in the CMSIS math library, the fast math functions
+ * operate on individual values and not arrays.
+ * There are separate functions for Q15, Q31, and floating-point data.
+ *
+ */
+
+/**
+ * @defgroup groupCmplxMath Complex Math Functions
+ * This set of functions operates on complex data vectors.
+ * The data in the complex arrays is stored in an interleaved fashion
+ * (real, imag, real, imag, ...).
+ * In the API functions, the number of samples in a complex array refers
+ * to the number of complex values; the array contains twice this number of
+ * real values.
+ */
+
+/**
+ * @defgroup groupFilters Filtering Functions
+ */
+
+/**
+ * @defgroup groupMatrix Matrix Functions
+ *
+ * This set of functions provides basic matrix math operations.
+ * The functions operate on matrix data structures. For example,
+ * the type
+ * definition for the floating-point matrix structure is shown
+ * below:
+ * <pre>
+ * typedef struct
+ * {
+ * uint16_t numRows; // number of rows of the matrix.
+ * uint16_t numCols; // number of columns of the matrix.
+ * float32_t *pData; // points to the data of the matrix.
+ * } arm_matrix_instance_f32;
+ * </pre>
+ * There are similar definitions for Q15 and Q31 data types.
+ *
+ * The structure specifies the size of the matrix and then points to
+ * an array of data. The array is of size <code>numRows X numCols</code>
+ * and the values are arranged in row order. That is, the
+ * matrix element (i, j) is stored at:
+ * <pre>
+ * pData[i*numCols + j]
+ * </pre>
+ *
+ * \par Init Functions
+ * There is an associated initialization function for each type of matrix
+ * data structure.
+ * The initialization function sets the values of the internal structure fields.
+ * Refer to the function <code>arm_mat_init_f32()</code>, <code>arm_mat_init_q31()</code>
+ * and <code>arm_mat_init_q15()</code> for floating-point, Q31 and Q15 types, respectively.
+ *
+ * \par
+ * Use of the initialization function is optional. However, if initialization function is used
+ * then the instance structure cannot be placed into a const data section.
+ * To place the instance structure in a const data
+ * section, manually initialize the data structure. For example:
+ * <pre>
+ * <code>arm_matrix_instance_f32 S = {nRows, nColumns, pData};</code>
+ * <code>arm_matrix_instance_q31 S = {nRows, nColumns, pData};</code>
+ * <code>arm_matrix_instance_q15 S = {nRows, nColumns, pData};</code>
+ * </pre>
+ * where <code>nRows</code> specifies the number of rows, <code>nColumns</code>
+ * specifies the number of columns, and <code>pData</code> points to the
+ * data array.
+ *
+ * \par Size Checking
+ * By default all of the matrix functions perform size checking on the input and
+ * output matrices. For example, the matrix addition function verifies that the
+ * two input matrices and the output matrix all have the same number of rows and
+ * columns. If the size check fails the functions return:
+ * <pre>
+ * ARM_MATH_SIZE_MISMATCH
+ * </pre>
+ * Otherwise the functions return
+ * <pre>
+ * ARM_MATH_SUCCESS
+ * </pre>
+ * There is some overhead associated with this matrix size checking.
+ * The matrix size checking is enabled via the \#define
+ * <pre>
+ * ARM_MATH_MATRIX_CHECK
+ * </pre>
+ * within the library project settings. By default this macro is defined
+ * and size checking is enabled. By changing the project settings and
+ * undefining this macro size checking is eliminated and the functions
+ * run a bit faster. With size checking disabled the functions always
+ * return <code>ARM_MATH_SUCCESS</code>.
+ */
+
+/**
+ * @defgroup groupTransforms Transform Functions
+ */
+
+/**
+ * @defgroup groupController Controller Functions
+ */
+
+/**
+ * @defgroup groupStats Statistics Functions
+ */
+/**
+ * @defgroup groupSupport Support Functions
+ */
+
+/**
+ * @defgroup groupInterpolation Interpolation Functions
+ * These functions perform 1- and 2-dimensional interpolation of data.
+ * Linear interpolation is used for 1-dimensional data and
+ * bilinear interpolation is used for 2-dimensional data.
+ */
+
+/**
+ * @defgroup groupExamples Examples
+ */
+#ifndef _ARM_MATH_H
+#define _ARM_MATH_H
+
+#define __CMSIS_GENERIC /* disable NVIC and Systick functions */
+
+/* PX4 */
+#include <nuttx/config.h>
+#ifdef CONFIG_ARCH_CORTEXM4
+# define ARM_MATH_CM4 1
+#endif
+#ifdef CONFIG_ARCH_CORTEXM3
+# define ARM_MATH_CM3 1
+#endif
+#ifdef CONFIG_ARCH_FPU
+# define __FPU_PRESENT 1
+#endif
+
+#if defined (ARM_MATH_CM4)
+#include "core_cm4.h"
+#elif defined (ARM_MATH_CM3)
+#include "core_cm3.h"
+#elif defined (ARM_MATH_CM0)
+#include "core_cm0.h"
+#define ARM_MATH_CM0_FAMILY
+#elif defined (ARM_MATH_CM0PLUS)
+#include "core_cm0plus.h"
+#define ARM_MATH_CM0_FAMILY
+#else
+#include "ARMCM4.h"
+#warning "Define either ARM_MATH_CM4 OR ARM_MATH_CM3...By Default building on ARM_MATH_CM4....."
+#endif
+
+#undef __CMSIS_GENERIC /* enable NVIC and Systick functions */
+#include "string.h"
+#include "math.h"
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+
+ /**
+ * @brief Macros required for reciprocal calculation in Normalized LMS
+ */
+
+#define DELTA_Q31 (0x100)
+#define DELTA_Q15 0x5
+#define INDEX_MASK 0x0000003F
+#ifndef PI
+#define PI 3.14159265358979f
+#endif
+
+ /**
+ * @brief Macros required for SINE and COSINE Fast math approximations
+ */
+
+#define TABLE_SIZE 256
+#define TABLE_SPACING_Q31 0x800000
+#define TABLE_SPACING_Q15 0x80
+
+ /**
+ * @brief Macros required for SINE and COSINE Controller functions
+ */
+ /* 1.31(q31) Fixed value of 2/360 */
+ /* -1 to +1 is divided into 360 values so total spacing is (2/360) */
+#define INPUT_SPACING 0xB60B61
+
+ /**
+ * @brief Macro for Unaligned Support
+ */
+#ifndef UNALIGNED_SUPPORT_DISABLE
+ #define ALIGN4
+#else
+ #if defined (__GNUC__)
+ #define ALIGN4 __attribute__((aligned(4)))
+ #else
+ #define ALIGN4 __align(4)
+ #endif
+#endif /* #ifndef UNALIGNED_SUPPORT_DISABLE */
+
+ /**
+ * @brief Error status returned by some functions in the library.
+ */
+
+ typedef enum
+ {
+ ARM_MATH_SUCCESS = 0, /**< No error */
+ ARM_MATH_ARGUMENT_ERROR = -1, /**< One or more arguments are incorrect */
+ ARM_MATH_LENGTH_ERROR = -2, /**< Length of data buffer is incorrect */
+ ARM_MATH_SIZE_MISMATCH = -3, /**< Size of matrices is not compatible with the operation. */
+ ARM_MATH_NANINF = -4, /**< Not-a-number (NaN) or infinity is generated */
+ ARM_MATH_SINGULAR = -5, /**< Generated by matrix inversion if the input matrix is singular and cannot be inverted. */
+ ARM_MATH_TEST_FAILURE = -6 /**< Test Failed */
+ } arm_status;
+
+ /**
+ * @brief 8-bit fractional data type in 1.7 format.
+ */
+ typedef int8_t q7_t;
+
+ /**
+ * @brief 16-bit fractional data type in 1.15 format.
+ */
+ typedef int16_t q15_t;
+
+ /**
+ * @brief 32-bit fractional data type in 1.31 format.
+ */
+ typedef int32_t q31_t;
+
+ /**
+ * @brief 64-bit fractional data type in 1.63 format.
+ */
+ typedef int64_t q63_t;
+
+ /**
+ * @brief 32-bit floating-point type definition.
+ */
+ typedef float float32_t;
+
+ /**
+ * @brief 64-bit floating-point type definition.
+ */
+ typedef double float64_t;
+
+ /**
+ * @brief definition to read/write two 16 bit values.
+ */
+#if defined __CC_ARM
+#define __SIMD32_TYPE int32_t __packed
+#define CMSIS_UNUSED __attribute__((unused))
+#elif defined __ICCARM__
+#define CMSIS_UNUSED
+#define __SIMD32_TYPE int32_t __packed
+#elif defined __GNUC__
+#define __SIMD32_TYPE int32_t
+#define CMSIS_UNUSED __attribute__((unused))
+#else
+#error Unknown compiler
+#endif
+
+#define __SIMD32(addr) (*(__SIMD32_TYPE **) & (addr))
+#define __SIMD32_CONST(addr) ((__SIMD32_TYPE *)(addr))
+
+#define _SIMD32_OFFSET(addr) (*(__SIMD32_TYPE *) (addr))
+
+#define __SIMD64(addr) (*(int64_t **) & (addr))
+
+#if defined (ARM_MATH_CM3) || defined (ARM_MATH_CM0_FAMILY)
+ /**
+ * @brief definition to pack two 16 bit values.
+ */
+#define __PKHBT(ARG1, ARG2, ARG3) ( (((int32_t)(ARG1) << 0) & (int32_t)0x0000FFFF) | \
+ (((int32_t)(ARG2) << ARG3) & (int32_t)0xFFFF0000) )
+#define __PKHTB(ARG1, ARG2, ARG3) ( (((int32_t)(ARG1) << 0) & (int32_t)0xFFFF0000) | \
+ (((int32_t)(ARG2) >> ARG3) & (int32_t)0x0000FFFF) )
+
+#endif
+
+
+ /**
+ * @brief definition to pack four 8 bit values.
+ */
+#ifndef ARM_MATH_BIG_ENDIAN
+
+#define __PACKq7(v0,v1,v2,v3) ( (((int32_t)(v0) << 0) & (int32_t)0x000000FF) | \
+ (((int32_t)(v1) << 8) & (int32_t)0x0000FF00) | \
+ (((int32_t)(v2) << 16) & (int32_t)0x00FF0000) | \
+ (((int32_t)(v3) << 24) & (int32_t)0xFF000000) )
+#else
+
+#define __PACKq7(v0,v1,v2,v3) ( (((int32_t)(v3) << 0) & (int32_t)0x000000FF) | \
+ (((int32_t)(v2) << 8) & (int32_t)0x0000FF00) | \
+ (((int32_t)(v1) << 16) & (int32_t)0x00FF0000) | \
+ (((int32_t)(v0) << 24) & (int32_t)0xFF000000) )
+
+#endif
+
+
+ /**
+ * @brief Clips Q63 to Q31 values.
+ */
+ static __INLINE q31_t clip_q63_to_q31(
+ q63_t x)
+ {
+ return ((q31_t) (x >> 32) != ((q31_t) x >> 31)) ?
+ ((0x7FFFFFFF ^ ((q31_t) (x >> 63)))) : (q31_t) x;
+ }
+
+ /**
+ * @brief Clips Q63 to Q15 values.
+ */
+ static __INLINE q15_t clip_q63_to_q15(
+ q63_t x)
+ {
+ return ((q31_t) (x >> 32) != ((q31_t) x >> 31)) ?
+ ((0x7FFF ^ ((q15_t) (x >> 63)))) : (q15_t) (x >> 15);
+ }
+
+ /**
+ * @brief Clips Q31 to Q7 values.
+ */
+ static __INLINE q7_t clip_q31_to_q7(
+ q31_t x)
+ {
+ return ((q31_t) (x >> 24) != ((q31_t) x >> 23)) ?
+ ((0x7F ^ ((q7_t) (x >> 31)))) : (q7_t) x;
+ }
+
+ /**
+ * @brief Clips Q31 to Q15 values.
+ */
+ static __INLINE q15_t clip_q31_to_q15(
+ q31_t x)
+ {
+ return ((q31_t) (x >> 16) != ((q31_t) x >> 15)) ?
+ ((0x7FFF ^ ((q15_t) (x >> 31)))) : (q15_t) x;
+ }
+
+ /**
+ * @brief Multiplies 32 X 64 and returns 32 bit result in 2.30 format.
+ */
+
+ static __INLINE q63_t mult32x64(
+ q63_t x,
+ q31_t y)
+ {
+ return ((((q63_t) (x & 0x00000000FFFFFFFF) * y) >> 32) +
+ (((q63_t) (x >> 32) * y)));
+ }
+
+
+#if defined (ARM_MATH_CM0_FAMILY) && defined ( __CC_ARM )
+#define __CLZ __clz
+#endif
+
+#if defined (ARM_MATH_CM0_FAMILY) && ((defined (__ICCARM__)) ||(defined (__GNUC__)) || defined (__TASKING__) )
+
+ static __INLINE uint32_t __CLZ(
+ q31_t data);
+
+
+ static __INLINE uint32_t __CLZ(
+ q31_t data)
+ {
+ uint32_t count = 0;
+ uint32_t mask = 0x80000000;
+
+ while((data & mask) == 0)
+ {
+ count += 1u;
+ mask = mask >> 1u;
+ }
+
+ return (count);
+
+ }
+
+#endif
+
+ /**
+ * @brief Function to Calculates 1/in (reciprocal) value of Q31 Data type.
+ */
+
+ static __INLINE uint32_t arm_recip_q31(
+ q31_t in,
+ q31_t * dst,
+ q31_t * pRecipTable)
+ {
+
+ uint32_t out, tempVal;
+ uint32_t index, i;
+ uint32_t signBits;
+
+ if(in > 0)
+ {
+ signBits = __CLZ(in) - 1;
+ }
+ else
+ {
+ signBits = __CLZ(-in) - 1;
+ }
+
+ /* Convert input sample to 1.31 format */
+ in = in << signBits;
+
+ /* calculation of index for initial approximated Val */
+ index = (uint32_t) (in >> 24u);
+ index = (index & INDEX_MASK);
+
+ /* 1.31 with exp 1 */
+ out = pRecipTable[index];
+
+ /* calculation of reciprocal value */
+ /* running approximation for two iterations */
+ for (i = 0u; i < 2u; i++)
+ {
+ tempVal = (q31_t) (((q63_t) in * out) >> 31u);
+ tempVal = 0x7FFFFFFF - tempVal;
+ /* 1.31 with exp 1 */
+ //out = (q31_t) (((q63_t) out * tempVal) >> 30u);
+ out = (q31_t) clip_q63_to_q31(((q63_t) out * tempVal) >> 30u);
+ }
+
+ /* write output */
+ *dst = out;
+
+ /* return num of signbits of out = 1/in value */
+ return (signBits + 1u);
+
+ }
+
+ /**
+ * @brief Function to Calculates 1/in (reciprocal) value of Q15 Data type.
+ */
+ static __INLINE uint32_t arm_recip_q15(
+ q15_t in,
+ q15_t * dst,
+ q15_t * pRecipTable)
+ {
+
+ uint32_t out = 0, tempVal = 0;
+ uint32_t index = 0, i = 0;
+ uint32_t signBits = 0;
+
+ if(in > 0)
+ {
+ signBits = __CLZ(in) - 17;
+ }
+ else
+ {
+ signBits = __CLZ(-in) - 17;
+ }
+
+ /* Convert input sample to 1.15 format */
+ in = in << signBits;
+
+ /* calculation of index for initial approximated Val */
+ index = in >> 8;
+ index = (index & INDEX_MASK);
+
+ /* 1.15 with exp 1 */
+ out = pRecipTable[index];
+
+ /* calculation of reciprocal value */
+ /* running approximation for two iterations */
+ for (i = 0; i < 2; i++)
+ {
+ tempVal = (q15_t) (((q31_t) in * out) >> 15);
+ tempVal = 0x7FFF - tempVal;
+ /* 1.15 with exp 1 */
+ out = (q15_t) (((q31_t) out * tempVal) >> 14);
+ }
+
+ /* write output */
+ *dst = out;
+
+ /* return num of signbits of out = 1/in value */
+ return (signBits + 1);
+
+ }
+
+
+ /*
+ * @brief C custom defined intrinisic function for only M0 processors
+ */
+#if defined(ARM_MATH_CM0_FAMILY)
+
+ static __INLINE q31_t __SSAT(
+ q31_t x,
+ uint32_t y)
+ {
+ int32_t posMax, negMin;
+ uint32_t i;
+
+ posMax = 1;
+ for (i = 0; i < (y - 1); i++)
+ {
+ posMax = posMax * 2;
+ }
+
+ if(x > 0)
+ {
+ posMax = (posMax - 1);
+
+ if(x > posMax)
+ {
+ x = posMax;
+ }
+ }
+ else
+ {
+ negMin = -posMax;
+
+ if(x < negMin)
+ {
+ x = negMin;
+ }
+ }
+ return (x);
+
+
+ }
+
+#endif /* end of ARM_MATH_CM0_FAMILY */
+
+
+
+ /*
+ * @brief C custom defined intrinsic function for M3 and M0 processors
+ */
+#if defined (ARM_MATH_CM3) || defined (ARM_MATH_CM0_FAMILY)
+
+ /*
+ * @brief C custom defined QADD8 for M3 and M0 processors
+ */
+ static __INLINE q31_t __QADD8(
+ q31_t x,
+ q31_t y)
+ {
+
+ q31_t sum;
+ q7_t r, s, t, u;
+
+ r = (q7_t) x;
+ s = (q7_t) y;
+
+ r = __SSAT((q31_t) (r + s), 8);
+ s = __SSAT(((q31_t) (((x << 16) >> 24) + ((y << 16) >> 24))), 8);
+ t = __SSAT(((q31_t) (((x << 8) >> 24) + ((y << 8) >> 24))), 8);
+ u = __SSAT(((q31_t) ((x >> 24) + (y >> 24))), 8);
+
+ sum =
+ (((q31_t) u << 24) & 0xFF000000) | (((q31_t) t << 16) & 0x00FF0000) |
+ (((q31_t) s << 8) & 0x0000FF00) | (r & 0x000000FF);
+
+ return sum;
+
+ }
+
+ /*
+ * @brief C custom defined QSUB8 for M3 and M0 processors
+ */
+ static __INLINE q31_t __QSUB8(
+ q31_t x,
+ q31_t y)
+ {
+
+ q31_t sum;
+ q31_t r, s, t, u;
+
+ r = (q7_t) x;
+ s = (q7_t) y;
+
+ r = __SSAT((r - s), 8);
+ s = __SSAT(((q31_t) (((x << 16) >> 24) - ((y << 16) >> 24))), 8) << 8;
+ t = __SSAT(((q31_t) (((x << 8) >> 24) - ((y << 8) >> 24))), 8) << 16;
+ u = __SSAT(((q31_t) ((x >> 24) - (y >> 24))), 8) << 24;
+
+ sum =
+ (u & 0xFF000000) | (t & 0x00FF0000) | (s & 0x0000FF00) | (r &
+ 0x000000FF);
+
+ return sum;
+ }
+
+ /*
+ * @brief C custom defined QADD16 for M3 and M0 processors
+ */
+
+ /*
+ * @brief C custom defined QADD16 for M3 and M0 processors
+ */
+ static __INLINE q31_t __QADD16(
+ q31_t x,
+ q31_t y)
+ {
+
+ q31_t sum;
+ q31_t r, s;
+
+ r = (short) x;
+ s = (short) y;
+
+ r = __SSAT(r + s, 16);
+ s = __SSAT(((q31_t) ((x >> 16) + (y >> 16))), 16) << 16;
+
+ sum = (s & 0xFFFF0000) | (r & 0x0000FFFF);
+
+ return sum;
+
+ }
+
+ /*
+ * @brief C custom defined SHADD16 for M3 and M0 processors
+ */
+ static __INLINE q31_t __SHADD16(
+ q31_t x,
+ q31_t y)
+ {
+
+ q31_t sum;
+ q31_t r, s;
+
+ r = (short) x;
+ s = (short) y;
+
+ r = ((r >> 1) + (s >> 1));
+ s = ((q31_t) ((x >> 17) + (y >> 17))) << 16;
+
+ sum = (s & 0xFFFF0000) | (r & 0x0000FFFF);
+
+ return sum;
+
+ }
+
+ /*
+ * @brief C custom defined QSUB16 for M3 and M0 processors
+ */
+ static __INLINE q31_t __QSUB16(
+ q31_t x,
+ q31_t y)
+ {
+
+ q31_t sum;
+ q31_t r, s;
+
+ r = (short) x;
+ s = (short) y;
+
+ r = __SSAT(r - s, 16);
+ s = __SSAT(((q31_t) ((x >> 16) - (y >> 16))), 16) << 16;
+
+ sum = (s & 0xFFFF0000) | (r & 0x0000FFFF);
+
+ return sum;
+ }
+
+ /*
+ * @brief C custom defined SHSUB16 for M3 and M0 processors
+ */
+ static __INLINE q31_t __SHSUB16(
+ q31_t x,
+ q31_t y)
+ {
+
+ q31_t diff;
+ q31_t r, s;
+
+ r = (short) x;
+ s = (short) y;
+
+ r = ((r >> 1) - (s >> 1));
+ s = (((x >> 17) - (y >> 17)) << 16);
+
+ diff = (s & 0xFFFF0000) | (r & 0x0000FFFF);
+
+ return diff;
+ }
+
+ /*
+ * @brief C custom defined QASX for M3 and M0 processors
+ */
+ static __INLINE q31_t __QASX(
+ q31_t x,
+ q31_t y)
+ {
+
+ q31_t sum = 0;
+
+ sum =
+ ((sum +
+ clip_q31_to_q15((q31_t) ((short) (x >> 16) + (short) y))) << 16) +
+ clip_q31_to_q15((q31_t) ((short) x - (short) (y >> 16)));
+
+ return sum;
+ }
+
+ /*
+ * @brief C custom defined SHASX for M3 and M0 processors
+ */
+ static __INLINE q31_t __SHASX(
+ q31_t x,
+ q31_t y)
+ {
+
+ q31_t sum;
+ q31_t r, s;
+
+ r = (short) x;
+ s = (short) y;
+
+ r = ((r >> 1) - (y >> 17));
+ s = (((x >> 17) + (s >> 1)) << 16);
+
+ sum = (s & 0xFFFF0000) | (r & 0x0000FFFF);
+
+ return sum;
+ }
+
+
+ /*
+ * @brief C custom defined QSAX for M3 and M0 processors
+ */
+ static __INLINE q31_t __QSAX(
+ q31_t x,
+ q31_t y)
+ {
+
+ q31_t sum = 0;
+
+ sum =
+ ((sum +
+ clip_q31_to_q15((q31_t) ((short) (x >> 16) - (short) y))) << 16) +
+ clip_q31_to_q15((q31_t) ((short) x + (short) (y >> 16)));
+
+ return sum;
+ }
+
+ /*
+ * @brief C custom defined SHSAX for M3 and M0 processors
+ */
+ static __INLINE q31_t __SHSAX(
+ q31_t x,
+ q31_t y)
+ {
+
+ q31_t sum;
+ q31_t r, s;
+
+ r = (short) x;
+ s = (short) y;
+
+ r = ((r >> 1) + (y >> 17));
+ s = (((x >> 17) - (s >> 1)) << 16);
+
+ sum = (s & 0xFFFF0000) | (r & 0x0000FFFF);
+
+ return sum;
+ }
+
+ /*
+ * @brief C custom defined SMUSDX for M3 and M0 processors
+ */
+ static __INLINE q31_t __SMUSDX(
+ q31_t x,
+ q31_t y)
+ {
+
+ return ((q31_t) (((short) x * (short) (y >> 16)) -
+ ((short) (x >> 16) * (short) y)));
+ }
+
+ /*
+ * @brief C custom defined SMUADX for M3 and M0 processors
+ */
+ static __INLINE q31_t __SMUADX(
+ q31_t x,
+ q31_t y)
+ {
+
+ return ((q31_t) (((short) x * (short) (y >> 16)) +
+ ((short) (x >> 16) * (short) y)));
+ }
+
+ /*
+ * @brief C custom defined QADD for M3 and M0 processors
+ */
+ static __INLINE q31_t __QADD(
+ q31_t x,
+ q31_t y)
+ {
+ return clip_q63_to_q31((q63_t) x + y);
+ }
+
+ /*
+ * @brief C custom defined QSUB for M3 and M0 processors
+ */
+ static __INLINE q31_t __QSUB(
+ q31_t x,
+ q31_t y)
+ {
+ return clip_q63_to_q31((q63_t) x - y);
+ }
+
+ /*
+ * @brief C custom defined SMLAD for M3 and M0 processors
+ */
+ static __INLINE q31_t __SMLAD(
+ q31_t x,
+ q31_t y,
+ q31_t sum)
+ {
+
+ return (sum + ((short) (x >> 16) * (short) (y >> 16)) +
+ ((short) x * (short) y));
+ }
+
+ /*
+ * @brief C custom defined SMLADX for M3 and M0 processors
+ */
+ static __INLINE q31_t __SMLADX(
+ q31_t x,
+ q31_t y,
+ q31_t sum)
+ {
+
+ return (sum + ((short) (x >> 16) * (short) (y)) +
+ ((short) x * (short) (y >> 16)));
+ }
+
+ /*
+ * @brief C custom defined SMLSDX for M3 and M0 processors
+ */
+ static __INLINE q31_t __SMLSDX(
+ q31_t x,
+ q31_t y,
+ q31_t sum)
+ {
+
+ return (sum - ((short) (x >> 16) * (short) (y)) +
+ ((short) x * (short) (y >> 16)));
+ }
+
+ /*
+ * @brief C custom defined SMLALD for M3 and M0 processors
+ */
+ static __INLINE q63_t __SMLALD(
+ q31_t x,
+ q31_t y,
+ q63_t sum)
+ {
+
+ return (sum + ((short) (x >> 16) * (short) (y >> 16)) +
+ ((short) x * (short) y));
+ }
+
+ /*
+ * @brief C custom defined SMLALDX for M3 and M0 processors
+ */
+ static __INLINE q63_t __SMLALDX(
+ q31_t x,
+ q31_t y,
+ q63_t sum)
+ {
+
+ return (sum + ((short) (x >> 16) * (short) y)) +
+ ((short) x * (short) (y >> 16));
+ }
+
+ /*
+ * @brief C custom defined SMUAD for M3 and M0 processors
+ */
+ static __INLINE q31_t __SMUAD(
+ q31_t x,
+ q31_t y)
+ {
+
+ return (((x >> 16) * (y >> 16)) +
+ (((x << 16) >> 16) * ((y << 16) >> 16)));
+ }
+
+ /*
+ * @brief C custom defined SMUSD for M3 and M0 processors
+ */
+ static __INLINE q31_t __SMUSD(
+ q31_t x,
+ q31_t y)
+ {
+
+ return (-((x >> 16) * (y >> 16)) +
+ (((x << 16) >> 16) * ((y << 16) >> 16)));
+ }
+
+
+ /*
+ * @brief C custom defined SXTB16 for M3 and M0 processors
+ */
+ static __INLINE q31_t __SXTB16(
+ q31_t x)
+ {
+
+ return ((((x << 24) >> 24) & 0x0000FFFF) |
+ (((x << 8) >> 8) & 0xFFFF0000));
+ }
+
+
+#endif /* defined (ARM_MATH_CM3) || defined (ARM_MATH_CM0_FAMILY) */
+
+
+ /**
+ * @brief Instance structure for the Q7 FIR filter.
+ */
+ typedef struct
+ {
+ uint16_t numTaps; /**< number of filter coefficients in the filter. */
+ q7_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
+ q7_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/
+ } arm_fir_instance_q7;
+
+ /**
+ * @brief Instance structure for the Q15 FIR filter.
+ */
+ typedef struct
+ {
+ uint16_t numTaps; /**< number of filter coefficients in the filter. */
+ q15_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
+ q15_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/
+ } arm_fir_instance_q15;
+
+ /**
+ * @brief Instance structure for the Q31 FIR filter.
+ */
+ typedef struct
+ {
+ uint16_t numTaps; /**< number of filter coefficients in the filter. */
+ q31_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
+ q31_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */
+ } arm_fir_instance_q31;
+
+ /**
+ * @brief Instance structure for the floating-point FIR filter.
+ */
+ typedef struct
+ {
+ uint16_t numTaps; /**< number of filter coefficients in the filter. */
+ float32_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
+ float32_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */
+ } arm_fir_instance_f32;
+
+
+ /**
+ * @brief Processing function for the Q7 FIR filter.
+ * @param[in] *S points to an instance of the Q7 FIR filter structure.
+ * @param[in] *pSrc points to the block of input data.
+ * @param[out] *pDst points to the block of output data.
+ * @param[in] blockSize number of samples to process.
+ * @return none.
+ */
+ void arm_fir_q7(
+ const arm_fir_instance_q7 * S,
+ q7_t * pSrc,
+ q7_t * pDst,
+ uint32_t blockSize);
+
+
+ /**
+ * @brief Initialization function for the Q7 FIR filter.
+ * @param[in,out] *S points to an instance of the Q7 FIR structure.
+ * @param[in] numTaps Number of filter coefficients in the filter.
+ * @param[in] *pCoeffs points to the filter coefficients.
+ * @param[in] *pState points to the state buffer.
+ * @param[in] blockSize number of samples that are processed.
+ * @return none
+ */
+ void arm_fir_init_q7(
+ arm_fir_instance_q7 * S,
+ uint16_t numTaps,
+ q7_t * pCoeffs,
+ q7_t * pState,
+ uint32_t blockSize);
+
+
+ /**
+ * @brief Processing function for the Q15 FIR filter.
+ * @param[in] *S points to an instance of the Q15 FIR structure.
+ * @param[in] *pSrc points to the block of input data.
+ * @param[out] *pDst points to the block of output data.
+ * @param[in] blockSize number of samples to process.
+ * @return none.
+ */
+ void arm_fir_q15(
+ const arm_fir_instance_q15 * S,
+ q15_t * pSrc,
+ q15_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Processing function for the fast Q15 FIR filter for Cortex-M3 and Cortex-M4.
+ * @param[in] *S points to an instance of the Q15 FIR filter structure.
+ * @param[in] *pSrc points to the block of input data.
+ * @param[out] *pDst points to the block of output data.
+ * @param[in] blockSize number of samples to process.
+ * @return none.
+ */
+ void arm_fir_fast_q15(
+ const arm_fir_instance_q15 * S,
+ q15_t * pSrc,
+ q15_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Initialization function for the Q15 FIR filter.
+ * @param[in,out] *S points to an instance of the Q15 FIR filter structure.
+ * @param[in] numTaps Number of filter coefficients in the filter. Must be even and greater than or equal to 4.
+ * @param[in] *pCoeffs points to the filter coefficients.
+ * @param[in] *pState points to the state buffer.
+ * @param[in] blockSize number of samples that are processed at a time.
+ * @return The function returns ARM_MATH_SUCCESS if initialization was successful or ARM_MATH_ARGUMENT_ERROR if
+ * <code>numTaps</code> is not a supported value.
+ */
+
+ arm_status arm_fir_init_q15(
+ arm_fir_instance_q15 * S,
+ uint16_t numTaps,
+ q15_t * pCoeffs,
+ q15_t * pState,
+ uint32_t blockSize);
+
+ /**
+ * @brief Processing function for the Q31 FIR filter.
+ * @param[in] *S points to an instance of the Q31 FIR filter structure.
+ * @param[in] *pSrc points to the block of input data.
+ * @param[out] *pDst points to the block of output data.
+ * @param[in] blockSize number of samples to process.
+ * @return none.
+ */
+ void arm_fir_q31(
+ const arm_fir_instance_q31 * S,
+ q31_t * pSrc,
+ q31_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Processing function for the fast Q31 FIR filter for Cortex-M3 and Cortex-M4.
+ * @param[in] *S points to an instance of the Q31 FIR structure.
+ * @param[in] *pSrc points to the block of input data.
+ * @param[out] *pDst points to the block of output data.
+ * @param[in] blockSize number of samples to process.
+ * @return none.
+ */
+ void arm_fir_fast_q31(
+ const arm_fir_instance_q31 * S,
+ q31_t * pSrc,
+ q31_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Initialization function for the Q31 FIR filter.
+ * @param[in,out] *S points to an instance of the Q31 FIR structure.
+ * @param[in] numTaps Number of filter coefficients in the filter.
+ * @param[in] *pCoeffs points to the filter coefficients.
+ * @param[in] *pState points to the state buffer.
+ * @param[in] blockSize number of samples that are processed at a time.
+ * @return none.
+ */
+ void arm_fir_init_q31(
+ arm_fir_instance_q31 * S,
+ uint16_t numTaps,
+ q31_t * pCoeffs,
+ q31_t * pState,
+ uint32_t blockSize);
+
+ /**
+ * @brief Processing function for the floating-point FIR filter.
+ * @param[in] *S points to an instance of the floating-point FIR structure.
+ * @param[in] *pSrc points to the block of input data.
+ * @param[out] *pDst points to the block of output data.
+ * @param[in] blockSize number of samples to process.
+ * @return none.
+ */
+ void arm_fir_f32(
+ const arm_fir_instance_f32 * S,
+ float32_t * pSrc,
+ float32_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Initialization function for the floating-point FIR filter.
+ * @param[in,out] *S points to an instance of the floating-point FIR filter structure.
+ * @param[in] numTaps Number of filter coefficients in the filter.
+ * @param[in] *pCoeffs points to the filter coefficients.
+ * @param[in] *pState points to the state buffer.
+ * @param[in] blockSize number of samples that are processed at a time.
+ * @return none.
+ */
+ void arm_fir_init_f32(
+ arm_fir_instance_f32 * S,
+ uint16_t numTaps,
+ float32_t * pCoeffs,
+ float32_t * pState,
+ uint32_t blockSize);
+
+
+ /**
+ * @brief Instance structure for the Q15 Biquad cascade filter.
+ */
+ typedef struct
+ {
+ int8_t numStages; /**< number of 2nd order stages in the filter. Overall order is 2*numStages. */
+ q15_t *pState; /**< Points to the array of state coefficients. The array is of length 4*numStages. */
+ q15_t *pCoeffs; /**< Points to the array of coefficients. The array is of length 5*numStages. */
+ int8_t postShift; /**< Additional shift, in bits, applied to each output sample. */
+
+ } arm_biquad_casd_df1_inst_q15;
+
+
+ /**
+ * @brief Instance structure for the Q31 Biquad cascade filter.
+ */
+ typedef struct
+ {
+ uint32_t numStages; /**< number of 2nd order stages in the filter. Overall order is 2*numStages. */
+ q31_t *pState; /**< Points to the array of state coefficients. The array is of length 4*numStages. */
+ q31_t *pCoeffs; /**< Points to the array of coefficients. The array is of length 5*numStages. */
+ uint8_t postShift; /**< Additional shift, in bits, applied to each output sample. */
+
+ } arm_biquad_casd_df1_inst_q31;
+
+ /**
+ * @brief Instance structure for the floating-point Biquad cascade filter.
+ */
+ typedef struct
+ {
+ uint32_t numStages; /**< number of 2nd order stages in the filter. Overall order is 2*numStages. */
+ float32_t *pState; /**< Points to the array of state coefficients. The array is of length 4*numStages. */
+ float32_t *pCoeffs; /**< Points to the array of coefficients. The array is of length 5*numStages. */
+
+
+ } arm_biquad_casd_df1_inst_f32;
+
+
+
+ /**
+ * @brief Processing function for the Q15 Biquad cascade filter.
+ * @param[in] *S points to an instance of the Q15 Biquad cascade structure.
+ * @param[in] *pSrc points to the block of input data.
+ * @param[out] *pDst points to the block of output data.
+ * @param[in] blockSize number of samples to process.
+ * @return none.
+ */
+
+ void arm_biquad_cascade_df1_q15(
+ const arm_biquad_casd_df1_inst_q15 * S,
+ q15_t * pSrc,
+ q15_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Initialization function for the Q15 Biquad cascade filter.
+ * @param[in,out] *S points to an instance of the Q15 Biquad cascade structure.
+ * @param[in] numStages number of 2nd order stages in the filter.
+ * @param[in] *pCoeffs points to the filter coefficients.
+ * @param[in] *pState points to the state buffer.
+ * @param[in] postShift Shift to be applied to the output. Varies according to the coefficients format
+ * @return none
+ */
+
+ void arm_biquad_cascade_df1_init_q15(
+ arm_biquad_casd_df1_inst_q15 * S,
+ uint8_t numStages,
+ q15_t * pCoeffs,
+ q15_t * pState,
+ int8_t postShift);
+
+
+ /**
+ * @brief Fast but less precise processing function for the Q15 Biquad cascade filter for Cortex-M3 and Cortex-M4.
+ * @param[in] *S points to an instance of the Q15 Biquad cascade structure.
+ * @param[in] *pSrc points to the block of input data.
+ * @param[out] *pDst points to the block of output data.
+ * @param[in] blockSize number of samples to process.
+ * @return none.
+ */
+
+ void arm_biquad_cascade_df1_fast_q15(
+ const arm_biquad_casd_df1_inst_q15 * S,
+ q15_t * pSrc,
+ q15_t * pDst,
+ uint32_t blockSize);
+
+
+ /**
+ * @brief Processing function for the Q31 Biquad cascade filter
+ * @param[in] *S points to an instance of the Q31 Biquad cascade structure.
+ * @param[in] *pSrc points to the block of input data.
+ * @param[out] *pDst points to the block of output data.
+ * @param[in] blockSize number of samples to process.
+ * @return none.
+ */
+
+ void arm_biquad_cascade_df1_q31(
+ const arm_biquad_casd_df1_inst_q31 * S,
+ q31_t * pSrc,
+ q31_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Fast but less precise processing function for the Q31 Biquad cascade filter for Cortex-M3 and Cortex-M4.
+ * @param[in] *S points to an instance of the Q31 Biquad cascade structure.
+ * @param[in] *pSrc points to the block of input data.
+ * @param[out] *pDst points to the block of output data.
+ * @param[in] blockSize number of samples to process.
+ * @return none.
+ */
+
+ void arm_biquad_cascade_df1_fast_q31(
+ const arm_biquad_casd_df1_inst_q31 * S,
+ q31_t * pSrc,
+ q31_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Initialization function for the Q31 Biquad cascade filter.
+ * @param[in,out] *S points to an instance of the Q31 Biquad cascade structure.
+ * @param[in] numStages number of 2nd order stages in the filter.
+ * @param[in] *pCoeffs points to the filter coefficients.
+ * @param[in] *pState points to the state buffer.
+ * @param[in] postShift Shift to be applied to the output. Varies according to the coefficients format
+ * @return none
+ */
+
+ void arm_biquad_cascade_df1_init_q31(
+ arm_biquad_casd_df1_inst_q31 * S,
+ uint8_t numStages,
+ q31_t * pCoeffs,
+ q31_t * pState,
+ int8_t postShift);
+
+ /**
+ * @brief Processing function for the floating-point Biquad cascade filter.
+ * @param[in] *S points to an instance of the floating-point Biquad cascade structure.
+ * @param[in] *pSrc points to the block of input data.
+ * @param[out] *pDst points to the block of output data.
+ * @param[in] blockSize number of samples to process.
+ * @return none.
+ */
+
+ void arm_biquad_cascade_df1_f32(
+ const arm_biquad_casd_df1_inst_f32 * S,
+ float32_t * pSrc,
+ float32_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Initialization function for the floating-point Biquad cascade filter.
+ * @param[in,out] *S points to an instance of the floating-point Biquad cascade structure.
+ * @param[in] numStages number of 2nd order stages in the filter.
+ * @param[in] *pCoeffs points to the filter coefficients.
+ * @param[in] *pState points to the state buffer.
+ * @return none
+ */
+
+ void arm_biquad_cascade_df1_init_f32(
+ arm_biquad_casd_df1_inst_f32 * S,
+ uint8_t numStages,
+ float32_t * pCoeffs,
+ float32_t * pState);
+
+
+ /**
+ * @brief Instance structure for the floating-point matrix structure.
+ */
+
+ typedef struct
+ {
+ uint16_t numRows; /**< number of rows of the matrix. */
+ uint16_t numCols; /**< number of columns of the matrix. */
+ float32_t *pData; /**< points to the data of the matrix. */
+ } arm_matrix_instance_f32;
+
+ /**
+ * @brief Instance structure for the Q15 matrix structure.
+ */
+
+ typedef struct
+ {
+ uint16_t numRows; /**< number of rows of the matrix. */
+ uint16_t numCols; /**< number of columns of the matrix. */
+ q15_t *pData; /**< points to the data of the matrix. */
+
+ } arm_matrix_instance_q15;
+
+ /**
+ * @brief Instance structure for the Q31 matrix structure.
+ */
+
+ typedef struct
+ {
+ uint16_t numRows; /**< number of rows of the matrix. */
+ uint16_t numCols; /**< number of columns of the matrix. */
+ q31_t *pData; /**< points to the data of the matrix. */
+
+ } arm_matrix_instance_q31;
+
+
+
+ /**
+ * @brief Floating-point matrix addition.
+ * @param[in] *pSrcA points to the first input matrix structure
+ * @param[in] *pSrcB points to the second input matrix structure
+ * @param[out] *pDst points to output matrix structure
+ * @return The function returns either
+ * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+ */
+
+ arm_status arm_mat_add_f32(
+ const arm_matrix_instance_f32 * pSrcA,
+ const arm_matrix_instance_f32 * pSrcB,
+ arm_matrix_instance_f32 * pDst);
+
+ /**
+ * @brief Q15 matrix addition.
+ * @param[in] *pSrcA points to the first input matrix structure
+ * @param[in] *pSrcB points to the second input matrix structure
+ * @param[out] *pDst points to output matrix structure
+ * @return The function returns either
+ * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+ */
+
+ arm_status arm_mat_add_q15(
+ const arm_matrix_instance_q15 * pSrcA,
+ const arm_matrix_instance_q15 * pSrcB,
+ arm_matrix_instance_q15 * pDst);
+
+ /**
+ * @brief Q31 matrix addition.
+ * @param[in] *pSrcA points to the first input matrix structure
+ * @param[in] *pSrcB points to the second input matrix structure
+ * @param[out] *pDst points to output matrix structure
+ * @return The function returns either
+ * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+ */
+
+ arm_status arm_mat_add_q31(
+ const arm_matrix_instance_q31 * pSrcA,
+ const arm_matrix_instance_q31 * pSrcB,
+ arm_matrix_instance_q31 * pDst);
+
+
+ /**
+ * @brief Floating-point matrix transpose.
+ * @param[in] *pSrc points to the input matrix
+ * @param[out] *pDst points to the output matrix
+ * @return The function returns either <code>ARM_MATH_SIZE_MISMATCH</code>
+ * or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+ */
+
+ arm_status arm_mat_trans_f32(
+ const arm_matrix_instance_f32 * pSrc,
+ arm_matrix_instance_f32 * pDst);
+
+
+ /**
+ * @brief Q15 matrix transpose.
+ * @param[in] *pSrc points to the input matrix
+ * @param[out] *pDst points to the output matrix
+ * @return The function returns either <code>ARM_MATH_SIZE_MISMATCH</code>
+ * or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+ */
+
+ arm_status arm_mat_trans_q15(
+ const arm_matrix_instance_q15 * pSrc,
+ arm_matrix_instance_q15 * pDst);
+
+ /**
+ * @brief Q31 matrix transpose.
+ * @param[in] *pSrc points to the input matrix
+ * @param[out] *pDst points to the output matrix
+ * @return The function returns either <code>ARM_MATH_SIZE_MISMATCH</code>
+ * or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+ */
+
+ arm_status arm_mat_trans_q31(
+ const arm_matrix_instance_q31 * pSrc,
+ arm_matrix_instance_q31 * pDst);
+
+
+ /**
+ * @brief Floating-point matrix multiplication
+ * @param[in] *pSrcA points to the first input matrix structure
+ * @param[in] *pSrcB points to the second input matrix structure
+ * @param[out] *pDst points to output matrix structure
+ * @return The function returns either
+ * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+ */
+
+ arm_status arm_mat_mult_f32(
+ const arm_matrix_instance_f32 * pSrcA,
+ const arm_matrix_instance_f32 * pSrcB,
+ arm_matrix_instance_f32 * pDst);
+
+ /**
+ * @brief Q15 matrix multiplication
+ * @param[in] *pSrcA points to the first input matrix structure
+ * @param[in] *pSrcB points to the second input matrix structure
+ * @param[out] *pDst points to output matrix structure
+ * @param[in] *pState points to the array for storing intermediate results
+ * @return The function returns either
+ * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+ */
+
+ arm_status arm_mat_mult_q15(
+ const arm_matrix_instance_q15 * pSrcA,
+ const arm_matrix_instance_q15 * pSrcB,
+ arm_matrix_instance_q15 * pDst,
+ q15_t * pState);
+
+ /**
+ * @brief Q15 matrix multiplication (fast variant) for Cortex-M3 and Cortex-M4
+ * @param[in] *pSrcA points to the first input matrix structure
+ * @param[in] *pSrcB points to the second input matrix structure
+ * @param[out] *pDst points to output matrix structure
+ * @param[in] *pState points to the array for storing intermediate results
+ * @return The function returns either
+ * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+ */
+
+ arm_status arm_mat_mult_fast_q15(
+ const arm_matrix_instance_q15 * pSrcA,
+ const arm_matrix_instance_q15 * pSrcB,
+ arm_matrix_instance_q15 * pDst,
+ q15_t * pState);
+
+ /**
+ * @brief Q31 matrix multiplication
+ * @param[in] *pSrcA points to the first input matrix structure
+ * @param[in] *pSrcB points to the second input matrix structure
+ * @param[out] *pDst points to output matrix structure
+ * @return The function returns either
+ * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+ */
+
+ arm_status arm_mat_mult_q31(
+ const arm_matrix_instance_q31 * pSrcA,
+ const arm_matrix_instance_q31 * pSrcB,
+ arm_matrix_instance_q31 * pDst);
+
+ /**
+ * @brief Q31 matrix multiplication (fast variant) for Cortex-M3 and Cortex-M4
+ * @param[in] *pSrcA points to the first input matrix structure
+ * @param[in] *pSrcB points to the second input matrix structure
+ * @param[out] *pDst points to output matrix structure
+ * @return The function returns either
+ * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+ */
+
+ arm_status arm_mat_mult_fast_q31(
+ const arm_matrix_instance_q31 * pSrcA,
+ const arm_matrix_instance_q31 * pSrcB,
+ arm_matrix_instance_q31 * pDst);
+
+
+ /**
+ * @brief Floating-point matrix subtraction
+ * @param[in] *pSrcA points to the first input matrix structure
+ * @param[in] *pSrcB points to the second input matrix structure
+ * @param[out] *pDst points to output matrix structure
+ * @return The function returns either
+ * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+ */
+
+ arm_status arm_mat_sub_f32(
+ const arm_matrix_instance_f32 * pSrcA,
+ const arm_matrix_instance_f32 * pSrcB,
+ arm_matrix_instance_f32 * pDst);
+
+ /**
+ * @brief Q15 matrix subtraction
+ * @param[in] *pSrcA points to the first input matrix structure
+ * @param[in] *pSrcB points to the second input matrix structure
+ * @param[out] *pDst points to output matrix structure
+ * @return The function returns either
+ * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+ */
+
+ arm_status arm_mat_sub_q15(
+ const arm_matrix_instance_q15 * pSrcA,
+ const arm_matrix_instance_q15 * pSrcB,
+ arm_matrix_instance_q15 * pDst);
+
+ /**
+ * @brief Q31 matrix subtraction
+ * @param[in] *pSrcA points to the first input matrix structure
+ * @param[in] *pSrcB points to the second input matrix structure
+ * @param[out] *pDst points to output matrix structure
+ * @return The function returns either
+ * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+ */
+
+ arm_status arm_mat_sub_q31(
+ const arm_matrix_instance_q31 * pSrcA,
+ const arm_matrix_instance_q31 * pSrcB,
+ arm_matrix_instance_q31 * pDst);
+
+ /**
+ * @brief Floating-point matrix scaling.
+ * @param[in] *pSrc points to the input matrix
+ * @param[in] scale scale factor
+ * @param[out] *pDst points to the output matrix
+ * @return The function returns either
+ * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+ */
+
+ arm_status arm_mat_scale_f32(
+ const arm_matrix_instance_f32 * pSrc,
+ float32_t scale,
+ arm_matrix_instance_f32 * pDst);
+
+ /**
+ * @brief Q15 matrix scaling.
+ * @param[in] *pSrc points to input matrix
+ * @param[in] scaleFract fractional portion of the scale factor
+ * @param[in] shift number of bits to shift the result by
+ * @param[out] *pDst points to output matrix
+ * @return The function returns either
+ * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+ */
+
+ arm_status arm_mat_scale_q15(
+ const arm_matrix_instance_q15 * pSrc,
+ q15_t scaleFract,
+ int32_t shift,
+ arm_matrix_instance_q15 * pDst);
+
+ /**
+ * @brief Q31 matrix scaling.
+ * @param[in] *pSrc points to input matrix
+ * @param[in] scaleFract fractional portion of the scale factor
+ * @param[in] shift number of bits to shift the result by
+ * @param[out] *pDst points to output matrix structure
+ * @return The function returns either
+ * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
+ */
+
+ arm_status arm_mat_scale_q31(
+ const arm_matrix_instance_q31 * pSrc,
+ q31_t scaleFract,
+ int32_t shift,
+ arm_matrix_instance_q31 * pDst);
+
+
+ /**
+ * @brief Q31 matrix initialization.
+ * @param[in,out] *S points to an instance of the floating-point matrix structure.
+ * @param[in] nRows number of rows in the matrix.
+ * @param[in] nColumns number of columns in the matrix.
+ * @param[in] *pData points to the matrix data array.
+ * @return none
+ */
+
+ void arm_mat_init_q31(
+ arm_matrix_instance_q31 * S,
+ uint16_t nRows,
+ uint16_t nColumns,
+ q31_t * pData);
+
+ /**
+ * @brief Q15 matrix initialization.
+ * @param[in,out] *S points to an instance of the floating-point matrix structure.
+ * @param[in] nRows number of rows in the matrix.
+ * @param[in] nColumns number of columns in the matrix.
+ * @param[in] *pData points to the matrix data array.
+ * @return none
+ */
+
+ void arm_mat_init_q15(
+ arm_matrix_instance_q15 * S,
+ uint16_t nRows,
+ uint16_t nColumns,
+ q15_t * pData);
+
+ /**
+ * @brief Floating-point matrix initialization.
+ * @param[in,out] *S points to an instance of the floating-point matrix structure.
+ * @param[in] nRows number of rows in the matrix.
+ * @param[in] nColumns number of columns in the matrix.
+ * @param[in] *pData points to the matrix data array.
+ * @return none
+ */
+
+ void arm_mat_init_f32(
+ arm_matrix_instance_f32 * S,
+ uint16_t nRows,
+ uint16_t nColumns,
+ float32_t * pData);
+
+
+
+ /**
+ * @brief Instance structure for the Q15 PID Control.
+ */
+ typedef struct
+ {
+ q15_t A0; /**< The derived gain, A0 = Kp + Ki + Kd . */
+#ifdef ARM_MATH_CM0_FAMILY
+ q15_t A1;
+ q15_t A2;
+#else
+ q31_t A1; /**< The derived gain A1 = -Kp - 2Kd | Kd.*/
+#endif
+ q15_t state[3]; /**< The state array of length 3. */
+ q15_t Kp; /**< The proportional gain. */
+ q15_t Ki; /**< The integral gain. */
+ q15_t Kd; /**< The derivative gain. */
+ } arm_pid_instance_q15;
+
+ /**
+ * @brief Instance structure for the Q31 PID Control.
+ */
+ typedef struct
+ {
+ q31_t A0; /**< The derived gain, A0 = Kp + Ki + Kd . */
+ q31_t A1; /**< The derived gain, A1 = -Kp - 2Kd. */
+ q31_t A2; /**< The derived gain, A2 = Kd . */
+ q31_t state[3]; /**< The state array of length 3. */
+ q31_t Kp; /**< The proportional gain. */
+ q31_t Ki; /**< The integral gain. */
+ q31_t Kd; /**< The derivative gain. */
+
+ } arm_pid_instance_q31;
+
+ /**
+ * @brief Instance structure for the floating-point PID Control.
+ */
+ typedef struct
+ {
+ float32_t A0; /**< The derived gain, A0 = Kp + Ki + Kd . */
+ float32_t A1; /**< The derived gain, A1 = -Kp - 2Kd. */
+ float32_t A2; /**< The derived gain, A2 = Kd . */
+ float32_t state[3]; /**< The state array of length 3. */
+ float32_t Kp; /**< The proportional gain. */
+ float32_t Ki; /**< The integral gain. */
+ float32_t Kd; /**< The derivative gain. */
+ } arm_pid_instance_f32;
+
+
+
+ /**
+ * @brief Initialization function for the floating-point PID Control.
+ * @param[in,out] *S points to an instance of the PID structure.
+ * @param[in] resetStateFlag flag to reset the state. 0 = no change in state 1 = reset the state.
+ * @return none.
+ */
+ void arm_pid_init_f32(
+ arm_pid_instance_f32 * S,
+ int32_t resetStateFlag);
+
+ /**
+ * @brief Reset function for the floating-point PID Control.
+ * @param[in,out] *S is an instance of the floating-point PID Control structure
+ * @return none
+ */
+ void arm_pid_reset_f32(
+ arm_pid_instance_f32 * S);
+
+
+ /**
+ * @brief Initialization function for the Q31 PID Control.
+ * @param[in,out] *S points to an instance of the Q15 PID structure.
+ * @param[in] resetStateFlag flag to reset the state. 0 = no change in state 1 = reset the state.
+ * @return none.
+ */
+ void arm_pid_init_q31(
+ arm_pid_instance_q31 * S,
+ int32_t resetStateFlag);
+
+
+ /**
+ * @brief Reset function for the Q31 PID Control.
+ * @param[in,out] *S points to an instance of the Q31 PID Control structure
+ * @return none
+ */
+
+ void arm_pid_reset_q31(
+ arm_pid_instance_q31 * S);
+
+ /**
+ * @brief Initialization function for the Q15 PID Control.
+ * @param[in,out] *S points to an instance of the Q15 PID structure.
+ * @param[in] resetStateFlag flag to reset the state. 0 = no change in state 1 = reset the state.
+ * @return none.
+ */
+ void arm_pid_init_q15(
+ arm_pid_instance_q15 * S,
+ int32_t resetStateFlag);
+
+ /**
+ * @brief Reset function for the Q15 PID Control.
+ * @param[in,out] *S points to an instance of the q15 PID Control structure
+ * @return none
+ */
+ void arm_pid_reset_q15(
+ arm_pid_instance_q15 * S);
+
+
+ /**
+ * @brief Instance structure for the floating-point Linear Interpolate function.
+ */
+ typedef struct
+ {
+ uint32_t nValues; /**< nValues */
+ float32_t x1; /**< x1 */
+ float32_t xSpacing; /**< xSpacing */
+ float32_t *pYData; /**< pointer to the table of Y values */
+ } arm_linear_interp_instance_f32;
+
+ /**
+ * @brief Instance structure for the floating-point bilinear interpolation function.
+ */
+
+ typedef struct
+ {
+ uint16_t numRows; /**< number of rows in the data table. */
+ uint16_t numCols; /**< number of columns in the data table. */
+ float32_t *pData; /**< points to the data table. */
+ } arm_bilinear_interp_instance_f32;
+
+ /**
+ * @brief Instance structure for the Q31 bilinear interpolation function.
+ */
+
+ typedef struct
+ {
+ uint16_t numRows; /**< number of rows in the data table. */
+ uint16_t numCols; /**< number of columns in the data table. */
+ q31_t *pData; /**< points to the data table. */
+ } arm_bilinear_interp_instance_q31;
+
+ /**
+ * @brief Instance structure for the Q15 bilinear interpolation function.
+ */
+
+ typedef struct
+ {
+ uint16_t numRows; /**< number of rows in the data table. */
+ uint16_t numCols; /**< number of columns in the data table. */
+ q15_t *pData; /**< points to the data table. */
+ } arm_bilinear_interp_instance_q15;
+
+ /**
+ * @brief Instance structure for the Q15 bilinear interpolation function.
+ */
+
+ typedef struct
+ {
+ uint16_t numRows; /**< number of rows in the data table. */
+ uint16_t numCols; /**< number of columns in the data table. */
+ q7_t *pData; /**< points to the data table. */
+ } arm_bilinear_interp_instance_q7;
+
+
+ /**
+ * @brief Q7 vector multiplication.
+ * @param[in] *pSrcA points to the first input vector
+ * @param[in] *pSrcB points to the second input vector
+ * @param[out] *pDst points to the output vector
+ * @param[in] blockSize number of samples in each vector
+ * @return none.
+ */
+
+ void arm_mult_q7(
+ q7_t * pSrcA,
+ q7_t * pSrcB,
+ q7_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Q15 vector multiplication.
+ * @param[in] *pSrcA points to the first input vector
+ * @param[in] *pSrcB points to the second input vector
+ * @param[out] *pDst points to the output vector
+ * @param[in] blockSize number of samples in each vector
+ * @return none.
+ */
+
+ void arm_mult_q15(
+ q15_t * pSrcA,
+ q15_t * pSrcB,
+ q15_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Q31 vector multiplication.
+ * @param[in] *pSrcA points to the first input vector
+ * @param[in] *pSrcB points to the second input vector
+ * @param[out] *pDst points to the output vector
+ * @param[in] blockSize number of samples in each vector
+ * @return none.
+ */
+
+ void arm_mult_q31(
+ q31_t * pSrcA,
+ q31_t * pSrcB,
+ q31_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Floating-point vector multiplication.
+ * @param[in] *pSrcA points to the first input vector
+ * @param[in] *pSrcB points to the second input vector
+ * @param[out] *pDst points to the output vector
+ * @param[in] blockSize number of samples in each vector
+ * @return none.
+ */
+
+ void arm_mult_f32(
+ float32_t * pSrcA,
+ float32_t * pSrcB,
+ float32_t * pDst,
+ uint32_t blockSize);
+
+
+
+
+
+
+ /**
+ * @brief Instance structure for the Q15 CFFT/CIFFT function.
+ */
+
+ typedef struct
+ {
+ uint16_t fftLen; /**< length of the FFT. */
+ uint8_t ifftFlag; /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */
+ uint8_t bitReverseFlag; /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */
+ q15_t *pTwiddle; /**< points to the Sin twiddle factor table. */
+ uint16_t *pBitRevTable; /**< points to the bit reversal table. */
+ uint16_t twidCoefModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
+ uint16_t bitRevFactor; /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */
+ } arm_cfft_radix2_instance_q15;
+
+ arm_status arm_cfft_radix2_init_q15(
+ arm_cfft_radix2_instance_q15 * S,
+ uint16_t fftLen,
+ uint8_t ifftFlag,
+ uint8_t bitReverseFlag);
+
+ void arm_cfft_radix2_q15(
+ const arm_cfft_radix2_instance_q15 * S,
+ q15_t * pSrc);
+
+
+
+ /**
+ * @brief Instance structure for the Q15 CFFT/CIFFT function.
+ */
+
+ typedef struct
+ {
+ uint16_t fftLen; /**< length of the FFT. */
+ uint8_t ifftFlag; /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */
+ uint8_t bitReverseFlag; /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */
+ q15_t *pTwiddle; /**< points to the twiddle factor table. */
+ uint16_t *pBitRevTable; /**< points to the bit reversal table. */
+ uint16_t twidCoefModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
+ uint16_t bitRevFactor; /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */
+ } arm_cfft_radix4_instance_q15;
+
+ arm_status arm_cfft_radix4_init_q15(
+ arm_cfft_radix4_instance_q15 * S,
+ uint16_t fftLen,
+ uint8_t ifftFlag,
+ uint8_t bitReverseFlag);
+
+ void arm_cfft_radix4_q15(
+ const arm_cfft_radix4_instance_q15 * S,
+ q15_t * pSrc);
+
+ /**
+ * @brief Instance structure for the Radix-2 Q31 CFFT/CIFFT function.
+ */
+
+ typedef struct
+ {
+ uint16_t fftLen; /**< length of the FFT. */
+ uint8_t ifftFlag; /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */
+ uint8_t bitReverseFlag; /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */
+ q31_t *pTwiddle; /**< points to the Twiddle factor table. */
+ uint16_t *pBitRevTable; /**< points to the bit reversal table. */
+ uint16_t twidCoefModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
+ uint16_t bitRevFactor; /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */
+ } arm_cfft_radix2_instance_q31;
+
+ arm_status arm_cfft_radix2_init_q31(
+ arm_cfft_radix2_instance_q31 * S,
+ uint16_t fftLen,
+ uint8_t ifftFlag,
+ uint8_t bitReverseFlag);
+
+ void arm_cfft_radix2_q31(
+ const arm_cfft_radix2_instance_q31 * S,
+ q31_t * pSrc);
+
+ /**
+ * @brief Instance structure for the Q31 CFFT/CIFFT function.
+ */
+
+ typedef struct
+ {
+ uint16_t fftLen; /**< length of the FFT. */
+ uint8_t ifftFlag; /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */
+ uint8_t bitReverseFlag; /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */
+ q31_t *pTwiddle; /**< points to the twiddle factor table. */
+ uint16_t *pBitRevTable; /**< points to the bit reversal table. */
+ uint16_t twidCoefModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
+ uint16_t bitRevFactor; /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */
+ } arm_cfft_radix4_instance_q31;
+
+
+ void arm_cfft_radix4_q31(
+ const arm_cfft_radix4_instance_q31 * S,
+ q31_t * pSrc);
+
+ arm_status arm_cfft_radix4_init_q31(
+ arm_cfft_radix4_instance_q31 * S,
+ uint16_t fftLen,
+ uint8_t ifftFlag,
+ uint8_t bitReverseFlag);
+
+ /**
+ * @brief Instance structure for the floating-point CFFT/CIFFT function.
+ */
+
+ typedef struct
+ {
+ uint16_t fftLen; /**< length of the FFT. */
+ uint8_t ifftFlag; /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */
+ uint8_t bitReverseFlag; /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */
+ float32_t *pTwiddle; /**< points to the Twiddle factor table. */
+ uint16_t *pBitRevTable; /**< points to the bit reversal table. */
+ uint16_t twidCoefModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
+ uint16_t bitRevFactor; /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */
+ float32_t onebyfftLen; /**< value of 1/fftLen. */
+ } arm_cfft_radix2_instance_f32;
+
+/* Deprecated */
+ arm_status arm_cfft_radix2_init_f32(
+ arm_cfft_radix2_instance_f32 * S,
+ uint16_t fftLen,
+ uint8_t ifftFlag,
+ uint8_t bitReverseFlag);
+
+/* Deprecated */
+ void arm_cfft_radix2_f32(
+ const arm_cfft_radix2_instance_f32 * S,
+ float32_t * pSrc);
+
+ /**
+ * @brief Instance structure for the floating-point CFFT/CIFFT function.
+ */
+
+ typedef struct
+ {
+ uint16_t fftLen; /**< length of the FFT. */
+ uint8_t ifftFlag; /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */
+ uint8_t bitReverseFlag; /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */
+ float32_t *pTwiddle; /**< points to the Twiddle factor table. */
+ uint16_t *pBitRevTable; /**< points to the bit reversal table. */
+ uint16_t twidCoefModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
+ uint16_t bitRevFactor; /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */
+ float32_t onebyfftLen; /**< value of 1/fftLen. */
+ } arm_cfft_radix4_instance_f32;
+
+/* Deprecated */
+ arm_status arm_cfft_radix4_init_f32(
+ arm_cfft_radix4_instance_f32 * S,
+ uint16_t fftLen,
+ uint8_t ifftFlag,
+ uint8_t bitReverseFlag);
+
+/* Deprecated */
+ void arm_cfft_radix4_f32(
+ const arm_cfft_radix4_instance_f32 * S,
+ float32_t * pSrc);
+
+ /**
+ * @brief Instance structure for the floating-point CFFT/CIFFT function.
+ */
+
+ typedef struct
+ {
+ uint16_t fftLen; /**< length of the FFT. */
+ const float32_t *pTwiddle; /**< points to the Twiddle factor table. */
+ const uint16_t *pBitRevTable; /**< points to the bit reversal table. */
+ uint16_t bitRevLength; /**< bit reversal table length. */
+ } arm_cfft_instance_f32;
+
+ void arm_cfft_f32(
+ const arm_cfft_instance_f32 * S,
+ float32_t * p1,
+ uint8_t ifftFlag,
+ uint8_t bitReverseFlag);
+
+ /**
+ * @brief Instance structure for the Q15 RFFT/RIFFT function.
+ */
+
+ typedef struct
+ {
+ uint32_t fftLenReal; /**< length of the real FFT. */
+ uint32_t fftLenBy2; /**< length of the complex FFT. */
+ uint8_t ifftFlagR; /**< flag that selects forward (ifftFlagR=0) or inverse (ifftFlagR=1) transform. */
+ uint8_t bitReverseFlagR; /**< flag that enables (bitReverseFlagR=1) or disables (bitReverseFlagR=0) bit reversal of output. */
+ uint32_t twidCoefRModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
+ q15_t *pTwiddleAReal; /**< points to the real twiddle factor table. */
+ q15_t *pTwiddleBReal; /**< points to the imag twiddle factor table. */
+ arm_cfft_radix4_instance_q15 *pCfft; /**< points to the complex FFT instance. */
+ } arm_rfft_instance_q15;
+
+ arm_status arm_rfft_init_q15(
+ arm_rfft_instance_q15 * S,
+ arm_cfft_radix4_instance_q15 * S_CFFT,
+ uint32_t fftLenReal,
+ uint32_t ifftFlagR,
+ uint32_t bitReverseFlag);
+
+ void arm_rfft_q15(
+ const arm_rfft_instance_q15 * S,
+ q15_t * pSrc,
+ q15_t * pDst);
+
+ /**
+ * @brief Instance structure for the Q31 RFFT/RIFFT function.
+ */
+
+ typedef struct
+ {
+ uint32_t fftLenReal; /**< length of the real FFT. */
+ uint32_t fftLenBy2; /**< length of the complex FFT. */
+ uint8_t ifftFlagR; /**< flag that selects forward (ifftFlagR=0) or inverse (ifftFlagR=1) transform. */
+ uint8_t bitReverseFlagR; /**< flag that enables (bitReverseFlagR=1) or disables (bitReverseFlagR=0) bit reversal of output. */
+ uint32_t twidCoefRModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
+ q31_t *pTwiddleAReal; /**< points to the real twiddle factor table. */
+ q31_t *pTwiddleBReal; /**< points to the imag twiddle factor table. */
+ arm_cfft_radix4_instance_q31 *pCfft; /**< points to the complex FFT instance. */
+ } arm_rfft_instance_q31;
+
+ arm_status arm_rfft_init_q31(
+ arm_rfft_instance_q31 * S,
+ arm_cfft_radix4_instance_q31 * S_CFFT,
+ uint32_t fftLenReal,
+ uint32_t ifftFlagR,
+ uint32_t bitReverseFlag);
+
+ void arm_rfft_q31(
+ const arm_rfft_instance_q31 * S,
+ q31_t * pSrc,
+ q31_t * pDst);
+
+ /**
+ * @brief Instance structure for the floating-point RFFT/RIFFT function.
+ */
+
+ typedef struct
+ {
+ uint32_t fftLenReal; /**< length of the real FFT. */
+ uint16_t fftLenBy2; /**< length of the complex FFT. */
+ uint8_t ifftFlagR; /**< flag that selects forward (ifftFlagR=0) or inverse (ifftFlagR=1) transform. */
+ uint8_t bitReverseFlagR; /**< flag that enables (bitReverseFlagR=1) or disables (bitReverseFlagR=0) bit reversal of output. */
+ uint32_t twidCoefRModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
+ float32_t *pTwiddleAReal; /**< points to the real twiddle factor table. */
+ float32_t *pTwiddleBReal; /**< points to the imag twiddle factor table. */
+ arm_cfft_radix4_instance_f32 *pCfft; /**< points to the complex FFT instance. */
+ } arm_rfft_instance_f32;
+
+ arm_status arm_rfft_init_f32(
+ arm_rfft_instance_f32 * S,
+ arm_cfft_radix4_instance_f32 * S_CFFT,
+ uint32_t fftLenReal,
+ uint32_t ifftFlagR,
+ uint32_t bitReverseFlag);
+
+ void arm_rfft_f32(
+ const arm_rfft_instance_f32 * S,
+ float32_t * pSrc,
+ float32_t * pDst);
+
+ /**
+ * @brief Instance structure for the floating-point RFFT/RIFFT function.
+ */
+
+typedef struct
+ {
+ arm_cfft_instance_f32 Sint; /**< Internal CFFT structure. */
+ uint16_t fftLenRFFT; /**< length of the real sequence */
+ float32_t * pTwiddleRFFT; /**< Twiddle factors real stage */
+ } arm_rfft_fast_instance_f32 ;
+
+arm_status arm_rfft_fast_init_f32 (
+ arm_rfft_fast_instance_f32 * S,
+ uint16_t fftLen);
+
+void arm_rfft_fast_f32(
+ arm_rfft_fast_instance_f32 * S,
+ float32_t * p, float32_t * pOut,
+ uint8_t ifftFlag);
+
+ /**
+ * @brief Instance structure for the floating-point DCT4/IDCT4 function.
+ */
+
+ typedef struct
+ {
+ uint16_t N; /**< length of the DCT4. */
+ uint16_t Nby2; /**< half of the length of the DCT4. */
+ float32_t normalize; /**< normalizing factor. */
+ float32_t *pTwiddle; /**< points to the twiddle factor table. */
+ float32_t *pCosFactor; /**< points to the cosFactor table. */
+ arm_rfft_instance_f32 *pRfft; /**< points to the real FFT instance. */
+ arm_cfft_radix4_instance_f32 *pCfft; /**< points to the complex FFT instance. */
+ } arm_dct4_instance_f32;
+
+ /**
+ * @brief Initialization function for the floating-point DCT4/IDCT4.
+ * @param[in,out] *S points to an instance of floating-point DCT4/IDCT4 structure.
+ * @param[in] *S_RFFT points to an instance of floating-point RFFT/RIFFT structure.
+ * @param[in] *S_CFFT points to an instance of floating-point CFFT/CIFFT structure.
+ * @param[in] N length of the DCT4.
+ * @param[in] Nby2 half of the length of the DCT4.
+ * @param[in] normalize normalizing factor.
+ * @return arm_status function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_ARGUMENT_ERROR if <code>fftLenReal</code> is not a supported transform length.
+ */
+
+ arm_status arm_dct4_init_f32(
+ arm_dct4_instance_f32 * S,
+ arm_rfft_instance_f32 * S_RFFT,
+ arm_cfft_radix4_instance_f32 * S_CFFT,
+ uint16_t N,
+ uint16_t Nby2,
+ float32_t normalize);
+
+ /**
+ * @brief Processing function for the floating-point DCT4/IDCT4.
+ * @param[in] *S points to an instance of the floating-point DCT4/IDCT4 structure.
+ * @param[in] *pState points to state buffer.
+ * @param[in,out] *pInlineBuffer points to the in-place input and output buffer.
+ * @return none.
+ */
+
+ void arm_dct4_f32(
+ const arm_dct4_instance_f32 * S,
+ float32_t * pState,
+ float32_t * pInlineBuffer);
+
+ /**
+ * @brief Instance structure for the Q31 DCT4/IDCT4 function.
+ */
+
+ typedef struct
+ {
+ uint16_t N; /**< length of the DCT4. */
+ uint16_t Nby2; /**< half of the length of the DCT4. */
+ q31_t normalize; /**< normalizing factor. */
+ q31_t *pTwiddle; /**< points to the twiddle factor table. */
+ q31_t *pCosFactor; /**< points to the cosFactor table. */
+ arm_rfft_instance_q31 *pRfft; /**< points to the real FFT instance. */
+ arm_cfft_radix4_instance_q31 *pCfft; /**< points to the complex FFT instance. */
+ } arm_dct4_instance_q31;
+
+ /**
+ * @brief Initialization function for the Q31 DCT4/IDCT4.
+ * @param[in,out] *S points to an instance of Q31 DCT4/IDCT4 structure.
+ * @param[in] *S_RFFT points to an instance of Q31 RFFT/RIFFT structure
+ * @param[in] *S_CFFT points to an instance of Q31 CFFT/CIFFT structure
+ * @param[in] N length of the DCT4.
+ * @param[in] Nby2 half of the length of the DCT4.
+ * @param[in] normalize normalizing factor.
+ * @return arm_status function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_ARGUMENT_ERROR if <code>N</code> is not a supported transform length.
+ */
+
+ arm_status arm_dct4_init_q31(
+ arm_dct4_instance_q31 * S,
+ arm_rfft_instance_q31 * S_RFFT,
+ arm_cfft_radix4_instance_q31 * S_CFFT,
+ uint16_t N,
+ uint16_t Nby2,
+ q31_t normalize);
+
+ /**
+ * @brief Processing function for the Q31 DCT4/IDCT4.
+ * @param[in] *S points to an instance of the Q31 DCT4 structure.
+ * @param[in] *pState points to state buffer.
+ * @param[in,out] *pInlineBuffer points to the in-place input and output buffer.
+ * @return none.
+ */
+
+ void arm_dct4_q31(
+ const arm_dct4_instance_q31 * S,
+ q31_t * pState,
+ q31_t * pInlineBuffer);
+
+ /**
+ * @brief Instance structure for the Q15 DCT4/IDCT4 function.
+ */
+
+ typedef struct
+ {
+ uint16_t N; /**< length of the DCT4. */
+ uint16_t Nby2; /**< half of the length of the DCT4. */
+ q15_t normalize; /**< normalizing factor. */
+ q15_t *pTwiddle; /**< points to the twiddle factor table. */
+ q15_t *pCosFactor; /**< points to the cosFactor table. */
+ arm_rfft_instance_q15 *pRfft; /**< points to the real FFT instance. */
+ arm_cfft_radix4_instance_q15 *pCfft; /**< points to the complex FFT instance. */
+ } arm_dct4_instance_q15;
+
+ /**
+ * @brief Initialization function for the Q15 DCT4/IDCT4.
+ * @param[in,out] *S points to an instance of Q15 DCT4/IDCT4 structure.
+ * @param[in] *S_RFFT points to an instance of Q15 RFFT/RIFFT structure.
+ * @param[in] *S_CFFT points to an instance of Q15 CFFT/CIFFT structure.
+ * @param[in] N length of the DCT4.
+ * @param[in] Nby2 half of the length of the DCT4.
+ * @param[in] normalize normalizing factor.
+ * @return arm_status function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_ARGUMENT_ERROR if <code>N</code> is not a supported transform length.
+ */
+
+ arm_status arm_dct4_init_q15(
+ arm_dct4_instance_q15 * S,
+ arm_rfft_instance_q15 * S_RFFT,
+ arm_cfft_radix4_instance_q15 * S_CFFT,
+ uint16_t N,
+ uint16_t Nby2,
+ q15_t normalize);
+
+ /**
+ * @brief Processing function for the Q15 DCT4/IDCT4.
+ * @param[in] *S points to an instance of the Q15 DCT4 structure.
+ * @param[in] *pState points to state buffer.
+ * @param[in,out] *pInlineBuffer points to the in-place input and output buffer.
+ * @return none.
+ */
+
+ void arm_dct4_q15(
+ const arm_dct4_instance_q15 * S,
+ q15_t * pState,
+ q15_t * pInlineBuffer);
+
+ /**
+ * @brief Floating-point vector addition.
+ * @param[in] *pSrcA points to the first input vector
+ * @param[in] *pSrcB points to the second input vector
+ * @param[out] *pDst points to the output vector
+ * @param[in] blockSize number of samples in each vector
+ * @return none.
+ */
+
+ void arm_add_f32(
+ float32_t * pSrcA,
+ float32_t * pSrcB,
+ float32_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Q7 vector addition.
+ * @param[in] *pSrcA points to the first input vector
+ * @param[in] *pSrcB points to the second input vector
+ * @param[out] *pDst points to the output vector
+ * @param[in] blockSize number of samples in each vector
+ * @return none.
+ */
+
+ void arm_add_q7(
+ q7_t * pSrcA,
+ q7_t * pSrcB,
+ q7_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Q15 vector addition.
+ * @param[in] *pSrcA points to the first input vector
+ * @param[in] *pSrcB points to the second input vector
+ * @param[out] *pDst points to the output vector
+ * @param[in] blockSize number of samples in each vector
+ * @return none.
+ */
+
+ void arm_add_q15(
+ q15_t * pSrcA,
+ q15_t * pSrcB,
+ q15_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Q31 vector addition.
+ * @param[in] *pSrcA points to the first input vector
+ * @param[in] *pSrcB points to the second input vector
+ * @param[out] *pDst points to the output vector
+ * @param[in] blockSize number of samples in each vector
+ * @return none.
+ */
+
+ void arm_add_q31(
+ q31_t * pSrcA,
+ q31_t * pSrcB,
+ q31_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Floating-point vector subtraction.
+ * @param[in] *pSrcA points to the first input vector
+ * @param[in] *pSrcB points to the second input vector
+ * @param[out] *pDst points to the output vector
+ * @param[in] blockSize number of samples in each vector
+ * @return none.
+ */
+
+ void arm_sub_f32(
+ float32_t * pSrcA,
+ float32_t * pSrcB,
+ float32_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Q7 vector subtraction.
+ * @param[in] *pSrcA points to the first input vector
+ * @param[in] *pSrcB points to the second input vector
+ * @param[out] *pDst points to the output vector
+ * @param[in] blockSize number of samples in each vector
+ * @return none.
+ */
+
+ void arm_sub_q7(
+ q7_t * pSrcA,
+ q7_t * pSrcB,
+ q7_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Q15 vector subtraction.
+ * @param[in] *pSrcA points to the first input vector
+ * @param[in] *pSrcB points to the second input vector
+ * @param[out] *pDst points to the output vector
+ * @param[in] blockSize number of samples in each vector
+ * @return none.
+ */
+
+ void arm_sub_q15(
+ q15_t * pSrcA,
+ q15_t * pSrcB,
+ q15_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Q31 vector subtraction.
+ * @param[in] *pSrcA points to the first input vector
+ * @param[in] *pSrcB points to the second input vector
+ * @param[out] *pDst points to the output vector
+ * @param[in] blockSize number of samples in each vector
+ * @return none.
+ */
+
+ void arm_sub_q31(
+ q31_t * pSrcA,
+ q31_t * pSrcB,
+ q31_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Multiplies a floating-point vector by a scalar.
+ * @param[in] *pSrc points to the input vector
+ * @param[in] scale scale factor to be applied
+ * @param[out] *pDst points to the output vector
+ * @param[in] blockSize number of samples in the vector
+ * @return none.
+ */
+
+ void arm_scale_f32(
+ float32_t * pSrc,
+ float32_t scale,
+ float32_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Multiplies a Q7 vector by a scalar.
+ * @param[in] *pSrc points to the input vector
+ * @param[in] scaleFract fractional portion of the scale value
+ * @param[in] shift number of bits to shift the result by
+ * @param[out] *pDst points to the output vector
+ * @param[in] blockSize number of samples in the vector
+ * @return none.
+ */
+
+ void arm_scale_q7(
+ q7_t * pSrc,
+ q7_t scaleFract,
+ int8_t shift,
+ q7_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Multiplies a Q15 vector by a scalar.
+ * @param[in] *pSrc points to the input vector
+ * @param[in] scaleFract fractional portion of the scale value
+ * @param[in] shift number of bits to shift the result by
+ * @param[out] *pDst points to the output vector
+ * @param[in] blockSize number of samples in the vector
+ * @return none.
+ */
+
+ void arm_scale_q15(
+ q15_t * pSrc,
+ q15_t scaleFract,
+ int8_t shift,
+ q15_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Multiplies a Q31 vector by a scalar.
+ * @param[in] *pSrc points to the input vector
+ * @param[in] scaleFract fractional portion of the scale value
+ * @param[in] shift number of bits to shift the result by
+ * @param[out] *pDst points to the output vector
+ * @param[in] blockSize number of samples in the vector
+ * @return none.
+ */
+
+ void arm_scale_q31(
+ q31_t * pSrc,
+ q31_t scaleFract,
+ int8_t shift,
+ q31_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Q7 vector absolute value.
+ * @param[in] *pSrc points to the input buffer
+ * @param[out] *pDst points to the output buffer
+ * @param[in] blockSize number of samples in each vector
+ * @return none.
+ */
+
+ void arm_abs_q7(
+ q7_t * pSrc,
+ q7_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Floating-point vector absolute value.
+ * @param[in] *pSrc points to the input buffer
+ * @param[out] *pDst points to the output buffer
+ * @param[in] blockSize number of samples in each vector
+ * @return none.
+ */
+
+ void arm_abs_f32(
+ float32_t * pSrc,
+ float32_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Q15 vector absolute value.
+ * @param[in] *pSrc points to the input buffer
+ * @param[out] *pDst points to the output buffer
+ * @param[in] blockSize number of samples in each vector
+ * @return none.
+ */
+
+ void arm_abs_q15(
+ q15_t * pSrc,
+ q15_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Q31 vector absolute value.
+ * @param[in] *pSrc points to the input buffer
+ * @param[out] *pDst points to the output buffer
+ * @param[in] blockSize number of samples in each vector
+ * @return none.
+ */
+
+ void arm_abs_q31(
+ q31_t * pSrc,
+ q31_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Dot product of floating-point vectors.
+ * @param[in] *pSrcA points to the first input vector
+ * @param[in] *pSrcB points to the second input vector
+ * @param[in] blockSize number of samples in each vector
+ * @param[out] *result output result returned here
+ * @return none.
+ */
+
+ void arm_dot_prod_f32(
+ float32_t * pSrcA,
+ float32_t * pSrcB,
+ uint32_t blockSize,
+ float32_t * result);
+
+ /**
+ * @brief Dot product of Q7 vectors.
+ * @param[in] *pSrcA points to the first input vector
+ * @param[in] *pSrcB points to the second input vector
+ * @param[in] blockSize number of samples in each vector
+ * @param[out] *result output result returned here
+ * @return none.
+ */
+
+ void arm_dot_prod_q7(
+ q7_t * pSrcA,
+ q7_t * pSrcB,
+ uint32_t blockSize,
+ q31_t * result);
+
+ /**
+ * @brief Dot product of Q15 vectors.
+ * @param[in] *pSrcA points to the first input vector
+ * @param[in] *pSrcB points to the second input vector
+ * @param[in] blockSize number of samples in each vector
+ * @param[out] *result output result returned here
+ * @return none.
+ */
+
+ void arm_dot_prod_q15(
+ q15_t * pSrcA,
+ q15_t * pSrcB,
+ uint32_t blockSize,
+ q63_t * result);
+
+ /**
+ * @brief Dot product of Q31 vectors.
+ * @param[in] *pSrcA points to the first input vector
+ * @param[in] *pSrcB points to the second input vector
+ * @param[in] blockSize number of samples in each vector
+ * @param[out] *result output result returned here
+ * @return none.
+ */
+
+ void arm_dot_prod_q31(
+ q31_t * pSrcA,
+ q31_t * pSrcB,
+ uint32_t blockSize,
+ q63_t * result);
+
+ /**
+ * @brief Shifts the elements of a Q7 vector a specified number of bits.
+ * @param[in] *pSrc points to the input vector
+ * @param[in] shiftBits number of bits to shift. A positive value shifts left; a negative value shifts right.
+ * @param[out] *pDst points to the output vector
+ * @param[in] blockSize number of samples in the vector
+ * @return none.
+ */
+
+ void arm_shift_q7(
+ q7_t * pSrc,
+ int8_t shiftBits,
+ q7_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Shifts the elements of a Q15 vector a specified number of bits.
+ * @param[in] *pSrc points to the input vector
+ * @param[in] shiftBits number of bits to shift. A positive value shifts left; a negative value shifts right.
+ * @param[out] *pDst points to the output vector
+ * @param[in] blockSize number of samples in the vector
+ * @return none.
+ */
+
+ void arm_shift_q15(
+ q15_t * pSrc,
+ int8_t shiftBits,
+ q15_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Shifts the elements of a Q31 vector a specified number of bits.
+ * @param[in] *pSrc points to the input vector
+ * @param[in] shiftBits number of bits to shift. A positive value shifts left; a negative value shifts right.
+ * @param[out] *pDst points to the output vector
+ * @param[in] blockSize number of samples in the vector
+ * @return none.
+ */
+
+ void arm_shift_q31(
+ q31_t * pSrc,
+ int8_t shiftBits,
+ q31_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Adds a constant offset to a floating-point vector.
+ * @param[in] *pSrc points to the input vector
+ * @param[in] offset is the offset to be added
+ * @param[out] *pDst points to the output vector
+ * @param[in] blockSize number of samples in the vector
+ * @return none.
+ */
+
+ void arm_offset_f32(
+ float32_t * pSrc,
+ float32_t offset,
+ float32_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Adds a constant offset to a Q7 vector.
+ * @param[in] *pSrc points to the input vector
+ * @param[in] offset is the offset to be added
+ * @param[out] *pDst points to the output vector
+ * @param[in] blockSize number of samples in the vector
+ * @return none.
+ */
+
+ void arm_offset_q7(
+ q7_t * pSrc,
+ q7_t offset,
+ q7_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Adds a constant offset to a Q15 vector.
+ * @param[in] *pSrc points to the input vector
+ * @param[in] offset is the offset to be added
+ * @param[out] *pDst points to the output vector
+ * @param[in] blockSize number of samples in the vector
+ * @return none.
+ */
+
+ void arm_offset_q15(
+ q15_t * pSrc,
+ q15_t offset,
+ q15_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Adds a constant offset to a Q31 vector.
+ * @param[in] *pSrc points to the input vector
+ * @param[in] offset is the offset to be added
+ * @param[out] *pDst points to the output vector
+ * @param[in] blockSize number of samples in the vector
+ * @return none.
+ */
+
+ void arm_offset_q31(
+ q31_t * pSrc,
+ q31_t offset,
+ q31_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Negates the elements of a floating-point vector.
+ * @param[in] *pSrc points to the input vector
+ * @param[out] *pDst points to the output vector
+ * @param[in] blockSize number of samples in the vector
+ * @return none.
+ */
+
+ void arm_negate_f32(
+ float32_t * pSrc,
+ float32_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Negates the elements of a Q7 vector.
+ * @param[in] *pSrc points to the input vector
+ * @param[out] *pDst points to the output vector
+ * @param[in] blockSize number of samples in the vector
+ * @return none.
+ */
+
+ void arm_negate_q7(
+ q7_t * pSrc,
+ q7_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Negates the elements of a Q15 vector.
+ * @param[in] *pSrc points to the input vector
+ * @param[out] *pDst points to the output vector
+ * @param[in] blockSize number of samples in the vector
+ * @return none.
+ */
+
+ void arm_negate_q15(
+ q15_t * pSrc,
+ q15_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Negates the elements of a Q31 vector.
+ * @param[in] *pSrc points to the input vector
+ * @param[out] *pDst points to the output vector
+ * @param[in] blockSize number of samples in the vector
+ * @return none.
+ */
+
+ void arm_negate_q31(
+ q31_t * pSrc,
+ q31_t * pDst,
+ uint32_t blockSize);
+ /**
+ * @brief Copies the elements of a floating-point vector.
+ * @param[in] *pSrc input pointer
+ * @param[out] *pDst output pointer
+ * @param[in] blockSize number of samples to process
+ * @return none.
+ */
+ void arm_copy_f32(
+ float32_t * pSrc,
+ float32_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Copies the elements of a Q7 vector.
+ * @param[in] *pSrc input pointer
+ * @param[out] *pDst output pointer
+ * @param[in] blockSize number of samples to process
+ * @return none.
+ */
+ void arm_copy_q7(
+ q7_t * pSrc,
+ q7_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Copies the elements of a Q15 vector.
+ * @param[in] *pSrc input pointer
+ * @param[out] *pDst output pointer
+ * @param[in] blockSize number of samples to process
+ * @return none.
+ */
+ void arm_copy_q15(
+ q15_t * pSrc,
+ q15_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Copies the elements of a Q31 vector.
+ * @param[in] *pSrc input pointer
+ * @param[out] *pDst output pointer
+ * @param[in] blockSize number of samples to process
+ * @return none.
+ */
+ void arm_copy_q31(
+ q31_t * pSrc,
+ q31_t * pDst,
+ uint32_t blockSize);
+ /**
+ * @brief Fills a constant value into a floating-point vector.
+ * @param[in] value input value to be filled
+ * @param[out] *pDst output pointer
+ * @param[in] blockSize number of samples to process
+ * @return none.
+ */
+ void arm_fill_f32(
+ float32_t value,
+ float32_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Fills a constant value into a Q7 vector.
+ * @param[in] value input value to be filled
+ * @param[out] *pDst output pointer
+ * @param[in] blockSize number of samples to process
+ * @return none.
+ */
+ void arm_fill_q7(
+ q7_t value,
+ q7_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Fills a constant value into a Q15 vector.
+ * @param[in] value input value to be filled
+ * @param[out] *pDst output pointer
+ * @param[in] blockSize number of samples to process
+ * @return none.
+ */
+ void arm_fill_q15(
+ q15_t value,
+ q15_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Fills a constant value into a Q31 vector.
+ * @param[in] value input value to be filled
+ * @param[out] *pDst output pointer
+ * @param[in] blockSize number of samples to process
+ * @return none.
+ */
+ void arm_fill_q31(
+ q31_t value,
+ q31_t * pDst,
+ uint32_t blockSize);
+
+/**
+ * @brief Convolution of floating-point sequences.
+ * @param[in] *pSrcA points to the first input sequence.
+ * @param[in] srcALen length of the first input sequence.
+ * @param[in] *pSrcB points to the second input sequence.
+ * @param[in] srcBLen length of the second input sequence.
+ * @param[out] *pDst points to the location where the output result is written. Length srcALen+srcBLen-1.
+ * @return none.
+ */
+
+ void arm_conv_f32(
+ float32_t * pSrcA,
+ uint32_t srcALen,
+ float32_t * pSrcB,
+ uint32_t srcBLen,
+ float32_t * pDst);
+
+
+ /**
+ * @brief Convolution of Q15 sequences.
+ * @param[in] *pSrcA points to the first input sequence.
+ * @param[in] srcALen length of the first input sequence.
+ * @param[in] *pSrcB points to the second input sequence.
+ * @param[in] srcBLen length of the second input sequence.
+ * @param[out] *pDst points to the block of output data Length srcALen+srcBLen-1.
+ * @param[in] *pScratch1 points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
+ * @param[in] *pScratch2 points to scratch buffer of size min(srcALen, srcBLen).
+ * @return none.
+ */
+
+
+ void arm_conv_opt_q15(
+ q15_t * pSrcA,
+ uint32_t srcALen,
+ q15_t * pSrcB,
+ uint32_t srcBLen,
+ q15_t * pDst,
+ q15_t * pScratch1,
+ q15_t * pScratch2);
+
+
+/**
+ * @brief Convolution of Q15 sequences.
+ * @param[in] *pSrcA points to the first input sequence.
+ * @param[in] srcALen length of the first input sequence.
+ * @param[in] *pSrcB points to the second input sequence.
+ * @param[in] srcBLen length of the second input sequence.
+ * @param[out] *pDst points to the location where the output result is written. Length srcALen+srcBLen-1.
+ * @return none.
+ */
+
+ void arm_conv_q15(
+ q15_t * pSrcA,
+ uint32_t srcALen,
+ q15_t * pSrcB,
+ uint32_t srcBLen,
+ q15_t * pDst);
+
+ /**
+ * @brief Convolution of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4
+ * @param[in] *pSrcA points to the first input sequence.
+ * @param[in] srcALen length of the first input sequence.
+ * @param[in] *pSrcB points to the second input sequence.
+ * @param[in] srcBLen length of the second input sequence.
+ * @param[out] *pDst points to the block of output data Length srcALen+srcBLen-1.
+ * @return none.
+ */
+
+ void arm_conv_fast_q15(
+ q15_t * pSrcA,
+ uint32_t srcALen,
+ q15_t * pSrcB,
+ uint32_t srcBLen,
+ q15_t * pDst);
+
+ /**
+ * @brief Convolution of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4
+ * @param[in] *pSrcA points to the first input sequence.
+ * @param[in] srcALen length of the first input sequence.
+ * @param[in] *pSrcB points to the second input sequence.
+ * @param[in] srcBLen length of the second input sequence.
+ * @param[out] *pDst points to the block of output data Length srcALen+srcBLen-1.
+ * @param[in] *pScratch1 points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
+ * @param[in] *pScratch2 points to scratch buffer of size min(srcALen, srcBLen).
+ * @return none.
+ */
+
+ void arm_conv_fast_opt_q15(
+ q15_t * pSrcA,
+ uint32_t srcALen,
+ q15_t * pSrcB,
+ uint32_t srcBLen,
+ q15_t * pDst,
+ q15_t * pScratch1,
+ q15_t * pScratch2);
+
+
+
+ /**
+ * @brief Convolution of Q31 sequences.
+ * @param[in] *pSrcA points to the first input sequence.
+ * @param[in] srcALen length of the first input sequence.
+ * @param[in] *pSrcB points to the second input sequence.
+ * @param[in] srcBLen length of the second input sequence.
+ * @param[out] *pDst points to the block of output data Length srcALen+srcBLen-1.
+ * @return none.
+ */
+
+ void arm_conv_q31(
+ q31_t * pSrcA,
+ uint32_t srcALen,
+ q31_t * pSrcB,
+ uint32_t srcBLen,
+ q31_t * pDst);
+
+ /**
+ * @brief Convolution of Q31 sequences (fast version) for Cortex-M3 and Cortex-M4
+ * @param[in] *pSrcA points to the first input sequence.
+ * @param[in] srcALen length of the first input sequence.
+ * @param[in] *pSrcB points to the second input sequence.
+ * @param[in] srcBLen length of the second input sequence.
+ * @param[out] *pDst points to the block of output data Length srcALen+srcBLen-1.
+ * @return none.
+ */
+
+ void arm_conv_fast_q31(
+ q31_t * pSrcA,
+ uint32_t srcALen,
+ q31_t * pSrcB,
+ uint32_t srcBLen,
+ q31_t * pDst);
+
+
+ /**
+ * @brief Convolution of Q7 sequences.
+ * @param[in] *pSrcA points to the first input sequence.
+ * @param[in] srcALen length of the first input sequence.
+ * @param[in] *pSrcB points to the second input sequence.
+ * @param[in] srcBLen length of the second input sequence.
+ * @param[out] *pDst points to the block of output data Length srcALen+srcBLen-1.
+ * @param[in] *pScratch1 points to scratch buffer(of type q15_t) of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
+ * @param[in] *pScratch2 points to scratch buffer (of type q15_t) of size min(srcALen, srcBLen).
+ * @return none.
+ */
+
+ void arm_conv_opt_q7(
+ q7_t * pSrcA,
+ uint32_t srcALen,
+ q7_t * pSrcB,
+ uint32_t srcBLen,
+ q7_t * pDst,
+ q15_t * pScratch1,
+ q15_t * pScratch2);
+
+
+
+ /**
+ * @brief Convolution of Q7 sequences.
+ * @param[in] *pSrcA points to the first input sequence.
+ * @param[in] srcALen length of the first input sequence.
+ * @param[in] *pSrcB points to the second input sequence.
+ * @param[in] srcBLen length of the second input sequence.
+ * @param[out] *pDst points to the block of output data Length srcALen+srcBLen-1.
+ * @return none.
+ */
+
+ void arm_conv_q7(
+ q7_t * pSrcA,
+ uint32_t srcALen,
+ q7_t * pSrcB,
+ uint32_t srcBLen,
+ q7_t * pDst);
+
+
+ /**
+ * @brief Partial convolution of floating-point sequences.
+ * @param[in] *pSrcA points to the first input sequence.
+ * @param[in] srcALen length of the first input sequence.
+ * @param[in] *pSrcB points to the second input sequence.
+ * @param[in] srcBLen length of the second input sequence.
+ * @param[out] *pDst points to the block of output data
+ * @param[in] firstIndex is the first output sample to start with.
+ * @param[in] numPoints is the number of output points to be computed.
+ * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
+ */
+
+ arm_status arm_conv_partial_f32(
+ float32_t * pSrcA,
+ uint32_t srcALen,
+ float32_t * pSrcB,
+ uint32_t srcBLen,
+ float32_t * pDst,
+ uint32_t firstIndex,
+ uint32_t numPoints);
+
+ /**
+ * @brief Partial convolution of Q15 sequences.
+ * @param[in] *pSrcA points to the first input sequence.
+ * @param[in] srcALen length of the first input sequence.
+ * @param[in] *pSrcB points to the second input sequence.
+ * @param[in] srcBLen length of the second input sequence.
+ * @param[out] *pDst points to the block of output data
+ * @param[in] firstIndex is the first output sample to start with.
+ * @param[in] numPoints is the number of output points to be computed.
+ * @param[in] * pScratch1 points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
+ * @param[in] * pScratch2 points to scratch buffer of size min(srcALen, srcBLen).
+ * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
+ */
+
+ arm_status arm_conv_partial_opt_q15(
+ q15_t * pSrcA,
+ uint32_t srcALen,
+ q15_t * pSrcB,
+ uint32_t srcBLen,
+ q15_t * pDst,
+ uint32_t firstIndex,
+ uint32_t numPoints,
+ q15_t * pScratch1,
+ q15_t * pScratch2);
+
+
+/**
+ * @brief Partial convolution of Q15 sequences.
+ * @param[in] *pSrcA points to the first input sequence.
+ * @param[in] srcALen length of the first input sequence.
+ * @param[in] *pSrcB points to the second input sequence.
+ * @param[in] srcBLen length of the second input sequence.
+ * @param[out] *pDst points to the block of output data
+ * @param[in] firstIndex is the first output sample to start with.
+ * @param[in] numPoints is the number of output points to be computed.
+ * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
+ */
+
+ arm_status arm_conv_partial_q15(
+ q15_t * pSrcA,
+ uint32_t srcALen,
+ q15_t * pSrcB,
+ uint32_t srcBLen,
+ q15_t * pDst,
+ uint32_t firstIndex,
+ uint32_t numPoints);
+
+ /**
+ * @brief Partial convolution of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4
+ * @param[in] *pSrcA points to the first input sequence.
+ * @param[in] srcALen length of the first input sequence.
+ * @param[in] *pSrcB points to the second input sequence.
+ * @param[in] srcBLen length of the second input sequence.
+ * @param[out] *pDst points to the block of output data
+ * @param[in] firstIndex is the first output sample to start with.
+ * @param[in] numPoints is the number of output points to be computed.
+ * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
+ */
+
+ arm_status arm_conv_partial_fast_q15(
+ q15_t * pSrcA,
+ uint32_t srcALen,
+ q15_t * pSrcB,
+ uint32_t srcBLen,
+ q15_t * pDst,
+ uint32_t firstIndex,
+ uint32_t numPoints);
+
+
+ /**
+ * @brief Partial convolution of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4
+ * @param[in] *pSrcA points to the first input sequence.
+ * @param[in] srcALen length of the first input sequence.
+ * @param[in] *pSrcB points to the second input sequence.
+ * @param[in] srcBLen length of the second input sequence.
+ * @param[out] *pDst points to the block of output data
+ * @param[in] firstIndex is the first output sample to start with.
+ * @param[in] numPoints is the number of output points to be computed.
+ * @param[in] * pScratch1 points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
+ * @param[in] * pScratch2 points to scratch buffer of size min(srcALen, srcBLen).
+ * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
+ */
+
+ arm_status arm_conv_partial_fast_opt_q15(
+ q15_t * pSrcA,
+ uint32_t srcALen,
+ q15_t * pSrcB,
+ uint32_t srcBLen,
+ q15_t * pDst,
+ uint32_t firstIndex,
+ uint32_t numPoints,
+ q15_t * pScratch1,
+ q15_t * pScratch2);
+
+
+ /**
+ * @brief Partial convolution of Q31 sequences.
+ * @param[in] *pSrcA points to the first input sequence.
+ * @param[in] srcALen length of the first input sequence.
+ * @param[in] *pSrcB points to the second input sequence.
+ * @param[in] srcBLen length of the second input sequence.
+ * @param[out] *pDst points to the block of output data
+ * @param[in] firstIndex is the first output sample to start with.
+ * @param[in] numPoints is the number of output points to be computed.
+ * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
+ */
+
+ arm_status arm_conv_partial_q31(
+ q31_t * pSrcA,
+ uint32_t srcALen,
+ q31_t * pSrcB,
+ uint32_t srcBLen,
+ q31_t * pDst,
+ uint32_t firstIndex,
+ uint32_t numPoints);
+
+
+ /**
+ * @brief Partial convolution of Q31 sequences (fast version) for Cortex-M3 and Cortex-M4
+ * @param[in] *pSrcA points to the first input sequence.
+ * @param[in] srcALen length of the first input sequence.
+ * @param[in] *pSrcB points to the second input sequence.
+ * @param[in] srcBLen length of the second input sequence.
+ * @param[out] *pDst points to the block of output data
+ * @param[in] firstIndex is the first output sample to start with.
+ * @param[in] numPoints is the number of output points to be computed.
+ * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
+ */
+
+ arm_status arm_conv_partial_fast_q31(
+ q31_t * pSrcA,
+ uint32_t srcALen,
+ q31_t * pSrcB,
+ uint32_t srcBLen,
+ q31_t * pDst,
+ uint32_t firstIndex,
+ uint32_t numPoints);
+
+
+ /**
+ * @brief Partial convolution of Q7 sequences
+ * @param[in] *pSrcA points to the first input sequence.
+ * @param[in] srcALen length of the first input sequence.
+ * @param[in] *pSrcB points to the second input sequence.
+ * @param[in] srcBLen length of the second input sequence.
+ * @param[out] *pDst points to the block of output data
+ * @param[in] firstIndex is the first output sample to start with.
+ * @param[in] numPoints is the number of output points to be computed.
+ * @param[in] *pScratch1 points to scratch buffer(of type q15_t) of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
+ * @param[in] *pScratch2 points to scratch buffer (of type q15_t) of size min(srcALen, srcBLen).
+ * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
+ */
+
+ arm_status arm_conv_partial_opt_q7(
+ q7_t * pSrcA,
+ uint32_t srcALen,
+ q7_t * pSrcB,
+ uint32_t srcBLen,
+ q7_t * pDst,
+ uint32_t firstIndex,
+ uint32_t numPoints,
+ q15_t * pScratch1,
+ q15_t * pScratch2);
+
+
+/**
+ * @brief Partial convolution of Q7 sequences.
+ * @param[in] *pSrcA points to the first input sequence.
+ * @param[in] srcALen length of the first input sequence.
+ * @param[in] *pSrcB points to the second input sequence.
+ * @param[in] srcBLen length of the second input sequence.
+ * @param[out] *pDst points to the block of output data
+ * @param[in] firstIndex is the first output sample to start with.
+ * @param[in] numPoints is the number of output points to be computed.
+ * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
+ */
+
+ arm_status arm_conv_partial_q7(
+ q7_t * pSrcA,
+ uint32_t srcALen,
+ q7_t * pSrcB,
+ uint32_t srcBLen,
+ q7_t * pDst,
+ uint32_t firstIndex,
+ uint32_t numPoints);
+
+
+
+ /**
+ * @brief Instance structure for the Q15 FIR decimator.
+ */
+
+ typedef struct
+ {
+ uint8_t M; /**< decimation factor. */
+ uint16_t numTaps; /**< number of coefficients in the filter. */
+ q15_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/
+ q15_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
+ } arm_fir_decimate_instance_q15;
+
+ /**
+ * @brief Instance structure for the Q31 FIR decimator.
+ */
+
+ typedef struct
+ {
+ uint8_t M; /**< decimation factor. */
+ uint16_t numTaps; /**< number of coefficients in the filter. */
+ q31_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/
+ q31_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
+
+ } arm_fir_decimate_instance_q31;
+
+ /**
+ * @brief Instance structure for the floating-point FIR decimator.
+ */
+
+ typedef struct
+ {
+ uint8_t M; /**< decimation factor. */
+ uint16_t numTaps; /**< number of coefficients in the filter. */
+ float32_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/
+ float32_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
+
+ } arm_fir_decimate_instance_f32;
+
+
+
+ /**
+ * @brief Processing function for the floating-point FIR decimator.
+ * @param[in] *S points to an instance of the floating-point FIR decimator structure.
+ * @param[in] *pSrc points to the block of input data.
+ * @param[out] *pDst points to the block of output data
+ * @param[in] blockSize number of input samples to process per call.
+ * @return none
+ */
+
+ void arm_fir_decimate_f32(
+ const arm_fir_decimate_instance_f32 * S,
+ float32_t * pSrc,
+ float32_t * pDst,
+ uint32_t blockSize);
+
+
+ /**
+ * @brief Initialization function for the floating-point FIR decimator.
+ * @param[in,out] *S points to an instance of the floating-point FIR decimator structure.
+ * @param[in] numTaps number of coefficients in the filter.
+ * @param[in] M decimation factor.
+ * @param[in] *pCoeffs points to the filter coefficients.
+ * @param[in] *pState points to the state buffer.
+ * @param[in] blockSize number of input samples to process per call.
+ * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if
+ * <code>blockSize</code> is not a multiple of <code>M</code>.
+ */
+
+ arm_status arm_fir_decimate_init_f32(
+ arm_fir_decimate_instance_f32 * S,
+ uint16_t numTaps,
+ uint8_t M,
+ float32_t * pCoeffs,
+ float32_t * pState,
+ uint32_t blockSize);
+
+ /**
+ * @brief Processing function for the Q15 FIR decimator.
+ * @param[in] *S points to an instance of the Q15 FIR decimator structure.
+ * @param[in] *pSrc points to the block of input data.
+ * @param[out] *pDst points to the block of output data
+ * @param[in] blockSize number of input samples to process per call.
+ * @return none
+ */
+
+ void arm_fir_decimate_q15(
+ const arm_fir_decimate_instance_q15 * S,
+ q15_t * pSrc,
+ q15_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Processing function for the Q15 FIR decimator (fast variant) for Cortex-M3 and Cortex-M4.
+ * @param[in] *S points to an instance of the Q15 FIR decimator structure.
+ * @param[in] *pSrc points to the block of input data.
+ * @param[out] *pDst points to the block of output data
+ * @param[in] blockSize number of input samples to process per call.
+ * @return none
+ */
+
+ void arm_fir_decimate_fast_q15(
+ const arm_fir_decimate_instance_q15 * S,
+ q15_t * pSrc,
+ q15_t * pDst,
+ uint32_t blockSize);
+
+
+
+ /**
+ * @brief Initialization function for the Q15 FIR decimator.
+ * @param[in,out] *S points to an instance of the Q15 FIR decimator structure.
+ * @param[in] numTaps number of coefficients in the filter.
+ * @param[in] M decimation factor.
+ * @param[in] *pCoeffs points to the filter coefficients.
+ * @param[in] *pState points to the state buffer.
+ * @param[in] blockSize number of input samples to process per call.
+ * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if
+ * <code>blockSize</code> is not a multiple of <code>M</code>.
+ */
+
+ arm_status arm_fir_decimate_init_q15(
+ arm_fir_decimate_instance_q15 * S,
+ uint16_t numTaps,
+ uint8_t M,
+ q15_t * pCoeffs,
+ q15_t * pState,
+ uint32_t blockSize);
+
+ /**
+ * @brief Processing function for the Q31 FIR decimator.
+ * @param[in] *S points to an instance of the Q31 FIR decimator structure.
+ * @param[in] *pSrc points to the block of input data.
+ * @param[out] *pDst points to the block of output data
+ * @param[in] blockSize number of input samples to process per call.
+ * @return none
+ */
+
+ void arm_fir_decimate_q31(
+ const arm_fir_decimate_instance_q31 * S,
+ q31_t * pSrc,
+ q31_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Processing function for the Q31 FIR decimator (fast variant) for Cortex-M3 and Cortex-M4.
+ * @param[in] *S points to an instance of the Q31 FIR decimator structure.
+ * @param[in] *pSrc points to the block of input data.
+ * @param[out] *pDst points to the block of output data
+ * @param[in] blockSize number of input samples to process per call.
+ * @return none
+ */
+
+ void arm_fir_decimate_fast_q31(
+ arm_fir_decimate_instance_q31 * S,
+ q31_t * pSrc,
+ q31_t * pDst,
+ uint32_t blockSize);
+
+
+ /**
+ * @brief Initialization function for the Q31 FIR decimator.
+ * @param[in,out] *S points to an instance of the Q31 FIR decimator structure.
+ * @param[in] numTaps number of coefficients in the filter.
+ * @param[in] M decimation factor.
+ * @param[in] *pCoeffs points to the filter coefficients.
+ * @param[in] *pState points to the state buffer.
+ * @param[in] blockSize number of input samples to process per call.
+ * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if
+ * <code>blockSize</code> is not a multiple of <code>M</code>.
+ */
+
+ arm_status arm_fir_decimate_init_q31(
+ arm_fir_decimate_instance_q31 * S,
+ uint16_t numTaps,
+ uint8_t M,
+ q31_t * pCoeffs,
+ q31_t * pState,
+ uint32_t blockSize);
+
+
+
+ /**
+ * @brief Instance structure for the Q15 FIR interpolator.
+ */
+
+ typedef struct
+ {
+ uint8_t L; /**< upsample factor. */
+ uint16_t phaseLength; /**< length of each polyphase filter component. */
+ q15_t *pCoeffs; /**< points to the coefficient array. The array is of length L*phaseLength. */
+ q15_t *pState; /**< points to the state variable array. The array is of length blockSize+phaseLength-1. */
+ } arm_fir_interpolate_instance_q15;
+
+ /**
+ * @brief Instance structure for the Q31 FIR interpolator.
+ */
+
+ typedef struct
+ {
+ uint8_t L; /**< upsample factor. */
+ uint16_t phaseLength; /**< length of each polyphase filter component. */
+ q31_t *pCoeffs; /**< points to the coefficient array. The array is of length L*phaseLength. */
+ q31_t *pState; /**< points to the state variable array. The array is of length blockSize+phaseLength-1. */
+ } arm_fir_interpolate_instance_q31;
+
+ /**
+ * @brief Instance structure for the floating-point FIR interpolator.
+ */
+
+ typedef struct
+ {
+ uint8_t L; /**< upsample factor. */
+ uint16_t phaseLength; /**< length of each polyphase filter component. */
+ float32_t *pCoeffs; /**< points to the coefficient array. The array is of length L*phaseLength. */
+ float32_t *pState; /**< points to the state variable array. The array is of length phaseLength+numTaps-1. */
+ } arm_fir_interpolate_instance_f32;
+
+
+ /**
+ * @brief Processing function for the Q15 FIR interpolator.
+ * @param[in] *S points to an instance of the Q15 FIR interpolator structure.
+ * @param[in] *pSrc points to the block of input data.
+ * @param[out] *pDst points to the block of output data.
+ * @param[in] blockSize number of input samples to process per call.
+ * @return none.
+ */
+
+ void arm_fir_interpolate_q15(
+ const arm_fir_interpolate_instance_q15 * S,
+ q15_t * pSrc,
+ q15_t * pDst,
+ uint32_t blockSize);
+
+
+ /**
+ * @brief Initialization function for the Q15 FIR interpolator.
+ * @param[in,out] *S points to an instance of the Q15 FIR interpolator structure.
+ * @param[in] L upsample factor.
+ * @param[in] numTaps number of filter coefficients in the filter.
+ * @param[in] *pCoeffs points to the filter coefficient buffer.
+ * @param[in] *pState points to the state buffer.
+ * @param[in] blockSize number of input samples to process per call.
+ * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if
+ * the filter length <code>numTaps</code> is not a multiple of the interpolation factor <code>L</code>.
+ */
+
+ arm_status arm_fir_interpolate_init_q15(
+ arm_fir_interpolate_instance_q15 * S,
+ uint8_t L,
+ uint16_t numTaps,
+ q15_t * pCoeffs,
+ q15_t * pState,
+ uint32_t blockSize);
+
+ /**
+ * @brief Processing function for the Q31 FIR interpolator.
+ * @param[in] *S points to an instance of the Q15 FIR interpolator structure.
+ * @param[in] *pSrc points to the block of input data.
+ * @param[out] *pDst points to the block of output data.
+ * @param[in] blockSize number of input samples to process per call.
+ * @return none.
+ */
+
+ void arm_fir_interpolate_q31(
+ const arm_fir_interpolate_instance_q31 * S,
+ q31_t * pSrc,
+ q31_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Initialization function for the Q31 FIR interpolator.
+ * @param[in,out] *S points to an instance of the Q31 FIR interpolator structure.
+ * @param[in] L upsample factor.
+ * @param[in] numTaps number of filter coefficients in the filter.
+ * @param[in] *pCoeffs points to the filter coefficient buffer.
+ * @param[in] *pState points to the state buffer.
+ * @param[in] blockSize number of input samples to process per call.
+ * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if
+ * the filter length <code>numTaps</code> is not a multiple of the interpolation factor <code>L</code>.
+ */
+
+ arm_status arm_fir_interpolate_init_q31(
+ arm_fir_interpolate_instance_q31 * S,
+ uint8_t L,
+ uint16_t numTaps,
+ q31_t * pCoeffs,
+ q31_t * pState,
+ uint32_t blockSize);
+
+
+ /**
+ * @brief Processing function for the floating-point FIR interpolator.
+ * @param[in] *S points to an instance of the floating-point FIR interpolator structure.
+ * @param[in] *pSrc points to the block of input data.
+ * @param[out] *pDst points to the block of output data.
+ * @param[in] blockSize number of input samples to process per call.
+ * @return none.
+ */
+
+ void arm_fir_interpolate_f32(
+ const arm_fir_interpolate_instance_f32 * S,
+ float32_t * pSrc,
+ float32_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Initialization function for the floating-point FIR interpolator.
+ * @param[in,out] *S points to an instance of the floating-point FIR interpolator structure.
+ * @param[in] L upsample factor.
+ * @param[in] numTaps number of filter coefficients in the filter.
+ * @param[in] *pCoeffs points to the filter coefficient buffer.
+ * @param[in] *pState points to the state buffer.
+ * @param[in] blockSize number of input samples to process per call.
+ * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if
+ * the filter length <code>numTaps</code> is not a multiple of the interpolation factor <code>L</code>.
+ */
+
+ arm_status arm_fir_interpolate_init_f32(
+ arm_fir_interpolate_instance_f32 * S,
+ uint8_t L,
+ uint16_t numTaps,
+ float32_t * pCoeffs,
+ float32_t * pState,
+ uint32_t blockSize);
+
+ /**
+ * @brief Instance structure for the high precision Q31 Biquad cascade filter.
+ */
+
+ typedef struct
+ {
+ uint8_t numStages; /**< number of 2nd order stages in the filter. Overall order is 2*numStages. */
+ q63_t *pState; /**< points to the array of state coefficients. The array is of length 4*numStages. */
+ q31_t *pCoeffs; /**< points to the array of coefficients. The array is of length 5*numStages. */
+ uint8_t postShift; /**< additional shift, in bits, applied to each output sample. */
+
+ } arm_biquad_cas_df1_32x64_ins_q31;
+
+
+ /**
+ * @param[in] *S points to an instance of the high precision Q31 Biquad cascade filter structure.
+ * @param[in] *pSrc points to the block of input data.
+ * @param[out] *pDst points to the block of output data
+ * @param[in] blockSize number of samples to process.
+ * @return none.
+ */
+
+ void arm_biquad_cas_df1_32x64_q31(
+ const arm_biquad_cas_df1_32x64_ins_q31 * S,
+ q31_t * pSrc,
+ q31_t * pDst,
+ uint32_t blockSize);
+
+
+ /**
+ * @param[in,out] *S points to an instance of the high precision Q31 Biquad cascade filter structure.
+ * @param[in] numStages number of 2nd order stages in the filter.
+ * @param[in] *pCoeffs points to the filter coefficients.
+ * @param[in] *pState points to the state buffer.
+ * @param[in] postShift shift to be applied to the output. Varies according to the coefficients format
+ * @return none
+ */
+
+ void arm_biquad_cas_df1_32x64_init_q31(
+ arm_biquad_cas_df1_32x64_ins_q31 * S,
+ uint8_t numStages,
+ q31_t * pCoeffs,
+ q63_t * pState,
+ uint8_t postShift);
+
+
+
+ /**
+ * @brief Instance structure for the floating-point transposed direct form II Biquad cascade filter.
+ */
+
+ typedef struct
+ {
+ uint8_t numStages; /**< number of 2nd order stages in the filter. Overall order is 2*numStages. */
+ float32_t *pState; /**< points to the array of state coefficients. The array is of length 2*numStages. */
+ float32_t *pCoeffs; /**< points to the array of coefficients. The array is of length 5*numStages. */
+ } arm_biquad_cascade_df2T_instance_f32;
+
+
+ /**
+ * @brief Processing function for the floating-point transposed direct form II Biquad cascade filter.
+ * @param[in] *S points to an instance of the filter data structure.
+ * @param[in] *pSrc points to the block of input data.
+ * @param[out] *pDst points to the block of output data
+ * @param[in] blockSize number of samples to process.
+ * @return none.
+ */
+
+ void arm_biquad_cascade_df2T_f32(
+ const arm_biquad_cascade_df2T_instance_f32 * S,
+ float32_t * pSrc,
+ float32_t * pDst,
+ uint32_t blockSize);
+
+
+ /**
+ * @brief Initialization function for the floating-point transposed direct form II Biquad cascade filter.
+ * @param[in,out] *S points to an instance of the filter data structure.
+ * @param[in] numStages number of 2nd order stages in the filter.
+ * @param[in] *pCoeffs points to the filter coefficients.
+ * @param[in] *pState points to the state buffer.
+ * @return none
+ */
+
+ void arm_biquad_cascade_df2T_init_f32(
+ arm_biquad_cascade_df2T_instance_f32 * S,
+ uint8_t numStages,
+ float32_t * pCoeffs,
+ float32_t * pState);
+
+
+
+ /**
+ * @brief Instance structure for the Q15 FIR lattice filter.
+ */
+
+ typedef struct
+ {
+ uint16_t numStages; /**< number of filter stages. */
+ q15_t *pState; /**< points to the state variable array. The array is of length numStages. */
+ q15_t *pCoeffs; /**< points to the coefficient array. The array is of length numStages. */
+ } arm_fir_lattice_instance_q15;
+
+ /**
+ * @brief Instance structure for the Q31 FIR lattice filter.
+ */
+
+ typedef struct
+ {
+ uint16_t numStages; /**< number of filter stages. */
+ q31_t *pState; /**< points to the state variable array. The array is of length numStages. */
+ q31_t *pCoeffs; /**< points to the coefficient array. The array is of length numStages. */
+ } arm_fir_lattice_instance_q31;
+
+ /**
+ * @brief Instance structure for the floating-point FIR lattice filter.
+ */
+
+ typedef struct
+ {
+ uint16_t numStages; /**< number of filter stages. */
+ float32_t *pState; /**< points to the state variable array. The array is of length numStages. */
+ float32_t *pCoeffs; /**< points to the coefficient array. The array is of length numStages. */
+ } arm_fir_lattice_instance_f32;
+
+ /**
+ * @brief Initialization function for the Q15 FIR lattice filter.
+ * @param[in] *S points to an instance of the Q15 FIR lattice structure.
+ * @param[in] numStages number of filter stages.
+ * @param[in] *pCoeffs points to the coefficient buffer. The array is of length numStages.
+ * @param[in] *pState points to the state buffer. The array is of length numStages.
+ * @return none.
+ */
+
+ void arm_fir_lattice_init_q15(
+ arm_fir_lattice_instance_q15 * S,
+ uint16_t numStages,
+ q15_t * pCoeffs,
+ q15_t * pState);
+
+
+ /**
+ * @brief Processing function for the Q15 FIR lattice filter.
+ * @param[in] *S points to an instance of the Q15 FIR lattice structure.
+ * @param[in] *pSrc points to the block of input data.
+ * @param[out] *pDst points to the block of output data.
+ * @param[in] blockSize number of samples to process.
+ * @return none.
+ */
+ void arm_fir_lattice_q15(
+ const arm_fir_lattice_instance_q15 * S,
+ q15_t * pSrc,
+ q15_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Initialization function for the Q31 FIR lattice filter.
+ * @param[in] *S points to an instance of the Q31 FIR lattice structure.
+ * @param[in] numStages number of filter stages.
+ * @param[in] *pCoeffs points to the coefficient buffer. The array is of length numStages.
+ * @param[in] *pState points to the state buffer. The array is of length numStages.
+ * @return none.
+ */
+
+ void arm_fir_lattice_init_q31(
+ arm_fir_lattice_instance_q31 * S,
+ uint16_t numStages,
+ q31_t * pCoeffs,
+ q31_t * pState);
+
+
+ /**
+ * @brief Processing function for the Q31 FIR lattice filter.
+ * @param[in] *S points to an instance of the Q31 FIR lattice structure.
+ * @param[in] *pSrc points to the block of input data.
+ * @param[out] *pDst points to the block of output data
+ * @param[in] blockSize number of samples to process.
+ * @return none.
+ */
+
+ void arm_fir_lattice_q31(
+ const arm_fir_lattice_instance_q31 * S,
+ q31_t * pSrc,
+ q31_t * pDst,
+ uint32_t blockSize);
+
+/**
+ * @brief Initialization function for the floating-point FIR lattice filter.
+ * @param[in] *S points to an instance of the floating-point FIR lattice structure.
+ * @param[in] numStages number of filter stages.
+ * @param[in] *pCoeffs points to the coefficient buffer. The array is of length numStages.
+ * @param[in] *pState points to the state buffer. The array is of length numStages.
+ * @return none.
+ */
+
+ void arm_fir_lattice_init_f32(
+ arm_fir_lattice_instance_f32 * S,
+ uint16_t numStages,
+ float32_t * pCoeffs,
+ float32_t * pState);
+
+ /**
+ * @brief Processing function for the floating-point FIR lattice filter.
+ * @param[in] *S points to an instance of the floating-point FIR lattice structure.
+ * @param[in] *pSrc points to the block of input data.
+ * @param[out] *pDst points to the block of output data
+ * @param[in] blockSize number of samples to process.
+ * @return none.
+ */
+
+ void arm_fir_lattice_f32(
+ const arm_fir_lattice_instance_f32 * S,
+ float32_t * pSrc,
+ float32_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Instance structure for the Q15 IIR lattice filter.
+ */
+ typedef struct
+ {
+ uint16_t numStages; /**< number of stages in the filter. */
+ q15_t *pState; /**< points to the state variable array. The array is of length numStages+blockSize. */
+ q15_t *pkCoeffs; /**< points to the reflection coefficient array. The array is of length numStages. */
+ q15_t *pvCoeffs; /**< points to the ladder coefficient array. The array is of length numStages+1. */
+ } arm_iir_lattice_instance_q15;
+
+ /**
+ * @brief Instance structure for the Q31 IIR lattice filter.
+ */
+ typedef struct
+ {
+ uint16_t numStages; /**< number of stages in the filter. */
+ q31_t *pState; /**< points to the state variable array. The array is of length numStages+blockSize. */
+ q31_t *pkCoeffs; /**< points to the reflection coefficient array. The array is of length numStages. */
+ q31_t *pvCoeffs; /**< points to the ladder coefficient array. The array is of length numStages+1. */
+ } arm_iir_lattice_instance_q31;
+
+ /**
+ * @brief Instance structure for the floating-point IIR lattice filter.
+ */
+ typedef struct
+ {
+ uint16_t numStages; /**< number of stages in the filter. */
+ float32_t *pState; /**< points to the state variable array. The array is of length numStages+blockSize. */
+ float32_t *pkCoeffs; /**< points to the reflection coefficient array. The array is of length numStages. */
+ float32_t *pvCoeffs; /**< points to the ladder coefficient array. The array is of length numStages+1. */
+ } arm_iir_lattice_instance_f32;
+
+ /**
+ * @brief Processing function for the floating-point IIR lattice filter.
+ * @param[in] *S points to an instance of the floating-point IIR lattice structure.
+ * @param[in] *pSrc points to the block of input data.
+ * @param[out] *pDst points to the block of output data.
+ * @param[in] blockSize number of samples to process.
+ * @return none.
+ */
+
+ void arm_iir_lattice_f32(
+ const arm_iir_lattice_instance_f32 * S,
+ float32_t * pSrc,
+ float32_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Initialization function for the floating-point IIR lattice filter.
+ * @param[in] *S points to an instance of the floating-point IIR lattice structure.
+ * @param[in] numStages number of stages in the filter.
+ * @param[in] *pkCoeffs points to the reflection coefficient buffer. The array is of length numStages.
+ * @param[in] *pvCoeffs points to the ladder coefficient buffer. The array is of length numStages+1.
+ * @param[in] *pState points to the state buffer. The array is of length numStages+blockSize-1.
+ * @param[in] blockSize number of samples to process.
+ * @return none.
+ */
+
+ void arm_iir_lattice_init_f32(
+ arm_iir_lattice_instance_f32 * S,
+ uint16_t numStages,
+ float32_t * pkCoeffs,
+ float32_t * pvCoeffs,
+ float32_t * pState,
+ uint32_t blockSize);
+
+
+ /**
+ * @brief Processing function for the Q31 IIR lattice filter.
+ * @param[in] *S points to an instance of the Q31 IIR lattice structure.
+ * @param[in] *pSrc points to the block of input data.
+ * @param[out] *pDst points to the block of output data.
+ * @param[in] blockSize number of samples to process.
+ * @return none.
+ */
+
+ void arm_iir_lattice_q31(
+ const arm_iir_lattice_instance_q31 * S,
+ q31_t * pSrc,
+ q31_t * pDst,
+ uint32_t blockSize);
+
+
+ /**
+ * @brief Initialization function for the Q31 IIR lattice filter.
+ * @param[in] *S points to an instance of the Q31 IIR lattice structure.
+ * @param[in] numStages number of stages in the filter.
+ * @param[in] *pkCoeffs points to the reflection coefficient buffer. The array is of length numStages.
+ * @param[in] *pvCoeffs points to the ladder coefficient buffer. The array is of length numStages+1.
+ * @param[in] *pState points to the state buffer. The array is of length numStages+blockSize.
+ * @param[in] blockSize number of samples to process.
+ * @return none.
+ */
+
+ void arm_iir_lattice_init_q31(
+ arm_iir_lattice_instance_q31 * S,
+ uint16_t numStages,
+ q31_t * pkCoeffs,
+ q31_t * pvCoeffs,
+ q31_t * pState,
+ uint32_t blockSize);
+
+
+ /**
+ * @brief Processing function for the Q15 IIR lattice filter.
+ * @param[in] *S points to an instance of the Q15 IIR lattice structure.
+ * @param[in] *pSrc points to the block of input data.
+ * @param[out] *pDst points to the block of output data.
+ * @param[in] blockSize number of samples to process.
+ * @return none.
+ */
+
+ void arm_iir_lattice_q15(
+ const arm_iir_lattice_instance_q15 * S,
+ q15_t * pSrc,
+ q15_t * pDst,
+ uint32_t blockSize);
+
+
+/**
+ * @brief Initialization function for the Q15 IIR lattice filter.
+ * @param[in] *S points to an instance of the fixed-point Q15 IIR lattice structure.
+ * @param[in] numStages number of stages in the filter.
+ * @param[in] *pkCoeffs points to reflection coefficient buffer. The array is of length numStages.
+ * @param[in] *pvCoeffs points to ladder coefficient buffer. The array is of length numStages+1.
+ * @param[in] *pState points to state buffer. The array is of length numStages+blockSize.
+ * @param[in] blockSize number of samples to process per call.
+ * @return none.
+ */
+
+ void arm_iir_lattice_init_q15(
+ arm_iir_lattice_instance_q15 * S,
+ uint16_t numStages,
+ q15_t * pkCoeffs,
+ q15_t * pvCoeffs,
+ q15_t * pState,
+ uint32_t blockSize);
+
+ /**
+ * @brief Instance structure for the floating-point LMS filter.
+ */
+
+ typedef struct
+ {
+ uint16_t numTaps; /**< number of coefficients in the filter. */
+ float32_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
+ float32_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */
+ float32_t mu; /**< step size that controls filter coefficient updates. */
+ } arm_lms_instance_f32;
+
+ /**
+ * @brief Processing function for floating-point LMS filter.
+ * @param[in] *S points to an instance of the floating-point LMS filter structure.
+ * @param[in] *pSrc points to the block of input data.
+ * @param[in] *pRef points to the block of reference data.
+ * @param[out] *pOut points to the block of output data.
+ * @param[out] *pErr points to the block of error data.
+ * @param[in] blockSize number of samples to process.
+ * @return none.
+ */
+
+ void arm_lms_f32(
+ const arm_lms_instance_f32 * S,
+ float32_t * pSrc,
+ float32_t * pRef,
+ float32_t * pOut,
+ float32_t * pErr,
+ uint32_t blockSize);
+
+ /**
+ * @brief Initialization function for floating-point LMS filter.
+ * @param[in] *S points to an instance of the floating-point LMS filter structure.
+ * @param[in] numTaps number of filter coefficients.
+ * @param[in] *pCoeffs points to the coefficient buffer.
+ * @param[in] *pState points to state buffer.
+ * @param[in] mu step size that controls filter coefficient updates.
+ * @param[in] blockSize number of samples to process.
+ * @return none.
+ */
+
+ void arm_lms_init_f32(
+ arm_lms_instance_f32 * S,
+ uint16_t numTaps,
+ float32_t * pCoeffs,
+ float32_t * pState,
+ float32_t mu,
+ uint32_t blockSize);
+
+ /**
+ * @brief Instance structure for the Q15 LMS filter.
+ */
+
+ typedef struct
+ {
+ uint16_t numTaps; /**< number of coefficients in the filter. */
+ q15_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
+ q15_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */
+ q15_t mu; /**< step size that controls filter coefficient updates. */
+ uint32_t postShift; /**< bit shift applied to coefficients. */
+ } arm_lms_instance_q15;
+
+
+ /**
+ * @brief Initialization function for the Q15 LMS filter.
+ * @param[in] *S points to an instance of the Q15 LMS filter structure.
+ * @param[in] numTaps number of filter coefficients.
+ * @param[in] *pCoeffs points to the coefficient buffer.
+ * @param[in] *pState points to the state buffer.
+ * @param[in] mu step size that controls filter coefficient updates.
+ * @param[in] blockSize number of samples to process.
+ * @param[in] postShift bit shift applied to coefficients.
+ * @return none.
+ */
+
+ void arm_lms_init_q15(
+ arm_lms_instance_q15 * S,
+ uint16_t numTaps,
+ q15_t * pCoeffs,
+ q15_t * pState,
+ q15_t mu,
+ uint32_t blockSize,
+ uint32_t postShift);
+
+ /**
+ * @brief Processing function for Q15 LMS filter.
+ * @param[in] *S points to an instance of the Q15 LMS filter structure.
+ * @param[in] *pSrc points to the block of input data.
+ * @param[in] *pRef points to the block of reference data.
+ * @param[out] *pOut points to the block of output data.
+ * @param[out] *pErr points to the block of error data.
+ * @param[in] blockSize number of samples to process.
+ * @return none.
+ */
+
+ void arm_lms_q15(
+ const arm_lms_instance_q15 * S,
+ q15_t * pSrc,
+ q15_t * pRef,
+ q15_t * pOut,
+ q15_t * pErr,
+ uint32_t blockSize);
+
+
+ /**
+ * @brief Instance structure for the Q31 LMS filter.
+ */
+
+ typedef struct
+ {
+ uint16_t numTaps; /**< number of coefficients in the filter. */
+ q31_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
+ q31_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */
+ q31_t mu; /**< step size that controls filter coefficient updates. */
+ uint32_t postShift; /**< bit shift applied to coefficients. */
+
+ } arm_lms_instance_q31;
+
+ /**
+ * @brief Processing function for Q31 LMS filter.
+ * @param[in] *S points to an instance of the Q15 LMS filter structure.
+ * @param[in] *pSrc points to the block of input data.
+ * @param[in] *pRef points to the block of reference data.
+ * @param[out] *pOut points to the block of output data.
+ * @param[out] *pErr points to the block of error data.
+ * @param[in] blockSize number of samples to process.
+ * @return none.
+ */
+
+ void arm_lms_q31(
+ const arm_lms_instance_q31 * S,
+ q31_t * pSrc,
+ q31_t * pRef,
+ q31_t * pOut,
+ q31_t * pErr,
+ uint32_t blockSize);
+
+ /**
+ * @brief Initialization function for Q31 LMS filter.
+ * @param[in] *S points to an instance of the Q31 LMS filter structure.
+ * @param[in] numTaps number of filter coefficients.
+ * @param[in] *pCoeffs points to coefficient buffer.
+ * @param[in] *pState points to state buffer.
+ * @param[in] mu step size that controls filter coefficient updates.
+ * @param[in] blockSize number of samples to process.
+ * @param[in] postShift bit shift applied to coefficients.
+ * @return none.
+ */
+
+ void arm_lms_init_q31(
+ arm_lms_instance_q31 * S,
+ uint16_t numTaps,
+ q31_t * pCoeffs,
+ q31_t * pState,
+ q31_t mu,
+ uint32_t blockSize,
+ uint32_t postShift);
+
+ /**
+ * @brief Instance structure for the floating-point normalized LMS filter.
+ */
+
+ typedef struct
+ {
+ uint16_t numTaps; /**< number of coefficients in the filter. */
+ float32_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
+ float32_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */
+ float32_t mu; /**< step size that control filter coefficient updates. */
+ float32_t energy; /**< saves previous frame energy. */
+ float32_t x0; /**< saves previous input sample. */
+ } arm_lms_norm_instance_f32;
+
+ /**
+ * @brief Processing function for floating-point normalized LMS filter.
+ * @param[in] *S points to an instance of the floating-point normalized LMS filter structure.
+ * @param[in] *pSrc points to the block of input data.
+ * @param[in] *pRef points to the block of reference data.
+ * @param[out] *pOut points to the block of output data.
+ * @param[out] *pErr points to the block of error data.
+ * @param[in] blockSize number of samples to process.
+ * @return none.
+ */
+
+ void arm_lms_norm_f32(
+ arm_lms_norm_instance_f32 * S,
+ float32_t * pSrc,
+ float32_t * pRef,
+ float32_t * pOut,
+ float32_t * pErr,
+ uint32_t blockSize);
+
+ /**
+ * @brief Initialization function for floating-point normalized LMS filter.
+ * @param[in] *S points to an instance of the floating-point LMS filter structure.
+ * @param[in] numTaps number of filter coefficients.
+ * @param[in] *pCoeffs points to coefficient buffer.
+ * @param[in] *pState points to state buffer.
+ * @param[in] mu step size that controls filter coefficient updates.
+ * @param[in] blockSize number of samples to process.
+ * @return none.
+ */
+
+ void arm_lms_norm_init_f32(
+ arm_lms_norm_instance_f32 * S,
+ uint16_t numTaps,
+ float32_t * pCoeffs,
+ float32_t * pState,
+ float32_t mu,
+ uint32_t blockSize);
+
+
+ /**
+ * @brief Instance structure for the Q31 normalized LMS filter.
+ */
+ typedef struct
+ {
+ uint16_t numTaps; /**< number of coefficients in the filter. */
+ q31_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
+ q31_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */
+ q31_t mu; /**< step size that controls filter coefficient updates. */
+ uint8_t postShift; /**< bit shift applied to coefficients. */
+ q31_t *recipTable; /**< points to the reciprocal initial value table. */
+ q31_t energy; /**< saves previous frame energy. */
+ q31_t x0; /**< saves previous input sample. */
+ } arm_lms_norm_instance_q31;
+
+ /**
+ * @brief Processing function for Q31 normalized LMS filter.
+ * @param[in] *S points to an instance of the Q31 normalized LMS filter structure.
+ * @param[in] *pSrc points to the block of input data.
+ * @param[in] *pRef points to the block of reference data.
+ * @param[out] *pOut points to the block of output data.
+ * @param[out] *pErr points to the block of error data.
+ * @param[in] blockSize number of samples to process.
+ * @return none.
+ */
+
+ void arm_lms_norm_q31(
+ arm_lms_norm_instance_q31 * S,
+ q31_t * pSrc,
+ q31_t * pRef,
+ q31_t * pOut,
+ q31_t * pErr,
+ uint32_t blockSize);
+
+ /**
+ * @brief Initialization function for Q31 normalized LMS filter.
+ * @param[in] *S points to an instance of the Q31 normalized LMS filter structure.
+ * @param[in] numTaps number of filter coefficients.
+ * @param[in] *pCoeffs points to coefficient buffer.
+ * @param[in] *pState points to state buffer.
+ * @param[in] mu step size that controls filter coefficient updates.
+ * @param[in] blockSize number of samples to process.
+ * @param[in] postShift bit shift applied to coefficients.
+ * @return none.
+ */
+
+ void arm_lms_norm_init_q31(
+ arm_lms_norm_instance_q31 * S,
+ uint16_t numTaps,
+ q31_t * pCoeffs,
+ q31_t * pState,
+ q31_t mu,
+ uint32_t blockSize,
+ uint8_t postShift);
+
+ /**
+ * @brief Instance structure for the Q15 normalized LMS filter.
+ */
+
+ typedef struct
+ {
+ uint16_t numTaps; /**< Number of coefficients in the filter. */
+ q15_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
+ q15_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */
+ q15_t mu; /**< step size that controls filter coefficient updates. */
+ uint8_t postShift; /**< bit shift applied to coefficients. */
+ q15_t *recipTable; /**< Points to the reciprocal initial value table. */
+ q15_t energy; /**< saves previous frame energy. */
+ q15_t x0; /**< saves previous input sample. */
+ } arm_lms_norm_instance_q15;
+
+ /**
+ * @brief Processing function for Q15 normalized LMS filter.
+ * @param[in] *S points to an instance of the Q15 normalized LMS filter structure.
+ * @param[in] *pSrc points to the block of input data.
+ * @param[in] *pRef points to the block of reference data.
+ * @param[out] *pOut points to the block of output data.
+ * @param[out] *pErr points to the block of error data.
+ * @param[in] blockSize number of samples to process.
+ * @return none.
+ */
+
+ void arm_lms_norm_q15(
+ arm_lms_norm_instance_q15 * S,
+ q15_t * pSrc,
+ q15_t * pRef,
+ q15_t * pOut,
+ q15_t * pErr,
+ uint32_t blockSize);
+
+
+ /**
+ * @brief Initialization function for Q15 normalized LMS filter.
+ * @param[in] *S points to an instance of the Q15 normalized LMS filter structure.
+ * @param[in] numTaps number of filter coefficients.
+ * @param[in] *pCoeffs points to coefficient buffer.
+ * @param[in] *pState points to state buffer.
+ * @param[in] mu step size that controls filter coefficient updates.
+ * @param[in] blockSize number of samples to process.
+ * @param[in] postShift bit shift applied to coefficients.
+ * @return none.
+ */
+
+ void arm_lms_norm_init_q15(
+ arm_lms_norm_instance_q15 * S,
+ uint16_t numTaps,
+ q15_t * pCoeffs,
+ q15_t * pState,
+ q15_t mu,
+ uint32_t blockSize,
+ uint8_t postShift);
+
+ /**
+ * @brief Correlation of floating-point sequences.
+ * @param[in] *pSrcA points to the first input sequence.
+ * @param[in] srcALen length of the first input sequence.
+ * @param[in] *pSrcB points to the second input sequence.
+ * @param[in] srcBLen length of the second input sequence.
+ * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1.
+ * @return none.
+ */
+
+ void arm_correlate_f32(
+ float32_t * pSrcA,
+ uint32_t srcALen,
+ float32_t * pSrcB,
+ uint32_t srcBLen,
+ float32_t * pDst);
+
+
+ /**
+ * @brief Correlation of Q15 sequences
+ * @param[in] *pSrcA points to the first input sequence.
+ * @param[in] srcALen length of the first input sequence.
+ * @param[in] *pSrcB points to the second input sequence.
+ * @param[in] srcBLen length of the second input sequence.
+ * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1.
+ * @param[in] *pScratch points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
+ * @return none.
+ */
+ void arm_correlate_opt_q15(
+ q15_t * pSrcA,
+ uint32_t srcALen,
+ q15_t * pSrcB,
+ uint32_t srcBLen,
+ q15_t * pDst,
+ q15_t * pScratch);
+
+
+ /**
+ * @brief Correlation of Q15 sequences.
+ * @param[in] *pSrcA points to the first input sequence.
+ * @param[in] srcALen length of the first input sequence.
+ * @param[in] *pSrcB points to the second input sequence.
+ * @param[in] srcBLen length of the second input sequence.
+ * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1.
+ * @return none.
+ */
+
+ void arm_correlate_q15(
+ q15_t * pSrcA,
+ uint32_t srcALen,
+ q15_t * pSrcB,
+ uint32_t srcBLen,
+ q15_t * pDst);
+
+ /**
+ * @brief Correlation of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4.
+ * @param[in] *pSrcA points to the first input sequence.
+ * @param[in] srcALen length of the first input sequence.
+ * @param[in] *pSrcB points to the second input sequence.
+ * @param[in] srcBLen length of the second input sequence.
+ * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1.
+ * @return none.
+ */
+
+ void arm_correlate_fast_q15(
+ q15_t * pSrcA,
+ uint32_t srcALen,
+ q15_t * pSrcB,
+ uint32_t srcBLen,
+ q15_t * pDst);
+
+
+
+ /**
+ * @brief Correlation of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4.
+ * @param[in] *pSrcA points to the first input sequence.
+ * @param[in] srcALen length of the first input sequence.
+ * @param[in] *pSrcB points to the second input sequence.
+ * @param[in] srcBLen length of the second input sequence.
+ * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1.
+ * @param[in] *pScratch points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
+ * @return none.
+ */
+
+ void arm_correlate_fast_opt_q15(
+ q15_t * pSrcA,
+ uint32_t srcALen,
+ q15_t * pSrcB,
+ uint32_t srcBLen,
+ q15_t * pDst,
+ q15_t * pScratch);
+
+ /**
+ * @brief Correlation of Q31 sequences.
+ * @param[in] *pSrcA points to the first input sequence.
+ * @param[in] srcALen length of the first input sequence.
+ * @param[in] *pSrcB points to the second input sequence.
+ * @param[in] srcBLen length of the second input sequence.
+ * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1.
+ * @return none.
+ */
+
+ void arm_correlate_q31(
+ q31_t * pSrcA,
+ uint32_t srcALen,
+ q31_t * pSrcB,
+ uint32_t srcBLen,
+ q31_t * pDst);
+
+ /**
+ * @brief Correlation of Q31 sequences (fast version) for Cortex-M3 and Cortex-M4
+ * @param[in] *pSrcA points to the first input sequence.
+ * @param[in] srcALen length of the first input sequence.
+ * @param[in] *pSrcB points to the second input sequence.
+ * @param[in] srcBLen length of the second input sequence.
+ * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1.
+ * @return none.
+ */
+
+ void arm_correlate_fast_q31(
+ q31_t * pSrcA,
+ uint32_t srcALen,
+ q31_t * pSrcB,
+ uint32_t srcBLen,
+ q31_t * pDst);
+
+
+
+ /**
+ * @brief Correlation of Q7 sequences.
+ * @param[in] *pSrcA points to the first input sequence.
+ * @param[in] srcALen length of the first input sequence.
+ * @param[in] *pSrcB points to the second input sequence.
+ * @param[in] srcBLen length of the second input sequence.
+ * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1.
+ * @param[in] *pScratch1 points to scratch buffer(of type q15_t) of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
+ * @param[in] *pScratch2 points to scratch buffer (of type q15_t) of size min(srcALen, srcBLen).
+ * @return none.
+ */
+
+ void arm_correlate_opt_q7(
+ q7_t * pSrcA,
+ uint32_t srcALen,
+ q7_t * pSrcB,
+ uint32_t srcBLen,
+ q7_t * pDst,
+ q15_t * pScratch1,
+ q15_t * pScratch2);
+
+
+ /**
+ * @brief Correlation of Q7 sequences.
+ * @param[in] *pSrcA points to the first input sequence.
+ * @param[in] srcALen length of the first input sequence.
+ * @param[in] *pSrcB points to the second input sequence.
+ * @param[in] srcBLen length of the second input sequence.
+ * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1.
+ * @return none.
+ */
+
+ void arm_correlate_q7(
+ q7_t * pSrcA,
+ uint32_t srcALen,
+ q7_t * pSrcB,
+ uint32_t srcBLen,
+ q7_t * pDst);
+
+
+ /**
+ * @brief Instance structure for the floating-point sparse FIR filter.
+ */
+ typedef struct
+ {
+ uint16_t numTaps; /**< number of coefficients in the filter. */
+ uint16_t stateIndex; /**< state buffer index. Points to the oldest sample in the state buffer. */
+ float32_t *pState; /**< points to the state buffer array. The array is of length maxDelay+blockSize-1. */
+ float32_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/
+ uint16_t maxDelay; /**< maximum offset specified by the pTapDelay array. */
+ int32_t *pTapDelay; /**< points to the array of delay values. The array is of length numTaps. */
+ } arm_fir_sparse_instance_f32;
+
+ /**
+ * @brief Instance structure for the Q31 sparse FIR filter.
+ */
+
+ typedef struct
+ {
+ uint16_t numTaps; /**< number of coefficients in the filter. */
+ uint16_t stateIndex; /**< state buffer index. Points to the oldest sample in the state buffer. */
+ q31_t *pState; /**< points to the state buffer array. The array is of length maxDelay+blockSize-1. */
+ q31_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/
+ uint16_t maxDelay; /**< maximum offset specified by the pTapDelay array. */
+ int32_t *pTapDelay; /**< points to the array of delay values. The array is of length numTaps. */
+ } arm_fir_sparse_instance_q31;
+
+ /**
+ * @brief Instance structure for the Q15 sparse FIR filter.
+ */
+
+ typedef struct
+ {
+ uint16_t numTaps; /**< number of coefficients in the filter. */
+ uint16_t stateIndex; /**< state buffer index. Points to the oldest sample in the state buffer. */
+ q15_t *pState; /**< points to the state buffer array. The array is of length maxDelay+blockSize-1. */
+ q15_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/
+ uint16_t maxDelay; /**< maximum offset specified by the pTapDelay array. */
+ int32_t *pTapDelay; /**< points to the array of delay values. The array is of length numTaps. */
+ } arm_fir_sparse_instance_q15;
+
+ /**
+ * @brief Instance structure for the Q7 sparse FIR filter.
+ */
+
+ typedef struct
+ {
+ uint16_t numTaps; /**< number of coefficients in the filter. */
+ uint16_t stateIndex; /**< state buffer index. Points to the oldest sample in the state buffer. */
+ q7_t *pState; /**< points to the state buffer array. The array is of length maxDelay+blockSize-1. */
+ q7_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/
+ uint16_t maxDelay; /**< maximum offset specified by the pTapDelay array. */
+ int32_t *pTapDelay; /**< points to the array of delay values. The array is of length numTaps. */
+ } arm_fir_sparse_instance_q7;
+
+ /**
+ * @brief Processing function for the floating-point sparse FIR filter.
+ * @param[in] *S points to an instance of the floating-point sparse FIR structure.
+ * @param[in] *pSrc points to the block of input data.
+ * @param[out] *pDst points to the block of output data
+ * @param[in] *pScratchIn points to a temporary buffer of size blockSize.
+ * @param[in] blockSize number of input samples to process per call.
+ * @return none.
+ */
+
+ void arm_fir_sparse_f32(
+ arm_fir_sparse_instance_f32 * S,
+ float32_t * pSrc,
+ float32_t * pDst,
+ float32_t * pScratchIn,
+ uint32_t blockSize);
+
+ /**
+ * @brief Initialization function for the floating-point sparse FIR filter.
+ * @param[in,out] *S points to an instance of the floating-point sparse FIR structure.
+ * @param[in] numTaps number of nonzero coefficients in the filter.
+ * @param[in] *pCoeffs points to the array of filter coefficients.
+ * @param[in] *pState points to the state buffer.
+ * @param[in] *pTapDelay points to the array of offset times.
+ * @param[in] maxDelay maximum offset time supported.
+ * @param[in] blockSize number of samples that will be processed per block.
+ * @return none
+ */
+
+ void arm_fir_sparse_init_f32(
+ arm_fir_sparse_instance_f32 * S,
+ uint16_t numTaps,
+ float32_t * pCoeffs,
+ float32_t * pState,
+ int32_t * pTapDelay,
+ uint16_t maxDelay,
+ uint32_t blockSize);
+
+ /**
+ * @brief Processing function for the Q31 sparse FIR filter.
+ * @param[in] *S points to an instance of the Q31 sparse FIR structure.
+ * @param[in] *pSrc points to the block of input data.
+ * @param[out] *pDst points to the block of output data
+ * @param[in] *pScratchIn points to a temporary buffer of size blockSize.
+ * @param[in] blockSize number of input samples to process per call.
+ * @return none.
+ */
+
+ void arm_fir_sparse_q31(
+ arm_fir_sparse_instance_q31 * S,
+ q31_t * pSrc,
+ q31_t * pDst,
+ q31_t * pScratchIn,
+ uint32_t blockSize);
+
+ /**
+ * @brief Initialization function for the Q31 sparse FIR filter.
+ * @param[in,out] *S points to an instance of the Q31 sparse FIR structure.
+ * @param[in] numTaps number of nonzero coefficients in the filter.
+ * @param[in] *pCoeffs points to the array of filter coefficients.
+ * @param[in] *pState points to the state buffer.
+ * @param[in] *pTapDelay points to the array of offset times.
+ * @param[in] maxDelay maximum offset time supported.
+ * @param[in] blockSize number of samples that will be processed per block.
+ * @return none
+ */
+
+ void arm_fir_sparse_init_q31(
+ arm_fir_sparse_instance_q31 * S,
+ uint16_t numTaps,
+ q31_t * pCoeffs,
+ q31_t * pState,
+ int32_t * pTapDelay,
+ uint16_t maxDelay,
+ uint32_t blockSize);
+
+ /**
+ * @brief Processing function for the Q15 sparse FIR filter.
+ * @param[in] *S points to an instance of the Q15 sparse FIR structure.
+ * @param[in] *pSrc points to the block of input data.
+ * @param[out] *pDst points to the block of output data
+ * @param[in] *pScratchIn points to a temporary buffer of size blockSize.
+ * @param[in] *pScratchOut points to a temporary buffer of size blockSize.
+ * @param[in] blockSize number of input samples to process per call.
+ * @return none.
+ */
+
+ void arm_fir_sparse_q15(
+ arm_fir_sparse_instance_q15 * S,
+ q15_t * pSrc,
+ q15_t * pDst,
+ q15_t * pScratchIn,
+ q31_t * pScratchOut,
+ uint32_t blockSize);
+
+
+ /**
+ * @brief Initialization function for the Q15 sparse FIR filter.
+ * @param[in,out] *S points to an instance of the Q15 sparse FIR structure.
+ * @param[in] numTaps number of nonzero coefficients in the filter.
+ * @param[in] *pCoeffs points to the array of filter coefficients.
+ * @param[in] *pState points to the state buffer.
+ * @param[in] *pTapDelay points to the array of offset times.
+ * @param[in] maxDelay maximum offset time supported.
+ * @param[in] blockSize number of samples that will be processed per block.
+ * @return none
+ */
+
+ void arm_fir_sparse_init_q15(
+ arm_fir_sparse_instance_q15 * S,
+ uint16_t numTaps,
+ q15_t * pCoeffs,
+ q15_t * pState,
+ int32_t * pTapDelay,
+ uint16_t maxDelay,
+ uint32_t blockSize);
+
+ /**
+ * @brief Processing function for the Q7 sparse FIR filter.
+ * @param[in] *S points to an instance of the Q7 sparse FIR structure.
+ * @param[in] *pSrc points to the block of input data.
+ * @param[out] *pDst points to the block of output data
+ * @param[in] *pScratchIn points to a temporary buffer of size blockSize.
+ * @param[in] *pScratchOut points to a temporary buffer of size blockSize.
+ * @param[in] blockSize number of input samples to process per call.
+ * @return none.
+ */
+
+ void arm_fir_sparse_q7(
+ arm_fir_sparse_instance_q7 * S,
+ q7_t * pSrc,
+ q7_t * pDst,
+ q7_t * pScratchIn,
+ q31_t * pScratchOut,
+ uint32_t blockSize);
+
+ /**
+ * @brief Initialization function for the Q7 sparse FIR filter.
+ * @param[in,out] *S points to an instance of the Q7 sparse FIR structure.
+ * @param[in] numTaps number of nonzero coefficients in the filter.
+ * @param[in] *pCoeffs points to the array of filter coefficients.
+ * @param[in] *pState points to the state buffer.
+ * @param[in] *pTapDelay points to the array of offset times.
+ * @param[in] maxDelay maximum offset time supported.
+ * @param[in] blockSize number of samples that will be processed per block.
+ * @return none
+ */
+
+ void arm_fir_sparse_init_q7(
+ arm_fir_sparse_instance_q7 * S,
+ uint16_t numTaps,
+ q7_t * pCoeffs,
+ q7_t * pState,
+ int32_t * pTapDelay,
+ uint16_t maxDelay,
+ uint32_t blockSize);
+
+
+ /*
+ * @brief Floating-point sin_cos function.
+ * @param[in] theta input value in degrees
+ * @param[out] *pSinVal points to the processed sine output.
+ * @param[out] *pCosVal points to the processed cos output.
+ * @return none.
+ */
+
+ void arm_sin_cos_f32(
+ float32_t theta,
+ float32_t * pSinVal,
+ float32_t * pCcosVal);
+
+ /*
+ * @brief Q31 sin_cos function.
+ * @param[in] theta scaled input value in degrees
+ * @param[out] *pSinVal points to the processed sine output.
+ * @param[out] *pCosVal points to the processed cosine output.
+ * @return none.
+ */
+
+ void arm_sin_cos_q31(
+ q31_t theta,
+ q31_t * pSinVal,
+ q31_t * pCosVal);
+
+
+ /**
+ * @brief Floating-point complex conjugate.
+ * @param[in] *pSrc points to the input vector
+ * @param[out] *pDst points to the output vector
+ * @param[in] numSamples number of complex samples in each vector
+ * @return none.
+ */
+
+ void arm_cmplx_conj_f32(
+ float32_t * pSrc,
+ float32_t * pDst,
+ uint32_t numSamples);
+
+ /**
+ * @brief Q31 complex conjugate.
+ * @param[in] *pSrc points to the input vector
+ * @param[out] *pDst points to the output vector
+ * @param[in] numSamples number of complex samples in each vector
+ * @return none.
+ */
+
+ void arm_cmplx_conj_q31(
+ q31_t * pSrc,
+ q31_t * pDst,
+ uint32_t numSamples);
+
+ /**
+ * @brief Q15 complex conjugate.
+ * @param[in] *pSrc points to the input vector
+ * @param[out] *pDst points to the output vector
+ * @param[in] numSamples number of complex samples in each vector
+ * @return none.
+ */
+
+ void arm_cmplx_conj_q15(
+ q15_t * pSrc,
+ q15_t * pDst,
+ uint32_t numSamples);
+
+
+
+ /**
+ * @brief Floating-point complex magnitude squared
+ * @param[in] *pSrc points to the complex input vector
+ * @param[out] *pDst points to the real output vector
+ * @param[in] numSamples number of complex samples in the input vector
+ * @return none.
+ */
+
+ void arm_cmplx_mag_squared_f32(
+ float32_t * pSrc,
+ float32_t * pDst,
+ uint32_t numSamples);
+
+ /**
+ * @brief Q31 complex magnitude squared
+ * @param[in] *pSrc points to the complex input vector
+ * @param[out] *pDst points to the real output vector
+ * @param[in] numSamples number of complex samples in the input vector
+ * @return none.
+ */
+
+ void arm_cmplx_mag_squared_q31(
+ q31_t * pSrc,
+ q31_t * pDst,
+ uint32_t numSamples);
+
+ /**
+ * @brief Q15 complex magnitude squared
+ * @param[in] *pSrc points to the complex input vector
+ * @param[out] *pDst points to the real output vector
+ * @param[in] numSamples number of complex samples in the input vector
+ * @return none.
+ */
+
+ void arm_cmplx_mag_squared_q15(
+ q15_t * pSrc,
+ q15_t * pDst,
+ uint32_t numSamples);
+
+
+ /**
+ * @ingroup groupController
+ */
+
+ /**
+ * @defgroup PID PID Motor Control
+ *
+ * A Proportional Integral Derivative (PID) controller is a generic feedback control
+ * loop mechanism widely used in industrial control systems.
+ * A PID controller is the most commonly used type of feedback controller.
+ *
+ * This set of functions implements (PID) controllers
+ * for Q15, Q31, and floating-point data types. The functions operate on a single sample
+ * of data and each call to the function returns a single processed value.
+ * <code>S</code> points to an instance of the PID control data structure. <code>in</code>
+ * is the input sample value. The functions return the output value.
+ *
+ * \par Algorithm:
+ * <pre>
+ * y[n] = y[n-1] + A0 * x[n] + A1 * x[n-1] + A2 * x[n-2]
+ * A0 = Kp + Ki + Kd
+ * A1 = (-Kp ) - (2 * Kd )
+ * A2 = Kd </pre>
+ *
+ * \par
+ * where \c Kp is proportional constant, \c Ki is Integral constant and \c Kd is Derivative constant
+ *
+ * \par
+ * \image html PID.gif "Proportional Integral Derivative Controller"
+ *
+ * \par
+ * The PID controller calculates an "error" value as the difference between
+ * the measured output and the reference input.
+ * The controller attempts to minimize the error by adjusting the process control inputs.
+ * The proportional value determines the reaction to the current error,
+ * the integral value determines the reaction based on the sum of recent errors,
+ * and the derivative value determines the reaction based on the rate at which the error has been changing.
+ *
+ * \par Instance Structure
+ * The Gains A0, A1, A2 and state variables for a PID controller are stored together in an instance data structure.
+ * A separate instance structure must be defined for each PID Controller.
+ * There are separate instance structure declarations for each of the 3 supported data types.
+ *
+ * \par Reset Functions
+ * There is also an associated reset function for each data type which clears the state array.
+ *
+ * \par Initialization Functions
+ * There is also an associated initialization function for each data type.
+ * The initialization function performs the following operations:
+ * - Initializes the Gains A0, A1, A2 from Kp,Ki, Kd gains.
+ * - Zeros out the values in the state buffer.
+ *
+ * \par
+ * Instance structure cannot be placed into a const data section and it is recommended to use the initialization function.
+ *
+ * \par Fixed-Point Behavior
+ * Care must be taken when using the fixed-point versions of the PID Controller functions.
+ * In particular, the overflow and saturation behavior of the accumulator used in each function must be considered.
+ * Refer to the function specific documentation below for usage guidelines.
+ */
+
+ /**
+ * @addtogroup PID
+ * @{
+ */
+
+ /**
+ * @brief Process function for the floating-point PID Control.
+ * @param[in,out] *S is an instance of the floating-point PID Control structure
+ * @param[in] in input sample to process
+ * @return out processed output sample.
+ */
+
+
+ static __INLINE float32_t arm_pid_f32(
+ arm_pid_instance_f32 * S,
+ float32_t in)
+ {
+ float32_t out;
+
+ /* y[n] = y[n-1] + A0 * x[n] + A1 * x[n-1] + A2 * x[n-2] */
+ out = (S->A0 * in) +
+ (S->A1 * S->state[0]) + (S->A2 * S->state[1]) + (S->state[2]);
+
+ /* Update state */
+ S->state[1] = S->state[0];
+ S->state[0] = in;
+ S->state[2] = out;
+
+ /* return to application */
+ return (out);
+
+ }
+
+ /**
+ * @brief Process function for the Q31 PID Control.
+ * @param[in,out] *S points to an instance of the Q31 PID Control structure
+ * @param[in] in input sample to process
+ * @return out processed output sample.
+ *
+ * <b>Scaling and Overflow Behavior:</b>
+ * \par
+ * The function is implemented using an internal 64-bit accumulator.
+ * The accumulator has a 2.62 format and maintains full precision of the intermediate multiplication results but provides only a single guard bit.
+ * Thus, if the accumulator result overflows it wraps around rather than clip.
+ * In order to avoid overflows completely the input signal must be scaled down by 2 bits as there are four additions.
+ * After all multiply-accumulates are performed, the 2.62 accumulator is truncated to 1.32 format and then saturated to 1.31 format.
+ */
+
+ static __INLINE q31_t arm_pid_q31(
+ arm_pid_instance_q31 * S,
+ q31_t in)
+ {
+ q63_t acc;
+ q31_t out;
+
+ /* acc = A0 * x[n] */
+ acc = (q63_t) S->A0 * in;
+
+ /* acc += A1 * x[n-1] */
+ acc += (q63_t) S->A1 * S->state[0];
+
+ /* acc += A2 * x[n-2] */
+ acc += (q63_t) S->A2 * S->state[1];
+
+ /* convert output to 1.31 format to add y[n-1] */
+ out = (q31_t) (acc >> 31u);
+
+ /* out += y[n-1] */
+ out += S->state[2];
+
+ /* Update state */
+ S->state[1] = S->state[0];
+ S->state[0] = in;
+ S->state[2] = out;
+
+ /* return to application */
+ return (out);
+
+ }
+
+ /**
+ * @brief Process function for the Q15 PID Control.
+ * @param[in,out] *S points to an instance of the Q15 PID Control structure
+ * @param[in] in input sample to process
+ * @return out processed output sample.
+ *
+ * <b>Scaling and Overflow Behavior:</b>
+ * \par
+ * The function is implemented using a 64-bit internal accumulator.
+ * Both Gains and state variables are represented in 1.15 format and multiplications yield a 2.30 result.
+ * The 2.30 intermediate results are accumulated in a 64-bit accumulator in 34.30 format.
+ * There is no risk of internal overflow with this approach and the full precision of intermediate multiplications is preserved.
+ * After all additions have been performed, the accumulator is truncated to 34.15 format by discarding low 15 bits.
+ * Lastly, the accumulator is saturated to yield a result in 1.15 format.
+ */
+
+ static __INLINE q15_t arm_pid_q15(
+ arm_pid_instance_q15 * S,
+ q15_t in)
+ {
+ q63_t acc;
+ q15_t out;
+
+#ifndef ARM_MATH_CM0_FAMILY
+ __SIMD32_TYPE *vstate;
+
+ /* Implementation of PID controller */
+
+ /* acc = A0 * x[n] */
+ acc = (q31_t) __SMUAD(S->A0, in);
+
+ /* acc += A1 * x[n-1] + A2 * x[n-2] */
+ vstate = __SIMD32_CONST(S->state);
+ acc = __SMLALD(S->A1, (q31_t) *vstate, acc);
+
+#else
+ /* acc = A0 * x[n] */
+ acc = ((q31_t) S->A0) * in;
+
+ /* acc += A1 * x[n-1] + A2 * x[n-2] */
+ acc += (q31_t) S->A1 * S->state[0];
+ acc += (q31_t) S->A2 * S->state[1];
+
+#endif
+
+ /* acc += y[n-1] */
+ acc += (q31_t) S->state[2] << 15;
+
+ /* saturate the output */
+ out = (q15_t) (__SSAT((acc >> 15), 16));
+
+ /* Update state */
+ S->state[1] = S->state[0];
+ S->state[0] = in;
+ S->state[2] = out;
+
+ /* return to application */
+ return (out);
+
+ }
+
+ /**
+ * @} end of PID group
+ */
+
+
+ /**
+ * @brief Floating-point matrix inverse.
+ * @param[in] *src points to the instance of the input floating-point matrix structure.
+ * @param[out] *dst points to the instance of the output floating-point matrix structure.
+ * @return The function returns ARM_MATH_SIZE_MISMATCH, if the dimensions do not match.
+ * If the input matrix is singular (does not have an inverse), then the algorithm terminates and returns error status ARM_MATH_SINGULAR.
+ */
+
+ arm_status arm_mat_inverse_f32(
+ const arm_matrix_instance_f32 * src,
+ arm_matrix_instance_f32 * dst);
+
+
+
+ /**
+ * @ingroup groupController
+ */
+
+
+ /**
+ * @defgroup clarke Vector Clarke Transform
+ * Forward Clarke transform converts the instantaneous stator phases into a two-coordinate time invariant vector.
+ * Generally the Clarke transform uses three-phase currents <code>Ia, Ib and Ic</code> to calculate currents
+ * in the two-phase orthogonal stator axis <code>Ialpha</code> and <code>Ibeta</code>.
+ * When <code>Ialpha</code> is superposed with <code>Ia</code> as shown in the figure below
+ * \image html clarke.gif Stator current space vector and its components in (a,b).
+ * and <code>Ia + Ib + Ic = 0</code>, in this condition <code>Ialpha</code> and <code>Ibeta</code>
+ * can be calculated using only <code>Ia</code> and <code>Ib</code>.
+ *
+ * The function operates on a single sample of data and each call to the function returns the processed output.
+ * The library provides separate functions for Q31 and floating-point data types.
+ * \par Algorithm
+ * \image html clarkeFormula.gif
+ * where <code>Ia</code> and <code>Ib</code> are the instantaneous stator phases and
+ * <code>pIalpha</code> and <code>pIbeta</code> are the two coordinates of time invariant vector.
+ * \par Fixed-Point Behavior
+ * Care must be taken when using the Q31 version of the Clarke transform.
+ * In particular, the overflow and saturation behavior of the accumulator used must be considered.
+ * Refer to the function specific documentation below for usage guidelines.
+ */
+
+ /**
+ * @addtogroup clarke
+ * @{
+ */
+
+ /**
+ *
+ * @brief Floating-point Clarke transform
+ * @param[in] Ia input three-phase coordinate <code>a</code>
+ * @param[in] Ib input three-phase coordinate <code>b</code>
+ * @param[out] *pIalpha points to output two-phase orthogonal vector axis alpha
+ * @param[out] *pIbeta points to output two-phase orthogonal vector axis beta
+ * @return none.
+ */
+
+ static __INLINE void arm_clarke_f32(
+ float32_t Ia,
+ float32_t Ib,
+ float32_t * pIalpha,
+ float32_t * pIbeta)
+ {
+ /* Calculate pIalpha using the equation, pIalpha = Ia */
+ *pIalpha = Ia;
+
+ /* Calculate pIbeta using the equation, pIbeta = (1/sqrt(3)) * Ia + (2/sqrt(3)) * Ib */
+ *pIbeta =
+ ((float32_t) 0.57735026919 * Ia + (float32_t) 1.15470053838 * Ib);
+
+ }
+
+ /**
+ * @brief Clarke transform for Q31 version
+ * @param[in] Ia input three-phase coordinate <code>a</code>
+ * @param[in] Ib input three-phase coordinate <code>b</code>
+ * @param[out] *pIalpha points to output two-phase orthogonal vector axis alpha
+ * @param[out] *pIbeta points to output two-phase orthogonal vector axis beta
+ * @return none.
+ *
+ * <b>Scaling and Overflow Behavior:</b>
+ * \par
+ * The function is implemented using an internal 32-bit accumulator.
+ * The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format.
+ * There is saturation on the addition, hence there is no risk of overflow.
+ */
+
+ static __INLINE void arm_clarke_q31(
+ q31_t Ia,
+ q31_t Ib,
+ q31_t * pIalpha,
+ q31_t * pIbeta)
+ {
+ q31_t product1, product2; /* Temporary variables used to store intermediate results */
+
+ /* Calculating pIalpha from Ia by equation pIalpha = Ia */
+ *pIalpha = Ia;
+
+ /* Intermediate product is calculated by (1/(sqrt(3)) * Ia) */
+ product1 = (q31_t) (((q63_t) Ia * 0x24F34E8B) >> 30);
+
+ /* Intermediate product is calculated by (2/sqrt(3) * Ib) */
+ product2 = (q31_t) (((q63_t) Ib * 0x49E69D16) >> 30);
+
+ /* pIbeta is calculated by adding the intermediate products */
+ *pIbeta = __QADD(product1, product2);
+ }
+
+ /**
+ * @} end of clarke group
+ */
+
+ /**
+ * @brief Converts the elements of the Q7 vector to Q31 vector.
+ * @param[in] *pSrc input pointer
+ * @param[out] *pDst output pointer
+ * @param[in] blockSize number of samples to process
+ * @return none.
+ */
+ void arm_q7_to_q31(
+ q7_t * pSrc,
+ q31_t * pDst,
+ uint32_t blockSize);
+
+
+
+
+ /**
+ * @ingroup groupController
+ */
+
+ /**
+ * @defgroup inv_clarke Vector Inverse Clarke Transform
+ * Inverse Clarke transform converts the two-coordinate time invariant vector into instantaneous stator phases.
+ *
+ * The function operates on a single sample of data and each call to the function returns the processed output.
+ * The library provides separate functions for Q31 and floating-point data types.
+ * \par Algorithm
+ * \image html clarkeInvFormula.gif
+ * where <code>pIa</code> and <code>pIb</code> are the instantaneous stator phases and
+ * <code>Ialpha</code> and <code>Ibeta</code> are the two coordinates of time invariant vector.
+ * \par Fixed-Point Behavior
+ * Care must be taken when using the Q31 version of the Clarke transform.
+ * In particular, the overflow and saturation behavior of the accumulator used must be considered.
+ * Refer to the function specific documentation below for usage guidelines.
+ */
+
+ /**
+ * @addtogroup inv_clarke
+ * @{
+ */
+
+ /**
+ * @brief Floating-point Inverse Clarke transform
+ * @param[in] Ialpha input two-phase orthogonal vector axis alpha
+ * @param[in] Ibeta input two-phase orthogonal vector axis beta
+ * @param[out] *pIa points to output three-phase coordinate <code>a</code>
+ * @param[out] *pIb points to output three-phase coordinate <code>b</code>
+ * @return none.
+ */
+
+
+ static __INLINE void arm_inv_clarke_f32(
+ float32_t Ialpha,
+ float32_t Ibeta,
+ float32_t * pIa,
+ float32_t * pIb)
+ {
+ /* Calculating pIa from Ialpha by equation pIa = Ialpha */
+ *pIa = Ialpha;
+
+ /* Calculating pIb from Ialpha and Ibeta by equation pIb = -(1/2) * Ialpha + (sqrt(3)/2) * Ibeta */
+ *pIb = -0.5 * Ialpha + (float32_t) 0.8660254039 *Ibeta;
+
+ }
+
+ /**
+ * @brief Inverse Clarke transform for Q31 version
+ * @param[in] Ialpha input two-phase orthogonal vector axis alpha
+ * @param[in] Ibeta input two-phase orthogonal vector axis beta
+ * @param[out] *pIa points to output three-phase coordinate <code>a</code>
+ * @param[out] *pIb points to output three-phase coordinate <code>b</code>
+ * @return none.
+ *
+ * <b>Scaling and Overflow Behavior:</b>
+ * \par
+ * The function is implemented using an internal 32-bit accumulator.
+ * The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format.
+ * There is saturation on the subtraction, hence there is no risk of overflow.
+ */
+
+ static __INLINE void arm_inv_clarke_q31(
+ q31_t Ialpha,
+ q31_t Ibeta,
+ q31_t * pIa,
+ q31_t * pIb)
+ {
+ q31_t product1, product2; /* Temporary variables used to store intermediate results */
+
+ /* Calculating pIa from Ialpha by equation pIa = Ialpha */
+ *pIa = Ialpha;
+
+ /* Intermediate product is calculated by (1/(2*sqrt(3)) * Ia) */
+ product1 = (q31_t) (((q63_t) (Ialpha) * (0x40000000)) >> 31);
+
+ /* Intermediate product is calculated by (1/sqrt(3) * pIb) */
+ product2 = (q31_t) (((q63_t) (Ibeta) * (0x6ED9EBA1)) >> 31);
+
+ /* pIb is calculated by subtracting the products */
+ *pIb = __QSUB(product2, product1);
+
+ }
+
+ /**
+ * @} end of inv_clarke group
+ */
+
+ /**
+ * @brief Converts the elements of the Q7 vector to Q15 vector.
+ * @param[in] *pSrc input pointer
+ * @param[out] *pDst output pointer
+ * @param[in] blockSize number of samples to process
+ * @return none.
+ */
+ void arm_q7_to_q15(
+ q7_t * pSrc,
+ q15_t * pDst,
+ uint32_t blockSize);
+
+
+
+ /**
+ * @ingroup groupController
+ */
+
+ /**
+ * @defgroup park Vector Park Transform
+ *
+ * Forward Park transform converts the input two-coordinate vector to flux and torque components.
+ * The Park transform can be used to realize the transformation of the <code>Ialpha</code> and the <code>Ibeta</code> currents
+ * from the stationary to the moving reference frame and control the spatial relationship between
+ * the stator vector current and rotor flux vector.
+ * If we consider the d axis aligned with the rotor flux, the diagram below shows the
+ * current vector and the relationship from the two reference frames:
+ * \image html park.gif "Stator current space vector and its component in (a,b) and in the d,q rotating reference frame"
+ *
+ * The function operates on a single sample of data and each call to the function returns the processed output.
+ * The library provides separate functions for Q31 and floating-point data types.
+ * \par Algorithm
+ * \image html parkFormula.gif
+ * where <code>Ialpha</code> and <code>Ibeta</code> are the stator vector components,
+ * <code>pId</code> and <code>pIq</code> are rotor vector components and <code>cosVal</code> and <code>sinVal</code> are the
+ * cosine and sine values of theta (rotor flux position).
+ * \par Fixed-Point Behavior
+ * Care must be taken when using the Q31 version of the Park transform.
+ * In particular, the overflow and saturation behavior of the accumulator used must be considered.
+ * Refer to the function specific documentation below for usage guidelines.
+ */
+
+ /**
+ * @addtogroup park
+ * @{
+ */
+
+ /**
+ * @brief Floating-point Park transform
+ * @param[in] Ialpha input two-phase vector coordinate alpha
+ * @param[in] Ibeta input two-phase vector coordinate beta
+ * @param[out] *pId points to output rotor reference frame d
+ * @param[out] *pIq points to output rotor reference frame q
+ * @param[in] sinVal sine value of rotation angle theta
+ * @param[in] cosVal cosine value of rotation angle theta
+ * @return none.
+ *
+ * The function implements the forward Park transform.
+ *
+ */
+
+ static __INLINE void arm_park_f32(
+ float32_t Ialpha,
+ float32_t Ibeta,
+ float32_t * pId,
+ float32_t * pIq,
+ float32_t sinVal,
+ float32_t cosVal)
+ {
+ /* Calculate pId using the equation, pId = Ialpha * cosVal + Ibeta * sinVal */
+ *pId = Ialpha * cosVal + Ibeta * sinVal;
+
+ /* Calculate pIq using the equation, pIq = - Ialpha * sinVal + Ibeta * cosVal */
+ *pIq = -Ialpha * sinVal + Ibeta * cosVal;
+
+ }
+
+ /**
+ * @brief Park transform for Q31 version
+ * @param[in] Ialpha input two-phase vector coordinate alpha
+ * @param[in] Ibeta input two-phase vector coordinate beta
+ * @param[out] *pId points to output rotor reference frame d
+ * @param[out] *pIq points to output rotor reference frame q
+ * @param[in] sinVal sine value of rotation angle theta
+ * @param[in] cosVal cosine value of rotation angle theta
+ * @return none.
+ *
+ * <b>Scaling and Overflow Behavior:</b>
+ * \par
+ * The function is implemented using an internal 32-bit accumulator.
+ * The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format.
+ * There is saturation on the addition and subtraction, hence there is no risk of overflow.
+ */
+
+
+ static __INLINE void arm_park_q31(
+ q31_t Ialpha,
+ q31_t Ibeta,
+ q31_t * pId,
+ q31_t * pIq,
+ q31_t sinVal,
+ q31_t cosVal)
+ {
+ q31_t product1, product2; /* Temporary variables used to store intermediate results */
+ q31_t product3, product4; /* Temporary variables used to store intermediate results */
+
+ /* Intermediate product is calculated by (Ialpha * cosVal) */
+ product1 = (q31_t) (((q63_t) (Ialpha) * (cosVal)) >> 31);
+
+ /* Intermediate product is calculated by (Ibeta * sinVal) */
+ product2 = (q31_t) (((q63_t) (Ibeta) * (sinVal)) >> 31);
+
+
+ /* Intermediate product is calculated by (Ialpha * sinVal) */
+ product3 = (q31_t) (((q63_t) (Ialpha) * (sinVal)) >> 31);
+
+ /* Intermediate product is calculated by (Ibeta * cosVal) */
+ product4 = (q31_t) (((q63_t) (Ibeta) * (cosVal)) >> 31);
+
+ /* Calculate pId by adding the two intermediate products 1 and 2 */
+ *pId = __QADD(product1, product2);
+
+ /* Calculate pIq by subtracting the two intermediate products 3 from 4 */
+ *pIq = __QSUB(product4, product3);
+ }
+
+ /**
+ * @} end of park group
+ */
+
+ /**
+ * @brief Converts the elements of the Q7 vector to floating-point vector.
+ * @param[in] *pSrc is input pointer
+ * @param[out] *pDst is output pointer
+ * @param[in] blockSize is the number of samples to process
+ * @return none.
+ */
+ void arm_q7_to_float(
+ q7_t * pSrc,
+ float32_t * pDst,
+ uint32_t blockSize);
+
+
+ /**
+ * @ingroup groupController
+ */
+
+ /**
+ * @defgroup inv_park Vector Inverse Park transform
+ * Inverse Park transform converts the input flux and torque components to two-coordinate vector.
+ *
+ * The function operates on a single sample of data and each call to the function returns the processed output.
+ * The library provides separate functions for Q31 and floating-point data types.
+ * \par Algorithm
+ * \image html parkInvFormula.gif
+ * where <code>pIalpha</code> and <code>pIbeta</code> are the stator vector components,
+ * <code>Id</code> and <code>Iq</code> are rotor vector components and <code>cosVal</code> and <code>sinVal</code> are the
+ * cosine and sine values of theta (rotor flux position).
+ * \par Fixed-Point Behavior
+ * Care must be taken when using the Q31 version of the Park transform.
+ * In particular, the overflow and saturation behavior of the accumulator used must be considered.
+ * Refer to the function specific documentation below for usage guidelines.
+ */
+
+ /**
+ * @addtogroup inv_park
+ * @{
+ */
+
+ /**
+ * @brief Floating-point Inverse Park transform
+ * @param[in] Id input coordinate of rotor reference frame d
+ * @param[in] Iq input coordinate of rotor reference frame q
+ * @param[out] *pIalpha points to output two-phase orthogonal vector axis alpha
+ * @param[out] *pIbeta points to output two-phase orthogonal vector axis beta
+ * @param[in] sinVal sine value of rotation angle theta
+ * @param[in] cosVal cosine value of rotation angle theta
+ * @return none.
+ */
+
+ static __INLINE void arm_inv_park_f32(
+ float32_t Id,
+ float32_t Iq,
+ float32_t * pIalpha,
+ float32_t * pIbeta,
+ float32_t sinVal,
+ float32_t cosVal)
+ {
+ /* Calculate pIalpha using the equation, pIalpha = Id * cosVal - Iq * sinVal */
+ *pIalpha = Id * cosVal - Iq * sinVal;
+
+ /* Calculate pIbeta using the equation, pIbeta = Id * sinVal + Iq * cosVal */
+ *pIbeta = Id * sinVal + Iq * cosVal;
+
+ }
+
+
+ /**
+ * @brief Inverse Park transform for Q31 version
+ * @param[in] Id input coordinate of rotor reference frame d
+ * @param[in] Iq input coordinate of rotor reference frame q
+ * @param[out] *pIalpha points to output two-phase orthogonal vector axis alpha
+ * @param[out] *pIbeta points to output two-phase orthogonal vector axis beta
+ * @param[in] sinVal sine value of rotation angle theta
+ * @param[in] cosVal cosine value of rotation angle theta
+ * @return none.
+ *
+ * <b>Scaling and Overflow Behavior:</b>
+ * \par
+ * The function is implemented using an internal 32-bit accumulator.
+ * The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format.
+ * There is saturation on the addition, hence there is no risk of overflow.
+ */
+
+
+ static __INLINE void arm_inv_park_q31(
+ q31_t Id,
+ q31_t Iq,
+ q31_t * pIalpha,
+ q31_t * pIbeta,
+ q31_t sinVal,
+ q31_t cosVal)
+ {
+ q31_t product1, product2; /* Temporary variables used to store intermediate results */
+ q31_t product3, product4; /* Temporary variables used to store intermediate results */
+
+ /* Intermediate product is calculated by (Id * cosVal) */
+ product1 = (q31_t) (((q63_t) (Id) * (cosVal)) >> 31);
+
+ /* Intermediate product is calculated by (Iq * sinVal) */
+ product2 = (q31_t) (((q63_t) (Iq) * (sinVal)) >> 31);
+
+
+ /* Intermediate product is calculated by (Id * sinVal) */
+ product3 = (q31_t) (((q63_t) (Id) * (sinVal)) >> 31);
+
+ /* Intermediate product is calculated by (Iq * cosVal) */
+ product4 = (q31_t) (((q63_t) (Iq) * (cosVal)) >> 31);
+
+ /* Calculate pIalpha by using the two intermediate products 1 and 2 */
+ *pIalpha = __QSUB(product1, product2);
+
+ /* Calculate pIbeta by using the two intermediate products 3 and 4 */
+ *pIbeta = __QADD(product4, product3);
+
+ }
+
+ /**
+ * @} end of Inverse park group
+ */
+
+
+ /**
+ * @brief Converts the elements of the Q31 vector to floating-point vector.
+ * @param[in] *pSrc is input pointer
+ * @param[out] *pDst is output pointer
+ * @param[in] blockSize is the number of samples to process
+ * @return none.
+ */
+ void arm_q31_to_float(
+ q31_t * pSrc,
+ float32_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @ingroup groupInterpolation
+ */
+
+ /**
+ * @defgroup LinearInterpolate Linear Interpolation
+ *
+ * Linear interpolation is a method of curve fitting using linear polynomials.
+ * Linear interpolation works by effectively drawing a straight line between two neighboring samples and returning the appropriate point along that line
+ *
+ * \par
+ * \image html LinearInterp.gif "Linear interpolation"
+ *
+ * \par
+ * A Linear Interpolate function calculates an output value(y), for the input(x)
+ * using linear interpolation of the input values x0, x1( nearest input values) and the output values y0 and y1(nearest output values)
+ *
+ * \par Algorithm:
+ * <pre>
+ * y = y0 + (x - x0) * ((y1 - y0)/(x1-x0))
+ * where x0, x1 are nearest values of input x
+ * y0, y1 are nearest values to output y
+ * </pre>
+ *
+ * \par
+ * This set of functions implements Linear interpolation process
+ * for Q7, Q15, Q31, and floating-point data types. The functions operate on a single
+ * sample of data and each call to the function returns a single processed value.
+ * <code>S</code> points to an instance of the Linear Interpolate function data structure.
+ * <code>x</code> is the input sample value. The functions returns the output value.
+ *
+ * \par
+ * if x is outside of the table boundary, Linear interpolation returns first value of the table
+ * if x is below input range and returns last value of table if x is above range.
+ */
+
+ /**
+ * @addtogroup LinearInterpolate
+ * @{
+ */
+
+ /**
+ * @brief Process function for the floating-point Linear Interpolation Function.
+ * @param[in,out] *S is an instance of the floating-point Linear Interpolation structure
+ * @param[in] x input sample to process
+ * @return y processed output sample.
+ *
+ */
+
+ static __INLINE float32_t arm_linear_interp_f32(
+ arm_linear_interp_instance_f32 * S,
+ float32_t x)
+ {
+
+ float32_t y;
+ float32_t x0, x1; /* Nearest input values */
+ float32_t y0, y1; /* Nearest output values */
+ float32_t xSpacing = S->xSpacing; /* spacing between input values */
+ int32_t i; /* Index variable */
+ float32_t *pYData = S->pYData; /* pointer to output table */
+
+ /* Calculation of index */
+ i = (int32_t) ((x - S->x1) / xSpacing);
+
+ if(i < 0)
+ {
+ /* Iniatilize output for below specified range as least output value of table */
+ y = pYData[0];
+ }
+ else if((uint32_t)i >= S->nValues)
+ {
+ /* Iniatilize output for above specified range as last output value of table */
+ y = pYData[S->nValues - 1];
+ }
+ else
+ {
+ /* Calculation of nearest input values */
+ x0 = S->x1 + i * xSpacing;
+ x1 = S->x1 + (i + 1) * xSpacing;
+
+ /* Read of nearest output values */
+ y0 = pYData[i];
+ y1 = pYData[i + 1];
+
+ /* Calculation of output */
+ y = y0 + (x - x0) * ((y1 - y0) / (x1 - x0));
+
+ }
+
+ /* returns output value */
+ return (y);
+ }
+
+ /**
+ *
+ * @brief Process function for the Q31 Linear Interpolation Function.
+ * @param[in] *pYData pointer to Q31 Linear Interpolation table
+ * @param[in] x input sample to process
+ * @param[in] nValues number of table values
+ * @return y processed output sample.
+ *
+ * \par
+ * Input sample <code>x</code> is in 12.20 format which contains 12 bits for table index and 20 bits for fractional part.
+ * This function can support maximum of table size 2^12.
+ *
+ */
+
+
+ static __INLINE q31_t arm_linear_interp_q31(
+ q31_t * pYData,
+ q31_t x,
+ uint32_t nValues)
+ {
+ q31_t y; /* output */
+ q31_t y0, y1; /* Nearest output values */
+ q31_t fract; /* fractional part */
+ int32_t index; /* Index to read nearest output values */
+
+ /* Input is in 12.20 format */
+ /* 12 bits for the table index */
+ /* Index value calculation */
+ index = ((x & 0xFFF00000) >> 20);
+
+ if(index >= (int32_t)(nValues - 1))
+ {
+ return (pYData[nValues - 1]);
+ }
+ else if(index < 0)
+ {
+ return (pYData[0]);
+ }
+ else
+ {
+
+ /* 20 bits for the fractional part */
+ /* shift left by 11 to keep fract in 1.31 format */
+ fract = (x & 0x000FFFFF) << 11;
+
+ /* Read two nearest output values from the index in 1.31(q31) format */
+ y0 = pYData[index];
+ y1 = pYData[index + 1u];
+
+ /* Calculation of y0 * (1-fract) and y is in 2.30 format */
+ y = ((q31_t) ((q63_t) y0 * (0x7FFFFFFF - fract) >> 32));
+
+ /* Calculation of y0 * (1-fract) + y1 *fract and y is in 2.30 format */
+ y += ((q31_t) (((q63_t) y1 * fract) >> 32));
+
+ /* Convert y to 1.31 format */
+ return (y << 1u);
+
+ }
+
+ }
+
+ /**
+ *
+ * @brief Process function for the Q15 Linear Interpolation Function.
+ * @param[in] *pYData pointer to Q15 Linear Interpolation table
+ * @param[in] x input sample to process
+ * @param[in] nValues number of table values
+ * @return y processed output sample.
+ *
+ * \par
+ * Input sample <code>x</code> is in 12.20 format which contains 12 bits for table index and 20 bits for fractional part.
+ * This function can support maximum of table size 2^12.
+ *
+ */
+
+
+ static __INLINE q15_t arm_linear_interp_q15(
+ q15_t * pYData,
+ q31_t x,
+ uint32_t nValues)
+ {
+ q63_t y; /* output */
+ q15_t y0, y1; /* Nearest output values */
+ q31_t fract; /* fractional part */
+ int32_t index; /* Index to read nearest output values */
+
+ /* Input is in 12.20 format */
+ /* 12 bits for the table index */
+ /* Index value calculation */
+ index = ((x & 0xFFF00000) >> 20u);
+
+ if(index >= (int32_t)(nValues - 1))
+ {
+ return (pYData[nValues - 1]);
+ }
+ else if(index < 0)
+ {
+ return (pYData[0]);
+ }
+ else
+ {
+ /* 20 bits for the fractional part */
+ /* fract is in 12.20 format */
+ fract = (x & 0x000FFFFF);
+
+ /* Read two nearest output values from the index */
+ y0 = pYData[index];
+ y1 = pYData[index + 1u];
+
+ /* Calculation of y0 * (1-fract) and y is in 13.35 format */
+ y = ((q63_t) y0 * (0xFFFFF - fract));
+
+ /* Calculation of (y0 * (1-fract) + y1 * fract) and y is in 13.35 format */
+ y += ((q63_t) y1 * (fract));
+
+ /* convert y to 1.15 format */
+ return (y >> 20);
+ }
+
+
+ }
+
+ /**
+ *
+ * @brief Process function for the Q7 Linear Interpolation Function.
+ * @param[in] *pYData pointer to Q7 Linear Interpolation table
+ * @param[in] x input sample to process
+ * @param[in] nValues number of table values
+ * @return y processed output sample.
+ *
+ * \par
+ * Input sample <code>x</code> is in 12.20 format which contains 12 bits for table index and 20 bits for fractional part.
+ * This function can support maximum of table size 2^12.
+ */
+
+
+ static __INLINE q7_t arm_linear_interp_q7(
+ q7_t * pYData,
+ q31_t x,
+ uint32_t nValues)
+ {
+ q31_t y; /* output */
+ q7_t y0, y1; /* Nearest output values */
+ q31_t fract; /* fractional part */
+ uint32_t index; /* Index to read nearest output values */
+
+ /* Input is in 12.20 format */
+ /* 12 bits for the table index */
+ /* Index value calculation */
+ if (x < 0)
+ {
+ return (pYData[0]);
+ }
+ index = (x >> 20) & 0xfff;
+
+
+ if(index >= (nValues - 1))
+ {
+ return (pYData[nValues - 1]);
+ }
+ else
+ {
+
+ /* 20 bits for the fractional part */
+ /* fract is in 12.20 format */
+ fract = (x & 0x000FFFFF);
+
+ /* Read two nearest output values from the index and are in 1.7(q7) format */
+ y0 = pYData[index];
+ y1 = pYData[index + 1u];
+
+ /* Calculation of y0 * (1-fract ) and y is in 13.27(q27) format */
+ y = ((y0 * (0xFFFFF - fract)));
+
+ /* Calculation of y1 * fract + y0 * (1-fract) and y is in 13.27(q27) format */
+ y += (y1 * fract);
+
+ /* convert y to 1.7(q7) format */
+ return (y >> 20u);
+
+ }
+
+ }
+ /**
+ * @} end of LinearInterpolate group
+ */
+
+ /**
+ * @brief Fast approximation to the trigonometric sine function for floating-point data.
+ * @param[in] x input value in radians.
+ * @return sin(x).
+ */
+
+ float32_t arm_sin_f32(
+ float32_t x);
+
+ /**
+ * @brief Fast approximation to the trigonometric sine function for Q31 data.
+ * @param[in] x Scaled input value in radians.
+ * @return sin(x).
+ */
+
+ q31_t arm_sin_q31(
+ q31_t x);
+
+ /**
+ * @brief Fast approximation to the trigonometric sine function for Q15 data.
+ * @param[in] x Scaled input value in radians.
+ * @return sin(x).
+ */
+
+ q15_t arm_sin_q15(
+ q15_t x);
+
+ /**
+ * @brief Fast approximation to the trigonometric cosine function for floating-point data.
+ * @param[in] x input value in radians.
+ * @return cos(x).
+ */
+
+ float32_t arm_cos_f32(
+ float32_t x);
+
+ /**
+ * @brief Fast approximation to the trigonometric cosine function for Q31 data.
+ * @param[in] x Scaled input value in radians.
+ * @return cos(x).
+ */
+
+ q31_t arm_cos_q31(
+ q31_t x);
+
+ /**
+ * @brief Fast approximation to the trigonometric cosine function for Q15 data.
+ * @param[in] x Scaled input value in radians.
+ * @return cos(x).
+ */
+
+ q15_t arm_cos_q15(
+ q15_t x);
+
+
+ /**
+ * @ingroup groupFastMath
+ */
+
+
+ /**
+ * @defgroup SQRT Square Root
+ *
+ * Computes the square root of a number.
+ * There are separate functions for Q15, Q31, and floating-point data types.
+ * The square root function is computed using the Newton-Raphson algorithm.
+ * This is an iterative algorithm of the form:
+ * <pre>
+ * x1 = x0 - f(x0)/f'(x0)
+ * </pre>
+ * where <code>x1</code> is the current estimate,
+ * <code>x0</code> is the previous estimate, and
+ * <code>f'(x0)</code> is the derivative of <code>f()</code> evaluated at <code>x0</code>.
+ * For the square root function, the algorithm reduces to:
+ * <pre>
+ * x0 = in/2 [initial guess]
+ * x1 = 1/2 * ( x0 + in / x0) [each iteration]
+ * </pre>
+ */
+
+
+ /**
+ * @addtogroup SQRT
+ * @{
+ */
+
+ /**
+ * @brief Floating-point square root function.
+ * @param[in] in input value.
+ * @param[out] *pOut square root of input value.
+ * @return The function returns ARM_MATH_SUCCESS if input value is positive value or ARM_MATH_ARGUMENT_ERROR if
+ * <code>in</code> is negative value and returns zero output for negative values.
+ */
+
+ static __INLINE arm_status arm_sqrt_f32(
+ float32_t in,
+ float32_t * pOut)
+ {
+ if(in > 0)
+ {
+
+// #if __FPU_USED
+#if (__FPU_USED == 1) && defined ( __CC_ARM )
+ *pOut = __sqrtf(in);
+#else
+ *pOut = sqrtf(in);
+#endif
+
+ return (ARM_MATH_SUCCESS);
+ }
+ else
+ {
+ *pOut = 0.0f;
+ return (ARM_MATH_ARGUMENT_ERROR);
+ }
+
+ }
+
+
+ /**
+ * @brief Q31 square root function.
+ * @param[in] in input value. The range of the input value is [0 +1) or 0x00000000 to 0x7FFFFFFF.
+ * @param[out] *pOut square root of input value.
+ * @return The function returns ARM_MATH_SUCCESS if input value is positive value or ARM_MATH_ARGUMENT_ERROR if
+ * <code>in</code> is negative value and returns zero output for negative values.
+ */
+ arm_status arm_sqrt_q31(
+ q31_t in,
+ q31_t * pOut);
+
+ /**
+ * @brief Q15 square root function.
+ * @param[in] in input value. The range of the input value is [0 +1) or 0x0000 to 0x7FFF.
+ * @param[out] *pOut square root of input value.
+ * @return The function returns ARM_MATH_SUCCESS if input value is positive value or ARM_MATH_ARGUMENT_ERROR if
+ * <code>in</code> is negative value and returns zero output for negative values.
+ */
+ arm_status arm_sqrt_q15(
+ q15_t in,
+ q15_t * pOut);
+
+ /**
+ * @} end of SQRT group
+ */
+
+
+
+
+
+
+ /**
+ * @brief floating-point Circular write function.
+ */
+
+ static __INLINE void arm_circularWrite_f32(
+ int32_t * circBuffer,
+ int32_t L,
+ uint16_t * writeOffset,
+ int32_t bufferInc,
+ const int32_t * src,
+ int32_t srcInc,
+ uint32_t blockSize)
+ {
+ uint32_t i = 0u;
+ int32_t wOffset;
+
+ /* Copy the value of Index pointer that points
+ * to the current location where the input samples to be copied */
+ wOffset = *writeOffset;
+
+ /* Loop over the blockSize */
+ i = blockSize;
+
+ while(i > 0u)
+ {
+ /* copy the input sample to the circular buffer */
+ circBuffer[wOffset] = *src;
+
+ /* Update the input pointer */
+ src += srcInc;
+
+ /* Circularly update wOffset. Watch out for positive and negative value */
+ wOffset += bufferInc;
+ if(wOffset >= L)
+ wOffset -= L;
+
+ /* Decrement the loop counter */
+ i--;
+ }
+
+ /* Update the index pointer */
+ *writeOffset = wOffset;
+ }
+
+
+
+ /**
+ * @brief floating-point Circular Read function.
+ */
+ static __INLINE void arm_circularRead_f32(
+ int32_t * circBuffer,
+ int32_t L,
+ int32_t * readOffset,
+ int32_t bufferInc,
+ int32_t * dst,
+ int32_t * dst_base,
+ int32_t dst_length,
+ int32_t dstInc,
+ uint32_t blockSize)
+ {
+ uint32_t i = 0u;
+ int32_t rOffset, dst_end;
+
+ /* Copy the value of Index pointer that points
+ * to the current location from where the input samples to be read */
+ rOffset = *readOffset;
+ dst_end = (int32_t) (dst_base + dst_length);
+
+ /* Loop over the blockSize */
+ i = blockSize;
+
+ while(i > 0u)
+ {
+ /* copy the sample from the circular buffer to the destination buffer */
+ *dst = circBuffer[rOffset];
+
+ /* Update the input pointer */
+ dst += dstInc;
+
+ if(dst == (int32_t *) dst_end)
+ {
+ dst = dst_base;
+ }
+
+ /* Circularly update rOffset. Watch out for positive and negative value */
+ rOffset += bufferInc;
+
+ if(rOffset >= L)
+ {
+ rOffset -= L;
+ }
+
+ /* Decrement the loop counter */
+ i--;
+ }
+
+ /* Update the index pointer */
+ *readOffset = rOffset;
+ }
+
+ /**
+ * @brief Q15 Circular write function.
+ */
+
+ static __INLINE void arm_circularWrite_q15(
+ q15_t * circBuffer,
+ int32_t L,
+ uint16_t * writeOffset,
+ int32_t bufferInc,
+ const q15_t * src,
+ int32_t srcInc,
+ uint32_t blockSize)
+ {
+ uint32_t i = 0u;
+ int32_t wOffset;
+
+ /* Copy the value of Index pointer that points
+ * to the current location where the input samples to be copied */
+ wOffset = *writeOffset;
+
+ /* Loop over the blockSize */
+ i = blockSize;
+
+ while(i > 0u)
+ {
+ /* copy the input sample to the circular buffer */
+ circBuffer[wOffset] = *src;
+
+ /* Update the input pointer */
+ src += srcInc;
+
+ /* Circularly update wOffset. Watch out for positive and negative value */
+ wOffset += bufferInc;
+ if(wOffset >= L)
+ wOffset -= L;
+
+ /* Decrement the loop counter */
+ i--;
+ }
+
+ /* Update the index pointer */
+ *writeOffset = wOffset;
+ }
+
+
+
+ /**
+ * @brief Q15 Circular Read function.
+ */
+ static __INLINE void arm_circularRead_q15(
+ q15_t * circBuffer,
+ int32_t L,
+ int32_t * readOffset,
+ int32_t bufferInc,
+ q15_t * dst,
+ q15_t * dst_base,
+ int32_t dst_length,
+ int32_t dstInc,
+ uint32_t blockSize)
+ {
+ uint32_t i = 0;
+ int32_t rOffset, dst_end;
+
+ /* Copy the value of Index pointer that points
+ * to the current location from where the input samples to be read */
+ rOffset = *readOffset;
+
+ dst_end = (int32_t) (dst_base + dst_length);
+
+ /* Loop over the blockSize */
+ i = blockSize;
+
+ while(i > 0u)
+ {
+ /* copy the sample from the circular buffer to the destination buffer */
+ *dst = circBuffer[rOffset];
+
+ /* Update the input pointer */
+ dst += dstInc;
+
+ if(dst == (q15_t *) dst_end)
+ {
+ dst = dst_base;
+ }
+
+ /* Circularly update wOffset. Watch out for positive and negative value */
+ rOffset += bufferInc;
+
+ if(rOffset >= L)
+ {
+ rOffset -= L;
+ }
+
+ /* Decrement the loop counter */
+ i--;
+ }
+
+ /* Update the index pointer */
+ *readOffset = rOffset;
+ }
+
+
+ /**
+ * @brief Q7 Circular write function.
+ */
+
+ static __INLINE void arm_circularWrite_q7(
+ q7_t * circBuffer,
+ int32_t L,
+ uint16_t * writeOffset,
+ int32_t bufferInc,
+ const q7_t * src,
+ int32_t srcInc,
+ uint32_t blockSize)
+ {
+ uint32_t i = 0u;
+ int32_t wOffset;
+
+ /* Copy the value of Index pointer that points
+ * to the current location where the input samples to be copied */
+ wOffset = *writeOffset;
+
+ /* Loop over the blockSize */
+ i = blockSize;
+
+ while(i > 0u)
+ {
+ /* copy the input sample to the circular buffer */
+ circBuffer[wOffset] = *src;
+
+ /* Update the input pointer */
+ src += srcInc;
+
+ /* Circularly update wOffset. Watch out for positive and negative value */
+ wOffset += bufferInc;
+ if(wOffset >= L)
+ wOffset -= L;
+
+ /* Decrement the loop counter */
+ i--;
+ }
+
+ /* Update the index pointer */
+ *writeOffset = wOffset;
+ }
+
+
+
+ /**
+ * @brief Q7 Circular Read function.
+ */
+ static __INLINE void arm_circularRead_q7(
+ q7_t * circBuffer,
+ int32_t L,
+ int32_t * readOffset,
+ int32_t bufferInc,
+ q7_t * dst,
+ q7_t * dst_base,
+ int32_t dst_length,
+ int32_t dstInc,
+ uint32_t blockSize)
+ {
+ uint32_t i = 0;
+ int32_t rOffset, dst_end;
+
+ /* Copy the value of Index pointer that points
+ * to the current location from where the input samples to be read */
+ rOffset = *readOffset;
+
+ dst_end = (int32_t) (dst_base + dst_length);
+
+ /* Loop over the blockSize */
+ i = blockSize;
+
+ while(i > 0u)
+ {
+ /* copy the sample from the circular buffer to the destination buffer */
+ *dst = circBuffer[rOffset];
+
+ /* Update the input pointer */
+ dst += dstInc;
+
+ if(dst == (q7_t *) dst_end)
+ {
+ dst = dst_base;
+ }
+
+ /* Circularly update rOffset. Watch out for positive and negative value */
+ rOffset += bufferInc;
+
+ if(rOffset >= L)
+ {
+ rOffset -= L;
+ }
+
+ /* Decrement the loop counter */
+ i--;
+ }
+
+ /* Update the index pointer */
+ *readOffset = rOffset;
+ }
+
+
+ /**
+ * @brief Sum of the squares of the elements of a Q31 vector.
+ * @param[in] *pSrc is input pointer
+ * @param[in] blockSize is the number of samples to process
+ * @param[out] *pResult is output value.
+ * @return none.
+ */
+
+ void arm_power_q31(
+ q31_t * pSrc,
+ uint32_t blockSize,
+ q63_t * pResult);
+
+ /**
+ * @brief Sum of the squares of the elements of a floating-point vector.
+ * @param[in] *pSrc is input pointer
+ * @param[in] blockSize is the number of samples to process
+ * @param[out] *pResult is output value.
+ * @return none.
+ */
+
+ void arm_power_f32(
+ float32_t * pSrc,
+ uint32_t blockSize,
+ float32_t * pResult);
+
+ /**
+ * @brief Sum of the squares of the elements of a Q15 vector.
+ * @param[in] *pSrc is input pointer
+ * @param[in] blockSize is the number of samples to process
+ * @param[out] *pResult is output value.
+ * @return none.
+ */
+
+ void arm_power_q15(
+ q15_t * pSrc,
+ uint32_t blockSize,
+ q63_t * pResult);
+
+ /**
+ * @brief Sum of the squares of the elements of a Q7 vector.
+ * @param[in] *pSrc is input pointer
+ * @param[in] blockSize is the number of samples to process
+ * @param[out] *pResult is output value.
+ * @return none.
+ */
+
+ void arm_power_q7(
+ q7_t * pSrc,
+ uint32_t blockSize,
+ q31_t * pResult);
+
+ /**
+ * @brief Mean value of a Q7 vector.
+ * @param[in] *pSrc is input pointer
+ * @param[in] blockSize is the number of samples to process
+ * @param[out] *pResult is output value.
+ * @return none.
+ */
+
+ void arm_mean_q7(
+ q7_t * pSrc,
+ uint32_t blockSize,
+ q7_t * pResult);
+
+ /**
+ * @brief Mean value of a Q15 vector.
+ * @param[in] *pSrc is input pointer
+ * @param[in] blockSize is the number of samples to process
+ * @param[out] *pResult is output value.
+ * @return none.
+ */
+ void arm_mean_q15(
+ q15_t * pSrc,
+ uint32_t blockSize,
+ q15_t * pResult);
+
+ /**
+ * @brief Mean value of a Q31 vector.
+ * @param[in] *pSrc is input pointer
+ * @param[in] blockSize is the number of samples to process
+ * @param[out] *pResult is output value.
+ * @return none.
+ */
+ void arm_mean_q31(
+ q31_t * pSrc,
+ uint32_t blockSize,
+ q31_t * pResult);
+
+ /**
+ * @brief Mean value of a floating-point vector.
+ * @param[in] *pSrc is input pointer
+ * @param[in] blockSize is the number of samples to process
+ * @param[out] *pResult is output value.
+ * @return none.
+ */
+ void arm_mean_f32(
+ float32_t * pSrc,
+ uint32_t blockSize,
+ float32_t * pResult);
+
+ /**
+ * @brief Variance of the elements of a floating-point vector.
+ * @param[in] *pSrc is input pointer
+ * @param[in] blockSize is the number of samples to process
+ * @param[out] *pResult is output value.
+ * @return none.
+ */
+
+ void arm_var_f32(
+ float32_t * pSrc,
+ uint32_t blockSize,
+ float32_t * pResult);
+
+ /**
+ * @brief Variance of the elements of a Q31 vector.
+ * @param[in] *pSrc is input pointer
+ * @param[in] blockSize is the number of samples to process
+ * @param[out] *pResult is output value.
+ * @return none.
+ */
+
+ void arm_var_q31(
+ q31_t * pSrc,
+ uint32_t blockSize,
+ q63_t * pResult);
+
+ /**
+ * @brief Variance of the elements of a Q15 vector.
+ * @param[in] *pSrc is input pointer
+ * @param[in] blockSize is the number of samples to process
+ * @param[out] *pResult is output value.
+ * @return none.
+ */
+
+ void arm_var_q15(
+ q15_t * pSrc,
+ uint32_t blockSize,
+ q31_t * pResult);
+
+ /**
+ * @brief Root Mean Square of the elements of a floating-point vector.
+ * @param[in] *pSrc is input pointer
+ * @param[in] blockSize is the number of samples to process
+ * @param[out] *pResult is output value.
+ * @return none.
+ */
+
+ void arm_rms_f32(
+ float32_t * pSrc,
+ uint32_t blockSize,
+ float32_t * pResult);
+
+ /**
+ * @brief Root Mean Square of the elements of a Q31 vector.
+ * @param[in] *pSrc is input pointer
+ * @param[in] blockSize is the number of samples to process
+ * @param[out] *pResult is output value.
+ * @return none.
+ */
+
+ void arm_rms_q31(
+ q31_t * pSrc,
+ uint32_t blockSize,
+ q31_t * pResult);
+
+ /**
+ * @brief Root Mean Square of the elements of a Q15 vector.
+ * @param[in] *pSrc is input pointer
+ * @param[in] blockSize is the number of samples to process
+ * @param[out] *pResult is output value.
+ * @return none.
+ */
+
+ void arm_rms_q15(
+ q15_t * pSrc,
+ uint32_t blockSize,
+ q15_t * pResult);
+
+ /**
+ * @brief Standard deviation of the elements of a floating-point vector.
+ * @param[in] *pSrc is input pointer
+ * @param[in] blockSize is the number of samples to process
+ * @param[out] *pResult is output value.
+ * @return none.
+ */
+
+ void arm_std_f32(
+ float32_t * pSrc,
+ uint32_t blockSize,
+ float32_t * pResult);
+
+ /**
+ * @brief Standard deviation of the elements of a Q31 vector.
+ * @param[in] *pSrc is input pointer
+ * @param[in] blockSize is the number of samples to process
+ * @param[out] *pResult is output value.
+ * @return none.
+ */
+
+ void arm_std_q31(
+ q31_t * pSrc,
+ uint32_t blockSize,
+ q31_t * pResult);
+
+ /**
+ * @brief Standard deviation of the elements of a Q15 vector.
+ * @param[in] *pSrc is input pointer
+ * @param[in] blockSize is the number of samples to process
+ * @param[out] *pResult is output value.
+ * @return none.
+ */
+
+ void arm_std_q15(
+ q15_t * pSrc,
+ uint32_t blockSize,
+ q15_t * pResult);
+
+ /**
+ * @brief Floating-point complex magnitude
+ * @param[in] *pSrc points to the complex input vector
+ * @param[out] *pDst points to the real output vector
+ * @param[in] numSamples number of complex samples in the input vector
+ * @return none.
+ */
+
+ void arm_cmplx_mag_f32(
+ float32_t * pSrc,
+ float32_t * pDst,
+ uint32_t numSamples);
+
+ /**
+ * @brief Q31 complex magnitude
+ * @param[in] *pSrc points to the complex input vector
+ * @param[out] *pDst points to the real output vector
+ * @param[in] numSamples number of complex samples in the input vector
+ * @return none.
+ */
+
+ void arm_cmplx_mag_q31(
+ q31_t * pSrc,
+ q31_t * pDst,
+ uint32_t numSamples);
+
+ /**
+ * @brief Q15 complex magnitude
+ * @param[in] *pSrc points to the complex input vector
+ * @param[out] *pDst points to the real output vector
+ * @param[in] numSamples number of complex samples in the input vector
+ * @return none.
+ */
+
+ void arm_cmplx_mag_q15(
+ q15_t * pSrc,
+ q15_t * pDst,
+ uint32_t numSamples);
+
+ /**
+ * @brief Q15 complex dot product
+ * @param[in] *pSrcA points to the first input vector
+ * @param[in] *pSrcB points to the second input vector
+ * @param[in] numSamples number of complex samples in each vector
+ * @param[out] *realResult real part of the result returned here
+ * @param[out] *imagResult imaginary part of the result returned here
+ * @return none.
+ */
+
+ void arm_cmplx_dot_prod_q15(
+ q15_t * pSrcA,
+ q15_t * pSrcB,
+ uint32_t numSamples,
+ q31_t * realResult,
+ q31_t * imagResult);
+
+ /**
+ * @brief Q31 complex dot product
+ * @param[in] *pSrcA points to the first input vector
+ * @param[in] *pSrcB points to the second input vector
+ * @param[in] numSamples number of complex samples in each vector
+ * @param[out] *realResult real part of the result returned here
+ * @param[out] *imagResult imaginary part of the result returned here
+ * @return none.
+ */
+
+ void arm_cmplx_dot_prod_q31(
+ q31_t * pSrcA,
+ q31_t * pSrcB,
+ uint32_t numSamples,
+ q63_t * realResult,
+ q63_t * imagResult);
+
+ /**
+ * @brief Floating-point complex dot product
+ * @param[in] *pSrcA points to the first input vector
+ * @param[in] *pSrcB points to the second input vector
+ * @param[in] numSamples number of complex samples in each vector
+ * @param[out] *realResult real part of the result returned here
+ * @param[out] *imagResult imaginary part of the result returned here
+ * @return none.
+ */
+
+ void arm_cmplx_dot_prod_f32(
+ float32_t * pSrcA,
+ float32_t * pSrcB,
+ uint32_t numSamples,
+ float32_t * realResult,
+ float32_t * imagResult);
+
+ /**
+ * @brief Q15 complex-by-real multiplication
+ * @param[in] *pSrcCmplx points to the complex input vector
+ * @param[in] *pSrcReal points to the real input vector
+ * @param[out] *pCmplxDst points to the complex output vector
+ * @param[in] numSamples number of samples in each vector
+ * @return none.
+ */
+
+ void arm_cmplx_mult_real_q15(
+ q15_t * pSrcCmplx,
+ q15_t * pSrcReal,
+ q15_t * pCmplxDst,
+ uint32_t numSamples);
+
+ /**
+ * @brief Q31 complex-by-real multiplication
+ * @param[in] *pSrcCmplx points to the complex input vector
+ * @param[in] *pSrcReal points to the real input vector
+ * @param[out] *pCmplxDst points to the complex output vector
+ * @param[in] numSamples number of samples in each vector
+ * @return none.
+ */
+
+ void arm_cmplx_mult_real_q31(
+ q31_t * pSrcCmplx,
+ q31_t * pSrcReal,
+ q31_t * pCmplxDst,
+ uint32_t numSamples);
+
+ /**
+ * @brief Floating-point complex-by-real multiplication
+ * @param[in] *pSrcCmplx points to the complex input vector
+ * @param[in] *pSrcReal points to the real input vector
+ * @param[out] *pCmplxDst points to the complex output vector
+ * @param[in] numSamples number of samples in each vector
+ * @return none.
+ */
+
+ void arm_cmplx_mult_real_f32(
+ float32_t * pSrcCmplx,
+ float32_t * pSrcReal,
+ float32_t * pCmplxDst,
+ uint32_t numSamples);
+
+ /**
+ * @brief Minimum value of a Q7 vector.
+ * @param[in] *pSrc is input pointer
+ * @param[in] blockSize is the number of samples to process
+ * @param[out] *result is output pointer
+ * @param[in] index is the array index of the minimum value in the input buffer.
+ * @return none.
+ */
+
+ void arm_min_q7(
+ q7_t * pSrc,
+ uint32_t blockSize,
+ q7_t * result,
+ uint32_t * index);
+
+ /**
+ * @brief Minimum value of a Q15 vector.
+ * @param[in] *pSrc is input pointer
+ * @param[in] blockSize is the number of samples to process
+ * @param[out] *pResult is output pointer
+ * @param[in] *pIndex is the array index of the minimum value in the input buffer.
+ * @return none.
+ */
+
+ void arm_min_q15(
+ q15_t * pSrc,
+ uint32_t blockSize,
+ q15_t * pResult,
+ uint32_t * pIndex);
+
+ /**
+ * @brief Minimum value of a Q31 vector.
+ * @param[in] *pSrc is input pointer
+ * @param[in] blockSize is the number of samples to process
+ * @param[out] *pResult is output pointer
+ * @param[out] *pIndex is the array index of the minimum value in the input buffer.
+ * @return none.
+ */
+ void arm_min_q31(
+ q31_t * pSrc,
+ uint32_t blockSize,
+ q31_t * pResult,
+ uint32_t * pIndex);
+
+ /**
+ * @brief Minimum value of a floating-point vector.
+ * @param[in] *pSrc is input pointer
+ * @param[in] blockSize is the number of samples to process
+ * @param[out] *pResult is output pointer
+ * @param[out] *pIndex is the array index of the minimum value in the input buffer.
+ * @return none.
+ */
+
+ void arm_min_f32(
+ float32_t * pSrc,
+ uint32_t blockSize,
+ float32_t * pResult,
+ uint32_t * pIndex);
+
+/**
+ * @brief Maximum value of a Q7 vector.
+ * @param[in] *pSrc points to the input buffer
+ * @param[in] blockSize length of the input vector
+ * @param[out] *pResult maximum value returned here
+ * @param[out] *pIndex index of maximum value returned here
+ * @return none.
+ */
+
+ void arm_max_q7(
+ q7_t * pSrc,
+ uint32_t blockSize,
+ q7_t * pResult,
+ uint32_t * pIndex);
+
+/**
+ * @brief Maximum value of a Q15 vector.
+ * @param[in] *pSrc points to the input buffer
+ * @param[in] blockSize length of the input vector
+ * @param[out] *pResult maximum value returned here
+ * @param[out] *pIndex index of maximum value returned here
+ * @return none.
+ */
+
+ void arm_max_q15(
+ q15_t * pSrc,
+ uint32_t blockSize,
+ q15_t * pResult,
+ uint32_t * pIndex);
+
+/**
+ * @brief Maximum value of a Q31 vector.
+ * @param[in] *pSrc points to the input buffer
+ * @param[in] blockSize length of the input vector
+ * @param[out] *pResult maximum value returned here
+ * @param[out] *pIndex index of maximum value returned here
+ * @return none.
+ */
+
+ void arm_max_q31(
+ q31_t * pSrc,
+ uint32_t blockSize,
+ q31_t * pResult,
+ uint32_t * pIndex);
+
+/**
+ * @brief Maximum value of a floating-point vector.
+ * @param[in] *pSrc points to the input buffer
+ * @param[in] blockSize length of the input vector
+ * @param[out] *pResult maximum value returned here
+ * @param[out] *pIndex index of maximum value returned here
+ * @return none.
+ */
+
+ void arm_max_f32(
+ float32_t * pSrc,
+ uint32_t blockSize,
+ float32_t * pResult,
+ uint32_t * pIndex);
+
+ /**
+ * @brief Q15 complex-by-complex multiplication
+ * @param[in] *pSrcA points to the first input vector
+ * @param[in] *pSrcB points to the second input vector
+ * @param[out] *pDst points to the output vector
+ * @param[in] numSamples number of complex samples in each vector
+ * @return none.
+ */
+
+ void arm_cmplx_mult_cmplx_q15(
+ q15_t * pSrcA,
+ q15_t * pSrcB,
+ q15_t * pDst,
+ uint32_t numSamples);
+
+ /**
+ * @brief Q31 complex-by-complex multiplication
+ * @param[in] *pSrcA points to the first input vector
+ * @param[in] *pSrcB points to the second input vector
+ * @param[out] *pDst points to the output vector
+ * @param[in] numSamples number of complex samples in each vector
+ * @return none.
+ */
+
+ void arm_cmplx_mult_cmplx_q31(
+ q31_t * pSrcA,
+ q31_t * pSrcB,
+ q31_t * pDst,
+ uint32_t numSamples);
+
+ /**
+ * @brief Floating-point complex-by-complex multiplication
+ * @param[in] *pSrcA points to the first input vector
+ * @param[in] *pSrcB points to the second input vector
+ * @param[out] *pDst points to the output vector
+ * @param[in] numSamples number of complex samples in each vector
+ * @return none.
+ */
+
+ void arm_cmplx_mult_cmplx_f32(
+ float32_t * pSrcA,
+ float32_t * pSrcB,
+ float32_t * pDst,
+ uint32_t numSamples);
+
+ /**
+ * @brief Converts the elements of the floating-point vector to Q31 vector.
+ * @param[in] *pSrc points to the floating-point input vector
+ * @param[out] *pDst points to the Q31 output vector
+ * @param[in] blockSize length of the input vector
+ * @return none.
+ */
+ void arm_float_to_q31(
+ float32_t * pSrc,
+ q31_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Converts the elements of the floating-point vector to Q15 vector.
+ * @param[in] *pSrc points to the floating-point input vector
+ * @param[out] *pDst points to the Q15 output vector
+ * @param[in] blockSize length of the input vector
+ * @return none
+ */
+ void arm_float_to_q15(
+ float32_t * pSrc,
+ q15_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Converts the elements of the floating-point vector to Q7 vector.
+ * @param[in] *pSrc points to the floating-point input vector
+ * @param[out] *pDst points to the Q7 output vector
+ * @param[in] blockSize length of the input vector
+ * @return none
+ */
+ void arm_float_to_q7(
+ float32_t * pSrc,
+ q7_t * pDst,
+ uint32_t blockSize);
+
+
+ /**
+ * @brief Converts the elements of the Q31 vector to Q15 vector.
+ * @param[in] *pSrc is input pointer
+ * @param[out] *pDst is output pointer
+ * @param[in] blockSize is the number of samples to process
+ * @return none.
+ */
+ void arm_q31_to_q15(
+ q31_t * pSrc,
+ q15_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Converts the elements of the Q31 vector to Q7 vector.
+ * @param[in] *pSrc is input pointer
+ * @param[out] *pDst is output pointer
+ * @param[in] blockSize is the number of samples to process
+ * @return none.
+ */
+ void arm_q31_to_q7(
+ q31_t * pSrc,
+ q7_t * pDst,
+ uint32_t blockSize);
+
+ /**
+ * @brief Converts the elements of the Q15 vector to floating-point vector.
+ * @param[in] *pSrc is input pointer
+ * @param[out] *pDst is output pointer
+ * @param[in] blockSize is the number of samples to process
+ * @return none.
+ */
+ void arm_q15_to_float(
+ q15_t * pSrc,
+ float32_t * pDst,
+ uint32_t blockSize);
+
+
+ /**
+ * @brief Converts the elements of the Q15 vector to Q31 vector.
+ * @param[in] *pSrc is input pointer
+ * @param[out] *pDst is output pointer
+ * @param[in] blockSize is the number of samples to process
+ * @return none.
+ */
+ void arm_q15_to_q31(
+ q15_t * pSrc,
+ q31_t * pDst,
+ uint32_t blockSize);
+
+
+ /**
+ * @brief Converts the elements of the Q15 vector to Q7 vector.
+ * @param[in] *pSrc is input pointer
+ * @param[out] *pDst is output pointer
+ * @param[in] blockSize is the number of samples to process
+ * @return none.
+ */
+ void arm_q15_to_q7(
+ q15_t * pSrc,
+ q7_t * pDst,
+ uint32_t blockSize);
+
+
+ /**
+ * @ingroup groupInterpolation
+ */
+
+ /**
+ * @defgroup BilinearInterpolate Bilinear Interpolation
+ *
+ * Bilinear interpolation is an extension of linear interpolation applied to a two dimensional grid.
+ * The underlying function <code>f(x, y)</code> is sampled on a regular grid and the interpolation process
+ * determines values between the grid points.
+ * Bilinear interpolation is equivalent to two step linear interpolation, first in the x-dimension and then in the y-dimension.
+ * Bilinear interpolation is often used in image processing to rescale images.
+ * The CMSIS DSP library provides bilinear interpolation functions for Q7, Q15, Q31, and floating-point data types.
+ *
+ * <b>Algorithm</b>
+ * \par
+ * The instance structure used by the bilinear interpolation functions describes a two dimensional data table.
+ * For floating-point, the instance structure is defined as:
+ * <pre>
+ * typedef struct
+ * {
+ * uint16_t numRows;
+ * uint16_t numCols;
+ * float32_t *pData;
+ * } arm_bilinear_interp_instance_f32;
+ * </pre>
+ *
+ * \par
+ * where <code>numRows</code> specifies the number of rows in the table;
+ * <code>numCols</code> specifies the number of columns in the table;
+ * and <code>pData</code> points to an array of size <code>numRows*numCols</code> values.
+ * The data table <code>pTable</code> is organized in row order and the supplied data values fall on integer indexes.
+ * That is, table element (x,y) is located at <code>pTable[x + y*numCols]</code> where x and y are integers.
+ *
+ * \par
+ * Let <code>(x, y)</code> specify the desired interpolation point. Then define:
+ * <pre>
+ * XF = floor(x)
+ * YF = floor(y)
+ * </pre>
+ * \par
+ * The interpolated output point is computed as:
+ * <pre>
+ * f(x, y) = f(XF, YF) * (1-(x-XF)) * (1-(y-YF))
+ * + f(XF+1, YF) * (x-XF)*(1-(y-YF))
+ * + f(XF, YF+1) * (1-(x-XF))*(y-YF)
+ * + f(XF+1, YF+1) * (x-XF)*(y-YF)
+ * </pre>
+ * Note that the coordinates (x, y) contain integer and fractional components.
+ * The integer components specify which portion of the table to use while the
+ * fractional components control the interpolation processor.
+ *
+ * \par
+ * if (x,y) are outside of the table boundary, Bilinear interpolation returns zero output.
+ */
+
+ /**
+ * @addtogroup BilinearInterpolate
+ * @{
+ */
+
+ /**
+ *
+ * @brief Floating-point bilinear interpolation.
+ * @param[in,out] *S points to an instance of the interpolation structure.
+ * @param[in] X interpolation coordinate.
+ * @param[in] Y interpolation coordinate.
+ * @return out interpolated value.
+ */
+
+
+ static __INLINE float32_t arm_bilinear_interp_f32(
+ const arm_bilinear_interp_instance_f32 * S,
+ float32_t X,
+ float32_t Y)
+ {
+ float32_t out;
+ float32_t f00, f01, f10, f11;
+ float32_t *pData = S->pData;
+ int32_t xIndex, yIndex, index;
+ float32_t xdiff, ydiff;
+ float32_t b1, b2, b3, b4;
+
+ xIndex = (int32_t) X;
+ yIndex = (int32_t) Y;
+
+ /* Care taken for table outside boundary */
+ /* Returns zero output when values are outside table boundary */
+ if(xIndex < 0 || xIndex > (S->numRows - 1) || yIndex < 0
+ || yIndex > (S->numCols - 1))
+ {
+ return (0);
+ }
+
+ /* Calculation of index for two nearest points in X-direction */
+ index = (xIndex - 1) + (yIndex - 1) * S->numCols;
+
+
+ /* Read two nearest points in X-direction */
+ f00 = pData[index];
+ f01 = pData[index + 1];
+
+ /* Calculation of index for two nearest points in Y-direction */
+ index = (xIndex - 1) + (yIndex) * S->numCols;
+
+
+ /* Read two nearest points in Y-direction */
+ f10 = pData[index];
+ f11 = pData[index + 1];
+
+ /* Calculation of intermediate values */
+ b1 = f00;
+ b2 = f01 - f00;
+ b3 = f10 - f00;
+ b4 = f00 - f01 - f10 + f11;
+
+ /* Calculation of fractional part in X */
+ xdiff = X - xIndex;
+
+ /* Calculation of fractional part in Y */
+ ydiff = Y - yIndex;
+
+ /* Calculation of bi-linear interpolated output */
+ out = b1 + b2 * xdiff + b3 * ydiff + b4 * xdiff * ydiff;
+
+ /* return to application */
+ return (out);
+
+ }
+
+ /**
+ *
+ * @brief Q31 bilinear interpolation.
+ * @param[in,out] *S points to an instance of the interpolation structure.
+ * @param[in] X interpolation coordinate in 12.20 format.
+ * @param[in] Y interpolation coordinate in 12.20 format.
+ * @return out interpolated value.
+ */
+
+ static __INLINE q31_t arm_bilinear_interp_q31(
+ arm_bilinear_interp_instance_q31 * S,
+ q31_t X,
+ q31_t Y)
+ {
+ q31_t out; /* Temporary output */
+ q31_t acc = 0; /* output */
+ q31_t xfract, yfract; /* X, Y fractional parts */
+ q31_t x1, x2, y1, y2; /* Nearest output values */
+ int32_t rI, cI; /* Row and column indices */
+ q31_t *pYData = S->pData; /* pointer to output table values */
+ uint32_t nCols = S->numCols; /* num of rows */
+
+
+ /* Input is in 12.20 format */
+ /* 12 bits for the table index */
+ /* Index value calculation */
+ rI = ((X & 0xFFF00000) >> 20u);
+
+ /* Input is in 12.20 format */
+ /* 12 bits for the table index */
+ /* Index value calculation */
+ cI = ((Y & 0xFFF00000) >> 20u);
+
+ /* Care taken for table outside boundary */
+ /* Returns zero output when values are outside table boundary */
+ if(rI < 0 || rI > (S->numRows - 1) || cI < 0 || cI > (S->numCols - 1))
+ {
+ return (0);
+ }
+
+ /* 20 bits for the fractional part */
+ /* shift left xfract by 11 to keep 1.31 format */
+ xfract = (X & 0x000FFFFF) << 11u;
+
+ /* Read two nearest output values from the index */
+ x1 = pYData[(rI) + nCols * (cI)];
+ x2 = pYData[(rI) + nCols * (cI) + 1u];
+
+ /* 20 bits for the fractional part */
+ /* shift left yfract by 11 to keep 1.31 format */
+ yfract = (Y & 0x000FFFFF) << 11u;
+
+ /* Read two nearest output values from the index */
+ y1 = pYData[(rI) + nCols * (cI + 1)];
+ y2 = pYData[(rI) + nCols * (cI + 1) + 1u];
+
+ /* Calculation of x1 * (1-xfract ) * (1-yfract) and acc is in 3.29(q29) format */
+ out = ((q31_t) (((q63_t) x1 * (0x7FFFFFFF - xfract)) >> 32));
+ acc = ((q31_t) (((q63_t) out * (0x7FFFFFFF - yfract)) >> 32));
+
+ /* x2 * (xfract) * (1-yfract) in 3.29(q29) and adding to acc */
+ out = ((q31_t) ((q63_t) x2 * (0x7FFFFFFF - yfract) >> 32));
+ acc += ((q31_t) ((q63_t) out * (xfract) >> 32));
+
+ /* y1 * (1 - xfract) * (yfract) in 3.29(q29) and adding to acc */
+ out = ((q31_t) ((q63_t) y1 * (0x7FFFFFFF - xfract) >> 32));
+ acc += ((q31_t) ((q63_t) out * (yfract) >> 32));
+
+ /* y2 * (xfract) * (yfract) in 3.29(q29) and adding to acc */
+ out = ((q31_t) ((q63_t) y2 * (xfract) >> 32));
+ acc += ((q31_t) ((q63_t) out * (yfract) >> 32));
+
+ /* Convert acc to 1.31(q31) format */
+ return (acc << 2u);
+
+ }
+
+ /**
+ * @brief Q15 bilinear interpolation.
+ * @param[in,out] *S points to an instance of the interpolation structure.
+ * @param[in] X interpolation coordinate in 12.20 format.
+ * @param[in] Y interpolation coordinate in 12.20 format.
+ * @return out interpolated value.
+ */
+
+ static __INLINE q15_t arm_bilinear_interp_q15(
+ arm_bilinear_interp_instance_q15 * S,
+ q31_t X,
+ q31_t Y)
+ {
+ q63_t acc = 0; /* output */
+ q31_t out; /* Temporary output */
+ q15_t x1, x2, y1, y2; /* Nearest output values */
+ q31_t xfract, yfract; /* X, Y fractional parts */
+ int32_t rI, cI; /* Row and column indices */
+ q15_t *pYData = S->pData; /* pointer to output table values */
+ uint32_t nCols = S->numCols; /* num of rows */
+
+ /* Input is in 12.20 format */
+ /* 12 bits for the table index */
+ /* Index value calculation */
+ rI = ((X & 0xFFF00000) >> 20);
+
+ /* Input is in 12.20 format */
+ /* 12 bits for the table index */
+ /* Index value calculation */
+ cI = ((Y & 0xFFF00000) >> 20);
+
+ /* Care taken for table outside boundary */
+ /* Returns zero output when values are outside table boundary */
+ if(rI < 0 || rI > (S->numRows - 1) || cI < 0 || cI > (S->numCols - 1))
+ {
+ return (0);
+ }
+
+ /* 20 bits for the fractional part */
+ /* xfract should be in 12.20 format */
+ xfract = (X & 0x000FFFFF);
+
+ /* Read two nearest output values from the index */
+ x1 = pYData[(rI) + nCols * (cI)];
+ x2 = pYData[(rI) + nCols * (cI) + 1u];
+
+
+ /* 20 bits for the fractional part */
+ /* yfract should be in 12.20 format */
+ yfract = (Y & 0x000FFFFF);
+
+ /* Read two nearest output values from the index */
+ y1 = pYData[(rI) + nCols * (cI + 1)];
+ y2 = pYData[(rI) + nCols * (cI + 1) + 1u];
+
+ /* Calculation of x1 * (1-xfract ) * (1-yfract) and acc is in 13.51 format */
+
+ /* x1 is in 1.15(q15), xfract in 12.20 format and out is in 13.35 format */
+ /* convert 13.35 to 13.31 by right shifting and out is in 1.31 */
+ out = (q31_t) (((q63_t) x1 * (0xFFFFF - xfract)) >> 4u);
+ acc = ((q63_t) out * (0xFFFFF - yfract));
+
+ /* x2 * (xfract) * (1-yfract) in 1.51 and adding to acc */
+ out = (q31_t) (((q63_t) x2 * (0xFFFFF - yfract)) >> 4u);
+ acc += ((q63_t) out * (xfract));
+
+ /* y1 * (1 - xfract) * (yfract) in 1.51 and adding to acc */
+ out = (q31_t) (((q63_t) y1 * (0xFFFFF - xfract)) >> 4u);
+ acc += ((q63_t) out * (yfract));
+
+ /* y2 * (xfract) * (yfract) in 1.51 and adding to acc */
+ out = (q31_t) (((q63_t) y2 * (xfract)) >> 4u);
+ acc += ((q63_t) out * (yfract));
+
+ /* acc is in 13.51 format and down shift acc by 36 times */
+ /* Convert out to 1.15 format */
+ return (acc >> 36);
+
+ }
+
+ /**
+ * @brief Q7 bilinear interpolation.
+ * @param[in,out] *S points to an instance of the interpolation structure.
+ * @param[in] X interpolation coordinate in 12.20 format.
+ * @param[in] Y interpolation coordinate in 12.20 format.
+ * @return out interpolated value.
+ */
+
+ static __INLINE q7_t arm_bilinear_interp_q7(
+ arm_bilinear_interp_instance_q7 * S,
+ q31_t X,
+ q31_t Y)
+ {
+ q63_t acc = 0; /* output */
+ q31_t out; /* Temporary output */
+ q31_t xfract, yfract; /* X, Y fractional parts */
+ q7_t x1, x2, y1, y2; /* Nearest output values */
+ int32_t rI, cI; /* Row and column indices */
+ q7_t *pYData = S->pData; /* pointer to output table values */
+ uint32_t nCols = S->numCols; /* num of rows */
+
+ /* Input is in 12.20 format */
+ /* 12 bits for the table index */
+ /* Index value calculation */
+ rI = ((X & 0xFFF00000) >> 20);
+
+ /* Input is in 12.20 format */
+ /* 12 bits for the table index */
+ /* Index value calculation */
+ cI = ((Y & 0xFFF00000) >> 20);
+
+ /* Care taken for table outside boundary */
+ /* Returns zero output when values are outside table boundary */
+ if(rI < 0 || rI > (S->numRows - 1) || cI < 0 || cI > (S->numCols - 1))
+ {
+ return (0);
+ }
+
+ /* 20 bits for the fractional part */
+ /* xfract should be in 12.20 format */
+ xfract = (X & 0x000FFFFF);
+
+ /* Read two nearest output values from the index */
+ x1 = pYData[(rI) + nCols * (cI)];
+ x2 = pYData[(rI) + nCols * (cI) + 1u];
+
+
+ /* 20 bits for the fractional part */
+ /* yfract should be in 12.20 format */
+ yfract = (Y & 0x000FFFFF);
+
+ /* Read two nearest output values from the index */
+ y1 = pYData[(rI) + nCols * (cI + 1)];
+ y2 = pYData[(rI) + nCols * (cI + 1) + 1u];
+
+ /* Calculation of x1 * (1-xfract ) * (1-yfract) and acc is in 16.47 format */
+ out = ((x1 * (0xFFFFF - xfract)));
+ acc = (((q63_t) out * (0xFFFFF - yfract)));
+
+ /* x2 * (xfract) * (1-yfract) in 2.22 and adding to acc */
+ out = ((x2 * (0xFFFFF - yfract)));
+ acc += (((q63_t) out * (xfract)));
+
+ /* y1 * (1 - xfract) * (yfract) in 2.22 and adding to acc */
+ out = ((y1 * (0xFFFFF - xfract)));
+ acc += (((q63_t) out * (yfract)));
+
+ /* y2 * (xfract) * (yfract) in 2.22 and adding to acc */
+ out = ((y2 * (yfract)));
+ acc += (((q63_t) out * (xfract)));
+
+ /* acc in 16.47 format and down shift by 40 to convert to 1.7 format */
+ return (acc >> 40);
+
+ }
+
+ /**
+ * @} end of BilinearInterpolate group
+ */
+
+
+#if defined ( __CC_ARM ) //Keil
+//SMMLAR
+ #define multAcc_32x32_keep32_R(a, x, y) \
+ a = (q31_t) (((((q63_t) a) << 32) + ((q63_t) x * y) + 0x80000000LL ) >> 32)
+
+//SMMLSR
+ #define multSub_32x32_keep32_R(a, x, y) \
+ a = (q31_t) (((((q63_t) a) << 32) - ((q63_t) x * y) + 0x80000000LL ) >> 32)
+
+//SMMULR
+ #define mult_32x32_keep32_R(a, x, y) \
+ a = (q31_t) (((q63_t) x * y + 0x80000000LL ) >> 32)
+
+//Enter low optimization region - place directly above function definition
+ #define LOW_OPTIMIZATION_ENTER \
+ _Pragma ("push") \
+ _Pragma ("O1")
+
+//Exit low optimization region - place directly after end of function definition
+ #define LOW_OPTIMIZATION_EXIT \
+ _Pragma ("pop")
+
+//Enter low optimization region - place directly above function definition
+ #define IAR_ONLY_LOW_OPTIMIZATION_ENTER
+
+//Exit low optimization region - place directly after end of function definition
+ #define IAR_ONLY_LOW_OPTIMIZATION_EXIT
+
+#elif defined(__ICCARM__) //IAR
+ //SMMLA
+ #define multAcc_32x32_keep32_R(a, x, y) \
+ a += (q31_t) (((q63_t) x * y) >> 32)
+
+ //SMMLS
+ #define multSub_32x32_keep32_R(a, x, y) \
+ a -= (q31_t) (((q63_t) x * y) >> 32)
+
+//SMMUL
+ #define mult_32x32_keep32_R(a, x, y) \
+ a = (q31_t) (((q63_t) x * y ) >> 32)
+
+//Enter low optimization region - place directly above function definition
+ #define LOW_OPTIMIZATION_ENTER \
+ _Pragma ("optimize=low")
+
+//Exit low optimization region - place directly after end of function definition
+ #define LOW_OPTIMIZATION_EXIT
+
+//Enter low optimization region - place directly above function definition
+ #define IAR_ONLY_LOW_OPTIMIZATION_ENTER \
+ _Pragma ("optimize=low")
+
+//Exit low optimization region - place directly after end of function definition
+ #define IAR_ONLY_LOW_OPTIMIZATION_EXIT
+
+#elif defined(__GNUC__)
+ //SMMLA
+ #define multAcc_32x32_keep32_R(a, x, y) \
+ a += (q31_t) (((q63_t) x * y) >> 32)
+
+ //SMMLS
+ #define multSub_32x32_keep32_R(a, x, y) \
+ a -= (q31_t) (((q63_t) x * y) >> 32)
+
+//SMMUL
+ #define mult_32x32_keep32_R(a, x, y) \
+ a = (q31_t) (((q63_t) x * y ) >> 32)
+
+ #define LOW_OPTIMIZATION_ENTER __attribute__(( optimize("-O1") ))
+
+ #define LOW_OPTIMIZATION_EXIT
+
+ #define IAR_ONLY_LOW_OPTIMIZATION_ENTER
+
+ #define IAR_ONLY_LOW_OPTIMIZATION_EXIT
+
+#endif
+
+
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif /* _ARM_MATH_H */
+
+
+/**
+ *
+ * End of file.
+ */
diff --git a/src/modules/mathlib/CMSIS/Include/core_cm3.h b/src/modules/mathlib/CMSIS/Include/core_cm3.h new file mode 100644 index 000000000..8ac6dc078 --- /dev/null +++ b/src/modules/mathlib/CMSIS/Include/core_cm3.h @@ -0,0 +1,1627 @@ +/**************************************************************************//**
+ * @file core_cm3.h
+ * @brief CMSIS Cortex-M3 Core Peripheral Access Layer Header File
+ * @version V3.20
+ * @date 25. February 2013
+ *
+ * @note
+ *
+ ******************************************************************************/
+/* Copyright (c) 2009 - 2013 ARM LIMITED
+
+ All rights reserved.
+ Redistribution and use in source and binary forms, with or without
+ modification, are permitted provided that the following conditions are met:
+ - Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+ - 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.
+ - Neither the name of ARM 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 COPYRIGHT HOLDERS AND 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.
+ ---------------------------------------------------------------------------*/
+
+
+#if defined ( __ICCARM__ )
+ #pragma system_include /* treat file as system include file for MISRA check */
+#endif
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+#ifndef __CORE_CM3_H_GENERIC
+#define __CORE_CM3_H_GENERIC
+
+/** \page CMSIS_MISRA_Exceptions MISRA-C:2004 Compliance Exceptions
+ CMSIS violates the following MISRA-C:2004 rules:
+
+ \li Required Rule 8.5, object/function definition in header file.<br>
+ Function definitions in header files are used to allow 'inlining'.
+
+ \li Required Rule 18.4, declaration of union type or object of union type: '{...}'.<br>
+ Unions are used for effective representation of core registers.
+
+ \li Advisory Rule 19.7, Function-like macro defined.<br>
+ Function-like macros are used to allow more efficient code.
+ */
+
+
+/*******************************************************************************
+ * CMSIS definitions
+ ******************************************************************************/
+/** \ingroup Cortex_M3
+ @{
+ */
+
+/* CMSIS CM3 definitions */
+#define __CM3_CMSIS_VERSION_MAIN (0x03) /*!< [31:16] CMSIS HAL main version */
+#define __CM3_CMSIS_VERSION_SUB (0x20) /*!< [15:0] CMSIS HAL sub version */
+#define __CM3_CMSIS_VERSION ((__CM3_CMSIS_VERSION_MAIN << 16) | \
+ __CM3_CMSIS_VERSION_SUB ) /*!< CMSIS HAL version number */
+
+#define __CORTEX_M (0x03) /*!< Cortex-M Core */
+
+
+#if defined ( __CC_ARM )
+ #define __ASM __asm /*!< asm keyword for ARM Compiler */
+ #define __INLINE __inline /*!< inline keyword for ARM Compiler */
+ #define __STATIC_INLINE static __inline
+
+#elif defined ( __ICCARM__ )
+ #define __ASM __asm /*!< asm keyword for IAR Compiler */
+ #define __INLINE inline /*!< inline keyword for IAR Compiler. Only available in High optimization mode! */
+ #define __STATIC_INLINE static inline
+
+#elif defined ( __TMS470__ )
+ #define __ASM __asm /*!< asm keyword for TI CCS Compiler */
+ #define __STATIC_INLINE static inline
+
+#elif defined ( __GNUC__ )
+ #define __ASM __asm /*!< asm keyword for GNU Compiler */
+ #define __INLINE inline /*!< inline keyword for GNU Compiler */
+ #define __STATIC_INLINE static inline
+
+#elif defined ( __TASKING__ )
+ #define __ASM __asm /*!< asm keyword for TASKING Compiler */
+ #define __INLINE inline /*!< inline keyword for TASKING Compiler */
+ #define __STATIC_INLINE static inline
+
+#endif
+
+/** __FPU_USED indicates whether an FPU is used or not. This core does not support an FPU at all
+*/
+#define __FPU_USED 0
+
+#if defined ( __CC_ARM )
+ #if defined __TARGET_FPU_VFP
+ #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+ #endif
+
+#elif defined ( __ICCARM__ )
+ #if defined __ARMVFP__
+ #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+ #endif
+
+#elif defined ( __TMS470__ )
+ #if defined __TI__VFP_SUPPORT____
+ #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+ #endif
+
+#elif defined ( __GNUC__ )
+ #if defined (__VFP_FP__) && !defined(__SOFTFP__)
+ #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+ #endif
+
+#elif defined ( __TASKING__ )
+ #if defined __FPU_VFP__
+ #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+ #endif
+#endif
+
+#include <stdint.h> /* standard types definitions */
+#include "core_cmInstr.h" /* Core Instruction Access */
+#include "core_cmFunc.h" /* Core Function Access */
+
+#endif /* __CORE_CM3_H_GENERIC */
+
+#ifndef __CMSIS_GENERIC
+
+#ifndef __CORE_CM3_H_DEPENDANT
+#define __CORE_CM3_H_DEPENDANT
+
+/* check device defines and use defaults */
+#if defined __CHECK_DEVICE_DEFINES
+ #ifndef __CM3_REV
+ #define __CM3_REV 0x0200
+ #warning "__CM3_REV not defined in device header file; using default!"
+ #endif
+
+ #ifndef __MPU_PRESENT
+ #define __MPU_PRESENT 0
+ #warning "__MPU_PRESENT not defined in device header file; using default!"
+ #endif
+
+ #ifndef __NVIC_PRIO_BITS
+ #define __NVIC_PRIO_BITS 4
+ #warning "__NVIC_PRIO_BITS not defined in device header file; using default!"
+ #endif
+
+ #ifndef __Vendor_SysTickConfig
+ #define __Vendor_SysTickConfig 0
+ #warning "__Vendor_SysTickConfig not defined in device header file; using default!"
+ #endif
+#endif
+
+/* IO definitions (access restrictions to peripheral registers) */
+/**
+ \defgroup CMSIS_glob_defs CMSIS Global Defines
+
+ <strong>IO Type Qualifiers</strong> are used
+ \li to specify the access to peripheral variables.
+ \li for automatic generation of peripheral register debug information.
+*/
+#ifdef __cplusplus
+ #define __I volatile /*!< Defines 'read only' permissions */
+#else
+ #define __I volatile const /*!< Defines 'read only' permissions */
+#endif
+#define __O volatile /*!< Defines 'write only' permissions */
+#define __IO volatile /*!< Defines 'read / write' permissions */
+
+/*@} end of group Cortex_M3 */
+
+
+
+/*******************************************************************************
+ * Register Abstraction
+ Core Register contain:
+ - Core Register
+ - Core NVIC Register
+ - Core SCB Register
+ - Core SysTick Register
+ - Core Debug Register
+ - Core MPU Register
+ ******************************************************************************/
+/** \defgroup CMSIS_core_register Defines and Type Definitions
+ \brief Type definitions and defines for Cortex-M processor based devices.
+*/
+
+/** \ingroup CMSIS_core_register
+ \defgroup CMSIS_CORE Status and Control Registers
+ \brief Core Register type definitions.
+ @{
+ */
+
+/** \brief Union type to access the Application Program Status Register (APSR).
+ */
+typedef union
+{
+ struct
+ {
+#if (__CORTEX_M != 0x04)
+ uint32_t _reserved0:27; /*!< bit: 0..26 Reserved */
+#else
+ uint32_t _reserved0:16; /*!< bit: 0..15 Reserved */
+ uint32_t GE:4; /*!< bit: 16..19 Greater than or Equal flags */
+ uint32_t _reserved1:7; /*!< bit: 20..26 Reserved */
+#endif
+ uint32_t Q:1; /*!< bit: 27 Saturation condition flag */
+ uint32_t V:1; /*!< bit: 28 Overflow condition code flag */
+ uint32_t C:1; /*!< bit: 29 Carry condition code flag */
+ uint32_t Z:1; /*!< bit: 30 Zero condition code flag */
+ uint32_t N:1; /*!< bit: 31 Negative condition code flag */
+ } b; /*!< Structure used for bit access */
+ uint32_t w; /*!< Type used for word access */
+} APSR_Type;
+
+
+/** \brief Union type to access the Interrupt Program Status Register (IPSR).
+ */
+typedef union
+{
+ struct
+ {
+ uint32_t ISR:9; /*!< bit: 0.. 8 Exception number */
+ uint32_t _reserved0:23; /*!< bit: 9..31 Reserved */
+ } b; /*!< Structure used for bit access */
+ uint32_t w; /*!< Type used for word access */
+} IPSR_Type;
+
+
+/** \brief Union type to access the Special-Purpose Program Status Registers (xPSR).
+ */
+typedef union
+{
+ struct
+ {
+ uint32_t ISR:9; /*!< bit: 0.. 8 Exception number */
+#if (__CORTEX_M != 0x04)
+ uint32_t _reserved0:15; /*!< bit: 9..23 Reserved */
+#else
+ uint32_t _reserved0:7; /*!< bit: 9..15 Reserved */
+ uint32_t GE:4; /*!< bit: 16..19 Greater than or Equal flags */
+ uint32_t _reserved1:4; /*!< bit: 20..23 Reserved */
+#endif
+ uint32_t T:1; /*!< bit: 24 Thumb bit (read 0) */
+ uint32_t IT:2; /*!< bit: 25..26 saved IT state (read 0) */
+ uint32_t Q:1; /*!< bit: 27 Saturation condition flag */
+ uint32_t V:1; /*!< bit: 28 Overflow condition code flag */
+ uint32_t C:1; /*!< bit: 29 Carry condition code flag */
+ uint32_t Z:1; /*!< bit: 30 Zero condition code flag */
+ uint32_t N:1; /*!< bit: 31 Negative condition code flag */
+ } b; /*!< Structure used for bit access */
+ uint32_t w; /*!< Type used for word access */
+} xPSR_Type;
+
+
+/** \brief Union type to access the Control Registers (CONTROL).
+ */
+typedef union
+{
+ struct
+ {
+ uint32_t nPRIV:1; /*!< bit: 0 Execution privilege in Thread mode */
+ uint32_t SPSEL:1; /*!< bit: 1 Stack to be used */
+ uint32_t FPCA:1; /*!< bit: 2 FP extension active flag */
+ uint32_t _reserved0:29; /*!< bit: 3..31 Reserved */
+ } b; /*!< Structure used for bit access */
+ uint32_t w; /*!< Type used for word access */
+} CONTROL_Type;
+
+/*@} end of group CMSIS_CORE */
+
+
+/** \ingroup CMSIS_core_register
+ \defgroup CMSIS_NVIC Nested Vectored Interrupt Controller (NVIC)
+ \brief Type definitions for the NVIC Registers
+ @{
+ */
+
+/** \brief Structure type to access the Nested Vectored Interrupt Controller (NVIC).
+ */
+typedef struct
+{
+ __IO uint32_t ISER[8]; /*!< Offset: 0x000 (R/W) Interrupt Set Enable Register */
+ uint32_t RESERVED0[24];
+ __IO uint32_t ICER[8]; /*!< Offset: 0x080 (R/W) Interrupt Clear Enable Register */
+ uint32_t RSERVED1[24];
+ __IO uint32_t ISPR[8]; /*!< Offset: 0x100 (R/W) Interrupt Set Pending Register */
+ uint32_t RESERVED2[24];
+ __IO uint32_t ICPR[8]; /*!< Offset: 0x180 (R/W) Interrupt Clear Pending Register */
+ uint32_t RESERVED3[24];
+ __IO uint32_t IABR[8]; /*!< Offset: 0x200 (R/W) Interrupt Active bit Register */
+ uint32_t RESERVED4[56];
+ __IO uint8_t IP[240]; /*!< Offset: 0x300 (R/W) Interrupt Priority Register (8Bit wide) */
+ uint32_t RESERVED5[644];
+ __O uint32_t STIR; /*!< Offset: 0xE00 ( /W) Software Trigger Interrupt Register */
+} NVIC_Type;
+
+/* Software Triggered Interrupt Register Definitions */
+#define NVIC_STIR_INTID_Pos 0 /*!< STIR: INTLINESNUM Position */
+#define NVIC_STIR_INTID_Msk (0x1FFUL << NVIC_STIR_INTID_Pos) /*!< STIR: INTLINESNUM Mask */
+
+/*@} end of group CMSIS_NVIC */
+
+
+/** \ingroup CMSIS_core_register
+ \defgroup CMSIS_SCB System Control Block (SCB)
+ \brief Type definitions for the System Control Block Registers
+ @{
+ */
+
+/** \brief Structure type to access the System Control Block (SCB).
+ */
+typedef struct
+{
+ __I uint32_t CPUID; /*!< Offset: 0x000 (R/ ) CPUID Base Register */
+ __IO uint32_t ICSR; /*!< Offset: 0x004 (R/W) Interrupt Control and State Register */
+ __IO uint32_t VTOR; /*!< Offset: 0x008 (R/W) Vector Table Offset Register */
+ __IO uint32_t AIRCR; /*!< Offset: 0x00C (R/W) Application Interrupt and Reset Control Register */
+ __IO uint32_t SCR; /*!< Offset: 0x010 (R/W) System Control Register */
+ __IO uint32_t CCR; /*!< Offset: 0x014 (R/W) Configuration Control Register */
+ __IO uint8_t SHP[12]; /*!< Offset: 0x018 (R/W) System Handlers Priority Registers (4-7, 8-11, 12-15) */
+ __IO uint32_t SHCSR; /*!< Offset: 0x024 (R/W) System Handler Control and State Register */
+ __IO uint32_t CFSR; /*!< Offset: 0x028 (R/W) Configurable Fault Status Register */
+ __IO uint32_t HFSR; /*!< Offset: 0x02C (R/W) HardFault Status Register */
+ __IO uint32_t DFSR; /*!< Offset: 0x030 (R/W) Debug Fault Status Register */
+ __IO uint32_t MMFAR; /*!< Offset: 0x034 (R/W) MemManage Fault Address Register */
+ __IO uint32_t BFAR; /*!< Offset: 0x038 (R/W) BusFault Address Register */
+ __IO uint32_t AFSR; /*!< Offset: 0x03C (R/W) Auxiliary Fault Status Register */
+ __I uint32_t PFR[2]; /*!< Offset: 0x040 (R/ ) Processor Feature Register */
+ __I uint32_t DFR; /*!< Offset: 0x048 (R/ ) Debug Feature Register */
+ __I uint32_t ADR; /*!< Offset: 0x04C (R/ ) Auxiliary Feature Register */
+ __I uint32_t MMFR[4]; /*!< Offset: 0x050 (R/ ) Memory Model Feature Register */
+ __I uint32_t ISAR[5]; /*!< Offset: 0x060 (R/ ) Instruction Set Attributes Register */
+ uint32_t RESERVED0[5];
+ __IO uint32_t CPACR; /*!< Offset: 0x088 (R/W) Coprocessor Access Control Register */
+} SCB_Type;
+
+/* SCB CPUID Register Definitions */
+#define SCB_CPUID_IMPLEMENTER_Pos 24 /*!< SCB CPUID: IMPLEMENTER Position */
+#define SCB_CPUID_IMPLEMENTER_Msk (0xFFUL << SCB_CPUID_IMPLEMENTER_Pos) /*!< SCB CPUID: IMPLEMENTER Mask */
+
+#define SCB_CPUID_VARIANT_Pos 20 /*!< SCB CPUID: VARIANT Position */
+#define SCB_CPUID_VARIANT_Msk (0xFUL << SCB_CPUID_VARIANT_Pos) /*!< SCB CPUID: VARIANT Mask */
+
+#define SCB_CPUID_ARCHITECTURE_Pos 16 /*!< SCB CPUID: ARCHITECTURE Position */
+#define SCB_CPUID_ARCHITECTURE_Msk (0xFUL << SCB_CPUID_ARCHITECTURE_Pos) /*!< SCB CPUID: ARCHITECTURE Mask */
+
+#define SCB_CPUID_PARTNO_Pos 4 /*!< SCB CPUID: PARTNO Position */
+#define SCB_CPUID_PARTNO_Msk (0xFFFUL << SCB_CPUID_PARTNO_Pos) /*!< SCB CPUID: PARTNO Mask */
+
+#define SCB_CPUID_REVISION_Pos 0 /*!< SCB CPUID: REVISION Position */
+#define SCB_CPUID_REVISION_Msk (0xFUL << SCB_CPUID_REVISION_Pos) /*!< SCB CPUID: REVISION Mask */
+
+/* SCB Interrupt Control State Register Definitions */
+#define SCB_ICSR_NMIPENDSET_Pos 31 /*!< SCB ICSR: NMIPENDSET Position */
+#define SCB_ICSR_NMIPENDSET_Msk (1UL << SCB_ICSR_NMIPENDSET_Pos) /*!< SCB ICSR: NMIPENDSET Mask */
+
+#define SCB_ICSR_PENDSVSET_Pos 28 /*!< SCB ICSR: PENDSVSET Position */
+#define SCB_ICSR_PENDSVSET_Msk (1UL << SCB_ICSR_PENDSVSET_Pos) /*!< SCB ICSR: PENDSVSET Mask */
+
+#define SCB_ICSR_PENDSVCLR_Pos 27 /*!< SCB ICSR: PENDSVCLR Position */
+#define SCB_ICSR_PENDSVCLR_Msk (1UL << SCB_ICSR_PENDSVCLR_Pos) /*!< SCB ICSR: PENDSVCLR Mask */
+
+#define SCB_ICSR_PENDSTSET_Pos 26 /*!< SCB ICSR: PENDSTSET Position */
+#define SCB_ICSR_PENDSTSET_Msk (1UL << SCB_ICSR_PENDSTSET_Pos) /*!< SCB ICSR: PENDSTSET Mask */
+
+#define SCB_ICSR_PENDSTCLR_Pos 25 /*!< SCB ICSR: PENDSTCLR Position */
+#define SCB_ICSR_PENDSTCLR_Msk (1UL << SCB_ICSR_PENDSTCLR_Pos) /*!< SCB ICSR: PENDSTCLR Mask */
+
+#define SCB_ICSR_ISRPREEMPT_Pos 23 /*!< SCB ICSR: ISRPREEMPT Position */
+#define SCB_ICSR_ISRPREEMPT_Msk (1UL << SCB_ICSR_ISRPREEMPT_Pos) /*!< SCB ICSR: ISRPREEMPT Mask */
+
+#define SCB_ICSR_ISRPENDING_Pos 22 /*!< SCB ICSR: ISRPENDING Position */
+#define SCB_ICSR_ISRPENDING_Msk (1UL << SCB_ICSR_ISRPENDING_Pos) /*!< SCB ICSR: ISRPENDING Mask */
+
+#define SCB_ICSR_VECTPENDING_Pos 12 /*!< SCB ICSR: VECTPENDING Position */
+#define SCB_ICSR_VECTPENDING_Msk (0x1FFUL << SCB_ICSR_VECTPENDING_Pos) /*!< SCB ICSR: VECTPENDING Mask */
+
+#define SCB_ICSR_RETTOBASE_Pos 11 /*!< SCB ICSR: RETTOBASE Position */
+#define SCB_ICSR_RETTOBASE_Msk (1UL << SCB_ICSR_RETTOBASE_Pos) /*!< SCB ICSR: RETTOBASE Mask */
+
+#define SCB_ICSR_VECTACTIVE_Pos 0 /*!< SCB ICSR: VECTACTIVE Position */
+#define SCB_ICSR_VECTACTIVE_Msk (0x1FFUL << SCB_ICSR_VECTACTIVE_Pos) /*!< SCB ICSR: VECTACTIVE Mask */
+
+/* SCB Vector Table Offset Register Definitions */
+#if (__CM3_REV < 0x0201) /* core r2p1 */
+#define SCB_VTOR_TBLBASE_Pos 29 /*!< SCB VTOR: TBLBASE Position */
+#define SCB_VTOR_TBLBASE_Msk (1UL << SCB_VTOR_TBLBASE_Pos) /*!< SCB VTOR: TBLBASE Mask */
+
+#define SCB_VTOR_TBLOFF_Pos 7 /*!< SCB VTOR: TBLOFF Position */
+#define SCB_VTOR_TBLOFF_Msk (0x3FFFFFUL << SCB_VTOR_TBLOFF_Pos) /*!< SCB VTOR: TBLOFF Mask */
+#else
+#define SCB_VTOR_TBLOFF_Pos 7 /*!< SCB VTOR: TBLOFF Position */
+#define SCB_VTOR_TBLOFF_Msk (0x1FFFFFFUL << SCB_VTOR_TBLOFF_Pos) /*!< SCB VTOR: TBLOFF Mask */
+#endif
+
+/* SCB Application Interrupt and Reset Control Register Definitions */
+#define SCB_AIRCR_VECTKEY_Pos 16 /*!< SCB AIRCR: VECTKEY Position */
+#define SCB_AIRCR_VECTKEY_Msk (0xFFFFUL << SCB_AIRCR_VECTKEY_Pos) /*!< SCB AIRCR: VECTKEY Mask */
+
+#define SCB_AIRCR_VECTKEYSTAT_Pos 16 /*!< SCB AIRCR: VECTKEYSTAT Position */
+#define SCB_AIRCR_VECTKEYSTAT_Msk (0xFFFFUL << SCB_AIRCR_VECTKEYSTAT_Pos) /*!< SCB AIRCR: VECTKEYSTAT Mask */
+
+#define SCB_AIRCR_ENDIANESS_Pos 15 /*!< SCB AIRCR: ENDIANESS Position */
+#define SCB_AIRCR_ENDIANESS_Msk (1UL << SCB_AIRCR_ENDIANESS_Pos) /*!< SCB AIRCR: ENDIANESS Mask */
+
+#define SCB_AIRCR_PRIGROUP_Pos 8 /*!< SCB AIRCR: PRIGROUP Position */
+#define SCB_AIRCR_PRIGROUP_Msk (7UL << SCB_AIRCR_PRIGROUP_Pos) /*!< SCB AIRCR: PRIGROUP Mask */
+
+#define SCB_AIRCR_SYSRESETREQ_Pos 2 /*!< SCB AIRCR: SYSRESETREQ Position */
+#define SCB_AIRCR_SYSRESETREQ_Msk (1UL << SCB_AIRCR_SYSRESETREQ_Pos) /*!< SCB AIRCR: SYSRESETREQ Mask */
+
+#define SCB_AIRCR_VECTCLRACTIVE_Pos 1 /*!< SCB AIRCR: VECTCLRACTIVE Position */
+#define SCB_AIRCR_VECTCLRACTIVE_Msk (1UL << SCB_AIRCR_VECTCLRACTIVE_Pos) /*!< SCB AIRCR: VECTCLRACTIVE Mask */
+
+#define SCB_AIRCR_VECTRESET_Pos 0 /*!< SCB AIRCR: VECTRESET Position */
+#define SCB_AIRCR_VECTRESET_Msk (1UL << SCB_AIRCR_VECTRESET_Pos) /*!< SCB AIRCR: VECTRESET Mask */
+
+/* SCB System Control Register Definitions */
+#define SCB_SCR_SEVONPEND_Pos 4 /*!< SCB SCR: SEVONPEND Position */
+#define SCB_SCR_SEVONPEND_Msk (1UL << SCB_SCR_SEVONPEND_Pos) /*!< SCB SCR: SEVONPEND Mask */
+
+#define SCB_SCR_SLEEPDEEP_Pos 2 /*!< SCB SCR: SLEEPDEEP Position */
+#define SCB_SCR_SLEEPDEEP_Msk (1UL << SCB_SCR_SLEEPDEEP_Pos) /*!< SCB SCR: SLEEPDEEP Mask */
+
+#define SCB_SCR_SLEEPONEXIT_Pos 1 /*!< SCB SCR: SLEEPONEXIT Position */
+#define SCB_SCR_SLEEPONEXIT_Msk (1UL << SCB_SCR_SLEEPONEXIT_Pos) /*!< SCB SCR: SLEEPONEXIT Mask */
+
+/* SCB Configuration Control Register Definitions */
+#define SCB_CCR_STKALIGN_Pos 9 /*!< SCB CCR: STKALIGN Position */
+#define SCB_CCR_STKALIGN_Msk (1UL << SCB_CCR_STKALIGN_Pos) /*!< SCB CCR: STKALIGN Mask */
+
+#define SCB_CCR_BFHFNMIGN_Pos 8 /*!< SCB CCR: BFHFNMIGN Position */
+#define SCB_CCR_BFHFNMIGN_Msk (1UL << SCB_CCR_BFHFNMIGN_Pos) /*!< SCB CCR: BFHFNMIGN Mask */
+
+#define SCB_CCR_DIV_0_TRP_Pos 4 /*!< SCB CCR: DIV_0_TRP Position */
+#define SCB_CCR_DIV_0_TRP_Msk (1UL << SCB_CCR_DIV_0_TRP_Pos) /*!< SCB CCR: DIV_0_TRP Mask */
+
+#define SCB_CCR_UNALIGN_TRP_Pos 3 /*!< SCB CCR: UNALIGN_TRP Position */
+#define SCB_CCR_UNALIGN_TRP_Msk (1UL << SCB_CCR_UNALIGN_TRP_Pos) /*!< SCB CCR: UNALIGN_TRP Mask */
+
+#define SCB_CCR_USERSETMPEND_Pos 1 /*!< SCB CCR: USERSETMPEND Position */
+#define SCB_CCR_USERSETMPEND_Msk (1UL << SCB_CCR_USERSETMPEND_Pos) /*!< SCB CCR: USERSETMPEND Mask */
+
+#define SCB_CCR_NONBASETHRDENA_Pos 0 /*!< SCB CCR: NONBASETHRDENA Position */
+#define SCB_CCR_NONBASETHRDENA_Msk (1UL << SCB_CCR_NONBASETHRDENA_Pos) /*!< SCB CCR: NONBASETHRDENA Mask */
+
+/* SCB System Handler Control and State Register Definitions */
+#define SCB_SHCSR_USGFAULTENA_Pos 18 /*!< SCB SHCSR: USGFAULTENA Position */
+#define SCB_SHCSR_USGFAULTENA_Msk (1UL << SCB_SHCSR_USGFAULTENA_Pos) /*!< SCB SHCSR: USGFAULTENA Mask */
+
+#define SCB_SHCSR_BUSFAULTENA_Pos 17 /*!< SCB SHCSR: BUSFAULTENA Position */
+#define SCB_SHCSR_BUSFAULTENA_Msk (1UL << SCB_SHCSR_BUSFAULTENA_Pos) /*!< SCB SHCSR: BUSFAULTENA Mask */
+
+#define SCB_SHCSR_MEMFAULTENA_Pos 16 /*!< SCB SHCSR: MEMFAULTENA Position */
+#define SCB_SHCSR_MEMFAULTENA_Msk (1UL << SCB_SHCSR_MEMFAULTENA_Pos) /*!< SCB SHCSR: MEMFAULTENA Mask */
+
+#define SCB_SHCSR_SVCALLPENDED_Pos 15 /*!< SCB SHCSR: SVCALLPENDED Position */
+#define SCB_SHCSR_SVCALLPENDED_Msk (1UL << SCB_SHCSR_SVCALLPENDED_Pos) /*!< SCB SHCSR: SVCALLPENDED Mask */
+
+#define SCB_SHCSR_BUSFAULTPENDED_Pos 14 /*!< SCB SHCSR: BUSFAULTPENDED Position */
+#define SCB_SHCSR_BUSFAULTPENDED_Msk (1UL << SCB_SHCSR_BUSFAULTPENDED_Pos) /*!< SCB SHCSR: BUSFAULTPENDED Mask */
+
+#define SCB_SHCSR_MEMFAULTPENDED_Pos 13 /*!< SCB SHCSR: MEMFAULTPENDED Position */
+#define SCB_SHCSR_MEMFAULTPENDED_Msk (1UL << SCB_SHCSR_MEMFAULTPENDED_Pos) /*!< SCB SHCSR: MEMFAULTPENDED Mask */
+
+#define SCB_SHCSR_USGFAULTPENDED_Pos 12 /*!< SCB SHCSR: USGFAULTPENDED Position */
+#define SCB_SHCSR_USGFAULTPENDED_Msk (1UL << SCB_SHCSR_USGFAULTPENDED_Pos) /*!< SCB SHCSR: USGFAULTPENDED Mask */
+
+#define SCB_SHCSR_SYSTICKACT_Pos 11 /*!< SCB SHCSR: SYSTICKACT Position */
+#define SCB_SHCSR_SYSTICKACT_Msk (1UL << SCB_SHCSR_SYSTICKACT_Pos) /*!< SCB SHCSR: SYSTICKACT Mask */
+
+#define SCB_SHCSR_PENDSVACT_Pos 10 /*!< SCB SHCSR: PENDSVACT Position */
+#define SCB_SHCSR_PENDSVACT_Msk (1UL << SCB_SHCSR_PENDSVACT_Pos) /*!< SCB SHCSR: PENDSVACT Mask */
+
+#define SCB_SHCSR_MONITORACT_Pos 8 /*!< SCB SHCSR: MONITORACT Position */
+#define SCB_SHCSR_MONITORACT_Msk (1UL << SCB_SHCSR_MONITORACT_Pos) /*!< SCB SHCSR: MONITORACT Mask */
+
+#define SCB_SHCSR_SVCALLACT_Pos 7 /*!< SCB SHCSR: SVCALLACT Position */
+#define SCB_SHCSR_SVCALLACT_Msk (1UL << SCB_SHCSR_SVCALLACT_Pos) /*!< SCB SHCSR: SVCALLACT Mask */
+
+#define SCB_SHCSR_USGFAULTACT_Pos 3 /*!< SCB SHCSR: USGFAULTACT Position */
+#define SCB_SHCSR_USGFAULTACT_Msk (1UL << SCB_SHCSR_USGFAULTACT_Pos) /*!< SCB SHCSR: USGFAULTACT Mask */
+
+#define SCB_SHCSR_BUSFAULTACT_Pos 1 /*!< SCB SHCSR: BUSFAULTACT Position */
+#define SCB_SHCSR_BUSFAULTACT_Msk (1UL << SCB_SHCSR_BUSFAULTACT_Pos) /*!< SCB SHCSR: BUSFAULTACT Mask */
+
+#define SCB_SHCSR_MEMFAULTACT_Pos 0 /*!< SCB SHCSR: MEMFAULTACT Position */
+#define SCB_SHCSR_MEMFAULTACT_Msk (1UL << SCB_SHCSR_MEMFAULTACT_Pos) /*!< SCB SHCSR: MEMFAULTACT Mask */
+
+/* SCB Configurable Fault Status Registers Definitions */
+#define SCB_CFSR_USGFAULTSR_Pos 16 /*!< SCB CFSR: Usage Fault Status Register Position */
+#define SCB_CFSR_USGFAULTSR_Msk (0xFFFFUL << SCB_CFSR_USGFAULTSR_Pos) /*!< SCB CFSR: Usage Fault Status Register Mask */
+
+#define SCB_CFSR_BUSFAULTSR_Pos 8 /*!< SCB CFSR: Bus Fault Status Register Position */
+#define SCB_CFSR_BUSFAULTSR_Msk (0xFFUL << SCB_CFSR_BUSFAULTSR_Pos) /*!< SCB CFSR: Bus Fault Status Register Mask */
+
+#define SCB_CFSR_MEMFAULTSR_Pos 0 /*!< SCB CFSR: Memory Manage Fault Status Register Position */
+#define SCB_CFSR_MEMFAULTSR_Msk (0xFFUL << SCB_CFSR_MEMFAULTSR_Pos) /*!< SCB CFSR: Memory Manage Fault Status Register Mask */
+
+/* SCB Hard Fault Status Registers Definitions */
+#define SCB_HFSR_DEBUGEVT_Pos 31 /*!< SCB HFSR: DEBUGEVT Position */
+#define SCB_HFSR_DEBUGEVT_Msk (1UL << SCB_HFSR_DEBUGEVT_Pos) /*!< SCB HFSR: DEBUGEVT Mask */
+
+#define SCB_HFSR_FORCED_Pos 30 /*!< SCB HFSR: FORCED Position */
+#define SCB_HFSR_FORCED_Msk (1UL << SCB_HFSR_FORCED_Pos) /*!< SCB HFSR: FORCED Mask */
+
+#define SCB_HFSR_VECTTBL_Pos 1 /*!< SCB HFSR: VECTTBL Position */
+#define SCB_HFSR_VECTTBL_Msk (1UL << SCB_HFSR_VECTTBL_Pos) /*!< SCB HFSR: VECTTBL Mask */
+
+/* SCB Debug Fault Status Register Definitions */
+#define SCB_DFSR_EXTERNAL_Pos 4 /*!< SCB DFSR: EXTERNAL Position */
+#define SCB_DFSR_EXTERNAL_Msk (1UL << SCB_DFSR_EXTERNAL_Pos) /*!< SCB DFSR: EXTERNAL Mask */
+
+#define SCB_DFSR_VCATCH_Pos 3 /*!< SCB DFSR: VCATCH Position */
+#define SCB_DFSR_VCATCH_Msk (1UL << SCB_DFSR_VCATCH_Pos) /*!< SCB DFSR: VCATCH Mask */
+
+#define SCB_DFSR_DWTTRAP_Pos 2 /*!< SCB DFSR: DWTTRAP Position */
+#define SCB_DFSR_DWTTRAP_Msk (1UL << SCB_DFSR_DWTTRAP_Pos) /*!< SCB DFSR: DWTTRAP Mask */
+
+#define SCB_DFSR_BKPT_Pos 1 /*!< SCB DFSR: BKPT Position */
+#define SCB_DFSR_BKPT_Msk (1UL << SCB_DFSR_BKPT_Pos) /*!< SCB DFSR: BKPT Mask */
+
+#define SCB_DFSR_HALTED_Pos 0 /*!< SCB DFSR: HALTED Position */
+#define SCB_DFSR_HALTED_Msk (1UL << SCB_DFSR_HALTED_Pos) /*!< SCB DFSR: HALTED Mask */
+
+/*@} end of group CMSIS_SCB */
+
+
+/** \ingroup CMSIS_core_register
+ \defgroup CMSIS_SCnSCB System Controls not in SCB (SCnSCB)
+ \brief Type definitions for the System Control and ID Register not in the SCB
+ @{
+ */
+
+/** \brief Structure type to access the System Control and ID Register not in the SCB.
+ */
+typedef struct
+{
+ uint32_t RESERVED0[1];
+ __I uint32_t ICTR; /*!< Offset: 0x004 (R/ ) Interrupt Controller Type Register */
+#if ((defined __CM3_REV) && (__CM3_REV >= 0x200))
+ __IO uint32_t ACTLR; /*!< Offset: 0x008 (R/W) Auxiliary Control Register */
+#else
+ uint32_t RESERVED1[1];
+#endif
+} SCnSCB_Type;
+
+/* Interrupt Controller Type Register Definitions */
+#define SCnSCB_ICTR_INTLINESNUM_Pos 0 /*!< ICTR: INTLINESNUM Position */
+#define SCnSCB_ICTR_INTLINESNUM_Msk (0xFUL << SCnSCB_ICTR_INTLINESNUM_Pos) /*!< ICTR: INTLINESNUM Mask */
+
+/* Auxiliary Control Register Definitions */
+
+#define SCnSCB_ACTLR_DISFOLD_Pos 2 /*!< ACTLR: DISFOLD Position */
+#define SCnSCB_ACTLR_DISFOLD_Msk (1UL << SCnSCB_ACTLR_DISFOLD_Pos) /*!< ACTLR: DISFOLD Mask */
+
+#define SCnSCB_ACTLR_DISDEFWBUF_Pos 1 /*!< ACTLR: DISDEFWBUF Position */
+#define SCnSCB_ACTLR_DISDEFWBUF_Msk (1UL << SCnSCB_ACTLR_DISDEFWBUF_Pos) /*!< ACTLR: DISDEFWBUF Mask */
+
+#define SCnSCB_ACTLR_DISMCYCINT_Pos 0 /*!< ACTLR: DISMCYCINT Position */
+#define SCnSCB_ACTLR_DISMCYCINT_Msk (1UL << SCnSCB_ACTLR_DISMCYCINT_Pos) /*!< ACTLR: DISMCYCINT Mask */
+
+/*@} end of group CMSIS_SCnotSCB */
+
+
+/** \ingroup CMSIS_core_register
+ \defgroup CMSIS_SysTick System Tick Timer (SysTick)
+ \brief Type definitions for the System Timer Registers.
+ @{
+ */
+
+/** \brief Structure type to access the System Timer (SysTick).
+ */
+typedef struct
+{
+ __IO uint32_t CTRL; /*!< Offset: 0x000 (R/W) SysTick Control and Status Register */
+ __IO uint32_t LOAD; /*!< Offset: 0x004 (R/W) SysTick Reload Value Register */
+ __IO uint32_t VAL; /*!< Offset: 0x008 (R/W) SysTick Current Value Register */
+ __I uint32_t CALIB; /*!< Offset: 0x00C (R/ ) SysTick Calibration Register */
+} SysTick_Type;
+
+/* SysTick Control / Status Register Definitions */
+#define SysTick_CTRL_COUNTFLAG_Pos 16 /*!< SysTick CTRL: COUNTFLAG Position */
+#define SysTick_CTRL_COUNTFLAG_Msk (1UL << SysTick_CTRL_COUNTFLAG_Pos) /*!< SysTick CTRL: COUNTFLAG Mask */
+
+#define SysTick_CTRL_CLKSOURCE_Pos 2 /*!< SysTick CTRL: CLKSOURCE Position */
+#define SysTick_CTRL_CLKSOURCE_Msk (1UL << SysTick_CTRL_CLKSOURCE_Pos) /*!< SysTick CTRL: CLKSOURCE Mask */
+
+#define SysTick_CTRL_TICKINT_Pos 1 /*!< SysTick CTRL: TICKINT Position */
+#define SysTick_CTRL_TICKINT_Msk (1UL << SysTick_CTRL_TICKINT_Pos) /*!< SysTick CTRL: TICKINT Mask */
+
+#define SysTick_CTRL_ENABLE_Pos 0 /*!< SysTick CTRL: ENABLE Position */
+#define SysTick_CTRL_ENABLE_Msk (1UL << SysTick_CTRL_ENABLE_Pos) /*!< SysTick CTRL: ENABLE Mask */
+
+/* SysTick Reload Register Definitions */
+#define SysTick_LOAD_RELOAD_Pos 0 /*!< SysTick LOAD: RELOAD Position */
+#define SysTick_LOAD_RELOAD_Msk (0xFFFFFFUL << SysTick_LOAD_RELOAD_Pos) /*!< SysTick LOAD: RELOAD Mask */
+
+/* SysTick Current Register Definitions */
+#define SysTick_VAL_CURRENT_Pos 0 /*!< SysTick VAL: CURRENT Position */
+#define SysTick_VAL_CURRENT_Msk (0xFFFFFFUL << SysTick_VAL_CURRENT_Pos) /*!< SysTick VAL: CURRENT Mask */
+
+/* SysTick Calibration Register Definitions */
+#define SysTick_CALIB_NOREF_Pos 31 /*!< SysTick CALIB: NOREF Position */
+#define SysTick_CALIB_NOREF_Msk (1UL << SysTick_CALIB_NOREF_Pos) /*!< SysTick CALIB: NOREF Mask */
+
+#define SysTick_CALIB_SKEW_Pos 30 /*!< SysTick CALIB: SKEW Position */
+#define SysTick_CALIB_SKEW_Msk (1UL << SysTick_CALIB_SKEW_Pos) /*!< SysTick CALIB: SKEW Mask */
+
+#define SysTick_CALIB_TENMS_Pos 0 /*!< SysTick CALIB: TENMS Position */
+#define SysTick_CALIB_TENMS_Msk (0xFFFFFFUL << SysTick_VAL_CURRENT_Pos) /*!< SysTick CALIB: TENMS Mask */
+
+/*@} end of group CMSIS_SysTick */
+
+
+/** \ingroup CMSIS_core_register
+ \defgroup CMSIS_ITM Instrumentation Trace Macrocell (ITM)
+ \brief Type definitions for the Instrumentation Trace Macrocell (ITM)
+ @{
+ */
+
+/** \brief Structure type to access the Instrumentation Trace Macrocell Register (ITM).
+ */
+typedef struct
+{
+ __O union
+ {
+ __O uint8_t u8; /*!< Offset: 0x000 ( /W) ITM Stimulus Port 8-bit */
+ __O uint16_t u16; /*!< Offset: 0x000 ( /W) ITM Stimulus Port 16-bit */
+ __O uint32_t u32; /*!< Offset: 0x000 ( /W) ITM Stimulus Port 32-bit */
+ } PORT [32]; /*!< Offset: 0x000 ( /W) ITM Stimulus Port Registers */
+ uint32_t RESERVED0[864];
+ __IO uint32_t TER; /*!< Offset: 0xE00 (R/W) ITM Trace Enable Register */
+ uint32_t RESERVED1[15];
+ __IO uint32_t TPR; /*!< Offset: 0xE40 (R/W) ITM Trace Privilege Register */
+ uint32_t RESERVED2[15];
+ __IO uint32_t TCR; /*!< Offset: 0xE80 (R/W) ITM Trace Control Register */
+ uint32_t RESERVED3[29];
+ __O uint32_t IWR; /*!< Offset: 0xEF8 ( /W) ITM Integration Write Register */
+ __I uint32_t IRR; /*!< Offset: 0xEFC (R/ ) ITM Integration Read Register */
+ __IO uint32_t IMCR; /*!< Offset: 0xF00 (R/W) ITM Integration Mode Control Register */
+ uint32_t RESERVED4[43];
+ __O uint32_t LAR; /*!< Offset: 0xFB0 ( /W) ITM Lock Access Register */
+ __I uint32_t LSR; /*!< Offset: 0xFB4 (R/ ) ITM Lock Status Register */
+ uint32_t RESERVED5[6];
+ __I uint32_t PID4; /*!< Offset: 0xFD0 (R/ ) ITM Peripheral Identification Register #4 */
+ __I uint32_t PID5; /*!< Offset: 0xFD4 (R/ ) ITM Peripheral Identification Register #5 */
+ __I uint32_t PID6; /*!< Offset: 0xFD8 (R/ ) ITM Peripheral Identification Register #6 */
+ __I uint32_t PID7; /*!< Offset: 0xFDC (R/ ) ITM Peripheral Identification Register #7 */
+ __I uint32_t PID0; /*!< Offset: 0xFE0 (R/ ) ITM Peripheral Identification Register #0 */
+ __I uint32_t PID1; /*!< Offset: 0xFE4 (R/ ) ITM Peripheral Identification Register #1 */
+ __I uint32_t PID2; /*!< Offset: 0xFE8 (R/ ) ITM Peripheral Identification Register #2 */
+ __I uint32_t PID3; /*!< Offset: 0xFEC (R/ ) ITM Peripheral Identification Register #3 */
+ __I uint32_t CID0; /*!< Offset: 0xFF0 (R/ ) ITM Component Identification Register #0 */
+ __I uint32_t CID1; /*!< Offset: 0xFF4 (R/ ) ITM Component Identification Register #1 */
+ __I uint32_t CID2; /*!< Offset: 0xFF8 (R/ ) ITM Component Identification Register #2 */
+ __I uint32_t CID3; /*!< Offset: 0xFFC (R/ ) ITM Component Identification Register #3 */
+} ITM_Type;
+
+/* ITM Trace Privilege Register Definitions */
+#define ITM_TPR_PRIVMASK_Pos 0 /*!< ITM TPR: PRIVMASK Position */
+#define ITM_TPR_PRIVMASK_Msk (0xFUL << ITM_TPR_PRIVMASK_Pos) /*!< ITM TPR: PRIVMASK Mask */
+
+/* ITM Trace Control Register Definitions */
+#define ITM_TCR_BUSY_Pos 23 /*!< ITM TCR: BUSY Position */
+#define ITM_TCR_BUSY_Msk (1UL << ITM_TCR_BUSY_Pos) /*!< ITM TCR: BUSY Mask */
+
+#define ITM_TCR_TraceBusID_Pos 16 /*!< ITM TCR: ATBID Position */
+#define ITM_TCR_TraceBusID_Msk (0x7FUL << ITM_TCR_TraceBusID_Pos) /*!< ITM TCR: ATBID Mask */
+
+#define ITM_TCR_GTSFREQ_Pos 10 /*!< ITM TCR: Global timestamp frequency Position */
+#define ITM_TCR_GTSFREQ_Msk (3UL << ITM_TCR_GTSFREQ_Pos) /*!< ITM TCR: Global timestamp frequency Mask */
+
+#define ITM_TCR_TSPrescale_Pos 8 /*!< ITM TCR: TSPrescale Position */
+#define ITM_TCR_TSPrescale_Msk (3UL << ITM_TCR_TSPrescale_Pos) /*!< ITM TCR: TSPrescale Mask */
+
+#define ITM_TCR_SWOENA_Pos 4 /*!< ITM TCR: SWOENA Position */
+#define ITM_TCR_SWOENA_Msk (1UL << ITM_TCR_SWOENA_Pos) /*!< ITM TCR: SWOENA Mask */
+
+#define ITM_TCR_DWTENA_Pos 3 /*!< ITM TCR: DWTENA Position */
+#define ITM_TCR_DWTENA_Msk (1UL << ITM_TCR_DWTENA_Pos) /*!< ITM TCR: DWTENA Mask */
+
+#define ITM_TCR_SYNCENA_Pos 2 /*!< ITM TCR: SYNCENA Position */
+#define ITM_TCR_SYNCENA_Msk (1UL << ITM_TCR_SYNCENA_Pos) /*!< ITM TCR: SYNCENA Mask */
+
+#define ITM_TCR_TSENA_Pos 1 /*!< ITM TCR: TSENA Position */
+#define ITM_TCR_TSENA_Msk (1UL << ITM_TCR_TSENA_Pos) /*!< ITM TCR: TSENA Mask */
+
+#define ITM_TCR_ITMENA_Pos 0 /*!< ITM TCR: ITM Enable bit Position */
+#define ITM_TCR_ITMENA_Msk (1UL << ITM_TCR_ITMENA_Pos) /*!< ITM TCR: ITM Enable bit Mask */
+
+/* ITM Integration Write Register Definitions */
+#define ITM_IWR_ATVALIDM_Pos 0 /*!< ITM IWR: ATVALIDM Position */
+#define ITM_IWR_ATVALIDM_Msk (1UL << ITM_IWR_ATVALIDM_Pos) /*!< ITM IWR: ATVALIDM Mask */
+
+/* ITM Integration Read Register Definitions */
+#define ITM_IRR_ATREADYM_Pos 0 /*!< ITM IRR: ATREADYM Position */
+#define ITM_IRR_ATREADYM_Msk (1UL << ITM_IRR_ATREADYM_Pos) /*!< ITM IRR: ATREADYM Mask */
+
+/* ITM Integration Mode Control Register Definitions */
+#define ITM_IMCR_INTEGRATION_Pos 0 /*!< ITM IMCR: INTEGRATION Position */
+#define ITM_IMCR_INTEGRATION_Msk (1UL << ITM_IMCR_INTEGRATION_Pos) /*!< ITM IMCR: INTEGRATION Mask */
+
+/* ITM Lock Status Register Definitions */
+#define ITM_LSR_ByteAcc_Pos 2 /*!< ITM LSR: ByteAcc Position */
+#define ITM_LSR_ByteAcc_Msk (1UL << ITM_LSR_ByteAcc_Pos) /*!< ITM LSR: ByteAcc Mask */
+
+#define ITM_LSR_Access_Pos 1 /*!< ITM LSR: Access Position */
+#define ITM_LSR_Access_Msk (1UL << ITM_LSR_Access_Pos) /*!< ITM LSR: Access Mask */
+
+#define ITM_LSR_Present_Pos 0 /*!< ITM LSR: Present Position */
+#define ITM_LSR_Present_Msk (1UL << ITM_LSR_Present_Pos) /*!< ITM LSR: Present Mask */
+
+/*@}*/ /* end of group CMSIS_ITM */
+
+
+/** \ingroup CMSIS_core_register
+ \defgroup CMSIS_DWT Data Watchpoint and Trace (DWT)
+ \brief Type definitions for the Data Watchpoint and Trace (DWT)
+ @{
+ */
+
+/** \brief Structure type to access the Data Watchpoint and Trace Register (DWT).
+ */
+typedef struct
+{
+ __IO uint32_t CTRL; /*!< Offset: 0x000 (R/W) Control Register */
+ __IO uint32_t CYCCNT; /*!< Offset: 0x004 (R/W) Cycle Count Register */
+ __IO uint32_t CPICNT; /*!< Offset: 0x008 (R/W) CPI Count Register */
+ __IO uint32_t EXCCNT; /*!< Offset: 0x00C (R/W) Exception Overhead Count Register */
+ __IO uint32_t SLEEPCNT; /*!< Offset: 0x010 (R/W) Sleep Count Register */
+ __IO uint32_t LSUCNT; /*!< Offset: 0x014 (R/W) LSU Count Register */
+ __IO uint32_t FOLDCNT; /*!< Offset: 0x018 (R/W) Folded-instruction Count Register */
+ __I uint32_t PCSR; /*!< Offset: 0x01C (R/ ) Program Counter Sample Register */
+ __IO uint32_t COMP0; /*!< Offset: 0x020 (R/W) Comparator Register 0 */
+ __IO uint32_t MASK0; /*!< Offset: 0x024 (R/W) Mask Register 0 */
+ __IO uint32_t FUNCTION0; /*!< Offset: 0x028 (R/W) Function Register 0 */
+ uint32_t RESERVED0[1];
+ __IO uint32_t COMP1; /*!< Offset: 0x030 (R/W) Comparator Register 1 */
+ __IO uint32_t MASK1; /*!< Offset: 0x034 (R/W) Mask Register 1 */
+ __IO uint32_t FUNCTION1; /*!< Offset: 0x038 (R/W) Function Register 1 */
+ uint32_t RESERVED1[1];
+ __IO uint32_t COMP2; /*!< Offset: 0x040 (R/W) Comparator Register 2 */
+ __IO uint32_t MASK2; /*!< Offset: 0x044 (R/W) Mask Register 2 */
+ __IO uint32_t FUNCTION2; /*!< Offset: 0x048 (R/W) Function Register 2 */
+ uint32_t RESERVED2[1];
+ __IO uint32_t COMP3; /*!< Offset: 0x050 (R/W) Comparator Register 3 */
+ __IO uint32_t MASK3; /*!< Offset: 0x054 (R/W) Mask Register 3 */
+ __IO uint32_t FUNCTION3; /*!< Offset: 0x058 (R/W) Function Register 3 */
+} DWT_Type;
+
+/* DWT Control Register Definitions */
+#define DWT_CTRL_NUMCOMP_Pos 28 /*!< DWT CTRL: NUMCOMP Position */
+#define DWT_CTRL_NUMCOMP_Msk (0xFUL << DWT_CTRL_NUMCOMP_Pos) /*!< DWT CTRL: NUMCOMP Mask */
+
+#define DWT_CTRL_NOTRCPKT_Pos 27 /*!< DWT CTRL: NOTRCPKT Position */
+#define DWT_CTRL_NOTRCPKT_Msk (0x1UL << DWT_CTRL_NOTRCPKT_Pos) /*!< DWT CTRL: NOTRCPKT Mask */
+
+#define DWT_CTRL_NOEXTTRIG_Pos 26 /*!< DWT CTRL: NOEXTTRIG Position */
+#define DWT_CTRL_NOEXTTRIG_Msk (0x1UL << DWT_CTRL_NOEXTTRIG_Pos) /*!< DWT CTRL: NOEXTTRIG Mask */
+
+#define DWT_CTRL_NOCYCCNT_Pos 25 /*!< DWT CTRL: NOCYCCNT Position */
+#define DWT_CTRL_NOCYCCNT_Msk (0x1UL << DWT_CTRL_NOCYCCNT_Pos) /*!< DWT CTRL: NOCYCCNT Mask */
+
+#define DWT_CTRL_NOPRFCNT_Pos 24 /*!< DWT CTRL: NOPRFCNT Position */
+#define DWT_CTRL_NOPRFCNT_Msk (0x1UL << DWT_CTRL_NOPRFCNT_Pos) /*!< DWT CTRL: NOPRFCNT Mask */
+
+#define DWT_CTRL_CYCEVTENA_Pos 22 /*!< DWT CTRL: CYCEVTENA Position */
+#define DWT_CTRL_CYCEVTENA_Msk (0x1UL << DWT_CTRL_CYCEVTENA_Pos) /*!< DWT CTRL: CYCEVTENA Mask */
+
+#define DWT_CTRL_FOLDEVTENA_Pos 21 /*!< DWT CTRL: FOLDEVTENA Position */
+#define DWT_CTRL_FOLDEVTENA_Msk (0x1UL << DWT_CTRL_FOLDEVTENA_Pos) /*!< DWT CTRL: FOLDEVTENA Mask */
+
+#define DWT_CTRL_LSUEVTENA_Pos 20 /*!< DWT CTRL: LSUEVTENA Position */
+#define DWT_CTRL_LSUEVTENA_Msk (0x1UL << DWT_CTRL_LSUEVTENA_Pos) /*!< DWT CTRL: LSUEVTENA Mask */
+
+#define DWT_CTRL_SLEEPEVTENA_Pos 19 /*!< DWT CTRL: SLEEPEVTENA Position */
+#define DWT_CTRL_SLEEPEVTENA_Msk (0x1UL << DWT_CTRL_SLEEPEVTENA_Pos) /*!< DWT CTRL: SLEEPEVTENA Mask */
+
+#define DWT_CTRL_EXCEVTENA_Pos 18 /*!< DWT CTRL: EXCEVTENA Position */
+#define DWT_CTRL_EXCEVTENA_Msk (0x1UL << DWT_CTRL_EXCEVTENA_Pos) /*!< DWT CTRL: EXCEVTENA Mask */
+
+#define DWT_CTRL_CPIEVTENA_Pos 17 /*!< DWT CTRL: CPIEVTENA Position */
+#define DWT_CTRL_CPIEVTENA_Msk (0x1UL << DWT_CTRL_CPIEVTENA_Pos) /*!< DWT CTRL: CPIEVTENA Mask */
+
+#define DWT_CTRL_EXCTRCENA_Pos 16 /*!< DWT CTRL: EXCTRCENA Position */
+#define DWT_CTRL_EXCTRCENA_Msk (0x1UL << DWT_CTRL_EXCTRCENA_Pos) /*!< DWT CTRL: EXCTRCENA Mask */
+
+#define DWT_CTRL_PCSAMPLENA_Pos 12 /*!< DWT CTRL: PCSAMPLENA Position */
+#define DWT_CTRL_PCSAMPLENA_Msk (0x1UL << DWT_CTRL_PCSAMPLENA_Pos) /*!< DWT CTRL: PCSAMPLENA Mask */
+
+#define DWT_CTRL_SYNCTAP_Pos 10 /*!< DWT CTRL: SYNCTAP Position */
+#define DWT_CTRL_SYNCTAP_Msk (0x3UL << DWT_CTRL_SYNCTAP_Pos) /*!< DWT CTRL: SYNCTAP Mask */
+
+#define DWT_CTRL_CYCTAP_Pos 9 /*!< DWT CTRL: CYCTAP Position */
+#define DWT_CTRL_CYCTAP_Msk (0x1UL << DWT_CTRL_CYCTAP_Pos) /*!< DWT CTRL: CYCTAP Mask */
+
+#define DWT_CTRL_POSTINIT_Pos 5 /*!< DWT CTRL: POSTINIT Position */
+#define DWT_CTRL_POSTINIT_Msk (0xFUL << DWT_CTRL_POSTINIT_Pos) /*!< DWT CTRL: POSTINIT Mask */
+
+#define DWT_CTRL_POSTPRESET_Pos 1 /*!< DWT CTRL: POSTPRESET Position */
+#define DWT_CTRL_POSTPRESET_Msk (0xFUL << DWT_CTRL_POSTPRESET_Pos) /*!< DWT CTRL: POSTPRESET Mask */
+
+#define DWT_CTRL_CYCCNTENA_Pos 0 /*!< DWT CTRL: CYCCNTENA Position */
+#define DWT_CTRL_CYCCNTENA_Msk (0x1UL << DWT_CTRL_CYCCNTENA_Pos) /*!< DWT CTRL: CYCCNTENA Mask */
+
+/* DWT CPI Count Register Definitions */
+#define DWT_CPICNT_CPICNT_Pos 0 /*!< DWT CPICNT: CPICNT Position */
+#define DWT_CPICNT_CPICNT_Msk (0xFFUL << DWT_CPICNT_CPICNT_Pos) /*!< DWT CPICNT: CPICNT Mask */
+
+/* DWT Exception Overhead Count Register Definitions */
+#define DWT_EXCCNT_EXCCNT_Pos 0 /*!< DWT EXCCNT: EXCCNT Position */
+#define DWT_EXCCNT_EXCCNT_Msk (0xFFUL << DWT_EXCCNT_EXCCNT_Pos) /*!< DWT EXCCNT: EXCCNT Mask */
+
+/* DWT Sleep Count Register Definitions */
+#define DWT_SLEEPCNT_SLEEPCNT_Pos 0 /*!< DWT SLEEPCNT: SLEEPCNT Position */
+#define DWT_SLEEPCNT_SLEEPCNT_Msk (0xFFUL << DWT_SLEEPCNT_SLEEPCNT_Pos) /*!< DWT SLEEPCNT: SLEEPCNT Mask */
+
+/* DWT LSU Count Register Definitions */
+#define DWT_LSUCNT_LSUCNT_Pos 0 /*!< DWT LSUCNT: LSUCNT Position */
+#define DWT_LSUCNT_LSUCNT_Msk (0xFFUL << DWT_LSUCNT_LSUCNT_Pos) /*!< DWT LSUCNT: LSUCNT Mask */
+
+/* DWT Folded-instruction Count Register Definitions */
+#define DWT_FOLDCNT_FOLDCNT_Pos 0 /*!< DWT FOLDCNT: FOLDCNT Position */
+#define DWT_FOLDCNT_FOLDCNT_Msk (0xFFUL << DWT_FOLDCNT_FOLDCNT_Pos) /*!< DWT FOLDCNT: FOLDCNT Mask */
+
+/* DWT Comparator Mask Register Definitions */
+#define DWT_MASK_MASK_Pos 0 /*!< DWT MASK: MASK Position */
+#define DWT_MASK_MASK_Msk (0x1FUL << DWT_MASK_MASK_Pos) /*!< DWT MASK: MASK Mask */
+
+/* DWT Comparator Function Register Definitions */
+#define DWT_FUNCTION_MATCHED_Pos 24 /*!< DWT FUNCTION: MATCHED Position */
+#define DWT_FUNCTION_MATCHED_Msk (0x1UL << DWT_FUNCTION_MATCHED_Pos) /*!< DWT FUNCTION: MATCHED Mask */
+
+#define DWT_FUNCTION_DATAVADDR1_Pos 16 /*!< DWT FUNCTION: DATAVADDR1 Position */
+#define DWT_FUNCTION_DATAVADDR1_Msk (0xFUL << DWT_FUNCTION_DATAVADDR1_Pos) /*!< DWT FUNCTION: DATAVADDR1 Mask */
+
+#define DWT_FUNCTION_DATAVADDR0_Pos 12 /*!< DWT FUNCTION: DATAVADDR0 Position */
+#define DWT_FUNCTION_DATAVADDR0_Msk (0xFUL << DWT_FUNCTION_DATAVADDR0_Pos) /*!< DWT FUNCTION: DATAVADDR0 Mask */
+
+#define DWT_FUNCTION_DATAVSIZE_Pos 10 /*!< DWT FUNCTION: DATAVSIZE Position */
+#define DWT_FUNCTION_DATAVSIZE_Msk (0x3UL << DWT_FUNCTION_DATAVSIZE_Pos) /*!< DWT FUNCTION: DATAVSIZE Mask */
+
+#define DWT_FUNCTION_LNK1ENA_Pos 9 /*!< DWT FUNCTION: LNK1ENA Position */
+#define DWT_FUNCTION_LNK1ENA_Msk (0x1UL << DWT_FUNCTION_LNK1ENA_Pos) /*!< DWT FUNCTION: LNK1ENA Mask */
+
+#define DWT_FUNCTION_DATAVMATCH_Pos 8 /*!< DWT FUNCTION: DATAVMATCH Position */
+#define DWT_FUNCTION_DATAVMATCH_Msk (0x1UL << DWT_FUNCTION_DATAVMATCH_Pos) /*!< DWT FUNCTION: DATAVMATCH Mask */
+
+#define DWT_FUNCTION_CYCMATCH_Pos 7 /*!< DWT FUNCTION: CYCMATCH Position */
+#define DWT_FUNCTION_CYCMATCH_Msk (0x1UL << DWT_FUNCTION_CYCMATCH_Pos) /*!< DWT FUNCTION: CYCMATCH Mask */
+
+#define DWT_FUNCTION_EMITRANGE_Pos 5 /*!< DWT FUNCTION: EMITRANGE Position */
+#define DWT_FUNCTION_EMITRANGE_Msk (0x1UL << DWT_FUNCTION_EMITRANGE_Pos) /*!< DWT FUNCTION: EMITRANGE Mask */
+
+#define DWT_FUNCTION_FUNCTION_Pos 0 /*!< DWT FUNCTION: FUNCTION Position */
+#define DWT_FUNCTION_FUNCTION_Msk (0xFUL << DWT_FUNCTION_FUNCTION_Pos) /*!< DWT FUNCTION: FUNCTION Mask */
+
+/*@}*/ /* end of group CMSIS_DWT */
+
+
+/** \ingroup CMSIS_core_register
+ \defgroup CMSIS_TPI Trace Port Interface (TPI)
+ \brief Type definitions for the Trace Port Interface (TPI)
+ @{
+ */
+
+/** \brief Structure type to access the Trace Port Interface Register (TPI).
+ */
+typedef struct
+{
+ __IO uint32_t SSPSR; /*!< Offset: 0x000 (R/ ) Supported Parallel Port Size Register */
+ __IO uint32_t CSPSR; /*!< Offset: 0x004 (R/W) Current Parallel Port Size Register */
+ uint32_t RESERVED0[2];
+ __IO uint32_t ACPR; /*!< Offset: 0x010 (R/W) Asynchronous Clock Prescaler Register */
+ uint32_t RESERVED1[55];
+ __IO uint32_t SPPR; /*!< Offset: 0x0F0 (R/W) Selected Pin Protocol Register */
+ uint32_t RESERVED2[131];
+ __I uint32_t FFSR; /*!< Offset: 0x300 (R/ ) Formatter and Flush Status Register */
+ __IO uint32_t FFCR; /*!< Offset: 0x304 (R/W) Formatter and Flush Control Register */
+ __I uint32_t FSCR; /*!< Offset: 0x308 (R/ ) Formatter Synchronization Counter Register */
+ uint32_t RESERVED3[759];
+ __I uint32_t TRIGGER; /*!< Offset: 0xEE8 (R/ ) TRIGGER */
+ __I uint32_t FIFO0; /*!< Offset: 0xEEC (R/ ) Integration ETM Data */
+ __I uint32_t ITATBCTR2; /*!< Offset: 0xEF0 (R/ ) ITATBCTR2 */
+ uint32_t RESERVED4[1];
+ __I uint32_t ITATBCTR0; /*!< Offset: 0xEF8 (R/ ) ITATBCTR0 */
+ __I uint32_t FIFO1; /*!< Offset: 0xEFC (R/ ) Integration ITM Data */
+ __IO uint32_t ITCTRL; /*!< Offset: 0xF00 (R/W) Integration Mode Control */
+ uint32_t RESERVED5[39];
+ __IO uint32_t CLAIMSET; /*!< Offset: 0xFA0 (R/W) Claim tag set */
+ __IO uint32_t CLAIMCLR; /*!< Offset: 0xFA4 (R/W) Claim tag clear */
+ uint32_t RESERVED7[8];
+ __I uint32_t DEVID; /*!< Offset: 0xFC8 (R/ ) TPIU_DEVID */
+ __I uint32_t DEVTYPE; /*!< Offset: 0xFCC (R/ ) TPIU_DEVTYPE */
+} TPI_Type;
+
+/* TPI Asynchronous Clock Prescaler Register Definitions */
+#define TPI_ACPR_PRESCALER_Pos 0 /*!< TPI ACPR: PRESCALER Position */
+#define TPI_ACPR_PRESCALER_Msk (0x1FFFUL << TPI_ACPR_PRESCALER_Pos) /*!< TPI ACPR: PRESCALER Mask */
+
+/* TPI Selected Pin Protocol Register Definitions */
+#define TPI_SPPR_TXMODE_Pos 0 /*!< TPI SPPR: TXMODE Position */
+#define TPI_SPPR_TXMODE_Msk (0x3UL << TPI_SPPR_TXMODE_Pos) /*!< TPI SPPR: TXMODE Mask */
+
+/* TPI Formatter and Flush Status Register Definitions */
+#define TPI_FFSR_FtNonStop_Pos 3 /*!< TPI FFSR: FtNonStop Position */
+#define TPI_FFSR_FtNonStop_Msk (0x1UL << TPI_FFSR_FtNonStop_Pos) /*!< TPI FFSR: FtNonStop Mask */
+
+#define TPI_FFSR_TCPresent_Pos 2 /*!< TPI FFSR: TCPresent Position */
+#define TPI_FFSR_TCPresent_Msk (0x1UL << TPI_FFSR_TCPresent_Pos) /*!< TPI FFSR: TCPresent Mask */
+
+#define TPI_FFSR_FtStopped_Pos 1 /*!< TPI FFSR: FtStopped Position */
+#define TPI_FFSR_FtStopped_Msk (0x1UL << TPI_FFSR_FtStopped_Pos) /*!< TPI FFSR: FtStopped Mask */
+
+#define TPI_FFSR_FlInProg_Pos 0 /*!< TPI FFSR: FlInProg Position */
+#define TPI_FFSR_FlInProg_Msk (0x1UL << TPI_FFSR_FlInProg_Pos) /*!< TPI FFSR: FlInProg Mask */
+
+/* TPI Formatter and Flush Control Register Definitions */
+#define TPI_FFCR_TrigIn_Pos 8 /*!< TPI FFCR: TrigIn Position */
+#define TPI_FFCR_TrigIn_Msk (0x1UL << TPI_FFCR_TrigIn_Pos) /*!< TPI FFCR: TrigIn Mask */
+
+#define TPI_FFCR_EnFCont_Pos 1 /*!< TPI FFCR: EnFCont Position */
+#define TPI_FFCR_EnFCont_Msk (0x1UL << TPI_FFCR_EnFCont_Pos) /*!< TPI FFCR: EnFCont Mask */
+
+/* TPI TRIGGER Register Definitions */
+#define TPI_TRIGGER_TRIGGER_Pos 0 /*!< TPI TRIGGER: TRIGGER Position */
+#define TPI_TRIGGER_TRIGGER_Msk (0x1UL << TPI_TRIGGER_TRIGGER_Pos) /*!< TPI TRIGGER: TRIGGER Mask */
+
+/* TPI Integration ETM Data Register Definitions (FIFO0) */
+#define TPI_FIFO0_ITM_ATVALID_Pos 29 /*!< TPI FIFO0: ITM_ATVALID Position */
+#define TPI_FIFO0_ITM_ATVALID_Msk (0x3UL << TPI_FIFO0_ITM_ATVALID_Pos) /*!< TPI FIFO0: ITM_ATVALID Mask */
+
+#define TPI_FIFO0_ITM_bytecount_Pos 27 /*!< TPI FIFO0: ITM_bytecount Position */
+#define TPI_FIFO0_ITM_bytecount_Msk (0x3UL << TPI_FIFO0_ITM_bytecount_Pos) /*!< TPI FIFO0: ITM_bytecount Mask */
+
+#define TPI_FIFO0_ETM_ATVALID_Pos 26 /*!< TPI FIFO0: ETM_ATVALID Position */
+#define TPI_FIFO0_ETM_ATVALID_Msk (0x3UL << TPI_FIFO0_ETM_ATVALID_Pos) /*!< TPI FIFO0: ETM_ATVALID Mask */
+
+#define TPI_FIFO0_ETM_bytecount_Pos 24 /*!< TPI FIFO0: ETM_bytecount Position */
+#define TPI_FIFO0_ETM_bytecount_Msk (0x3UL << TPI_FIFO0_ETM_bytecount_Pos) /*!< TPI FIFO0: ETM_bytecount Mask */
+
+#define TPI_FIFO0_ETM2_Pos 16 /*!< TPI FIFO0: ETM2 Position */
+#define TPI_FIFO0_ETM2_Msk (0xFFUL << TPI_FIFO0_ETM2_Pos) /*!< TPI FIFO0: ETM2 Mask */
+
+#define TPI_FIFO0_ETM1_Pos 8 /*!< TPI FIFO0: ETM1 Position */
+#define TPI_FIFO0_ETM1_Msk (0xFFUL << TPI_FIFO0_ETM1_Pos) /*!< TPI FIFO0: ETM1 Mask */
+
+#define TPI_FIFO0_ETM0_Pos 0 /*!< TPI FIFO0: ETM0 Position */
+#define TPI_FIFO0_ETM0_Msk (0xFFUL << TPI_FIFO0_ETM0_Pos) /*!< TPI FIFO0: ETM0 Mask */
+
+/* TPI ITATBCTR2 Register Definitions */
+#define TPI_ITATBCTR2_ATREADY_Pos 0 /*!< TPI ITATBCTR2: ATREADY Position */
+#define TPI_ITATBCTR2_ATREADY_Msk (0x1UL << TPI_ITATBCTR2_ATREADY_Pos) /*!< TPI ITATBCTR2: ATREADY Mask */
+
+/* TPI Integration ITM Data Register Definitions (FIFO1) */
+#define TPI_FIFO1_ITM_ATVALID_Pos 29 /*!< TPI FIFO1: ITM_ATVALID Position */
+#define TPI_FIFO1_ITM_ATVALID_Msk (0x3UL << TPI_FIFO1_ITM_ATVALID_Pos) /*!< TPI FIFO1: ITM_ATVALID Mask */
+
+#define TPI_FIFO1_ITM_bytecount_Pos 27 /*!< TPI FIFO1: ITM_bytecount Position */
+#define TPI_FIFO1_ITM_bytecount_Msk (0x3UL << TPI_FIFO1_ITM_bytecount_Pos) /*!< TPI FIFO1: ITM_bytecount Mask */
+
+#define TPI_FIFO1_ETM_ATVALID_Pos 26 /*!< TPI FIFO1: ETM_ATVALID Position */
+#define TPI_FIFO1_ETM_ATVALID_Msk (0x3UL << TPI_FIFO1_ETM_ATVALID_Pos) /*!< TPI FIFO1: ETM_ATVALID Mask */
+
+#define TPI_FIFO1_ETM_bytecount_Pos 24 /*!< TPI FIFO1: ETM_bytecount Position */
+#define TPI_FIFO1_ETM_bytecount_Msk (0x3UL << TPI_FIFO1_ETM_bytecount_Pos) /*!< TPI FIFO1: ETM_bytecount Mask */
+
+#define TPI_FIFO1_ITM2_Pos 16 /*!< TPI FIFO1: ITM2 Position */
+#define TPI_FIFO1_ITM2_Msk (0xFFUL << TPI_FIFO1_ITM2_Pos) /*!< TPI FIFO1: ITM2 Mask */
+
+#define TPI_FIFO1_ITM1_Pos 8 /*!< TPI FIFO1: ITM1 Position */
+#define TPI_FIFO1_ITM1_Msk (0xFFUL << TPI_FIFO1_ITM1_Pos) /*!< TPI FIFO1: ITM1 Mask */
+
+#define TPI_FIFO1_ITM0_Pos 0 /*!< TPI FIFO1: ITM0 Position */
+#define TPI_FIFO1_ITM0_Msk (0xFFUL << TPI_FIFO1_ITM0_Pos) /*!< TPI FIFO1: ITM0 Mask */
+
+/* TPI ITATBCTR0 Register Definitions */
+#define TPI_ITATBCTR0_ATREADY_Pos 0 /*!< TPI ITATBCTR0: ATREADY Position */
+#define TPI_ITATBCTR0_ATREADY_Msk (0x1UL << TPI_ITATBCTR0_ATREADY_Pos) /*!< TPI ITATBCTR0: ATREADY Mask */
+
+/* TPI Integration Mode Control Register Definitions */
+#define TPI_ITCTRL_Mode_Pos 0 /*!< TPI ITCTRL: Mode Position */
+#define TPI_ITCTRL_Mode_Msk (0x1UL << TPI_ITCTRL_Mode_Pos) /*!< TPI ITCTRL: Mode Mask */
+
+/* TPI DEVID Register Definitions */
+#define TPI_DEVID_NRZVALID_Pos 11 /*!< TPI DEVID: NRZVALID Position */
+#define TPI_DEVID_NRZVALID_Msk (0x1UL << TPI_DEVID_NRZVALID_Pos) /*!< TPI DEVID: NRZVALID Mask */
+
+#define TPI_DEVID_MANCVALID_Pos 10 /*!< TPI DEVID: MANCVALID Position */
+#define TPI_DEVID_MANCVALID_Msk (0x1UL << TPI_DEVID_MANCVALID_Pos) /*!< TPI DEVID: MANCVALID Mask */
+
+#define TPI_DEVID_PTINVALID_Pos 9 /*!< TPI DEVID: PTINVALID Position */
+#define TPI_DEVID_PTINVALID_Msk (0x1UL << TPI_DEVID_PTINVALID_Pos) /*!< TPI DEVID: PTINVALID Mask */
+
+#define TPI_DEVID_MinBufSz_Pos 6 /*!< TPI DEVID: MinBufSz Position */
+#define TPI_DEVID_MinBufSz_Msk (0x7UL << TPI_DEVID_MinBufSz_Pos) /*!< TPI DEVID: MinBufSz Mask */
+
+#define TPI_DEVID_AsynClkIn_Pos 5 /*!< TPI DEVID: AsynClkIn Position */
+#define TPI_DEVID_AsynClkIn_Msk (0x1UL << TPI_DEVID_AsynClkIn_Pos) /*!< TPI DEVID: AsynClkIn Mask */
+
+#define TPI_DEVID_NrTraceInput_Pos 0 /*!< TPI DEVID: NrTraceInput Position */
+#define TPI_DEVID_NrTraceInput_Msk (0x1FUL << TPI_DEVID_NrTraceInput_Pos) /*!< TPI DEVID: NrTraceInput Mask */
+
+/* TPI DEVTYPE Register Definitions */
+#define TPI_DEVTYPE_SubType_Pos 0 /*!< TPI DEVTYPE: SubType Position */
+#define TPI_DEVTYPE_SubType_Msk (0xFUL << TPI_DEVTYPE_SubType_Pos) /*!< TPI DEVTYPE: SubType Mask */
+
+#define TPI_DEVTYPE_MajorType_Pos 4 /*!< TPI DEVTYPE: MajorType Position */
+#define TPI_DEVTYPE_MajorType_Msk (0xFUL << TPI_DEVTYPE_MajorType_Pos) /*!< TPI DEVTYPE: MajorType Mask */
+
+/*@}*/ /* end of group CMSIS_TPI */
+
+
+#if (__MPU_PRESENT == 1)
+/** \ingroup CMSIS_core_register
+ \defgroup CMSIS_MPU Memory Protection Unit (MPU)
+ \brief Type definitions for the Memory Protection Unit (MPU)
+ @{
+ */
+
+/** \brief Structure type to access the Memory Protection Unit (MPU).
+ */
+typedef struct
+{
+ __I uint32_t TYPE; /*!< Offset: 0x000 (R/ ) MPU Type Register */
+ __IO uint32_t CTRL; /*!< Offset: 0x004 (R/W) MPU Control Register */
+ __IO uint32_t RNR; /*!< Offset: 0x008 (R/W) MPU Region RNRber Register */
+ __IO uint32_t RBAR; /*!< Offset: 0x00C (R/W) MPU Region Base Address Register */
+ __IO uint32_t RASR; /*!< Offset: 0x010 (R/W) MPU Region Attribute and Size Register */
+ __IO uint32_t RBAR_A1; /*!< Offset: 0x014 (R/W) MPU Alias 1 Region Base Address Register */
+ __IO uint32_t RASR_A1; /*!< Offset: 0x018 (R/W) MPU Alias 1 Region Attribute and Size Register */
+ __IO uint32_t RBAR_A2; /*!< Offset: 0x01C (R/W) MPU Alias 2 Region Base Address Register */
+ __IO uint32_t RASR_A2; /*!< Offset: 0x020 (R/W) MPU Alias 2 Region Attribute and Size Register */
+ __IO uint32_t RBAR_A3; /*!< Offset: 0x024 (R/W) MPU Alias 3 Region Base Address Register */
+ __IO uint32_t RASR_A3; /*!< Offset: 0x028 (R/W) MPU Alias 3 Region Attribute and Size Register */
+} MPU_Type;
+
+/* MPU Type Register */
+#define MPU_TYPE_IREGION_Pos 16 /*!< MPU TYPE: IREGION Position */
+#define MPU_TYPE_IREGION_Msk (0xFFUL << MPU_TYPE_IREGION_Pos) /*!< MPU TYPE: IREGION Mask */
+
+#define MPU_TYPE_DREGION_Pos 8 /*!< MPU TYPE: DREGION Position */
+#define MPU_TYPE_DREGION_Msk (0xFFUL << MPU_TYPE_DREGION_Pos) /*!< MPU TYPE: DREGION Mask */
+
+#define MPU_TYPE_SEPARATE_Pos 0 /*!< MPU TYPE: SEPARATE Position */
+#define MPU_TYPE_SEPARATE_Msk (1UL << MPU_TYPE_SEPARATE_Pos) /*!< MPU TYPE: SEPARATE Mask */
+
+/* MPU Control Register */
+#define MPU_CTRL_PRIVDEFENA_Pos 2 /*!< MPU CTRL: PRIVDEFENA Position */
+#define MPU_CTRL_PRIVDEFENA_Msk (1UL << MPU_CTRL_PRIVDEFENA_Pos) /*!< MPU CTRL: PRIVDEFENA Mask */
+
+#define MPU_CTRL_HFNMIENA_Pos 1 /*!< MPU CTRL: HFNMIENA Position */
+#define MPU_CTRL_HFNMIENA_Msk (1UL << MPU_CTRL_HFNMIENA_Pos) /*!< MPU CTRL: HFNMIENA Mask */
+
+#define MPU_CTRL_ENABLE_Pos 0 /*!< MPU CTRL: ENABLE Position */
+#define MPU_CTRL_ENABLE_Msk (1UL << MPU_CTRL_ENABLE_Pos) /*!< MPU CTRL: ENABLE Mask */
+
+/* MPU Region Number Register */
+#define MPU_RNR_REGION_Pos 0 /*!< MPU RNR: REGION Position */
+#define MPU_RNR_REGION_Msk (0xFFUL << MPU_RNR_REGION_Pos) /*!< MPU RNR: REGION Mask */
+
+/* MPU Region Base Address Register */
+#define MPU_RBAR_ADDR_Pos 5 /*!< MPU RBAR: ADDR Position */
+#define MPU_RBAR_ADDR_Msk (0x7FFFFFFUL << MPU_RBAR_ADDR_Pos) /*!< MPU RBAR: ADDR Mask */
+
+#define MPU_RBAR_VALID_Pos 4 /*!< MPU RBAR: VALID Position */
+#define MPU_RBAR_VALID_Msk (1UL << MPU_RBAR_VALID_Pos) /*!< MPU RBAR: VALID Mask */
+
+#define MPU_RBAR_REGION_Pos 0 /*!< MPU RBAR: REGION Position */
+#define MPU_RBAR_REGION_Msk (0xFUL << MPU_RBAR_REGION_Pos) /*!< MPU RBAR: REGION Mask */
+
+/* MPU Region Attribute and Size Register */
+#define MPU_RASR_ATTRS_Pos 16 /*!< MPU RASR: MPU Region Attribute field Position */
+#define MPU_RASR_ATTRS_Msk (0xFFFFUL << MPU_RASR_ATTRS_Pos) /*!< MPU RASR: MPU Region Attribute field Mask */
+
+#define MPU_RASR_XN_Pos 28 /*!< MPU RASR: ATTRS.XN Position */
+#define MPU_RASR_XN_Msk (1UL << MPU_RASR_XN_Pos) /*!< MPU RASR: ATTRS.XN Mask */
+
+#define MPU_RASR_AP_Pos 24 /*!< MPU RASR: ATTRS.AP Position */
+#define MPU_RASR_AP_Msk (0x7UL << MPU_RASR_AP_Pos) /*!< MPU RASR: ATTRS.AP Mask */
+
+#define MPU_RASR_TEX_Pos 19 /*!< MPU RASR: ATTRS.TEX Position */
+#define MPU_RASR_TEX_Msk (0x7UL << MPU_RASR_TEX_Pos) /*!< MPU RASR: ATTRS.TEX Mask */
+
+#define MPU_RASR_S_Pos 18 /*!< MPU RASR: ATTRS.S Position */
+#define MPU_RASR_S_Msk (1UL << MPU_RASR_S_Pos) /*!< MPU RASR: ATTRS.S Mask */
+
+#define MPU_RASR_C_Pos 17 /*!< MPU RASR: ATTRS.C Position */
+#define MPU_RASR_C_Msk (1UL << MPU_RASR_C_Pos) /*!< MPU RASR: ATTRS.C Mask */
+
+#define MPU_RASR_B_Pos 16 /*!< MPU RASR: ATTRS.B Position */
+#define MPU_RASR_B_Msk (1UL << MPU_RASR_B_Pos) /*!< MPU RASR: ATTRS.B Mask */
+
+#define MPU_RASR_SRD_Pos 8 /*!< MPU RASR: Sub-Region Disable Position */
+#define MPU_RASR_SRD_Msk (0xFFUL << MPU_RASR_SRD_Pos) /*!< MPU RASR: Sub-Region Disable Mask */
+
+#define MPU_RASR_SIZE_Pos 1 /*!< MPU RASR: Region Size Field Position */
+#define MPU_RASR_SIZE_Msk (0x1FUL << MPU_RASR_SIZE_Pos) /*!< MPU RASR: Region Size Field Mask */
+
+#define MPU_RASR_ENABLE_Pos 0 /*!< MPU RASR: Region enable bit Position */
+#define MPU_RASR_ENABLE_Msk (1UL << MPU_RASR_ENABLE_Pos) /*!< MPU RASR: Region enable bit Disable Mask */
+
+/*@} end of group CMSIS_MPU */
+#endif
+
+
+/** \ingroup CMSIS_core_register
+ \defgroup CMSIS_CoreDebug Core Debug Registers (CoreDebug)
+ \brief Type definitions for the Core Debug Registers
+ @{
+ */
+
+/** \brief Structure type to access the Core Debug Register (CoreDebug).
+ */
+typedef struct
+{
+ __IO uint32_t DHCSR; /*!< Offset: 0x000 (R/W) Debug Halting Control and Status Register */
+ __O uint32_t DCRSR; /*!< Offset: 0x004 ( /W) Debug Core Register Selector Register */
+ __IO uint32_t DCRDR; /*!< Offset: 0x008 (R/W) Debug Core Register Data Register */
+ __IO uint32_t DEMCR; /*!< Offset: 0x00C (R/W) Debug Exception and Monitor Control Register */
+} CoreDebug_Type;
+
+/* Debug Halting Control and Status Register */
+#define CoreDebug_DHCSR_DBGKEY_Pos 16 /*!< CoreDebug DHCSR: DBGKEY Position */
+#define CoreDebug_DHCSR_DBGKEY_Msk (0xFFFFUL << CoreDebug_DHCSR_DBGKEY_Pos) /*!< CoreDebug DHCSR: DBGKEY Mask */
+
+#define CoreDebug_DHCSR_S_RESET_ST_Pos 25 /*!< CoreDebug DHCSR: S_RESET_ST Position */
+#define CoreDebug_DHCSR_S_RESET_ST_Msk (1UL << CoreDebug_DHCSR_S_RESET_ST_Pos) /*!< CoreDebug DHCSR: S_RESET_ST Mask */
+
+#define CoreDebug_DHCSR_S_RETIRE_ST_Pos 24 /*!< CoreDebug DHCSR: S_RETIRE_ST Position */
+#define CoreDebug_DHCSR_S_RETIRE_ST_Msk (1UL << CoreDebug_DHCSR_S_RETIRE_ST_Pos) /*!< CoreDebug DHCSR: S_RETIRE_ST Mask */
+
+#define CoreDebug_DHCSR_S_LOCKUP_Pos 19 /*!< CoreDebug DHCSR: S_LOCKUP Position */
+#define CoreDebug_DHCSR_S_LOCKUP_Msk (1UL << CoreDebug_DHCSR_S_LOCKUP_Pos) /*!< CoreDebug DHCSR: S_LOCKUP Mask */
+
+#define CoreDebug_DHCSR_S_SLEEP_Pos 18 /*!< CoreDebug DHCSR: S_SLEEP Position */
+#define CoreDebug_DHCSR_S_SLEEP_Msk (1UL << CoreDebug_DHCSR_S_SLEEP_Pos) /*!< CoreDebug DHCSR: S_SLEEP Mask */
+
+#define CoreDebug_DHCSR_S_HALT_Pos 17 /*!< CoreDebug DHCSR: S_HALT Position */
+#define CoreDebug_DHCSR_S_HALT_Msk (1UL << CoreDebug_DHCSR_S_HALT_Pos) /*!< CoreDebug DHCSR: S_HALT Mask */
+
+#define CoreDebug_DHCSR_S_REGRDY_Pos 16 /*!< CoreDebug DHCSR: S_REGRDY Position */
+#define CoreDebug_DHCSR_S_REGRDY_Msk (1UL << CoreDebug_DHCSR_S_REGRDY_Pos) /*!< CoreDebug DHCSR: S_REGRDY Mask */
+
+#define CoreDebug_DHCSR_C_SNAPSTALL_Pos 5 /*!< CoreDebug DHCSR: C_SNAPSTALL Position */
+#define CoreDebug_DHCSR_C_SNAPSTALL_Msk (1UL << CoreDebug_DHCSR_C_SNAPSTALL_Pos) /*!< CoreDebug DHCSR: C_SNAPSTALL Mask */
+
+#define CoreDebug_DHCSR_C_MASKINTS_Pos 3 /*!< CoreDebug DHCSR: C_MASKINTS Position */
+#define CoreDebug_DHCSR_C_MASKINTS_Msk (1UL << CoreDebug_DHCSR_C_MASKINTS_Pos) /*!< CoreDebug DHCSR: C_MASKINTS Mask */
+
+#define CoreDebug_DHCSR_C_STEP_Pos 2 /*!< CoreDebug DHCSR: C_STEP Position */
+#define CoreDebug_DHCSR_C_STEP_Msk (1UL << CoreDebug_DHCSR_C_STEP_Pos) /*!< CoreDebug DHCSR: C_STEP Mask */
+
+#define CoreDebug_DHCSR_C_HALT_Pos 1 /*!< CoreDebug DHCSR: C_HALT Position */
+#define CoreDebug_DHCSR_C_HALT_Msk (1UL << CoreDebug_DHCSR_C_HALT_Pos) /*!< CoreDebug DHCSR: C_HALT Mask */
+
+#define CoreDebug_DHCSR_C_DEBUGEN_Pos 0 /*!< CoreDebug DHCSR: C_DEBUGEN Position */
+#define CoreDebug_DHCSR_C_DEBUGEN_Msk (1UL << CoreDebug_DHCSR_C_DEBUGEN_Pos) /*!< CoreDebug DHCSR: C_DEBUGEN Mask */
+
+/* Debug Core Register Selector Register */
+#define CoreDebug_DCRSR_REGWnR_Pos 16 /*!< CoreDebug DCRSR: REGWnR Position */
+#define CoreDebug_DCRSR_REGWnR_Msk (1UL << CoreDebug_DCRSR_REGWnR_Pos) /*!< CoreDebug DCRSR: REGWnR Mask */
+
+#define CoreDebug_DCRSR_REGSEL_Pos 0 /*!< CoreDebug DCRSR: REGSEL Position */
+#define CoreDebug_DCRSR_REGSEL_Msk (0x1FUL << CoreDebug_DCRSR_REGSEL_Pos) /*!< CoreDebug DCRSR: REGSEL Mask */
+
+/* Debug Exception and Monitor Control Register */
+#define CoreDebug_DEMCR_TRCENA_Pos 24 /*!< CoreDebug DEMCR: TRCENA Position */
+#define CoreDebug_DEMCR_TRCENA_Msk (1UL << CoreDebug_DEMCR_TRCENA_Pos) /*!< CoreDebug DEMCR: TRCENA Mask */
+
+#define CoreDebug_DEMCR_MON_REQ_Pos 19 /*!< CoreDebug DEMCR: MON_REQ Position */
+#define CoreDebug_DEMCR_MON_REQ_Msk (1UL << CoreDebug_DEMCR_MON_REQ_Pos) /*!< CoreDebug DEMCR: MON_REQ Mask */
+
+#define CoreDebug_DEMCR_MON_STEP_Pos 18 /*!< CoreDebug DEMCR: MON_STEP Position */
+#define CoreDebug_DEMCR_MON_STEP_Msk (1UL << CoreDebug_DEMCR_MON_STEP_Pos) /*!< CoreDebug DEMCR: MON_STEP Mask */
+
+#define CoreDebug_DEMCR_MON_PEND_Pos 17 /*!< CoreDebug DEMCR: MON_PEND Position */
+#define CoreDebug_DEMCR_MON_PEND_Msk (1UL << CoreDebug_DEMCR_MON_PEND_Pos) /*!< CoreDebug DEMCR: MON_PEND Mask */
+
+#define CoreDebug_DEMCR_MON_EN_Pos 16 /*!< CoreDebug DEMCR: MON_EN Position */
+#define CoreDebug_DEMCR_MON_EN_Msk (1UL << CoreDebug_DEMCR_MON_EN_Pos) /*!< CoreDebug DEMCR: MON_EN Mask */
+
+#define CoreDebug_DEMCR_VC_HARDERR_Pos 10 /*!< CoreDebug DEMCR: VC_HARDERR Position */
+#define CoreDebug_DEMCR_VC_HARDERR_Msk (1UL << CoreDebug_DEMCR_VC_HARDERR_Pos) /*!< CoreDebug DEMCR: VC_HARDERR Mask */
+
+#define CoreDebug_DEMCR_VC_INTERR_Pos 9 /*!< CoreDebug DEMCR: VC_INTERR Position */
+#define CoreDebug_DEMCR_VC_INTERR_Msk (1UL << CoreDebug_DEMCR_VC_INTERR_Pos) /*!< CoreDebug DEMCR: VC_INTERR Mask */
+
+#define CoreDebug_DEMCR_VC_BUSERR_Pos 8 /*!< CoreDebug DEMCR: VC_BUSERR Position */
+#define CoreDebug_DEMCR_VC_BUSERR_Msk (1UL << CoreDebug_DEMCR_VC_BUSERR_Pos) /*!< CoreDebug DEMCR: VC_BUSERR Mask */
+
+#define CoreDebug_DEMCR_VC_STATERR_Pos 7 /*!< CoreDebug DEMCR: VC_STATERR Position */
+#define CoreDebug_DEMCR_VC_STATERR_Msk (1UL << CoreDebug_DEMCR_VC_STATERR_Pos) /*!< CoreDebug DEMCR: VC_STATERR Mask */
+
+#define CoreDebug_DEMCR_VC_CHKERR_Pos 6 /*!< CoreDebug DEMCR: VC_CHKERR Position */
+#define CoreDebug_DEMCR_VC_CHKERR_Msk (1UL << CoreDebug_DEMCR_VC_CHKERR_Pos) /*!< CoreDebug DEMCR: VC_CHKERR Mask */
+
+#define CoreDebug_DEMCR_VC_NOCPERR_Pos 5 /*!< CoreDebug DEMCR: VC_NOCPERR Position */
+#define CoreDebug_DEMCR_VC_NOCPERR_Msk (1UL << CoreDebug_DEMCR_VC_NOCPERR_Pos) /*!< CoreDebug DEMCR: VC_NOCPERR Mask */
+
+#define CoreDebug_DEMCR_VC_MMERR_Pos 4 /*!< CoreDebug DEMCR: VC_MMERR Position */
+#define CoreDebug_DEMCR_VC_MMERR_Msk (1UL << CoreDebug_DEMCR_VC_MMERR_Pos) /*!< CoreDebug DEMCR: VC_MMERR Mask */
+
+#define CoreDebug_DEMCR_VC_CORERESET_Pos 0 /*!< CoreDebug DEMCR: VC_CORERESET Position */
+#define CoreDebug_DEMCR_VC_CORERESET_Msk (1UL << CoreDebug_DEMCR_VC_CORERESET_Pos) /*!< CoreDebug DEMCR: VC_CORERESET Mask */
+
+/*@} end of group CMSIS_CoreDebug */
+
+
+/** \ingroup CMSIS_core_register
+ \defgroup CMSIS_core_base Core Definitions
+ \brief Definitions for base addresses, unions, and structures.
+ @{
+ */
+
+/* Memory mapping of Cortex-M3 Hardware */
+#define SCS_BASE (0xE000E000UL) /*!< System Control Space Base Address */
+#define ITM_BASE (0xE0000000UL) /*!< ITM Base Address */
+#define DWT_BASE (0xE0001000UL) /*!< DWT Base Address */
+#define TPI_BASE (0xE0040000UL) /*!< TPI Base Address */
+#define CoreDebug_BASE (0xE000EDF0UL) /*!< Core Debug Base Address */
+#define SysTick_BASE (SCS_BASE + 0x0010UL) /*!< SysTick Base Address */
+#define NVIC_BASE (SCS_BASE + 0x0100UL) /*!< NVIC Base Address */
+#define SCB_BASE (SCS_BASE + 0x0D00UL) /*!< System Control Block Base Address */
+
+#define SCnSCB ((SCnSCB_Type *) SCS_BASE ) /*!< System control Register not in SCB */
+#define SCB ((SCB_Type *) SCB_BASE ) /*!< SCB configuration struct */
+#define SysTick ((SysTick_Type *) SysTick_BASE ) /*!< SysTick configuration struct */
+#define NVIC ((NVIC_Type *) NVIC_BASE ) /*!< NVIC configuration struct */
+#define ITM ((ITM_Type *) ITM_BASE ) /*!< ITM configuration struct */
+#define DWT ((DWT_Type *) DWT_BASE ) /*!< DWT configuration struct */
+#define TPI ((TPI_Type *) TPI_BASE ) /*!< TPI configuration struct */
+#define CoreDebug ((CoreDebug_Type *) CoreDebug_BASE) /*!< Core Debug configuration struct */
+
+#if (__MPU_PRESENT == 1)
+ #define MPU_BASE (SCS_BASE + 0x0D90UL) /*!< Memory Protection Unit */
+ #define MPU ((MPU_Type *) MPU_BASE ) /*!< Memory Protection Unit */
+#endif
+
+/*@} */
+
+
+
+/*******************************************************************************
+ * Hardware Abstraction Layer
+ Core Function Interface contains:
+ - Core NVIC Functions
+ - Core SysTick Functions
+ - Core Debug Functions
+ - Core Register Access Functions
+ ******************************************************************************/
+/** \defgroup CMSIS_Core_FunctionInterface Functions and Instructions Reference
+*/
+
+
+
+/* ########################## NVIC functions #################################### */
+/** \ingroup CMSIS_Core_FunctionInterface
+ \defgroup CMSIS_Core_NVICFunctions NVIC Functions
+ \brief Functions that manage interrupts and exceptions via the NVIC.
+ @{
+ */
+
+/** \brief Set Priority Grouping
+
+ The function sets the priority grouping field using the required unlock sequence.
+ The parameter PriorityGroup is assigned to the field SCB->AIRCR [10:8] PRIGROUP field.
+ Only values from 0..7 are used.
+ In case of a conflict between priority grouping and available
+ priority bits (__NVIC_PRIO_BITS), the smallest possible priority group is set.
+
+ \param [in] PriorityGroup Priority grouping field.
+ */
+__STATIC_INLINE void NVIC_SetPriorityGrouping(uint32_t PriorityGroup)
+{
+ uint32_t reg_value;
+ uint32_t PriorityGroupTmp = (PriorityGroup & (uint32_t)0x07); /* only values 0..7 are used */
+
+ reg_value = SCB->AIRCR; /* read old register configuration */
+ reg_value &= ~(SCB_AIRCR_VECTKEY_Msk | SCB_AIRCR_PRIGROUP_Msk); /* clear bits to change */
+ reg_value = (reg_value |
+ ((uint32_t)0x5FA << SCB_AIRCR_VECTKEY_Pos) |
+ (PriorityGroupTmp << 8)); /* Insert write key and priorty group */
+ SCB->AIRCR = reg_value;
+}
+
+
+/** \brief Get Priority Grouping
+
+ The function reads the priority grouping field from the NVIC Interrupt Controller.
+
+ \return Priority grouping field (SCB->AIRCR [10:8] PRIGROUP field).
+ */
+__STATIC_INLINE uint32_t NVIC_GetPriorityGrouping(void)
+{
+ return ((SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) >> SCB_AIRCR_PRIGROUP_Pos); /* read priority grouping field */
+}
+
+
+/** \brief Enable External Interrupt
+
+ The function enables a device-specific interrupt in the NVIC interrupt controller.
+
+ \param [in] IRQn External interrupt number. Value cannot be negative.
+ */
+__STATIC_INLINE void NVIC_EnableIRQ(IRQn_Type IRQn)
+{
+ NVIC->ISER[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* enable interrupt */
+}
+
+
+/** \brief Disable External Interrupt
+
+ The function disables a device-specific interrupt in the NVIC interrupt controller.
+
+ \param [in] IRQn External interrupt number. Value cannot be negative.
+ */
+__STATIC_INLINE void NVIC_DisableIRQ(IRQn_Type IRQn)
+{
+ NVIC->ICER[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* disable interrupt */
+}
+
+
+/** \brief Get Pending Interrupt
+
+ The function reads the pending register in the NVIC and returns the pending bit
+ for the specified interrupt.
+
+ \param [in] IRQn Interrupt number.
+
+ \return 0 Interrupt status is not pending.
+ \return 1 Interrupt status is pending.
+ */
+__STATIC_INLINE uint32_t NVIC_GetPendingIRQ(IRQn_Type IRQn)
+{
+ return((uint32_t) ((NVIC->ISPR[(uint32_t)(IRQn) >> 5] & (1 << ((uint32_t)(IRQn) & 0x1F)))?1:0)); /* Return 1 if pending else 0 */
+}
+
+
+/** \brief Set Pending Interrupt
+
+ The function sets the pending bit of an external interrupt.
+
+ \param [in] IRQn Interrupt number. Value cannot be negative.
+ */
+__STATIC_INLINE void NVIC_SetPendingIRQ(IRQn_Type IRQn)
+{
+ NVIC->ISPR[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* set interrupt pending */
+}
+
+
+/** \brief Clear Pending Interrupt
+
+ The function clears the pending bit of an external interrupt.
+
+ \param [in] IRQn External interrupt number. Value cannot be negative.
+ */
+__STATIC_INLINE void NVIC_ClearPendingIRQ(IRQn_Type IRQn)
+{
+ NVIC->ICPR[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* Clear pending interrupt */
+}
+
+
+/** \brief Get Active Interrupt
+
+ The function reads the active register in NVIC and returns the active bit.
+
+ \param [in] IRQn Interrupt number.
+
+ \return 0 Interrupt status is not active.
+ \return 1 Interrupt status is active.
+ */
+__STATIC_INLINE uint32_t NVIC_GetActive(IRQn_Type IRQn)
+{
+ return((uint32_t)((NVIC->IABR[(uint32_t)(IRQn) >> 5] & (1 << ((uint32_t)(IRQn) & 0x1F)))?1:0)); /* Return 1 if active else 0 */
+}
+
+
+/** \brief Set Interrupt Priority
+
+ The function sets the priority of an interrupt.
+
+ \note The priority cannot be set for every core interrupt.
+
+ \param [in] IRQn Interrupt number.
+ \param [in] priority Priority to set.
+ */
+__STATIC_INLINE void NVIC_SetPriority(IRQn_Type IRQn, uint32_t priority)
+{
+ if(IRQn < 0) {
+ SCB->SHP[((uint32_t)(IRQn) & 0xF)-4] = ((priority << (8 - __NVIC_PRIO_BITS)) & 0xff); } /* set Priority for Cortex-M System Interrupts */
+ else {
+ NVIC->IP[(uint32_t)(IRQn)] = ((priority << (8 - __NVIC_PRIO_BITS)) & 0xff); } /* set Priority for device specific Interrupts */
+}
+
+
+/** \brief Get Interrupt Priority
+
+ The function reads the priority of an interrupt. The interrupt
+ number can be positive to specify an external (device specific)
+ interrupt, or negative to specify an internal (core) interrupt.
+
+
+ \param [in] IRQn Interrupt number.
+ \return Interrupt Priority. Value is aligned automatically to the implemented
+ priority bits of the microcontroller.
+ */
+__STATIC_INLINE uint32_t NVIC_GetPriority(IRQn_Type IRQn)
+{
+
+ if(IRQn < 0) {
+ return((uint32_t)(SCB->SHP[((uint32_t)(IRQn) & 0xF)-4] >> (8 - __NVIC_PRIO_BITS))); } /* get priority for Cortex-M system interrupts */
+ else {
+ return((uint32_t)(NVIC->IP[(uint32_t)(IRQn)] >> (8 - __NVIC_PRIO_BITS))); } /* get priority for device specific interrupts */
+}
+
+
+/** \brief Encode Priority
+
+ The function encodes the priority for an interrupt with the given priority group,
+ preemptive priority value, and subpriority value.
+ In case of a conflict between priority grouping and available
+ priority bits (__NVIC_PRIO_BITS), the samllest possible priority group is set.
+
+ \param [in] PriorityGroup Used priority group.
+ \param [in] PreemptPriority Preemptive priority value (starting from 0).
+ \param [in] SubPriority Subpriority value (starting from 0).
+ \return Encoded priority. Value can be used in the function \ref NVIC_SetPriority().
+ */
+__STATIC_INLINE uint32_t NVIC_EncodePriority (uint32_t PriorityGroup, uint32_t PreemptPriority, uint32_t SubPriority)
+{
+ uint32_t PriorityGroupTmp = (PriorityGroup & 0x07); /* only values 0..7 are used */
+ uint32_t PreemptPriorityBits;
+ uint32_t SubPriorityBits;
+
+ PreemptPriorityBits = ((7 - PriorityGroupTmp) > __NVIC_PRIO_BITS) ? __NVIC_PRIO_BITS : 7 - PriorityGroupTmp;
+ SubPriorityBits = ((PriorityGroupTmp + __NVIC_PRIO_BITS) < 7) ? 0 : PriorityGroupTmp - 7 + __NVIC_PRIO_BITS;
+
+ return (
+ ((PreemptPriority & ((1 << (PreemptPriorityBits)) - 1)) << SubPriorityBits) |
+ ((SubPriority & ((1 << (SubPriorityBits )) - 1)))
+ );
+}
+
+
+/** \brief Decode Priority
+
+ The function decodes an interrupt priority value with a given priority group to
+ preemptive priority value and subpriority value.
+ In case of a conflict between priority grouping and available
+ priority bits (__NVIC_PRIO_BITS) the samllest possible priority group is set.
+
+ \param [in] Priority Priority value, which can be retrieved with the function \ref NVIC_GetPriority().
+ \param [in] PriorityGroup Used priority group.
+ \param [out] pPreemptPriority Preemptive priority value (starting from 0).
+ \param [out] pSubPriority Subpriority value (starting from 0).
+ */
+__STATIC_INLINE void NVIC_DecodePriority (uint32_t Priority, uint32_t PriorityGroup, uint32_t* pPreemptPriority, uint32_t* pSubPriority)
+{
+ uint32_t PriorityGroupTmp = (PriorityGroup & 0x07); /* only values 0..7 are used */
+ uint32_t PreemptPriorityBits;
+ uint32_t SubPriorityBits;
+
+ PreemptPriorityBits = ((7 - PriorityGroupTmp) > __NVIC_PRIO_BITS) ? __NVIC_PRIO_BITS : 7 - PriorityGroupTmp;
+ SubPriorityBits = ((PriorityGroupTmp + __NVIC_PRIO_BITS) < 7) ? 0 : PriorityGroupTmp - 7 + __NVIC_PRIO_BITS;
+
+ *pPreemptPriority = (Priority >> SubPriorityBits) & ((1 << (PreemptPriorityBits)) - 1);
+ *pSubPriority = (Priority ) & ((1 << (SubPriorityBits )) - 1);
+}
+
+
+/** \brief System Reset
+
+ The function initiates a system reset request to reset the MCU.
+ */
+__STATIC_INLINE void NVIC_SystemReset(void)
+{
+ __DSB(); /* Ensure all outstanding memory accesses included
+ buffered write are completed before reset */
+ SCB->AIRCR = ((0x5FA << SCB_AIRCR_VECTKEY_Pos) |
+ (SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) |
+ SCB_AIRCR_SYSRESETREQ_Msk); /* Keep priority group unchanged */
+ __DSB(); /* Ensure completion of memory access */
+ while(1); /* wait until reset */
+}
+
+/*@} end of CMSIS_Core_NVICFunctions */
+
+
+
+/* ################################## SysTick function ############################################ */
+/** \ingroup CMSIS_Core_FunctionInterface
+ \defgroup CMSIS_Core_SysTickFunctions SysTick Functions
+ \brief Functions that configure the System.
+ @{
+ */
+
+#if (__Vendor_SysTickConfig == 0)
+
+/** \brief System Tick Configuration
+
+ The function initializes the System Timer and its interrupt, and starts the System Tick Timer.
+ Counter is in free running mode to generate periodic interrupts.
+
+ \param [in] ticks Number of ticks between two interrupts.
+
+ \return 0 Function succeeded.
+ \return 1 Function failed.
+
+ \note When the variable <b>__Vendor_SysTickConfig</b> is set to 1, then the
+ function <b>SysTick_Config</b> is not included. In this case, the file <b><i>device</i>.h</b>
+ must contain a vendor-specific implementation of this function.
+
+ */
+__STATIC_INLINE uint32_t SysTick_Config(uint32_t ticks)
+{
+ if ((ticks - 1) > SysTick_LOAD_RELOAD_Msk) return (1); /* Reload value impossible */
+
+ SysTick->LOAD = ticks - 1; /* set reload register */
+ NVIC_SetPriority (SysTick_IRQn, (1<<__NVIC_PRIO_BITS) - 1); /* set Priority for Systick Interrupt */
+ SysTick->VAL = 0; /* Load the SysTick Counter Value */
+ SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk |
+ SysTick_CTRL_TICKINT_Msk |
+ SysTick_CTRL_ENABLE_Msk; /* Enable SysTick IRQ and SysTick Timer */
+ return (0); /* Function successful */
+}
+
+#endif
+
+/*@} end of CMSIS_Core_SysTickFunctions */
+
+
+
+/* ##################################### Debug In/Output function ########################################### */
+/** \ingroup CMSIS_Core_FunctionInterface
+ \defgroup CMSIS_core_DebugFunctions ITM Functions
+ \brief Functions that access the ITM debug interface.
+ @{
+ */
+
+extern volatile int32_t ITM_RxBuffer; /*!< External variable to receive characters. */
+#define ITM_RXBUFFER_EMPTY 0x5AA55AA5 /*!< Value identifying \ref ITM_RxBuffer is ready for next character. */
+
+
+/** \brief ITM Send Character
+
+ The function transmits a character via the ITM channel 0, and
+ \li Just returns when no debugger is connected that has booked the output.
+ \li Is blocking when a debugger is connected, but the previous character sent has not been transmitted.
+
+ \param [in] ch Character to transmit.
+
+ \returns Character to transmit.
+ */
+__STATIC_INLINE uint32_t ITM_SendChar (uint32_t ch)
+{
+ if ((ITM->TCR & ITM_TCR_ITMENA_Msk) && /* ITM enabled */
+ (ITM->TER & (1UL << 0) ) ) /* ITM Port #0 enabled */
+ {
+ while (ITM->PORT[0].u32 == 0);
+ ITM->PORT[0].u8 = (uint8_t) ch;
+ }
+ return (ch);
+}
+
+
+/** \brief ITM Receive Character
+
+ The function inputs a character via the external variable \ref ITM_RxBuffer.
+
+ \return Received character.
+ \return -1 No character pending.
+ */
+__STATIC_INLINE int32_t ITM_ReceiveChar (void) {
+ int32_t ch = -1; /* no character available */
+
+ if (ITM_RxBuffer != ITM_RXBUFFER_EMPTY) {
+ ch = ITM_RxBuffer;
+ ITM_RxBuffer = ITM_RXBUFFER_EMPTY; /* ready for next character */
+ }
+
+ return (ch);
+}
+
+
+/** \brief ITM Check Character
+
+ The function checks whether a character is pending for reading in the variable \ref ITM_RxBuffer.
+
+ \return 0 No character available.
+ \return 1 Character available.
+ */
+__STATIC_INLINE int32_t ITM_CheckChar (void) {
+
+ if (ITM_RxBuffer == ITM_RXBUFFER_EMPTY) {
+ return (0); /* no character available */
+ } else {
+ return (1); /* character available */
+ }
+}
+
+/*@} end of CMSIS_core_DebugFunctions */
+
+#endif /* __CORE_CM3_H_DEPENDANT */
+
+#endif /* __CMSIS_GENERIC */
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/src/modules/mathlib/CMSIS/Include/core_cm4.h b/src/modules/mathlib/CMSIS/Include/core_cm4.h new file mode 100644 index 000000000..93efd3a7a --- /dev/null +++ b/src/modules/mathlib/CMSIS/Include/core_cm4.h @@ -0,0 +1,1772 @@ +/**************************************************************************//**
+ * @file core_cm4.h
+ * @brief CMSIS Cortex-M4 Core Peripheral Access Layer Header File
+ * @version V3.20
+ * @date 25. February 2013
+ *
+ * @note
+ *
+ ******************************************************************************/
+/* Copyright (c) 2009 - 2013 ARM LIMITED
+
+ All rights reserved.
+ Redistribution and use in source and binary forms, with or without
+ modification, are permitted provided that the following conditions are met:
+ - Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+ - 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.
+ - Neither the name of ARM 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 COPYRIGHT HOLDERS AND 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.
+ ---------------------------------------------------------------------------*/
+
+
+#if defined ( __ICCARM__ )
+ #pragma system_include /* treat file as system include file for MISRA check */
+#endif
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+#ifndef __CORE_CM4_H_GENERIC
+#define __CORE_CM4_H_GENERIC
+
+/** \page CMSIS_MISRA_Exceptions MISRA-C:2004 Compliance Exceptions
+ CMSIS violates the following MISRA-C:2004 rules:
+
+ \li Required Rule 8.5, object/function definition in header file.<br>
+ Function definitions in header files are used to allow 'inlining'.
+
+ \li Required Rule 18.4, declaration of union type or object of union type: '{...}'.<br>
+ Unions are used for effective representation of core registers.
+
+ \li Advisory Rule 19.7, Function-like macro defined.<br>
+ Function-like macros are used to allow more efficient code.
+ */
+
+
+/*******************************************************************************
+ * CMSIS definitions
+ ******************************************************************************/
+/** \ingroup Cortex_M4
+ @{
+ */
+
+/* CMSIS CM4 definitions */
+#define __CM4_CMSIS_VERSION_MAIN (0x03) /*!< [31:16] CMSIS HAL main version */
+#define __CM4_CMSIS_VERSION_SUB (0x20) /*!< [15:0] CMSIS HAL sub version */
+#define __CM4_CMSIS_VERSION ((__CM4_CMSIS_VERSION_MAIN << 16) | \
+ __CM4_CMSIS_VERSION_SUB ) /*!< CMSIS HAL version number */
+
+#define __CORTEX_M (0x04) /*!< Cortex-M Core */
+
+
+#if defined ( __CC_ARM )
+ #define __ASM __asm /*!< asm keyword for ARM Compiler */
+ #define __INLINE __inline /*!< inline keyword for ARM Compiler */
+ #define __STATIC_INLINE static __inline
+
+#elif defined ( __ICCARM__ )
+ #define __ASM __asm /*!< asm keyword for IAR Compiler */
+ #define __INLINE inline /*!< inline keyword for IAR Compiler. Only available in High optimization mode! */
+ #define __STATIC_INLINE static inline
+
+#elif defined ( __TMS470__ )
+ #define __ASM __asm /*!< asm keyword for TI CCS Compiler */
+ #define __STATIC_INLINE static inline
+
+#elif defined ( __GNUC__ )
+ #define __ASM __asm /*!< asm keyword for GNU Compiler */
+ #define __INLINE inline /*!< inline keyword for GNU Compiler */
+ #define __STATIC_INLINE static inline
+
+#elif defined ( __TASKING__ )
+ #define __ASM __asm /*!< asm keyword for TASKING Compiler */
+ #define __INLINE inline /*!< inline keyword for TASKING Compiler */
+ #define __STATIC_INLINE static inline
+
+#endif
+
+/** __FPU_USED indicates whether an FPU is used or not. For this, __FPU_PRESENT has to be checked prior to making use of FPU specific registers and functions.
+*/
+#if defined ( __CC_ARM )
+ #if defined __TARGET_FPU_VFP
+ #if (__FPU_PRESENT == 1)
+ #define __FPU_USED 1
+ #else
+ #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+ #define __FPU_USED 0
+ #endif
+ #else
+ #define __FPU_USED 0
+ #endif
+
+#elif defined ( __ICCARM__ )
+ #if defined __ARMVFP__
+ #if (__FPU_PRESENT == 1)
+ #define __FPU_USED 1
+ #else
+ #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+ #define __FPU_USED 0
+ #endif
+ #else
+ #define __FPU_USED 0
+ #endif
+
+#elif defined ( __TMS470__ )
+ #if defined __TI_VFP_SUPPORT__
+ #if (__FPU_PRESENT == 1)
+ #define __FPU_USED 1
+ #else
+ #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+ #define __FPU_USED 0
+ #endif
+ #else
+ #define __FPU_USED 0
+ #endif
+
+#elif defined ( __GNUC__ )
+ #if defined (__VFP_FP__) && !defined(__SOFTFP__)
+ #if (__FPU_PRESENT == 1)
+ #define __FPU_USED 1
+ #else
+ #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+ #define __FPU_USED 0
+ #endif
+ #else
+ #define __FPU_USED 0
+ #endif
+
+#elif defined ( __TASKING__ )
+ #if defined __FPU_VFP__
+ #if (__FPU_PRESENT == 1)
+ #define __FPU_USED 1
+ #else
+ #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+ #define __FPU_USED 0
+ #endif
+ #else
+ #define __FPU_USED 0
+ #endif
+#endif
+
+#include <stdint.h> /* standard types definitions */
+#include "core_cmInstr.h" /* Core Instruction Access */
+#include "core_cmFunc.h" /* Core Function Access */
+#include "core_cm4_simd.h" /* Compiler specific SIMD Intrinsics */
+
+#endif /* __CORE_CM4_H_GENERIC */
+
+#ifndef __CMSIS_GENERIC
+
+#ifndef __CORE_CM4_H_DEPENDANT
+#define __CORE_CM4_H_DEPENDANT
+
+/* check device defines and use defaults */
+#if defined __CHECK_DEVICE_DEFINES
+ #ifndef __CM4_REV
+ #define __CM4_REV 0x0000
+ #warning "__CM4_REV not defined in device header file; using default!"
+ #endif
+
+ #ifndef __FPU_PRESENT
+ #define __FPU_PRESENT 0
+ #warning "__FPU_PRESENT not defined in device header file; using default!"
+ #endif
+
+ #ifndef __MPU_PRESENT
+ #define __MPU_PRESENT 0
+ #warning "__MPU_PRESENT not defined in device header file; using default!"
+ #endif
+
+ #ifndef __NVIC_PRIO_BITS
+ #define __NVIC_PRIO_BITS 4
+ #warning "__NVIC_PRIO_BITS not defined in device header file; using default!"
+ #endif
+
+ #ifndef __Vendor_SysTickConfig
+ #define __Vendor_SysTickConfig 0
+ #warning "__Vendor_SysTickConfig not defined in device header file; using default!"
+ #endif
+#endif
+
+/* IO definitions (access restrictions to peripheral registers) */
+/**
+ \defgroup CMSIS_glob_defs CMSIS Global Defines
+
+ <strong>IO Type Qualifiers</strong> are used
+ \li to specify the access to peripheral variables.
+ \li for automatic generation of peripheral register debug information.
+*/
+#ifdef __cplusplus
+ #define __I volatile /*!< Defines 'read only' permissions */
+#else
+ #define __I volatile const /*!< Defines 'read only' permissions */
+#endif
+#define __O volatile /*!< Defines 'write only' permissions */
+#define __IO volatile /*!< Defines 'read / write' permissions */
+
+/*@} end of group Cortex_M4 */
+
+
+
+/*******************************************************************************
+ * Register Abstraction
+ Core Register contain:
+ - Core Register
+ - Core NVIC Register
+ - Core SCB Register
+ - Core SysTick Register
+ - Core Debug Register
+ - Core MPU Register
+ - Core FPU Register
+ ******************************************************************************/
+/** \defgroup CMSIS_core_register Defines and Type Definitions
+ \brief Type definitions and defines for Cortex-M processor based devices.
+*/
+
+/** \ingroup CMSIS_core_register
+ \defgroup CMSIS_CORE Status and Control Registers
+ \brief Core Register type definitions.
+ @{
+ */
+
+/** \brief Union type to access the Application Program Status Register (APSR).
+ */
+typedef union
+{
+ struct
+ {
+#if (__CORTEX_M != 0x04)
+ uint32_t _reserved0:27; /*!< bit: 0..26 Reserved */
+#else
+ uint32_t _reserved0:16; /*!< bit: 0..15 Reserved */
+ uint32_t GE:4; /*!< bit: 16..19 Greater than or Equal flags */
+ uint32_t _reserved1:7; /*!< bit: 20..26 Reserved */
+#endif
+ uint32_t Q:1; /*!< bit: 27 Saturation condition flag */
+ uint32_t V:1; /*!< bit: 28 Overflow condition code flag */
+ uint32_t C:1; /*!< bit: 29 Carry condition code flag */
+ uint32_t Z:1; /*!< bit: 30 Zero condition code flag */
+ uint32_t N:1; /*!< bit: 31 Negative condition code flag */
+ } b; /*!< Structure used for bit access */
+ uint32_t w; /*!< Type used for word access */
+} APSR_Type;
+
+
+/** \brief Union type to access the Interrupt Program Status Register (IPSR).
+ */
+typedef union
+{
+ struct
+ {
+ uint32_t ISR:9; /*!< bit: 0.. 8 Exception number */
+ uint32_t _reserved0:23; /*!< bit: 9..31 Reserved */
+ } b; /*!< Structure used for bit access */
+ uint32_t w; /*!< Type used for word access */
+} IPSR_Type;
+
+
+/** \brief Union type to access the Special-Purpose Program Status Registers (xPSR).
+ */
+typedef union
+{
+ struct
+ {
+ uint32_t ISR:9; /*!< bit: 0.. 8 Exception number */
+#if (__CORTEX_M != 0x04)
+ uint32_t _reserved0:15; /*!< bit: 9..23 Reserved */
+#else
+ uint32_t _reserved0:7; /*!< bit: 9..15 Reserved */
+ uint32_t GE:4; /*!< bit: 16..19 Greater than or Equal flags */
+ uint32_t _reserved1:4; /*!< bit: 20..23 Reserved */
+#endif
+ uint32_t T:1; /*!< bit: 24 Thumb bit (read 0) */
+ uint32_t IT:2; /*!< bit: 25..26 saved IT state (read 0) */
+ uint32_t Q:1; /*!< bit: 27 Saturation condition flag */
+ uint32_t V:1; /*!< bit: 28 Overflow condition code flag */
+ uint32_t C:1; /*!< bit: 29 Carry condition code flag */
+ uint32_t Z:1; /*!< bit: 30 Zero condition code flag */
+ uint32_t N:1; /*!< bit: 31 Negative condition code flag */
+ } b; /*!< Structure used for bit access */
+ uint32_t w; /*!< Type used for word access */
+} xPSR_Type;
+
+
+/** \brief Union type to access the Control Registers (CONTROL).
+ */
+typedef union
+{
+ struct
+ {
+ uint32_t nPRIV:1; /*!< bit: 0 Execution privilege in Thread mode */
+ uint32_t SPSEL:1; /*!< bit: 1 Stack to be used */
+ uint32_t FPCA:1; /*!< bit: 2 FP extension active flag */
+ uint32_t _reserved0:29; /*!< bit: 3..31 Reserved */
+ } b; /*!< Structure used for bit access */
+ uint32_t w; /*!< Type used for word access */
+} CONTROL_Type;
+
+/*@} end of group CMSIS_CORE */
+
+
+/** \ingroup CMSIS_core_register
+ \defgroup CMSIS_NVIC Nested Vectored Interrupt Controller (NVIC)
+ \brief Type definitions for the NVIC Registers
+ @{
+ */
+
+/** \brief Structure type to access the Nested Vectored Interrupt Controller (NVIC).
+ */
+typedef struct
+{
+ __IO uint32_t ISER[8]; /*!< Offset: 0x000 (R/W) Interrupt Set Enable Register */
+ uint32_t RESERVED0[24];
+ __IO uint32_t ICER[8]; /*!< Offset: 0x080 (R/W) Interrupt Clear Enable Register */
+ uint32_t RSERVED1[24];
+ __IO uint32_t ISPR[8]; /*!< Offset: 0x100 (R/W) Interrupt Set Pending Register */
+ uint32_t RESERVED2[24];
+ __IO uint32_t ICPR[8]; /*!< Offset: 0x180 (R/W) Interrupt Clear Pending Register */
+ uint32_t RESERVED3[24];
+ __IO uint32_t IABR[8]; /*!< Offset: 0x200 (R/W) Interrupt Active bit Register */
+ uint32_t RESERVED4[56];
+ __IO uint8_t IP[240]; /*!< Offset: 0x300 (R/W) Interrupt Priority Register (8Bit wide) */
+ uint32_t RESERVED5[644];
+ __O uint32_t STIR; /*!< Offset: 0xE00 ( /W) Software Trigger Interrupt Register */
+} NVIC_Type;
+
+/* Software Triggered Interrupt Register Definitions */
+#define NVIC_STIR_INTID_Pos 0 /*!< STIR: INTLINESNUM Position */
+#define NVIC_STIR_INTID_Msk (0x1FFUL << NVIC_STIR_INTID_Pos) /*!< STIR: INTLINESNUM Mask */
+
+/*@} end of group CMSIS_NVIC */
+
+
+/** \ingroup CMSIS_core_register
+ \defgroup CMSIS_SCB System Control Block (SCB)
+ \brief Type definitions for the System Control Block Registers
+ @{
+ */
+
+/** \brief Structure type to access the System Control Block (SCB).
+ */
+typedef struct
+{
+ __I uint32_t CPUID; /*!< Offset: 0x000 (R/ ) CPUID Base Register */
+ __IO uint32_t ICSR; /*!< Offset: 0x004 (R/W) Interrupt Control and State Register */
+ __IO uint32_t VTOR; /*!< Offset: 0x008 (R/W) Vector Table Offset Register */
+ __IO uint32_t AIRCR; /*!< Offset: 0x00C (R/W) Application Interrupt and Reset Control Register */
+ __IO uint32_t SCR; /*!< Offset: 0x010 (R/W) System Control Register */
+ __IO uint32_t CCR; /*!< Offset: 0x014 (R/W) Configuration Control Register */
+ __IO uint8_t SHP[12]; /*!< Offset: 0x018 (R/W) System Handlers Priority Registers (4-7, 8-11, 12-15) */
+ __IO uint32_t SHCSR; /*!< Offset: 0x024 (R/W) System Handler Control and State Register */
+ __IO uint32_t CFSR; /*!< Offset: 0x028 (R/W) Configurable Fault Status Register */
+ __IO uint32_t HFSR; /*!< Offset: 0x02C (R/W) HardFault Status Register */
+ __IO uint32_t DFSR; /*!< Offset: 0x030 (R/W) Debug Fault Status Register */
+ __IO uint32_t MMFAR; /*!< Offset: 0x034 (R/W) MemManage Fault Address Register */
+ __IO uint32_t BFAR; /*!< Offset: 0x038 (R/W) BusFault Address Register */
+ __IO uint32_t AFSR; /*!< Offset: 0x03C (R/W) Auxiliary Fault Status Register */
+ __I uint32_t PFR[2]; /*!< Offset: 0x040 (R/ ) Processor Feature Register */
+ __I uint32_t DFR; /*!< Offset: 0x048 (R/ ) Debug Feature Register */
+ __I uint32_t ADR; /*!< Offset: 0x04C (R/ ) Auxiliary Feature Register */
+ __I uint32_t MMFR[4]; /*!< Offset: 0x050 (R/ ) Memory Model Feature Register */
+ __I uint32_t ISAR[5]; /*!< Offset: 0x060 (R/ ) Instruction Set Attributes Register */
+ uint32_t RESERVED0[5];
+ __IO uint32_t CPACR; /*!< Offset: 0x088 (R/W) Coprocessor Access Control Register */
+} SCB_Type;
+
+/* SCB CPUID Register Definitions */
+#define SCB_CPUID_IMPLEMENTER_Pos 24 /*!< SCB CPUID: IMPLEMENTER Position */
+#define SCB_CPUID_IMPLEMENTER_Msk (0xFFUL << SCB_CPUID_IMPLEMENTER_Pos) /*!< SCB CPUID: IMPLEMENTER Mask */
+
+#define SCB_CPUID_VARIANT_Pos 20 /*!< SCB CPUID: VARIANT Position */
+#define SCB_CPUID_VARIANT_Msk (0xFUL << SCB_CPUID_VARIANT_Pos) /*!< SCB CPUID: VARIANT Mask */
+
+#define SCB_CPUID_ARCHITECTURE_Pos 16 /*!< SCB CPUID: ARCHITECTURE Position */
+#define SCB_CPUID_ARCHITECTURE_Msk (0xFUL << SCB_CPUID_ARCHITECTURE_Pos) /*!< SCB CPUID: ARCHITECTURE Mask */
+
+#define SCB_CPUID_PARTNO_Pos 4 /*!< SCB CPUID: PARTNO Position */
+#define SCB_CPUID_PARTNO_Msk (0xFFFUL << SCB_CPUID_PARTNO_Pos) /*!< SCB CPUID: PARTNO Mask */
+
+#define SCB_CPUID_REVISION_Pos 0 /*!< SCB CPUID: REVISION Position */
+#define SCB_CPUID_REVISION_Msk (0xFUL << SCB_CPUID_REVISION_Pos) /*!< SCB CPUID: REVISION Mask */
+
+/* SCB Interrupt Control State Register Definitions */
+#define SCB_ICSR_NMIPENDSET_Pos 31 /*!< SCB ICSR: NMIPENDSET Position */
+#define SCB_ICSR_NMIPENDSET_Msk (1UL << SCB_ICSR_NMIPENDSET_Pos) /*!< SCB ICSR: NMIPENDSET Mask */
+
+#define SCB_ICSR_PENDSVSET_Pos 28 /*!< SCB ICSR: PENDSVSET Position */
+#define SCB_ICSR_PENDSVSET_Msk (1UL << SCB_ICSR_PENDSVSET_Pos) /*!< SCB ICSR: PENDSVSET Mask */
+
+#define SCB_ICSR_PENDSVCLR_Pos 27 /*!< SCB ICSR: PENDSVCLR Position */
+#define SCB_ICSR_PENDSVCLR_Msk (1UL << SCB_ICSR_PENDSVCLR_Pos) /*!< SCB ICSR: PENDSVCLR Mask */
+
+#define SCB_ICSR_PENDSTSET_Pos 26 /*!< SCB ICSR: PENDSTSET Position */
+#define SCB_ICSR_PENDSTSET_Msk (1UL << SCB_ICSR_PENDSTSET_Pos) /*!< SCB ICSR: PENDSTSET Mask */
+
+#define SCB_ICSR_PENDSTCLR_Pos 25 /*!< SCB ICSR: PENDSTCLR Position */
+#define SCB_ICSR_PENDSTCLR_Msk (1UL << SCB_ICSR_PENDSTCLR_Pos) /*!< SCB ICSR: PENDSTCLR Mask */
+
+#define SCB_ICSR_ISRPREEMPT_Pos 23 /*!< SCB ICSR: ISRPREEMPT Position */
+#define SCB_ICSR_ISRPREEMPT_Msk (1UL << SCB_ICSR_ISRPREEMPT_Pos) /*!< SCB ICSR: ISRPREEMPT Mask */
+
+#define SCB_ICSR_ISRPENDING_Pos 22 /*!< SCB ICSR: ISRPENDING Position */
+#define SCB_ICSR_ISRPENDING_Msk (1UL << SCB_ICSR_ISRPENDING_Pos) /*!< SCB ICSR: ISRPENDING Mask */
+
+#define SCB_ICSR_VECTPENDING_Pos 12 /*!< SCB ICSR: VECTPENDING Position */
+#define SCB_ICSR_VECTPENDING_Msk (0x1FFUL << SCB_ICSR_VECTPENDING_Pos) /*!< SCB ICSR: VECTPENDING Mask */
+
+#define SCB_ICSR_RETTOBASE_Pos 11 /*!< SCB ICSR: RETTOBASE Position */
+#define SCB_ICSR_RETTOBASE_Msk (1UL << SCB_ICSR_RETTOBASE_Pos) /*!< SCB ICSR: RETTOBASE Mask */
+
+#define SCB_ICSR_VECTACTIVE_Pos 0 /*!< SCB ICSR: VECTACTIVE Position */
+#define SCB_ICSR_VECTACTIVE_Msk (0x1FFUL << SCB_ICSR_VECTACTIVE_Pos) /*!< SCB ICSR: VECTACTIVE Mask */
+
+/* SCB Vector Table Offset Register Definitions */
+#define SCB_VTOR_TBLOFF_Pos 7 /*!< SCB VTOR: TBLOFF Position */
+#define SCB_VTOR_TBLOFF_Msk (0x1FFFFFFUL << SCB_VTOR_TBLOFF_Pos) /*!< SCB VTOR: TBLOFF Mask */
+
+/* SCB Application Interrupt and Reset Control Register Definitions */
+#define SCB_AIRCR_VECTKEY_Pos 16 /*!< SCB AIRCR: VECTKEY Position */
+#define SCB_AIRCR_VECTKEY_Msk (0xFFFFUL << SCB_AIRCR_VECTKEY_Pos) /*!< SCB AIRCR: VECTKEY Mask */
+
+#define SCB_AIRCR_VECTKEYSTAT_Pos 16 /*!< SCB AIRCR: VECTKEYSTAT Position */
+#define SCB_AIRCR_VECTKEYSTAT_Msk (0xFFFFUL << SCB_AIRCR_VECTKEYSTAT_Pos) /*!< SCB AIRCR: VECTKEYSTAT Mask */
+
+#define SCB_AIRCR_ENDIANESS_Pos 15 /*!< SCB AIRCR: ENDIANESS Position */
+#define SCB_AIRCR_ENDIANESS_Msk (1UL << SCB_AIRCR_ENDIANESS_Pos) /*!< SCB AIRCR: ENDIANESS Mask */
+
+#define SCB_AIRCR_PRIGROUP_Pos 8 /*!< SCB AIRCR: PRIGROUP Position */
+#define SCB_AIRCR_PRIGROUP_Msk (7UL << SCB_AIRCR_PRIGROUP_Pos) /*!< SCB AIRCR: PRIGROUP Mask */
+
+#define SCB_AIRCR_SYSRESETREQ_Pos 2 /*!< SCB AIRCR: SYSRESETREQ Position */
+#define SCB_AIRCR_SYSRESETREQ_Msk (1UL << SCB_AIRCR_SYSRESETREQ_Pos) /*!< SCB AIRCR: SYSRESETREQ Mask */
+
+#define SCB_AIRCR_VECTCLRACTIVE_Pos 1 /*!< SCB AIRCR: VECTCLRACTIVE Position */
+#define SCB_AIRCR_VECTCLRACTIVE_Msk (1UL << SCB_AIRCR_VECTCLRACTIVE_Pos) /*!< SCB AIRCR: VECTCLRACTIVE Mask */
+
+#define SCB_AIRCR_VECTRESET_Pos 0 /*!< SCB AIRCR: VECTRESET Position */
+#define SCB_AIRCR_VECTRESET_Msk (1UL << SCB_AIRCR_VECTRESET_Pos) /*!< SCB AIRCR: VECTRESET Mask */
+
+/* SCB System Control Register Definitions */
+#define SCB_SCR_SEVONPEND_Pos 4 /*!< SCB SCR: SEVONPEND Position */
+#define SCB_SCR_SEVONPEND_Msk (1UL << SCB_SCR_SEVONPEND_Pos) /*!< SCB SCR: SEVONPEND Mask */
+
+#define SCB_SCR_SLEEPDEEP_Pos 2 /*!< SCB SCR: SLEEPDEEP Position */
+#define SCB_SCR_SLEEPDEEP_Msk (1UL << SCB_SCR_SLEEPDEEP_Pos) /*!< SCB SCR: SLEEPDEEP Mask */
+
+#define SCB_SCR_SLEEPONEXIT_Pos 1 /*!< SCB SCR: SLEEPONEXIT Position */
+#define SCB_SCR_SLEEPONEXIT_Msk (1UL << SCB_SCR_SLEEPONEXIT_Pos) /*!< SCB SCR: SLEEPONEXIT Mask */
+
+/* SCB Configuration Control Register Definitions */
+#define SCB_CCR_STKALIGN_Pos 9 /*!< SCB CCR: STKALIGN Position */
+#define SCB_CCR_STKALIGN_Msk (1UL << SCB_CCR_STKALIGN_Pos) /*!< SCB CCR: STKALIGN Mask */
+
+#define SCB_CCR_BFHFNMIGN_Pos 8 /*!< SCB CCR: BFHFNMIGN Position */
+#define SCB_CCR_BFHFNMIGN_Msk (1UL << SCB_CCR_BFHFNMIGN_Pos) /*!< SCB CCR: BFHFNMIGN Mask */
+
+#define SCB_CCR_DIV_0_TRP_Pos 4 /*!< SCB CCR: DIV_0_TRP Position */
+#define SCB_CCR_DIV_0_TRP_Msk (1UL << SCB_CCR_DIV_0_TRP_Pos) /*!< SCB CCR: DIV_0_TRP Mask */
+
+#define SCB_CCR_UNALIGN_TRP_Pos 3 /*!< SCB CCR: UNALIGN_TRP Position */
+#define SCB_CCR_UNALIGN_TRP_Msk (1UL << SCB_CCR_UNALIGN_TRP_Pos) /*!< SCB CCR: UNALIGN_TRP Mask */
+
+#define SCB_CCR_USERSETMPEND_Pos 1 /*!< SCB CCR: USERSETMPEND Position */
+#define SCB_CCR_USERSETMPEND_Msk (1UL << SCB_CCR_USERSETMPEND_Pos) /*!< SCB CCR: USERSETMPEND Mask */
+
+#define SCB_CCR_NONBASETHRDENA_Pos 0 /*!< SCB CCR: NONBASETHRDENA Position */
+#define SCB_CCR_NONBASETHRDENA_Msk (1UL << SCB_CCR_NONBASETHRDENA_Pos) /*!< SCB CCR: NONBASETHRDENA Mask */
+
+/* SCB System Handler Control and State Register Definitions */
+#define SCB_SHCSR_USGFAULTENA_Pos 18 /*!< SCB SHCSR: USGFAULTENA Position */
+#define SCB_SHCSR_USGFAULTENA_Msk (1UL << SCB_SHCSR_USGFAULTENA_Pos) /*!< SCB SHCSR: USGFAULTENA Mask */
+
+#define SCB_SHCSR_BUSFAULTENA_Pos 17 /*!< SCB SHCSR: BUSFAULTENA Position */
+#define SCB_SHCSR_BUSFAULTENA_Msk (1UL << SCB_SHCSR_BUSFAULTENA_Pos) /*!< SCB SHCSR: BUSFAULTENA Mask */
+
+#define SCB_SHCSR_MEMFAULTENA_Pos 16 /*!< SCB SHCSR: MEMFAULTENA Position */
+#define SCB_SHCSR_MEMFAULTENA_Msk (1UL << SCB_SHCSR_MEMFAULTENA_Pos) /*!< SCB SHCSR: MEMFAULTENA Mask */
+
+#define SCB_SHCSR_SVCALLPENDED_Pos 15 /*!< SCB SHCSR: SVCALLPENDED Position */
+#define SCB_SHCSR_SVCALLPENDED_Msk (1UL << SCB_SHCSR_SVCALLPENDED_Pos) /*!< SCB SHCSR: SVCALLPENDED Mask */
+
+#define SCB_SHCSR_BUSFAULTPENDED_Pos 14 /*!< SCB SHCSR: BUSFAULTPENDED Position */
+#define SCB_SHCSR_BUSFAULTPENDED_Msk (1UL << SCB_SHCSR_BUSFAULTPENDED_Pos) /*!< SCB SHCSR: BUSFAULTPENDED Mask */
+
+#define SCB_SHCSR_MEMFAULTPENDED_Pos 13 /*!< SCB SHCSR: MEMFAULTPENDED Position */
+#define SCB_SHCSR_MEMFAULTPENDED_Msk (1UL << SCB_SHCSR_MEMFAULTPENDED_Pos) /*!< SCB SHCSR: MEMFAULTPENDED Mask */
+
+#define SCB_SHCSR_USGFAULTPENDED_Pos 12 /*!< SCB SHCSR: USGFAULTPENDED Position */
+#define SCB_SHCSR_USGFAULTPENDED_Msk (1UL << SCB_SHCSR_USGFAULTPENDED_Pos) /*!< SCB SHCSR: USGFAULTPENDED Mask */
+
+#define SCB_SHCSR_SYSTICKACT_Pos 11 /*!< SCB SHCSR: SYSTICKACT Position */
+#define SCB_SHCSR_SYSTICKACT_Msk (1UL << SCB_SHCSR_SYSTICKACT_Pos) /*!< SCB SHCSR: SYSTICKACT Mask */
+
+#define SCB_SHCSR_PENDSVACT_Pos 10 /*!< SCB SHCSR: PENDSVACT Position */
+#define SCB_SHCSR_PENDSVACT_Msk (1UL << SCB_SHCSR_PENDSVACT_Pos) /*!< SCB SHCSR: PENDSVACT Mask */
+
+#define SCB_SHCSR_MONITORACT_Pos 8 /*!< SCB SHCSR: MONITORACT Position */
+#define SCB_SHCSR_MONITORACT_Msk (1UL << SCB_SHCSR_MONITORACT_Pos) /*!< SCB SHCSR: MONITORACT Mask */
+
+#define SCB_SHCSR_SVCALLACT_Pos 7 /*!< SCB SHCSR: SVCALLACT Position */
+#define SCB_SHCSR_SVCALLACT_Msk (1UL << SCB_SHCSR_SVCALLACT_Pos) /*!< SCB SHCSR: SVCALLACT Mask */
+
+#define SCB_SHCSR_USGFAULTACT_Pos 3 /*!< SCB SHCSR: USGFAULTACT Position */
+#define SCB_SHCSR_USGFAULTACT_Msk (1UL << SCB_SHCSR_USGFAULTACT_Pos) /*!< SCB SHCSR: USGFAULTACT Mask */
+
+#define SCB_SHCSR_BUSFAULTACT_Pos 1 /*!< SCB SHCSR: BUSFAULTACT Position */
+#define SCB_SHCSR_BUSFAULTACT_Msk (1UL << SCB_SHCSR_BUSFAULTACT_Pos) /*!< SCB SHCSR: BUSFAULTACT Mask */
+
+#define SCB_SHCSR_MEMFAULTACT_Pos 0 /*!< SCB SHCSR: MEMFAULTACT Position */
+#define SCB_SHCSR_MEMFAULTACT_Msk (1UL << SCB_SHCSR_MEMFAULTACT_Pos) /*!< SCB SHCSR: MEMFAULTACT Mask */
+
+/* SCB Configurable Fault Status Registers Definitions */
+#define SCB_CFSR_USGFAULTSR_Pos 16 /*!< SCB CFSR: Usage Fault Status Register Position */
+#define SCB_CFSR_USGFAULTSR_Msk (0xFFFFUL << SCB_CFSR_USGFAULTSR_Pos) /*!< SCB CFSR: Usage Fault Status Register Mask */
+
+#define SCB_CFSR_BUSFAULTSR_Pos 8 /*!< SCB CFSR: Bus Fault Status Register Position */
+#define SCB_CFSR_BUSFAULTSR_Msk (0xFFUL << SCB_CFSR_BUSFAULTSR_Pos) /*!< SCB CFSR: Bus Fault Status Register Mask */
+
+#define SCB_CFSR_MEMFAULTSR_Pos 0 /*!< SCB CFSR: Memory Manage Fault Status Register Position */
+#define SCB_CFSR_MEMFAULTSR_Msk (0xFFUL << SCB_CFSR_MEMFAULTSR_Pos) /*!< SCB CFSR: Memory Manage Fault Status Register Mask */
+
+/* SCB Hard Fault Status Registers Definitions */
+#define SCB_HFSR_DEBUGEVT_Pos 31 /*!< SCB HFSR: DEBUGEVT Position */
+#define SCB_HFSR_DEBUGEVT_Msk (1UL << SCB_HFSR_DEBUGEVT_Pos) /*!< SCB HFSR: DEBUGEVT Mask */
+
+#define SCB_HFSR_FORCED_Pos 30 /*!< SCB HFSR: FORCED Position */
+#define SCB_HFSR_FORCED_Msk (1UL << SCB_HFSR_FORCED_Pos) /*!< SCB HFSR: FORCED Mask */
+
+#define SCB_HFSR_VECTTBL_Pos 1 /*!< SCB HFSR: VECTTBL Position */
+#define SCB_HFSR_VECTTBL_Msk (1UL << SCB_HFSR_VECTTBL_Pos) /*!< SCB HFSR: VECTTBL Mask */
+
+/* SCB Debug Fault Status Register Definitions */
+#define SCB_DFSR_EXTERNAL_Pos 4 /*!< SCB DFSR: EXTERNAL Position */
+#define SCB_DFSR_EXTERNAL_Msk (1UL << SCB_DFSR_EXTERNAL_Pos) /*!< SCB DFSR: EXTERNAL Mask */
+
+#define SCB_DFSR_VCATCH_Pos 3 /*!< SCB DFSR: VCATCH Position */
+#define SCB_DFSR_VCATCH_Msk (1UL << SCB_DFSR_VCATCH_Pos) /*!< SCB DFSR: VCATCH Mask */
+
+#define SCB_DFSR_DWTTRAP_Pos 2 /*!< SCB DFSR: DWTTRAP Position */
+#define SCB_DFSR_DWTTRAP_Msk (1UL << SCB_DFSR_DWTTRAP_Pos) /*!< SCB DFSR: DWTTRAP Mask */
+
+#define SCB_DFSR_BKPT_Pos 1 /*!< SCB DFSR: BKPT Position */
+#define SCB_DFSR_BKPT_Msk (1UL << SCB_DFSR_BKPT_Pos) /*!< SCB DFSR: BKPT Mask */
+
+#define SCB_DFSR_HALTED_Pos 0 /*!< SCB DFSR: HALTED Position */
+#define SCB_DFSR_HALTED_Msk (1UL << SCB_DFSR_HALTED_Pos) /*!< SCB DFSR: HALTED Mask */
+
+/*@} end of group CMSIS_SCB */
+
+
+/** \ingroup CMSIS_core_register
+ \defgroup CMSIS_SCnSCB System Controls not in SCB (SCnSCB)
+ \brief Type definitions for the System Control and ID Register not in the SCB
+ @{
+ */
+
+/** \brief Structure type to access the System Control and ID Register not in the SCB.
+ */
+typedef struct
+{
+ uint32_t RESERVED0[1];
+ __I uint32_t ICTR; /*!< Offset: 0x004 (R/ ) Interrupt Controller Type Register */
+ __IO uint32_t ACTLR; /*!< Offset: 0x008 (R/W) Auxiliary Control Register */
+} SCnSCB_Type;
+
+/* Interrupt Controller Type Register Definitions */
+#define SCnSCB_ICTR_INTLINESNUM_Pos 0 /*!< ICTR: INTLINESNUM Position */
+#define SCnSCB_ICTR_INTLINESNUM_Msk (0xFUL << SCnSCB_ICTR_INTLINESNUM_Pos) /*!< ICTR: INTLINESNUM Mask */
+
+/* Auxiliary Control Register Definitions */
+#define SCnSCB_ACTLR_DISOOFP_Pos 9 /*!< ACTLR: DISOOFP Position */
+#define SCnSCB_ACTLR_DISOOFP_Msk (1UL << SCnSCB_ACTLR_DISOOFP_Pos) /*!< ACTLR: DISOOFP Mask */
+
+#define SCnSCB_ACTLR_DISFPCA_Pos 8 /*!< ACTLR: DISFPCA Position */
+#define SCnSCB_ACTLR_DISFPCA_Msk (1UL << SCnSCB_ACTLR_DISFPCA_Pos) /*!< ACTLR: DISFPCA Mask */
+
+#define SCnSCB_ACTLR_DISFOLD_Pos 2 /*!< ACTLR: DISFOLD Position */
+#define SCnSCB_ACTLR_DISFOLD_Msk (1UL << SCnSCB_ACTLR_DISFOLD_Pos) /*!< ACTLR: DISFOLD Mask */
+
+#define SCnSCB_ACTLR_DISDEFWBUF_Pos 1 /*!< ACTLR: DISDEFWBUF Position */
+#define SCnSCB_ACTLR_DISDEFWBUF_Msk (1UL << SCnSCB_ACTLR_DISDEFWBUF_Pos) /*!< ACTLR: DISDEFWBUF Mask */
+
+#define SCnSCB_ACTLR_DISMCYCINT_Pos 0 /*!< ACTLR: DISMCYCINT Position */
+#define SCnSCB_ACTLR_DISMCYCINT_Msk (1UL << SCnSCB_ACTLR_DISMCYCINT_Pos) /*!< ACTLR: DISMCYCINT Mask */
+
+/*@} end of group CMSIS_SCnotSCB */
+
+
+/** \ingroup CMSIS_core_register
+ \defgroup CMSIS_SysTick System Tick Timer (SysTick)
+ \brief Type definitions for the System Timer Registers.
+ @{
+ */
+
+/** \brief Structure type to access the System Timer (SysTick).
+ */
+typedef struct
+{
+ __IO uint32_t CTRL; /*!< Offset: 0x000 (R/W) SysTick Control and Status Register */
+ __IO uint32_t LOAD; /*!< Offset: 0x004 (R/W) SysTick Reload Value Register */
+ __IO uint32_t VAL; /*!< Offset: 0x008 (R/W) SysTick Current Value Register */
+ __I uint32_t CALIB; /*!< Offset: 0x00C (R/ ) SysTick Calibration Register */
+} SysTick_Type;
+
+/* SysTick Control / Status Register Definitions */
+#define SysTick_CTRL_COUNTFLAG_Pos 16 /*!< SysTick CTRL: COUNTFLAG Position */
+#define SysTick_CTRL_COUNTFLAG_Msk (1UL << SysTick_CTRL_COUNTFLAG_Pos) /*!< SysTick CTRL: COUNTFLAG Mask */
+
+#define SysTick_CTRL_CLKSOURCE_Pos 2 /*!< SysTick CTRL: CLKSOURCE Position */
+#define SysTick_CTRL_CLKSOURCE_Msk (1UL << SysTick_CTRL_CLKSOURCE_Pos) /*!< SysTick CTRL: CLKSOURCE Mask */
+
+#define SysTick_CTRL_TICKINT_Pos 1 /*!< SysTick CTRL: TICKINT Position */
+#define SysTick_CTRL_TICKINT_Msk (1UL << SysTick_CTRL_TICKINT_Pos) /*!< SysTick CTRL: TICKINT Mask */
+
+#define SysTick_CTRL_ENABLE_Pos 0 /*!< SysTick CTRL: ENABLE Position */
+#define SysTick_CTRL_ENABLE_Msk (1UL << SysTick_CTRL_ENABLE_Pos) /*!< SysTick CTRL: ENABLE Mask */
+
+/* SysTick Reload Register Definitions */
+#define SysTick_LOAD_RELOAD_Pos 0 /*!< SysTick LOAD: RELOAD Position */
+#define SysTick_LOAD_RELOAD_Msk (0xFFFFFFUL << SysTick_LOAD_RELOAD_Pos) /*!< SysTick LOAD: RELOAD Mask */
+
+/* SysTick Current Register Definitions */
+#define SysTick_VAL_CURRENT_Pos 0 /*!< SysTick VAL: CURRENT Position */
+#define SysTick_VAL_CURRENT_Msk (0xFFFFFFUL << SysTick_VAL_CURRENT_Pos) /*!< SysTick VAL: CURRENT Mask */
+
+/* SysTick Calibration Register Definitions */
+#define SysTick_CALIB_NOREF_Pos 31 /*!< SysTick CALIB: NOREF Position */
+#define SysTick_CALIB_NOREF_Msk (1UL << SysTick_CALIB_NOREF_Pos) /*!< SysTick CALIB: NOREF Mask */
+
+#define SysTick_CALIB_SKEW_Pos 30 /*!< SysTick CALIB: SKEW Position */
+#define SysTick_CALIB_SKEW_Msk (1UL << SysTick_CALIB_SKEW_Pos) /*!< SysTick CALIB: SKEW Mask */
+
+#define SysTick_CALIB_TENMS_Pos 0 /*!< SysTick CALIB: TENMS Position */
+#define SysTick_CALIB_TENMS_Msk (0xFFFFFFUL << SysTick_VAL_CURRENT_Pos) /*!< SysTick CALIB: TENMS Mask */
+
+/*@} end of group CMSIS_SysTick */
+
+
+/** \ingroup CMSIS_core_register
+ \defgroup CMSIS_ITM Instrumentation Trace Macrocell (ITM)
+ \brief Type definitions for the Instrumentation Trace Macrocell (ITM)
+ @{
+ */
+
+/** \brief Structure type to access the Instrumentation Trace Macrocell Register (ITM).
+ */
+typedef struct
+{
+ __O union
+ {
+ __O uint8_t u8; /*!< Offset: 0x000 ( /W) ITM Stimulus Port 8-bit */
+ __O uint16_t u16; /*!< Offset: 0x000 ( /W) ITM Stimulus Port 16-bit */
+ __O uint32_t u32; /*!< Offset: 0x000 ( /W) ITM Stimulus Port 32-bit */
+ } PORT [32]; /*!< Offset: 0x000 ( /W) ITM Stimulus Port Registers */
+ uint32_t RESERVED0[864];
+ __IO uint32_t TER; /*!< Offset: 0xE00 (R/W) ITM Trace Enable Register */
+ uint32_t RESERVED1[15];
+ __IO uint32_t TPR; /*!< Offset: 0xE40 (R/W) ITM Trace Privilege Register */
+ uint32_t RESERVED2[15];
+ __IO uint32_t TCR; /*!< Offset: 0xE80 (R/W) ITM Trace Control Register */
+ uint32_t RESERVED3[29];
+ __O uint32_t IWR; /*!< Offset: 0xEF8 ( /W) ITM Integration Write Register */
+ __I uint32_t IRR; /*!< Offset: 0xEFC (R/ ) ITM Integration Read Register */
+ __IO uint32_t IMCR; /*!< Offset: 0xF00 (R/W) ITM Integration Mode Control Register */
+ uint32_t RESERVED4[43];
+ __O uint32_t LAR; /*!< Offset: 0xFB0 ( /W) ITM Lock Access Register */
+ __I uint32_t LSR; /*!< Offset: 0xFB4 (R/ ) ITM Lock Status Register */
+ uint32_t RESERVED5[6];
+ __I uint32_t PID4; /*!< Offset: 0xFD0 (R/ ) ITM Peripheral Identification Register #4 */
+ __I uint32_t PID5; /*!< Offset: 0xFD4 (R/ ) ITM Peripheral Identification Register #5 */
+ __I uint32_t PID6; /*!< Offset: 0xFD8 (R/ ) ITM Peripheral Identification Register #6 */
+ __I uint32_t PID7; /*!< Offset: 0xFDC (R/ ) ITM Peripheral Identification Register #7 */
+ __I uint32_t PID0; /*!< Offset: 0xFE0 (R/ ) ITM Peripheral Identification Register #0 */
+ __I uint32_t PID1; /*!< Offset: 0xFE4 (R/ ) ITM Peripheral Identification Register #1 */
+ __I uint32_t PID2; /*!< Offset: 0xFE8 (R/ ) ITM Peripheral Identification Register #2 */
+ __I uint32_t PID3; /*!< Offset: 0xFEC (R/ ) ITM Peripheral Identification Register #3 */
+ __I uint32_t CID0; /*!< Offset: 0xFF0 (R/ ) ITM Component Identification Register #0 */
+ __I uint32_t CID1; /*!< Offset: 0xFF4 (R/ ) ITM Component Identification Register #1 */
+ __I uint32_t CID2; /*!< Offset: 0xFF8 (R/ ) ITM Component Identification Register #2 */
+ __I uint32_t CID3; /*!< Offset: 0xFFC (R/ ) ITM Component Identification Register #3 */
+} ITM_Type;
+
+/* ITM Trace Privilege Register Definitions */
+#define ITM_TPR_PRIVMASK_Pos 0 /*!< ITM TPR: PRIVMASK Position */
+#define ITM_TPR_PRIVMASK_Msk (0xFUL << ITM_TPR_PRIVMASK_Pos) /*!< ITM TPR: PRIVMASK Mask */
+
+/* ITM Trace Control Register Definitions */
+#define ITM_TCR_BUSY_Pos 23 /*!< ITM TCR: BUSY Position */
+#define ITM_TCR_BUSY_Msk (1UL << ITM_TCR_BUSY_Pos) /*!< ITM TCR: BUSY Mask */
+
+#define ITM_TCR_TraceBusID_Pos 16 /*!< ITM TCR: ATBID Position */
+#define ITM_TCR_TraceBusID_Msk (0x7FUL << ITM_TCR_TraceBusID_Pos) /*!< ITM TCR: ATBID Mask */
+
+#define ITM_TCR_GTSFREQ_Pos 10 /*!< ITM TCR: Global timestamp frequency Position */
+#define ITM_TCR_GTSFREQ_Msk (3UL << ITM_TCR_GTSFREQ_Pos) /*!< ITM TCR: Global timestamp frequency Mask */
+
+#define ITM_TCR_TSPrescale_Pos 8 /*!< ITM TCR: TSPrescale Position */
+#define ITM_TCR_TSPrescale_Msk (3UL << ITM_TCR_TSPrescale_Pos) /*!< ITM TCR: TSPrescale Mask */
+
+#define ITM_TCR_SWOENA_Pos 4 /*!< ITM TCR: SWOENA Position */
+#define ITM_TCR_SWOENA_Msk (1UL << ITM_TCR_SWOENA_Pos) /*!< ITM TCR: SWOENA Mask */
+
+#define ITM_TCR_DWTENA_Pos 3 /*!< ITM TCR: DWTENA Position */
+#define ITM_TCR_DWTENA_Msk (1UL << ITM_TCR_DWTENA_Pos) /*!< ITM TCR: DWTENA Mask */
+
+#define ITM_TCR_SYNCENA_Pos 2 /*!< ITM TCR: SYNCENA Position */
+#define ITM_TCR_SYNCENA_Msk (1UL << ITM_TCR_SYNCENA_Pos) /*!< ITM TCR: SYNCENA Mask */
+
+#define ITM_TCR_TSENA_Pos 1 /*!< ITM TCR: TSENA Position */
+#define ITM_TCR_TSENA_Msk (1UL << ITM_TCR_TSENA_Pos) /*!< ITM TCR: TSENA Mask */
+
+#define ITM_TCR_ITMENA_Pos 0 /*!< ITM TCR: ITM Enable bit Position */
+#define ITM_TCR_ITMENA_Msk (1UL << ITM_TCR_ITMENA_Pos) /*!< ITM TCR: ITM Enable bit Mask */
+
+/* ITM Integration Write Register Definitions */
+#define ITM_IWR_ATVALIDM_Pos 0 /*!< ITM IWR: ATVALIDM Position */
+#define ITM_IWR_ATVALIDM_Msk (1UL << ITM_IWR_ATVALIDM_Pos) /*!< ITM IWR: ATVALIDM Mask */
+
+/* ITM Integration Read Register Definitions */
+#define ITM_IRR_ATREADYM_Pos 0 /*!< ITM IRR: ATREADYM Position */
+#define ITM_IRR_ATREADYM_Msk (1UL << ITM_IRR_ATREADYM_Pos) /*!< ITM IRR: ATREADYM Mask */
+
+/* ITM Integration Mode Control Register Definitions */
+#define ITM_IMCR_INTEGRATION_Pos 0 /*!< ITM IMCR: INTEGRATION Position */
+#define ITM_IMCR_INTEGRATION_Msk (1UL << ITM_IMCR_INTEGRATION_Pos) /*!< ITM IMCR: INTEGRATION Mask */
+
+/* ITM Lock Status Register Definitions */
+#define ITM_LSR_ByteAcc_Pos 2 /*!< ITM LSR: ByteAcc Position */
+#define ITM_LSR_ByteAcc_Msk (1UL << ITM_LSR_ByteAcc_Pos) /*!< ITM LSR: ByteAcc Mask */
+
+#define ITM_LSR_Access_Pos 1 /*!< ITM LSR: Access Position */
+#define ITM_LSR_Access_Msk (1UL << ITM_LSR_Access_Pos) /*!< ITM LSR: Access Mask */
+
+#define ITM_LSR_Present_Pos 0 /*!< ITM LSR: Present Position */
+#define ITM_LSR_Present_Msk (1UL << ITM_LSR_Present_Pos) /*!< ITM LSR: Present Mask */
+
+/*@}*/ /* end of group CMSIS_ITM */
+
+
+/** \ingroup CMSIS_core_register
+ \defgroup CMSIS_DWT Data Watchpoint and Trace (DWT)
+ \brief Type definitions for the Data Watchpoint and Trace (DWT)
+ @{
+ */
+
+/** \brief Structure type to access the Data Watchpoint and Trace Register (DWT).
+ */
+typedef struct
+{
+ __IO uint32_t CTRL; /*!< Offset: 0x000 (R/W) Control Register */
+ __IO uint32_t CYCCNT; /*!< Offset: 0x004 (R/W) Cycle Count Register */
+ __IO uint32_t CPICNT; /*!< Offset: 0x008 (R/W) CPI Count Register */
+ __IO uint32_t EXCCNT; /*!< Offset: 0x00C (R/W) Exception Overhead Count Register */
+ __IO uint32_t SLEEPCNT; /*!< Offset: 0x010 (R/W) Sleep Count Register */
+ __IO uint32_t LSUCNT; /*!< Offset: 0x014 (R/W) LSU Count Register */
+ __IO uint32_t FOLDCNT; /*!< Offset: 0x018 (R/W) Folded-instruction Count Register */
+ __I uint32_t PCSR; /*!< Offset: 0x01C (R/ ) Program Counter Sample Register */
+ __IO uint32_t COMP0; /*!< Offset: 0x020 (R/W) Comparator Register 0 */
+ __IO uint32_t MASK0; /*!< Offset: 0x024 (R/W) Mask Register 0 */
+ __IO uint32_t FUNCTION0; /*!< Offset: 0x028 (R/W) Function Register 0 */
+ uint32_t RESERVED0[1];
+ __IO uint32_t COMP1; /*!< Offset: 0x030 (R/W) Comparator Register 1 */
+ __IO uint32_t MASK1; /*!< Offset: 0x034 (R/W) Mask Register 1 */
+ __IO uint32_t FUNCTION1; /*!< Offset: 0x038 (R/W) Function Register 1 */
+ uint32_t RESERVED1[1];
+ __IO uint32_t COMP2; /*!< Offset: 0x040 (R/W) Comparator Register 2 */
+ __IO uint32_t MASK2; /*!< Offset: 0x044 (R/W) Mask Register 2 */
+ __IO uint32_t FUNCTION2; /*!< Offset: 0x048 (R/W) Function Register 2 */
+ uint32_t RESERVED2[1];
+ __IO uint32_t COMP3; /*!< Offset: 0x050 (R/W) Comparator Register 3 */
+ __IO uint32_t MASK3; /*!< Offset: 0x054 (R/W) Mask Register 3 */
+ __IO uint32_t FUNCTION3; /*!< Offset: 0x058 (R/W) Function Register 3 */
+} DWT_Type;
+
+/* DWT Control Register Definitions */
+#define DWT_CTRL_NUMCOMP_Pos 28 /*!< DWT CTRL: NUMCOMP Position */
+#define DWT_CTRL_NUMCOMP_Msk (0xFUL << DWT_CTRL_NUMCOMP_Pos) /*!< DWT CTRL: NUMCOMP Mask */
+
+#define DWT_CTRL_NOTRCPKT_Pos 27 /*!< DWT CTRL: NOTRCPKT Position */
+#define DWT_CTRL_NOTRCPKT_Msk (0x1UL << DWT_CTRL_NOTRCPKT_Pos) /*!< DWT CTRL: NOTRCPKT Mask */
+
+#define DWT_CTRL_NOEXTTRIG_Pos 26 /*!< DWT CTRL: NOEXTTRIG Position */
+#define DWT_CTRL_NOEXTTRIG_Msk (0x1UL << DWT_CTRL_NOEXTTRIG_Pos) /*!< DWT CTRL: NOEXTTRIG Mask */
+
+#define DWT_CTRL_NOCYCCNT_Pos 25 /*!< DWT CTRL: NOCYCCNT Position */
+#define DWT_CTRL_NOCYCCNT_Msk (0x1UL << DWT_CTRL_NOCYCCNT_Pos) /*!< DWT CTRL: NOCYCCNT Mask */
+
+#define DWT_CTRL_NOPRFCNT_Pos 24 /*!< DWT CTRL: NOPRFCNT Position */
+#define DWT_CTRL_NOPRFCNT_Msk (0x1UL << DWT_CTRL_NOPRFCNT_Pos) /*!< DWT CTRL: NOPRFCNT Mask */
+
+#define DWT_CTRL_CYCEVTENA_Pos 22 /*!< DWT CTRL: CYCEVTENA Position */
+#define DWT_CTRL_CYCEVTENA_Msk (0x1UL << DWT_CTRL_CYCEVTENA_Pos) /*!< DWT CTRL: CYCEVTENA Mask */
+
+#define DWT_CTRL_FOLDEVTENA_Pos 21 /*!< DWT CTRL: FOLDEVTENA Position */
+#define DWT_CTRL_FOLDEVTENA_Msk (0x1UL << DWT_CTRL_FOLDEVTENA_Pos) /*!< DWT CTRL: FOLDEVTENA Mask */
+
+#define DWT_CTRL_LSUEVTENA_Pos 20 /*!< DWT CTRL: LSUEVTENA Position */
+#define DWT_CTRL_LSUEVTENA_Msk (0x1UL << DWT_CTRL_LSUEVTENA_Pos) /*!< DWT CTRL: LSUEVTENA Mask */
+
+#define DWT_CTRL_SLEEPEVTENA_Pos 19 /*!< DWT CTRL: SLEEPEVTENA Position */
+#define DWT_CTRL_SLEEPEVTENA_Msk (0x1UL << DWT_CTRL_SLEEPEVTENA_Pos) /*!< DWT CTRL: SLEEPEVTENA Mask */
+
+#define DWT_CTRL_EXCEVTENA_Pos 18 /*!< DWT CTRL: EXCEVTENA Position */
+#define DWT_CTRL_EXCEVTENA_Msk (0x1UL << DWT_CTRL_EXCEVTENA_Pos) /*!< DWT CTRL: EXCEVTENA Mask */
+
+#define DWT_CTRL_CPIEVTENA_Pos 17 /*!< DWT CTRL: CPIEVTENA Position */
+#define DWT_CTRL_CPIEVTENA_Msk (0x1UL << DWT_CTRL_CPIEVTENA_Pos) /*!< DWT CTRL: CPIEVTENA Mask */
+
+#define DWT_CTRL_EXCTRCENA_Pos 16 /*!< DWT CTRL: EXCTRCENA Position */
+#define DWT_CTRL_EXCTRCENA_Msk (0x1UL << DWT_CTRL_EXCTRCENA_Pos) /*!< DWT CTRL: EXCTRCENA Mask */
+
+#define DWT_CTRL_PCSAMPLENA_Pos 12 /*!< DWT CTRL: PCSAMPLENA Position */
+#define DWT_CTRL_PCSAMPLENA_Msk (0x1UL << DWT_CTRL_PCSAMPLENA_Pos) /*!< DWT CTRL: PCSAMPLENA Mask */
+
+#define DWT_CTRL_SYNCTAP_Pos 10 /*!< DWT CTRL: SYNCTAP Position */
+#define DWT_CTRL_SYNCTAP_Msk (0x3UL << DWT_CTRL_SYNCTAP_Pos) /*!< DWT CTRL: SYNCTAP Mask */
+
+#define DWT_CTRL_CYCTAP_Pos 9 /*!< DWT CTRL: CYCTAP Position */
+#define DWT_CTRL_CYCTAP_Msk (0x1UL << DWT_CTRL_CYCTAP_Pos) /*!< DWT CTRL: CYCTAP Mask */
+
+#define DWT_CTRL_POSTINIT_Pos 5 /*!< DWT CTRL: POSTINIT Position */
+#define DWT_CTRL_POSTINIT_Msk (0xFUL << DWT_CTRL_POSTINIT_Pos) /*!< DWT CTRL: POSTINIT Mask */
+
+#define DWT_CTRL_POSTPRESET_Pos 1 /*!< DWT CTRL: POSTPRESET Position */
+#define DWT_CTRL_POSTPRESET_Msk (0xFUL << DWT_CTRL_POSTPRESET_Pos) /*!< DWT CTRL: POSTPRESET Mask */
+
+#define DWT_CTRL_CYCCNTENA_Pos 0 /*!< DWT CTRL: CYCCNTENA Position */
+#define DWT_CTRL_CYCCNTENA_Msk (0x1UL << DWT_CTRL_CYCCNTENA_Pos) /*!< DWT CTRL: CYCCNTENA Mask */
+
+/* DWT CPI Count Register Definitions */
+#define DWT_CPICNT_CPICNT_Pos 0 /*!< DWT CPICNT: CPICNT Position */
+#define DWT_CPICNT_CPICNT_Msk (0xFFUL << DWT_CPICNT_CPICNT_Pos) /*!< DWT CPICNT: CPICNT Mask */
+
+/* DWT Exception Overhead Count Register Definitions */
+#define DWT_EXCCNT_EXCCNT_Pos 0 /*!< DWT EXCCNT: EXCCNT Position */
+#define DWT_EXCCNT_EXCCNT_Msk (0xFFUL << DWT_EXCCNT_EXCCNT_Pos) /*!< DWT EXCCNT: EXCCNT Mask */
+
+/* DWT Sleep Count Register Definitions */
+#define DWT_SLEEPCNT_SLEEPCNT_Pos 0 /*!< DWT SLEEPCNT: SLEEPCNT Position */
+#define DWT_SLEEPCNT_SLEEPCNT_Msk (0xFFUL << DWT_SLEEPCNT_SLEEPCNT_Pos) /*!< DWT SLEEPCNT: SLEEPCNT Mask */
+
+/* DWT LSU Count Register Definitions */
+#define DWT_LSUCNT_LSUCNT_Pos 0 /*!< DWT LSUCNT: LSUCNT Position */
+#define DWT_LSUCNT_LSUCNT_Msk (0xFFUL << DWT_LSUCNT_LSUCNT_Pos) /*!< DWT LSUCNT: LSUCNT Mask */
+
+/* DWT Folded-instruction Count Register Definitions */
+#define DWT_FOLDCNT_FOLDCNT_Pos 0 /*!< DWT FOLDCNT: FOLDCNT Position */
+#define DWT_FOLDCNT_FOLDCNT_Msk (0xFFUL << DWT_FOLDCNT_FOLDCNT_Pos) /*!< DWT FOLDCNT: FOLDCNT Mask */
+
+/* DWT Comparator Mask Register Definitions */
+#define DWT_MASK_MASK_Pos 0 /*!< DWT MASK: MASK Position */
+#define DWT_MASK_MASK_Msk (0x1FUL << DWT_MASK_MASK_Pos) /*!< DWT MASK: MASK Mask */
+
+/* DWT Comparator Function Register Definitions */
+#define DWT_FUNCTION_MATCHED_Pos 24 /*!< DWT FUNCTION: MATCHED Position */
+#define DWT_FUNCTION_MATCHED_Msk (0x1UL << DWT_FUNCTION_MATCHED_Pos) /*!< DWT FUNCTION: MATCHED Mask */
+
+#define DWT_FUNCTION_DATAVADDR1_Pos 16 /*!< DWT FUNCTION: DATAVADDR1 Position */
+#define DWT_FUNCTION_DATAVADDR1_Msk (0xFUL << DWT_FUNCTION_DATAVADDR1_Pos) /*!< DWT FUNCTION: DATAVADDR1 Mask */
+
+#define DWT_FUNCTION_DATAVADDR0_Pos 12 /*!< DWT FUNCTION: DATAVADDR0 Position */
+#define DWT_FUNCTION_DATAVADDR0_Msk (0xFUL << DWT_FUNCTION_DATAVADDR0_Pos) /*!< DWT FUNCTION: DATAVADDR0 Mask */
+
+#define DWT_FUNCTION_DATAVSIZE_Pos 10 /*!< DWT FUNCTION: DATAVSIZE Position */
+#define DWT_FUNCTION_DATAVSIZE_Msk (0x3UL << DWT_FUNCTION_DATAVSIZE_Pos) /*!< DWT FUNCTION: DATAVSIZE Mask */
+
+#define DWT_FUNCTION_LNK1ENA_Pos 9 /*!< DWT FUNCTION: LNK1ENA Position */
+#define DWT_FUNCTION_LNK1ENA_Msk (0x1UL << DWT_FUNCTION_LNK1ENA_Pos) /*!< DWT FUNCTION: LNK1ENA Mask */
+
+#define DWT_FUNCTION_DATAVMATCH_Pos 8 /*!< DWT FUNCTION: DATAVMATCH Position */
+#define DWT_FUNCTION_DATAVMATCH_Msk (0x1UL << DWT_FUNCTION_DATAVMATCH_Pos) /*!< DWT FUNCTION: DATAVMATCH Mask */
+
+#define DWT_FUNCTION_CYCMATCH_Pos 7 /*!< DWT FUNCTION: CYCMATCH Position */
+#define DWT_FUNCTION_CYCMATCH_Msk (0x1UL << DWT_FUNCTION_CYCMATCH_Pos) /*!< DWT FUNCTION: CYCMATCH Mask */
+
+#define DWT_FUNCTION_EMITRANGE_Pos 5 /*!< DWT FUNCTION: EMITRANGE Position */
+#define DWT_FUNCTION_EMITRANGE_Msk (0x1UL << DWT_FUNCTION_EMITRANGE_Pos) /*!< DWT FUNCTION: EMITRANGE Mask */
+
+#define DWT_FUNCTION_FUNCTION_Pos 0 /*!< DWT FUNCTION: FUNCTION Position */
+#define DWT_FUNCTION_FUNCTION_Msk (0xFUL << DWT_FUNCTION_FUNCTION_Pos) /*!< DWT FUNCTION: FUNCTION Mask */
+
+/*@}*/ /* end of group CMSIS_DWT */
+
+
+/** \ingroup CMSIS_core_register
+ \defgroup CMSIS_TPI Trace Port Interface (TPI)
+ \brief Type definitions for the Trace Port Interface (TPI)
+ @{
+ */
+
+/** \brief Structure type to access the Trace Port Interface Register (TPI).
+ */
+typedef struct
+{
+ __IO uint32_t SSPSR; /*!< Offset: 0x000 (R/ ) Supported Parallel Port Size Register */
+ __IO uint32_t CSPSR; /*!< Offset: 0x004 (R/W) Current Parallel Port Size Register */
+ uint32_t RESERVED0[2];
+ __IO uint32_t ACPR; /*!< Offset: 0x010 (R/W) Asynchronous Clock Prescaler Register */
+ uint32_t RESERVED1[55];
+ __IO uint32_t SPPR; /*!< Offset: 0x0F0 (R/W) Selected Pin Protocol Register */
+ uint32_t RESERVED2[131];
+ __I uint32_t FFSR; /*!< Offset: 0x300 (R/ ) Formatter and Flush Status Register */
+ __IO uint32_t FFCR; /*!< Offset: 0x304 (R/W) Formatter and Flush Control Register */
+ __I uint32_t FSCR; /*!< Offset: 0x308 (R/ ) Formatter Synchronization Counter Register */
+ uint32_t RESERVED3[759];
+ __I uint32_t TRIGGER; /*!< Offset: 0xEE8 (R/ ) TRIGGER */
+ __I uint32_t FIFO0; /*!< Offset: 0xEEC (R/ ) Integration ETM Data */
+ __I uint32_t ITATBCTR2; /*!< Offset: 0xEF0 (R/ ) ITATBCTR2 */
+ uint32_t RESERVED4[1];
+ __I uint32_t ITATBCTR0; /*!< Offset: 0xEF8 (R/ ) ITATBCTR0 */
+ __I uint32_t FIFO1; /*!< Offset: 0xEFC (R/ ) Integration ITM Data */
+ __IO uint32_t ITCTRL; /*!< Offset: 0xF00 (R/W) Integration Mode Control */
+ uint32_t RESERVED5[39];
+ __IO uint32_t CLAIMSET; /*!< Offset: 0xFA0 (R/W) Claim tag set */
+ __IO uint32_t CLAIMCLR; /*!< Offset: 0xFA4 (R/W) Claim tag clear */
+ uint32_t RESERVED7[8];
+ __I uint32_t DEVID; /*!< Offset: 0xFC8 (R/ ) TPIU_DEVID */
+ __I uint32_t DEVTYPE; /*!< Offset: 0xFCC (R/ ) TPIU_DEVTYPE */
+} TPI_Type;
+
+/* TPI Asynchronous Clock Prescaler Register Definitions */
+#define TPI_ACPR_PRESCALER_Pos 0 /*!< TPI ACPR: PRESCALER Position */
+#define TPI_ACPR_PRESCALER_Msk (0x1FFFUL << TPI_ACPR_PRESCALER_Pos) /*!< TPI ACPR: PRESCALER Mask */
+
+/* TPI Selected Pin Protocol Register Definitions */
+#define TPI_SPPR_TXMODE_Pos 0 /*!< TPI SPPR: TXMODE Position */
+#define TPI_SPPR_TXMODE_Msk (0x3UL << TPI_SPPR_TXMODE_Pos) /*!< TPI SPPR: TXMODE Mask */
+
+/* TPI Formatter and Flush Status Register Definitions */
+#define TPI_FFSR_FtNonStop_Pos 3 /*!< TPI FFSR: FtNonStop Position */
+#define TPI_FFSR_FtNonStop_Msk (0x1UL << TPI_FFSR_FtNonStop_Pos) /*!< TPI FFSR: FtNonStop Mask */
+
+#define TPI_FFSR_TCPresent_Pos 2 /*!< TPI FFSR: TCPresent Position */
+#define TPI_FFSR_TCPresent_Msk (0x1UL << TPI_FFSR_TCPresent_Pos) /*!< TPI FFSR: TCPresent Mask */
+
+#define TPI_FFSR_FtStopped_Pos 1 /*!< TPI FFSR: FtStopped Position */
+#define TPI_FFSR_FtStopped_Msk (0x1UL << TPI_FFSR_FtStopped_Pos) /*!< TPI FFSR: FtStopped Mask */
+
+#define TPI_FFSR_FlInProg_Pos 0 /*!< TPI FFSR: FlInProg Position */
+#define TPI_FFSR_FlInProg_Msk (0x1UL << TPI_FFSR_FlInProg_Pos) /*!< TPI FFSR: FlInProg Mask */
+
+/* TPI Formatter and Flush Control Register Definitions */
+#define TPI_FFCR_TrigIn_Pos 8 /*!< TPI FFCR: TrigIn Position */
+#define TPI_FFCR_TrigIn_Msk (0x1UL << TPI_FFCR_TrigIn_Pos) /*!< TPI FFCR: TrigIn Mask */
+
+#define TPI_FFCR_EnFCont_Pos 1 /*!< TPI FFCR: EnFCont Position */
+#define TPI_FFCR_EnFCont_Msk (0x1UL << TPI_FFCR_EnFCont_Pos) /*!< TPI FFCR: EnFCont Mask */
+
+/* TPI TRIGGER Register Definitions */
+#define TPI_TRIGGER_TRIGGER_Pos 0 /*!< TPI TRIGGER: TRIGGER Position */
+#define TPI_TRIGGER_TRIGGER_Msk (0x1UL << TPI_TRIGGER_TRIGGER_Pos) /*!< TPI TRIGGER: TRIGGER Mask */
+
+/* TPI Integration ETM Data Register Definitions (FIFO0) */
+#define TPI_FIFO0_ITM_ATVALID_Pos 29 /*!< TPI FIFO0: ITM_ATVALID Position */
+#define TPI_FIFO0_ITM_ATVALID_Msk (0x3UL << TPI_FIFO0_ITM_ATVALID_Pos) /*!< TPI FIFO0: ITM_ATVALID Mask */
+
+#define TPI_FIFO0_ITM_bytecount_Pos 27 /*!< TPI FIFO0: ITM_bytecount Position */
+#define TPI_FIFO0_ITM_bytecount_Msk (0x3UL << TPI_FIFO0_ITM_bytecount_Pos) /*!< TPI FIFO0: ITM_bytecount Mask */
+
+#define TPI_FIFO0_ETM_ATVALID_Pos 26 /*!< TPI FIFO0: ETM_ATVALID Position */
+#define TPI_FIFO0_ETM_ATVALID_Msk (0x3UL << TPI_FIFO0_ETM_ATVALID_Pos) /*!< TPI FIFO0: ETM_ATVALID Mask */
+
+#define TPI_FIFO0_ETM_bytecount_Pos 24 /*!< TPI FIFO0: ETM_bytecount Position */
+#define TPI_FIFO0_ETM_bytecount_Msk (0x3UL << TPI_FIFO0_ETM_bytecount_Pos) /*!< TPI FIFO0: ETM_bytecount Mask */
+
+#define TPI_FIFO0_ETM2_Pos 16 /*!< TPI FIFO0: ETM2 Position */
+#define TPI_FIFO0_ETM2_Msk (0xFFUL << TPI_FIFO0_ETM2_Pos) /*!< TPI FIFO0: ETM2 Mask */
+
+#define TPI_FIFO0_ETM1_Pos 8 /*!< TPI FIFO0: ETM1 Position */
+#define TPI_FIFO0_ETM1_Msk (0xFFUL << TPI_FIFO0_ETM1_Pos) /*!< TPI FIFO0: ETM1 Mask */
+
+#define TPI_FIFO0_ETM0_Pos 0 /*!< TPI FIFO0: ETM0 Position */
+#define TPI_FIFO0_ETM0_Msk (0xFFUL << TPI_FIFO0_ETM0_Pos) /*!< TPI FIFO0: ETM0 Mask */
+
+/* TPI ITATBCTR2 Register Definitions */
+#define TPI_ITATBCTR2_ATREADY_Pos 0 /*!< TPI ITATBCTR2: ATREADY Position */
+#define TPI_ITATBCTR2_ATREADY_Msk (0x1UL << TPI_ITATBCTR2_ATREADY_Pos) /*!< TPI ITATBCTR2: ATREADY Mask */
+
+/* TPI Integration ITM Data Register Definitions (FIFO1) */
+#define TPI_FIFO1_ITM_ATVALID_Pos 29 /*!< TPI FIFO1: ITM_ATVALID Position */
+#define TPI_FIFO1_ITM_ATVALID_Msk (0x3UL << TPI_FIFO1_ITM_ATVALID_Pos) /*!< TPI FIFO1: ITM_ATVALID Mask */
+
+#define TPI_FIFO1_ITM_bytecount_Pos 27 /*!< TPI FIFO1: ITM_bytecount Position */
+#define TPI_FIFO1_ITM_bytecount_Msk (0x3UL << TPI_FIFO1_ITM_bytecount_Pos) /*!< TPI FIFO1: ITM_bytecount Mask */
+
+#define TPI_FIFO1_ETM_ATVALID_Pos 26 /*!< TPI FIFO1: ETM_ATVALID Position */
+#define TPI_FIFO1_ETM_ATVALID_Msk (0x3UL << TPI_FIFO1_ETM_ATVALID_Pos) /*!< TPI FIFO1: ETM_ATVALID Mask */
+
+#define TPI_FIFO1_ETM_bytecount_Pos 24 /*!< TPI FIFO1: ETM_bytecount Position */
+#define TPI_FIFO1_ETM_bytecount_Msk (0x3UL << TPI_FIFO1_ETM_bytecount_Pos) /*!< TPI FIFO1: ETM_bytecount Mask */
+
+#define TPI_FIFO1_ITM2_Pos 16 /*!< TPI FIFO1: ITM2 Position */
+#define TPI_FIFO1_ITM2_Msk (0xFFUL << TPI_FIFO1_ITM2_Pos) /*!< TPI FIFO1: ITM2 Mask */
+
+#define TPI_FIFO1_ITM1_Pos 8 /*!< TPI FIFO1: ITM1 Position */
+#define TPI_FIFO1_ITM1_Msk (0xFFUL << TPI_FIFO1_ITM1_Pos) /*!< TPI FIFO1: ITM1 Mask */
+
+#define TPI_FIFO1_ITM0_Pos 0 /*!< TPI FIFO1: ITM0 Position */
+#define TPI_FIFO1_ITM0_Msk (0xFFUL << TPI_FIFO1_ITM0_Pos) /*!< TPI FIFO1: ITM0 Mask */
+
+/* TPI ITATBCTR0 Register Definitions */
+#define TPI_ITATBCTR0_ATREADY_Pos 0 /*!< TPI ITATBCTR0: ATREADY Position */
+#define TPI_ITATBCTR0_ATREADY_Msk (0x1UL << TPI_ITATBCTR0_ATREADY_Pos) /*!< TPI ITATBCTR0: ATREADY Mask */
+
+/* TPI Integration Mode Control Register Definitions */
+#define TPI_ITCTRL_Mode_Pos 0 /*!< TPI ITCTRL: Mode Position */
+#define TPI_ITCTRL_Mode_Msk (0x1UL << TPI_ITCTRL_Mode_Pos) /*!< TPI ITCTRL: Mode Mask */
+
+/* TPI DEVID Register Definitions */
+#define TPI_DEVID_NRZVALID_Pos 11 /*!< TPI DEVID: NRZVALID Position */
+#define TPI_DEVID_NRZVALID_Msk (0x1UL << TPI_DEVID_NRZVALID_Pos) /*!< TPI DEVID: NRZVALID Mask */
+
+#define TPI_DEVID_MANCVALID_Pos 10 /*!< TPI DEVID: MANCVALID Position */
+#define TPI_DEVID_MANCVALID_Msk (0x1UL << TPI_DEVID_MANCVALID_Pos) /*!< TPI DEVID: MANCVALID Mask */
+
+#define TPI_DEVID_PTINVALID_Pos 9 /*!< TPI DEVID: PTINVALID Position */
+#define TPI_DEVID_PTINVALID_Msk (0x1UL << TPI_DEVID_PTINVALID_Pos) /*!< TPI DEVID: PTINVALID Mask */
+
+#define TPI_DEVID_MinBufSz_Pos 6 /*!< TPI DEVID: MinBufSz Position */
+#define TPI_DEVID_MinBufSz_Msk (0x7UL << TPI_DEVID_MinBufSz_Pos) /*!< TPI DEVID: MinBufSz Mask */
+
+#define TPI_DEVID_AsynClkIn_Pos 5 /*!< TPI DEVID: AsynClkIn Position */
+#define TPI_DEVID_AsynClkIn_Msk (0x1UL << TPI_DEVID_AsynClkIn_Pos) /*!< TPI DEVID: AsynClkIn Mask */
+
+#define TPI_DEVID_NrTraceInput_Pos 0 /*!< TPI DEVID: NrTraceInput Position */
+#define TPI_DEVID_NrTraceInput_Msk (0x1FUL << TPI_DEVID_NrTraceInput_Pos) /*!< TPI DEVID: NrTraceInput Mask */
+
+/* TPI DEVTYPE Register Definitions */
+#define TPI_DEVTYPE_SubType_Pos 0 /*!< TPI DEVTYPE: SubType Position */
+#define TPI_DEVTYPE_SubType_Msk (0xFUL << TPI_DEVTYPE_SubType_Pos) /*!< TPI DEVTYPE: SubType Mask */
+
+#define TPI_DEVTYPE_MajorType_Pos 4 /*!< TPI DEVTYPE: MajorType Position */
+#define TPI_DEVTYPE_MajorType_Msk (0xFUL << TPI_DEVTYPE_MajorType_Pos) /*!< TPI DEVTYPE: MajorType Mask */
+
+/*@}*/ /* end of group CMSIS_TPI */
+
+
+#if (__MPU_PRESENT == 1)
+/** \ingroup CMSIS_core_register
+ \defgroup CMSIS_MPU Memory Protection Unit (MPU)
+ \brief Type definitions for the Memory Protection Unit (MPU)
+ @{
+ */
+
+/** \brief Structure type to access the Memory Protection Unit (MPU).
+ */
+typedef struct
+{
+ __I uint32_t TYPE; /*!< Offset: 0x000 (R/ ) MPU Type Register */
+ __IO uint32_t CTRL; /*!< Offset: 0x004 (R/W) MPU Control Register */
+ __IO uint32_t RNR; /*!< Offset: 0x008 (R/W) MPU Region RNRber Register */
+ __IO uint32_t RBAR; /*!< Offset: 0x00C (R/W) MPU Region Base Address Register */
+ __IO uint32_t RASR; /*!< Offset: 0x010 (R/W) MPU Region Attribute and Size Register */
+ __IO uint32_t RBAR_A1; /*!< Offset: 0x014 (R/W) MPU Alias 1 Region Base Address Register */
+ __IO uint32_t RASR_A1; /*!< Offset: 0x018 (R/W) MPU Alias 1 Region Attribute and Size Register */
+ __IO uint32_t RBAR_A2; /*!< Offset: 0x01C (R/W) MPU Alias 2 Region Base Address Register */
+ __IO uint32_t RASR_A2; /*!< Offset: 0x020 (R/W) MPU Alias 2 Region Attribute and Size Register */
+ __IO uint32_t RBAR_A3; /*!< Offset: 0x024 (R/W) MPU Alias 3 Region Base Address Register */
+ __IO uint32_t RASR_A3; /*!< Offset: 0x028 (R/W) MPU Alias 3 Region Attribute and Size Register */
+} MPU_Type;
+
+/* MPU Type Register */
+#define MPU_TYPE_IREGION_Pos 16 /*!< MPU TYPE: IREGION Position */
+#define MPU_TYPE_IREGION_Msk (0xFFUL << MPU_TYPE_IREGION_Pos) /*!< MPU TYPE: IREGION Mask */
+
+#define MPU_TYPE_DREGION_Pos 8 /*!< MPU TYPE: DREGION Position */
+#define MPU_TYPE_DREGION_Msk (0xFFUL << MPU_TYPE_DREGION_Pos) /*!< MPU TYPE: DREGION Mask */
+
+#define MPU_TYPE_SEPARATE_Pos 0 /*!< MPU TYPE: SEPARATE Position */
+#define MPU_TYPE_SEPARATE_Msk (1UL << MPU_TYPE_SEPARATE_Pos) /*!< MPU TYPE: SEPARATE Mask */
+
+/* MPU Control Register */
+#define MPU_CTRL_PRIVDEFENA_Pos 2 /*!< MPU CTRL: PRIVDEFENA Position */
+#define MPU_CTRL_PRIVDEFENA_Msk (1UL << MPU_CTRL_PRIVDEFENA_Pos) /*!< MPU CTRL: PRIVDEFENA Mask */
+
+#define MPU_CTRL_HFNMIENA_Pos 1 /*!< MPU CTRL: HFNMIENA Position */
+#define MPU_CTRL_HFNMIENA_Msk (1UL << MPU_CTRL_HFNMIENA_Pos) /*!< MPU CTRL: HFNMIENA Mask */
+
+#define MPU_CTRL_ENABLE_Pos 0 /*!< MPU CTRL: ENABLE Position */
+#define MPU_CTRL_ENABLE_Msk (1UL << MPU_CTRL_ENABLE_Pos) /*!< MPU CTRL: ENABLE Mask */
+
+/* MPU Region Number Register */
+#define MPU_RNR_REGION_Pos 0 /*!< MPU RNR: REGION Position */
+#define MPU_RNR_REGION_Msk (0xFFUL << MPU_RNR_REGION_Pos) /*!< MPU RNR: REGION Mask */
+
+/* MPU Region Base Address Register */
+#define MPU_RBAR_ADDR_Pos 5 /*!< MPU RBAR: ADDR Position */
+#define MPU_RBAR_ADDR_Msk (0x7FFFFFFUL << MPU_RBAR_ADDR_Pos) /*!< MPU RBAR: ADDR Mask */
+
+#define MPU_RBAR_VALID_Pos 4 /*!< MPU RBAR: VALID Position */
+#define MPU_RBAR_VALID_Msk (1UL << MPU_RBAR_VALID_Pos) /*!< MPU RBAR: VALID Mask */
+
+#define MPU_RBAR_REGION_Pos 0 /*!< MPU RBAR: REGION Position */
+#define MPU_RBAR_REGION_Msk (0xFUL << MPU_RBAR_REGION_Pos) /*!< MPU RBAR: REGION Mask */
+
+/* MPU Region Attribute and Size Register */
+#define MPU_RASR_ATTRS_Pos 16 /*!< MPU RASR: MPU Region Attribute field Position */
+#define MPU_RASR_ATTRS_Msk (0xFFFFUL << MPU_RASR_ATTRS_Pos) /*!< MPU RASR: MPU Region Attribute field Mask */
+
+#define MPU_RASR_XN_Pos 28 /*!< MPU RASR: ATTRS.XN Position */
+#define MPU_RASR_XN_Msk (1UL << MPU_RASR_XN_Pos) /*!< MPU RASR: ATTRS.XN Mask */
+
+#define MPU_RASR_AP_Pos 24 /*!< MPU RASR: ATTRS.AP Position */
+#define MPU_RASR_AP_Msk (0x7UL << MPU_RASR_AP_Pos) /*!< MPU RASR: ATTRS.AP Mask */
+
+#define MPU_RASR_TEX_Pos 19 /*!< MPU RASR: ATTRS.TEX Position */
+#define MPU_RASR_TEX_Msk (0x7UL << MPU_RASR_TEX_Pos) /*!< MPU RASR: ATTRS.TEX Mask */
+
+#define MPU_RASR_S_Pos 18 /*!< MPU RASR: ATTRS.S Position */
+#define MPU_RASR_S_Msk (1UL << MPU_RASR_S_Pos) /*!< MPU RASR: ATTRS.S Mask */
+
+#define MPU_RASR_C_Pos 17 /*!< MPU RASR: ATTRS.C Position */
+#define MPU_RASR_C_Msk (1UL << MPU_RASR_C_Pos) /*!< MPU RASR: ATTRS.C Mask */
+
+#define MPU_RASR_B_Pos 16 /*!< MPU RASR: ATTRS.B Position */
+#define MPU_RASR_B_Msk (1UL << MPU_RASR_B_Pos) /*!< MPU RASR: ATTRS.B Mask */
+
+#define MPU_RASR_SRD_Pos 8 /*!< MPU RASR: Sub-Region Disable Position */
+#define MPU_RASR_SRD_Msk (0xFFUL << MPU_RASR_SRD_Pos) /*!< MPU RASR: Sub-Region Disable Mask */
+
+#define MPU_RASR_SIZE_Pos 1 /*!< MPU RASR: Region Size Field Position */
+#define MPU_RASR_SIZE_Msk (0x1FUL << MPU_RASR_SIZE_Pos) /*!< MPU RASR: Region Size Field Mask */
+
+#define MPU_RASR_ENABLE_Pos 0 /*!< MPU RASR: Region enable bit Position */
+#define MPU_RASR_ENABLE_Msk (1UL << MPU_RASR_ENABLE_Pos) /*!< MPU RASR: Region enable bit Disable Mask */
+
+/*@} end of group CMSIS_MPU */
+#endif
+
+
+#if (__FPU_PRESENT == 1)
+/** \ingroup CMSIS_core_register
+ \defgroup CMSIS_FPU Floating Point Unit (FPU)
+ \brief Type definitions for the Floating Point Unit (FPU)
+ @{
+ */
+
+/** \brief Structure type to access the Floating Point Unit (FPU).
+ */
+typedef struct
+{
+ uint32_t RESERVED0[1];
+ __IO uint32_t FPCCR; /*!< Offset: 0x004 (R/W) Floating-Point Context Control Register */
+ __IO uint32_t FPCAR; /*!< Offset: 0x008 (R/W) Floating-Point Context Address Register */
+ __IO uint32_t FPDSCR; /*!< Offset: 0x00C (R/W) Floating-Point Default Status Control Register */
+ __I uint32_t MVFR0; /*!< Offset: 0x010 (R/ ) Media and FP Feature Register 0 */
+ __I uint32_t MVFR1; /*!< Offset: 0x014 (R/ ) Media and FP Feature Register 1 */
+} FPU_Type;
+
+/* Floating-Point Context Control Register */
+#define FPU_FPCCR_ASPEN_Pos 31 /*!< FPCCR: ASPEN bit Position */
+#define FPU_FPCCR_ASPEN_Msk (1UL << FPU_FPCCR_ASPEN_Pos) /*!< FPCCR: ASPEN bit Mask */
+
+#define FPU_FPCCR_LSPEN_Pos 30 /*!< FPCCR: LSPEN Position */
+#define FPU_FPCCR_LSPEN_Msk (1UL << FPU_FPCCR_LSPEN_Pos) /*!< FPCCR: LSPEN bit Mask */
+
+#define FPU_FPCCR_MONRDY_Pos 8 /*!< FPCCR: MONRDY Position */
+#define FPU_FPCCR_MONRDY_Msk (1UL << FPU_FPCCR_MONRDY_Pos) /*!< FPCCR: MONRDY bit Mask */
+
+#define FPU_FPCCR_BFRDY_Pos 6 /*!< FPCCR: BFRDY Position */
+#define FPU_FPCCR_BFRDY_Msk (1UL << FPU_FPCCR_BFRDY_Pos) /*!< FPCCR: BFRDY bit Mask */
+
+#define FPU_FPCCR_MMRDY_Pos 5 /*!< FPCCR: MMRDY Position */
+#define FPU_FPCCR_MMRDY_Msk (1UL << FPU_FPCCR_MMRDY_Pos) /*!< FPCCR: MMRDY bit Mask */
+
+#define FPU_FPCCR_HFRDY_Pos 4 /*!< FPCCR: HFRDY Position */
+#define FPU_FPCCR_HFRDY_Msk (1UL << FPU_FPCCR_HFRDY_Pos) /*!< FPCCR: HFRDY bit Mask */
+
+#define FPU_FPCCR_THREAD_Pos 3 /*!< FPCCR: processor mode bit Position */
+#define FPU_FPCCR_THREAD_Msk (1UL << FPU_FPCCR_THREAD_Pos) /*!< FPCCR: processor mode active bit Mask */
+
+#define FPU_FPCCR_USER_Pos 1 /*!< FPCCR: privilege level bit Position */
+#define FPU_FPCCR_USER_Msk (1UL << FPU_FPCCR_USER_Pos) /*!< FPCCR: privilege level bit Mask */
+
+#define FPU_FPCCR_LSPACT_Pos 0 /*!< FPCCR: Lazy state preservation active bit Position */
+#define FPU_FPCCR_LSPACT_Msk (1UL << FPU_FPCCR_LSPACT_Pos) /*!< FPCCR: Lazy state preservation active bit Mask */
+
+/* Floating-Point Context Address Register */
+#define FPU_FPCAR_ADDRESS_Pos 3 /*!< FPCAR: ADDRESS bit Position */
+#define FPU_FPCAR_ADDRESS_Msk (0x1FFFFFFFUL << FPU_FPCAR_ADDRESS_Pos) /*!< FPCAR: ADDRESS bit Mask */
+
+/* Floating-Point Default Status Control Register */
+#define FPU_FPDSCR_AHP_Pos 26 /*!< FPDSCR: AHP bit Position */
+#define FPU_FPDSCR_AHP_Msk (1UL << FPU_FPDSCR_AHP_Pos) /*!< FPDSCR: AHP bit Mask */
+
+#define FPU_FPDSCR_DN_Pos 25 /*!< FPDSCR: DN bit Position */
+#define FPU_FPDSCR_DN_Msk (1UL << FPU_FPDSCR_DN_Pos) /*!< FPDSCR: DN bit Mask */
+
+#define FPU_FPDSCR_FZ_Pos 24 /*!< FPDSCR: FZ bit Position */
+#define FPU_FPDSCR_FZ_Msk (1UL << FPU_FPDSCR_FZ_Pos) /*!< FPDSCR: FZ bit Mask */
+
+#define FPU_FPDSCR_RMode_Pos 22 /*!< FPDSCR: RMode bit Position */
+#define FPU_FPDSCR_RMode_Msk (3UL << FPU_FPDSCR_RMode_Pos) /*!< FPDSCR: RMode bit Mask */
+
+/* Media and FP Feature Register 0 */
+#define FPU_MVFR0_FP_rounding_modes_Pos 28 /*!< MVFR0: FP rounding modes bits Position */
+#define FPU_MVFR0_FP_rounding_modes_Msk (0xFUL << FPU_MVFR0_FP_rounding_modes_Pos) /*!< MVFR0: FP rounding modes bits Mask */
+
+#define FPU_MVFR0_Short_vectors_Pos 24 /*!< MVFR0: Short vectors bits Position */
+#define FPU_MVFR0_Short_vectors_Msk (0xFUL << FPU_MVFR0_Short_vectors_Pos) /*!< MVFR0: Short vectors bits Mask */
+
+#define FPU_MVFR0_Square_root_Pos 20 /*!< MVFR0: Square root bits Position */
+#define FPU_MVFR0_Square_root_Msk (0xFUL << FPU_MVFR0_Square_root_Pos) /*!< MVFR0: Square root bits Mask */
+
+#define FPU_MVFR0_Divide_Pos 16 /*!< MVFR0: Divide bits Position */
+#define FPU_MVFR0_Divide_Msk (0xFUL << FPU_MVFR0_Divide_Pos) /*!< MVFR0: Divide bits Mask */
+
+#define FPU_MVFR0_FP_excep_trapping_Pos 12 /*!< MVFR0: FP exception trapping bits Position */
+#define FPU_MVFR0_FP_excep_trapping_Msk (0xFUL << FPU_MVFR0_FP_excep_trapping_Pos) /*!< MVFR0: FP exception trapping bits Mask */
+
+#define FPU_MVFR0_Double_precision_Pos 8 /*!< MVFR0: Double-precision bits Position */
+#define FPU_MVFR0_Double_precision_Msk (0xFUL << FPU_MVFR0_Double_precision_Pos) /*!< MVFR0: Double-precision bits Mask */
+
+#define FPU_MVFR0_Single_precision_Pos 4 /*!< MVFR0: Single-precision bits Position */
+#define FPU_MVFR0_Single_precision_Msk (0xFUL << FPU_MVFR0_Single_precision_Pos) /*!< MVFR0: Single-precision bits Mask */
+
+#define FPU_MVFR0_A_SIMD_registers_Pos 0 /*!< MVFR0: A_SIMD registers bits Position */
+#define FPU_MVFR0_A_SIMD_registers_Msk (0xFUL << FPU_MVFR0_A_SIMD_registers_Pos) /*!< MVFR0: A_SIMD registers bits Mask */
+
+/* Media and FP Feature Register 1 */
+#define FPU_MVFR1_FP_fused_MAC_Pos 28 /*!< MVFR1: FP fused MAC bits Position */
+#define FPU_MVFR1_FP_fused_MAC_Msk (0xFUL << FPU_MVFR1_FP_fused_MAC_Pos) /*!< MVFR1: FP fused MAC bits Mask */
+
+#define FPU_MVFR1_FP_HPFP_Pos 24 /*!< MVFR1: FP HPFP bits Position */
+#define FPU_MVFR1_FP_HPFP_Msk (0xFUL << FPU_MVFR1_FP_HPFP_Pos) /*!< MVFR1: FP HPFP bits Mask */
+
+#define FPU_MVFR1_D_NaN_mode_Pos 4 /*!< MVFR1: D_NaN mode bits Position */
+#define FPU_MVFR1_D_NaN_mode_Msk (0xFUL << FPU_MVFR1_D_NaN_mode_Pos) /*!< MVFR1: D_NaN mode bits Mask */
+
+#define FPU_MVFR1_FtZ_mode_Pos 0 /*!< MVFR1: FtZ mode bits Position */
+#define FPU_MVFR1_FtZ_mode_Msk (0xFUL << FPU_MVFR1_FtZ_mode_Pos) /*!< MVFR1: FtZ mode bits Mask */
+
+/*@} end of group CMSIS_FPU */
+#endif
+
+
+/** \ingroup CMSIS_core_register
+ \defgroup CMSIS_CoreDebug Core Debug Registers (CoreDebug)
+ \brief Type definitions for the Core Debug Registers
+ @{
+ */
+
+/** \brief Structure type to access the Core Debug Register (CoreDebug).
+ */
+typedef struct
+{
+ __IO uint32_t DHCSR; /*!< Offset: 0x000 (R/W) Debug Halting Control and Status Register */
+ __O uint32_t DCRSR; /*!< Offset: 0x004 ( /W) Debug Core Register Selector Register */
+ __IO uint32_t DCRDR; /*!< Offset: 0x008 (R/W) Debug Core Register Data Register */
+ __IO uint32_t DEMCR; /*!< Offset: 0x00C (R/W) Debug Exception and Monitor Control Register */
+} CoreDebug_Type;
+
+/* Debug Halting Control and Status Register */
+#define CoreDebug_DHCSR_DBGKEY_Pos 16 /*!< CoreDebug DHCSR: DBGKEY Position */
+#define CoreDebug_DHCSR_DBGKEY_Msk (0xFFFFUL << CoreDebug_DHCSR_DBGKEY_Pos) /*!< CoreDebug DHCSR: DBGKEY Mask */
+
+#define CoreDebug_DHCSR_S_RESET_ST_Pos 25 /*!< CoreDebug DHCSR: S_RESET_ST Position */
+#define CoreDebug_DHCSR_S_RESET_ST_Msk (1UL << CoreDebug_DHCSR_S_RESET_ST_Pos) /*!< CoreDebug DHCSR: S_RESET_ST Mask */
+
+#define CoreDebug_DHCSR_S_RETIRE_ST_Pos 24 /*!< CoreDebug DHCSR: S_RETIRE_ST Position */
+#define CoreDebug_DHCSR_S_RETIRE_ST_Msk (1UL << CoreDebug_DHCSR_S_RETIRE_ST_Pos) /*!< CoreDebug DHCSR: S_RETIRE_ST Mask */
+
+#define CoreDebug_DHCSR_S_LOCKUP_Pos 19 /*!< CoreDebug DHCSR: S_LOCKUP Position */
+#define CoreDebug_DHCSR_S_LOCKUP_Msk (1UL << CoreDebug_DHCSR_S_LOCKUP_Pos) /*!< CoreDebug DHCSR: S_LOCKUP Mask */
+
+#define CoreDebug_DHCSR_S_SLEEP_Pos 18 /*!< CoreDebug DHCSR: S_SLEEP Position */
+#define CoreDebug_DHCSR_S_SLEEP_Msk (1UL << CoreDebug_DHCSR_S_SLEEP_Pos) /*!< CoreDebug DHCSR: S_SLEEP Mask */
+
+#define CoreDebug_DHCSR_S_HALT_Pos 17 /*!< CoreDebug DHCSR: S_HALT Position */
+#define CoreDebug_DHCSR_S_HALT_Msk (1UL << CoreDebug_DHCSR_S_HALT_Pos) /*!< CoreDebug DHCSR: S_HALT Mask */
+
+#define CoreDebug_DHCSR_S_REGRDY_Pos 16 /*!< CoreDebug DHCSR: S_REGRDY Position */
+#define CoreDebug_DHCSR_S_REGRDY_Msk (1UL << CoreDebug_DHCSR_S_REGRDY_Pos) /*!< CoreDebug DHCSR: S_REGRDY Mask */
+
+#define CoreDebug_DHCSR_C_SNAPSTALL_Pos 5 /*!< CoreDebug DHCSR: C_SNAPSTALL Position */
+#define CoreDebug_DHCSR_C_SNAPSTALL_Msk (1UL << CoreDebug_DHCSR_C_SNAPSTALL_Pos) /*!< CoreDebug DHCSR: C_SNAPSTALL Mask */
+
+#define CoreDebug_DHCSR_C_MASKINTS_Pos 3 /*!< CoreDebug DHCSR: C_MASKINTS Position */
+#define CoreDebug_DHCSR_C_MASKINTS_Msk (1UL << CoreDebug_DHCSR_C_MASKINTS_Pos) /*!< CoreDebug DHCSR: C_MASKINTS Mask */
+
+#define CoreDebug_DHCSR_C_STEP_Pos 2 /*!< CoreDebug DHCSR: C_STEP Position */
+#define CoreDebug_DHCSR_C_STEP_Msk (1UL << CoreDebug_DHCSR_C_STEP_Pos) /*!< CoreDebug DHCSR: C_STEP Mask */
+
+#define CoreDebug_DHCSR_C_HALT_Pos 1 /*!< CoreDebug DHCSR: C_HALT Position */
+#define CoreDebug_DHCSR_C_HALT_Msk (1UL << CoreDebug_DHCSR_C_HALT_Pos) /*!< CoreDebug DHCSR: C_HALT Mask */
+
+#define CoreDebug_DHCSR_C_DEBUGEN_Pos 0 /*!< CoreDebug DHCSR: C_DEBUGEN Position */
+#define CoreDebug_DHCSR_C_DEBUGEN_Msk (1UL << CoreDebug_DHCSR_C_DEBUGEN_Pos) /*!< CoreDebug DHCSR: C_DEBUGEN Mask */
+
+/* Debug Core Register Selector Register */
+#define CoreDebug_DCRSR_REGWnR_Pos 16 /*!< CoreDebug DCRSR: REGWnR Position */
+#define CoreDebug_DCRSR_REGWnR_Msk (1UL << CoreDebug_DCRSR_REGWnR_Pos) /*!< CoreDebug DCRSR: REGWnR Mask */
+
+#define CoreDebug_DCRSR_REGSEL_Pos 0 /*!< CoreDebug DCRSR: REGSEL Position */
+#define CoreDebug_DCRSR_REGSEL_Msk (0x1FUL << CoreDebug_DCRSR_REGSEL_Pos) /*!< CoreDebug DCRSR: REGSEL Mask */
+
+/* Debug Exception and Monitor Control Register */
+#define CoreDebug_DEMCR_TRCENA_Pos 24 /*!< CoreDebug DEMCR: TRCENA Position */
+#define CoreDebug_DEMCR_TRCENA_Msk (1UL << CoreDebug_DEMCR_TRCENA_Pos) /*!< CoreDebug DEMCR: TRCENA Mask */
+
+#define CoreDebug_DEMCR_MON_REQ_Pos 19 /*!< CoreDebug DEMCR: MON_REQ Position */
+#define CoreDebug_DEMCR_MON_REQ_Msk (1UL << CoreDebug_DEMCR_MON_REQ_Pos) /*!< CoreDebug DEMCR: MON_REQ Mask */
+
+#define CoreDebug_DEMCR_MON_STEP_Pos 18 /*!< CoreDebug DEMCR: MON_STEP Position */
+#define CoreDebug_DEMCR_MON_STEP_Msk (1UL << CoreDebug_DEMCR_MON_STEP_Pos) /*!< CoreDebug DEMCR: MON_STEP Mask */
+
+#define CoreDebug_DEMCR_MON_PEND_Pos 17 /*!< CoreDebug DEMCR: MON_PEND Position */
+#define CoreDebug_DEMCR_MON_PEND_Msk (1UL << CoreDebug_DEMCR_MON_PEND_Pos) /*!< CoreDebug DEMCR: MON_PEND Mask */
+
+#define CoreDebug_DEMCR_MON_EN_Pos 16 /*!< CoreDebug DEMCR: MON_EN Position */
+#define CoreDebug_DEMCR_MON_EN_Msk (1UL << CoreDebug_DEMCR_MON_EN_Pos) /*!< CoreDebug DEMCR: MON_EN Mask */
+
+#define CoreDebug_DEMCR_VC_HARDERR_Pos 10 /*!< CoreDebug DEMCR: VC_HARDERR Position */
+#define CoreDebug_DEMCR_VC_HARDERR_Msk (1UL << CoreDebug_DEMCR_VC_HARDERR_Pos) /*!< CoreDebug DEMCR: VC_HARDERR Mask */
+
+#define CoreDebug_DEMCR_VC_INTERR_Pos 9 /*!< CoreDebug DEMCR: VC_INTERR Position */
+#define CoreDebug_DEMCR_VC_INTERR_Msk (1UL << CoreDebug_DEMCR_VC_INTERR_Pos) /*!< CoreDebug DEMCR: VC_INTERR Mask */
+
+#define CoreDebug_DEMCR_VC_BUSERR_Pos 8 /*!< CoreDebug DEMCR: VC_BUSERR Position */
+#define CoreDebug_DEMCR_VC_BUSERR_Msk (1UL << CoreDebug_DEMCR_VC_BUSERR_Pos) /*!< CoreDebug DEMCR: VC_BUSERR Mask */
+
+#define CoreDebug_DEMCR_VC_STATERR_Pos 7 /*!< CoreDebug DEMCR: VC_STATERR Position */
+#define CoreDebug_DEMCR_VC_STATERR_Msk (1UL << CoreDebug_DEMCR_VC_STATERR_Pos) /*!< CoreDebug DEMCR: VC_STATERR Mask */
+
+#define CoreDebug_DEMCR_VC_CHKERR_Pos 6 /*!< CoreDebug DEMCR: VC_CHKERR Position */
+#define CoreDebug_DEMCR_VC_CHKERR_Msk (1UL << CoreDebug_DEMCR_VC_CHKERR_Pos) /*!< CoreDebug DEMCR: VC_CHKERR Mask */
+
+#define CoreDebug_DEMCR_VC_NOCPERR_Pos 5 /*!< CoreDebug DEMCR: VC_NOCPERR Position */
+#define CoreDebug_DEMCR_VC_NOCPERR_Msk (1UL << CoreDebug_DEMCR_VC_NOCPERR_Pos) /*!< CoreDebug DEMCR: VC_NOCPERR Mask */
+
+#define CoreDebug_DEMCR_VC_MMERR_Pos 4 /*!< CoreDebug DEMCR: VC_MMERR Position */
+#define CoreDebug_DEMCR_VC_MMERR_Msk (1UL << CoreDebug_DEMCR_VC_MMERR_Pos) /*!< CoreDebug DEMCR: VC_MMERR Mask */
+
+#define CoreDebug_DEMCR_VC_CORERESET_Pos 0 /*!< CoreDebug DEMCR: VC_CORERESET Position */
+#define CoreDebug_DEMCR_VC_CORERESET_Msk (1UL << CoreDebug_DEMCR_VC_CORERESET_Pos) /*!< CoreDebug DEMCR: VC_CORERESET Mask */
+
+/*@} end of group CMSIS_CoreDebug */
+
+
+/** \ingroup CMSIS_core_register
+ \defgroup CMSIS_core_base Core Definitions
+ \brief Definitions for base addresses, unions, and structures.
+ @{
+ */
+
+/* Memory mapping of Cortex-M4 Hardware */
+#define SCS_BASE (0xE000E000UL) /*!< System Control Space Base Address */
+#define ITM_BASE (0xE0000000UL) /*!< ITM Base Address */
+#define DWT_BASE (0xE0001000UL) /*!< DWT Base Address */
+#define TPI_BASE (0xE0040000UL) /*!< TPI Base Address */
+#define CoreDebug_BASE (0xE000EDF0UL) /*!< Core Debug Base Address */
+#define SysTick_BASE (SCS_BASE + 0x0010UL) /*!< SysTick Base Address */
+#define NVIC_BASE (SCS_BASE + 0x0100UL) /*!< NVIC Base Address */
+#define SCB_BASE (SCS_BASE + 0x0D00UL) /*!< System Control Block Base Address */
+
+#define SCnSCB ((SCnSCB_Type *) SCS_BASE ) /*!< System control Register not in SCB */
+#define SCB ((SCB_Type *) SCB_BASE ) /*!< SCB configuration struct */
+#define SysTick ((SysTick_Type *) SysTick_BASE ) /*!< SysTick configuration struct */
+#define NVIC ((NVIC_Type *) NVIC_BASE ) /*!< NVIC configuration struct */
+#define ITM ((ITM_Type *) ITM_BASE ) /*!< ITM configuration struct */
+#define DWT ((DWT_Type *) DWT_BASE ) /*!< DWT configuration struct */
+#define TPI ((TPI_Type *) TPI_BASE ) /*!< TPI configuration struct */
+#define CoreDebug ((CoreDebug_Type *) CoreDebug_BASE) /*!< Core Debug configuration struct */
+
+#if (__MPU_PRESENT == 1)
+ #define MPU_BASE (SCS_BASE + 0x0D90UL) /*!< Memory Protection Unit */
+ #define MPU ((MPU_Type *) MPU_BASE ) /*!< Memory Protection Unit */
+#endif
+
+#if (__FPU_PRESENT == 1)
+ #define FPU_BASE (SCS_BASE + 0x0F30UL) /*!< Floating Point Unit */
+ #define FPU ((FPU_Type *) FPU_BASE ) /*!< Floating Point Unit */
+#endif
+
+/*@} */
+
+
+
+/*******************************************************************************
+ * Hardware Abstraction Layer
+ Core Function Interface contains:
+ - Core NVIC Functions
+ - Core SysTick Functions
+ - Core Debug Functions
+ - Core Register Access Functions
+ ******************************************************************************/
+/** \defgroup CMSIS_Core_FunctionInterface Functions and Instructions Reference
+*/
+
+
+
+/* ########################## NVIC functions #################################### */
+/** \ingroup CMSIS_Core_FunctionInterface
+ \defgroup CMSIS_Core_NVICFunctions NVIC Functions
+ \brief Functions that manage interrupts and exceptions via the NVIC.
+ @{
+ */
+
+/** \brief Set Priority Grouping
+
+ The function sets the priority grouping field using the required unlock sequence.
+ The parameter PriorityGroup is assigned to the field SCB->AIRCR [10:8] PRIGROUP field.
+ Only values from 0..7 are used.
+ In case of a conflict between priority grouping and available
+ priority bits (__NVIC_PRIO_BITS), the smallest possible priority group is set.
+
+ \param [in] PriorityGroup Priority grouping field.
+ */
+__STATIC_INLINE void NVIC_SetPriorityGrouping(uint32_t PriorityGroup)
+{
+ uint32_t reg_value;
+ uint32_t PriorityGroupTmp = (PriorityGroup & (uint32_t)0x07); /* only values 0..7 are used */
+
+ reg_value = SCB->AIRCR; /* read old register configuration */
+ reg_value &= ~(SCB_AIRCR_VECTKEY_Msk | SCB_AIRCR_PRIGROUP_Msk); /* clear bits to change */
+ reg_value = (reg_value |
+ ((uint32_t)0x5FA << SCB_AIRCR_VECTKEY_Pos) |
+ (PriorityGroupTmp << 8)); /* Insert write key and priorty group */
+ SCB->AIRCR = reg_value;
+}
+
+
+/** \brief Get Priority Grouping
+
+ The function reads the priority grouping field from the NVIC Interrupt Controller.
+
+ \return Priority grouping field (SCB->AIRCR [10:8] PRIGROUP field).
+ */
+__STATIC_INLINE uint32_t NVIC_GetPriorityGrouping(void)
+{
+ return ((SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) >> SCB_AIRCR_PRIGROUP_Pos); /* read priority grouping field */
+}
+
+
+/** \brief Enable External Interrupt
+
+ The function enables a device-specific interrupt in the NVIC interrupt controller.
+
+ \param [in] IRQn External interrupt number. Value cannot be negative.
+ */
+__STATIC_INLINE void NVIC_EnableIRQ(IRQn_Type IRQn)
+{
+/* NVIC->ISER[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); enable interrupt */
+ NVIC->ISER[(uint32_t)((int32_t)IRQn) >> 5] = (uint32_t)(1 << ((uint32_t)((int32_t)IRQn) & (uint32_t)0x1F)); /* enable interrupt */
+}
+
+
+/** \brief Disable External Interrupt
+
+ The function disables a device-specific interrupt in the NVIC interrupt controller.
+
+ \param [in] IRQn External interrupt number. Value cannot be negative.
+ */
+__STATIC_INLINE void NVIC_DisableIRQ(IRQn_Type IRQn)
+{
+ NVIC->ICER[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* disable interrupt */
+}
+
+
+/** \brief Get Pending Interrupt
+
+ The function reads the pending register in the NVIC and returns the pending bit
+ for the specified interrupt.
+
+ \param [in] IRQn Interrupt number.
+
+ \return 0 Interrupt status is not pending.
+ \return 1 Interrupt status is pending.
+ */
+__STATIC_INLINE uint32_t NVIC_GetPendingIRQ(IRQn_Type IRQn)
+{
+ return((uint32_t) ((NVIC->ISPR[(uint32_t)(IRQn) >> 5] & (1 << ((uint32_t)(IRQn) & 0x1F)))?1:0)); /* Return 1 if pending else 0 */
+}
+
+
+/** \brief Set Pending Interrupt
+
+ The function sets the pending bit of an external interrupt.
+
+ \param [in] IRQn Interrupt number. Value cannot be negative.
+ */
+__STATIC_INLINE void NVIC_SetPendingIRQ(IRQn_Type IRQn)
+{
+ NVIC->ISPR[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* set interrupt pending */
+}
+
+
+/** \brief Clear Pending Interrupt
+
+ The function clears the pending bit of an external interrupt.
+
+ \param [in] IRQn External interrupt number. Value cannot be negative.
+ */
+__STATIC_INLINE void NVIC_ClearPendingIRQ(IRQn_Type IRQn)
+{
+ NVIC->ICPR[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* Clear pending interrupt */
+}
+
+
+/** \brief Get Active Interrupt
+
+ The function reads the active register in NVIC and returns the active bit.
+
+ \param [in] IRQn Interrupt number.
+
+ \return 0 Interrupt status is not active.
+ \return 1 Interrupt status is active.
+ */
+__STATIC_INLINE uint32_t NVIC_GetActive(IRQn_Type IRQn)
+{
+ return((uint32_t)((NVIC->IABR[(uint32_t)(IRQn) >> 5] & (1 << ((uint32_t)(IRQn) & 0x1F)))?1:0)); /* Return 1 if active else 0 */
+}
+
+
+/** \brief Set Interrupt Priority
+
+ The function sets the priority of an interrupt.
+
+ \note The priority cannot be set for every core interrupt.
+
+ \param [in] IRQn Interrupt number.
+ \param [in] priority Priority to set.
+ */
+__STATIC_INLINE void NVIC_SetPriority(IRQn_Type IRQn, uint32_t priority)
+{
+ if(IRQn < 0) {
+ SCB->SHP[((uint32_t)(IRQn) & 0xF)-4] = ((priority << (8 - __NVIC_PRIO_BITS)) & 0xff); } /* set Priority for Cortex-M System Interrupts */
+ else {
+ NVIC->IP[(uint32_t)(IRQn)] = ((priority << (8 - __NVIC_PRIO_BITS)) & 0xff); } /* set Priority for device specific Interrupts */
+}
+
+
+/** \brief Get Interrupt Priority
+
+ The function reads the priority of an interrupt. The interrupt
+ number can be positive to specify an external (device specific)
+ interrupt, or negative to specify an internal (core) interrupt.
+
+
+ \param [in] IRQn Interrupt number.
+ \return Interrupt Priority. Value is aligned automatically to the implemented
+ priority bits of the microcontroller.
+ */
+__STATIC_INLINE uint32_t NVIC_GetPriority(IRQn_Type IRQn)
+{
+
+ if(IRQn < 0) {
+ return((uint32_t)(SCB->SHP[((uint32_t)(IRQn) & 0xF)-4] >> (8 - __NVIC_PRIO_BITS))); } /* get priority for Cortex-M system interrupts */
+ else {
+ return((uint32_t)(NVIC->IP[(uint32_t)(IRQn)] >> (8 - __NVIC_PRIO_BITS))); } /* get priority for device specific interrupts */
+}
+
+
+/** \brief Encode Priority
+
+ The function encodes the priority for an interrupt with the given priority group,
+ preemptive priority value, and subpriority value.
+ In case of a conflict between priority grouping and available
+ priority bits (__NVIC_PRIO_BITS), the samllest possible priority group is set.
+
+ \param [in] PriorityGroup Used priority group.
+ \param [in] PreemptPriority Preemptive priority value (starting from 0).
+ \param [in] SubPriority Subpriority value (starting from 0).
+ \return Encoded priority. Value can be used in the function \ref NVIC_SetPriority().
+ */
+__STATIC_INLINE uint32_t NVIC_EncodePriority (uint32_t PriorityGroup, uint32_t PreemptPriority, uint32_t SubPriority)
+{
+ uint32_t PriorityGroupTmp = (PriorityGroup & 0x07); /* only values 0..7 are used */
+ uint32_t PreemptPriorityBits;
+ uint32_t SubPriorityBits;
+
+ PreemptPriorityBits = ((7 - PriorityGroupTmp) > __NVIC_PRIO_BITS) ? __NVIC_PRIO_BITS : 7 - PriorityGroupTmp;
+ SubPriorityBits = ((PriorityGroupTmp + __NVIC_PRIO_BITS) < 7) ? 0 : PriorityGroupTmp - 7 + __NVIC_PRIO_BITS;
+
+ return (
+ ((PreemptPriority & ((1 << (PreemptPriorityBits)) - 1)) << SubPriorityBits) |
+ ((SubPriority & ((1 << (SubPriorityBits )) - 1)))
+ );
+}
+
+
+/** \brief Decode Priority
+
+ The function decodes an interrupt priority value with a given priority group to
+ preemptive priority value and subpriority value.
+ In case of a conflict between priority grouping and available
+ priority bits (__NVIC_PRIO_BITS) the samllest possible priority group is set.
+
+ \param [in] Priority Priority value, which can be retrieved with the function \ref NVIC_GetPriority().
+ \param [in] PriorityGroup Used priority group.
+ \param [out] pPreemptPriority Preemptive priority value (starting from 0).
+ \param [out] pSubPriority Subpriority value (starting from 0).
+ */
+__STATIC_INLINE void NVIC_DecodePriority (uint32_t Priority, uint32_t PriorityGroup, uint32_t* pPreemptPriority, uint32_t* pSubPriority)
+{
+ uint32_t PriorityGroupTmp = (PriorityGroup & 0x07); /* only values 0..7 are used */
+ uint32_t PreemptPriorityBits;
+ uint32_t SubPriorityBits;
+
+ PreemptPriorityBits = ((7 - PriorityGroupTmp) > __NVIC_PRIO_BITS) ? __NVIC_PRIO_BITS : 7 - PriorityGroupTmp;
+ SubPriorityBits = ((PriorityGroupTmp + __NVIC_PRIO_BITS) < 7) ? 0 : PriorityGroupTmp - 7 + __NVIC_PRIO_BITS;
+
+ *pPreemptPriority = (Priority >> SubPriorityBits) & ((1 << (PreemptPriorityBits)) - 1);
+ *pSubPriority = (Priority ) & ((1 << (SubPriorityBits )) - 1);
+}
+
+
+/** \brief System Reset
+
+ The function initiates a system reset request to reset the MCU.
+ */
+__STATIC_INLINE void NVIC_SystemReset(void)
+{
+ __DSB(); /* Ensure all outstanding memory accesses included
+ buffered write are completed before reset */
+ SCB->AIRCR = ((0x5FA << SCB_AIRCR_VECTKEY_Pos) |
+ (SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) |
+ SCB_AIRCR_SYSRESETREQ_Msk); /* Keep priority group unchanged */
+ __DSB(); /* Ensure completion of memory access */
+ while(1); /* wait until reset */
+}
+
+/*@} end of CMSIS_Core_NVICFunctions */
+
+
+
+/* ################################## SysTick function ############################################ */
+/** \ingroup CMSIS_Core_FunctionInterface
+ \defgroup CMSIS_Core_SysTickFunctions SysTick Functions
+ \brief Functions that configure the System.
+ @{
+ */
+
+#if (__Vendor_SysTickConfig == 0)
+
+/** \brief System Tick Configuration
+
+ The function initializes the System Timer and its interrupt, and starts the System Tick Timer.
+ Counter is in free running mode to generate periodic interrupts.
+
+ \param [in] ticks Number of ticks between two interrupts.
+
+ \return 0 Function succeeded.
+ \return 1 Function failed.
+
+ \note When the variable <b>__Vendor_SysTickConfig</b> is set to 1, then the
+ function <b>SysTick_Config</b> is not included. In this case, the file <b><i>device</i>.h</b>
+ must contain a vendor-specific implementation of this function.
+
+ */
+__STATIC_INLINE uint32_t SysTick_Config(uint32_t ticks)
+{
+ if ((ticks - 1) > SysTick_LOAD_RELOAD_Msk) return (1); /* Reload value impossible */
+
+ SysTick->LOAD = ticks - 1; /* set reload register */
+ NVIC_SetPriority (SysTick_IRQn, (1<<__NVIC_PRIO_BITS) - 1); /* set Priority for Systick Interrupt */
+ SysTick->VAL = 0; /* Load the SysTick Counter Value */
+ SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk |
+ SysTick_CTRL_TICKINT_Msk |
+ SysTick_CTRL_ENABLE_Msk; /* Enable SysTick IRQ and SysTick Timer */
+ return (0); /* Function successful */
+}
+
+#endif
+
+/*@} end of CMSIS_Core_SysTickFunctions */
+
+
+
+/* ##################################### Debug In/Output function ########################################### */
+/** \ingroup CMSIS_Core_FunctionInterface
+ \defgroup CMSIS_core_DebugFunctions ITM Functions
+ \brief Functions that access the ITM debug interface.
+ @{
+ */
+
+extern volatile int32_t ITM_RxBuffer; /*!< External variable to receive characters. */
+#define ITM_RXBUFFER_EMPTY 0x5AA55AA5 /*!< Value identifying \ref ITM_RxBuffer is ready for next character. */
+
+
+/** \brief ITM Send Character
+
+ The function transmits a character via the ITM channel 0, and
+ \li Just returns when no debugger is connected that has booked the output.
+ \li Is blocking when a debugger is connected, but the previous character sent has not been transmitted.
+
+ \param [in] ch Character to transmit.
+
+ \returns Character to transmit.
+ */
+__STATIC_INLINE uint32_t ITM_SendChar (uint32_t ch)
+{
+ if ((ITM->TCR & ITM_TCR_ITMENA_Msk) && /* ITM enabled */
+ (ITM->TER & (1UL << 0) ) ) /* ITM Port #0 enabled */
+ {
+ while (ITM->PORT[0].u32 == 0);
+ ITM->PORT[0].u8 = (uint8_t) ch;
+ }
+ return (ch);
+}
+
+
+/** \brief ITM Receive Character
+
+ The function inputs a character via the external variable \ref ITM_RxBuffer.
+
+ \return Received character.
+ \return -1 No character pending.
+ */
+__STATIC_INLINE int32_t ITM_ReceiveChar (void) {
+ int32_t ch = -1; /* no character available */
+
+ if (ITM_RxBuffer != ITM_RXBUFFER_EMPTY) {
+ ch = ITM_RxBuffer;
+ ITM_RxBuffer = ITM_RXBUFFER_EMPTY; /* ready for next character */
+ }
+
+ return (ch);
+}
+
+
+/** \brief ITM Check Character
+
+ The function checks whether a character is pending for reading in the variable \ref ITM_RxBuffer.
+
+ \return 0 No character available.
+ \return 1 Character available.
+ */
+__STATIC_INLINE int32_t ITM_CheckChar (void) {
+
+ if (ITM_RxBuffer == ITM_RXBUFFER_EMPTY) {
+ return (0); /* no character available */
+ } else {
+ return (1); /* character available */
+ }
+}
+
+/*@} end of CMSIS_core_DebugFunctions */
+
+#endif /* __CORE_CM4_H_DEPENDANT */
+
+#endif /* __CMSIS_GENERIC */
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/src/modules/mathlib/CMSIS/Include/core_cm4_simd.h b/src/modules/mathlib/CMSIS/Include/core_cm4_simd.h new file mode 100644 index 000000000..af1831ee1 --- /dev/null +++ b/src/modules/mathlib/CMSIS/Include/core_cm4_simd.h @@ -0,0 +1,673 @@ +/**************************************************************************//**
+ * @file core_cm4_simd.h
+ * @brief CMSIS Cortex-M4 SIMD Header File
+ * @version V3.20
+ * @date 25. February 2013
+ *
+ * @note
+ *
+ ******************************************************************************/
+/* Copyright (c) 2009 - 2013 ARM LIMITED
+
+ All rights reserved.
+ Redistribution and use in source and binary forms, with or without
+ modification, are permitted provided that the following conditions are met:
+ - Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+ - 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.
+ - Neither the name of ARM 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 COPYRIGHT HOLDERS AND 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.
+ ---------------------------------------------------------------------------*/
+
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+#ifndef __CORE_CM4_SIMD_H
+#define __CORE_CM4_SIMD_H
+
+
+/*******************************************************************************
+ * Hardware Abstraction Layer
+ ******************************************************************************/
+
+
+/* ################### Compiler specific Intrinsics ########################### */
+/** \defgroup CMSIS_SIMD_intrinsics CMSIS SIMD Intrinsics
+ Access to dedicated SIMD instructions
+ @{
+*/
+
+#if defined ( __CC_ARM ) /*------------------RealView Compiler -----------------*/
+/* ARM armcc specific functions */
+
+/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/
+#define __SADD8 __sadd8
+#define __QADD8 __qadd8
+#define __SHADD8 __shadd8
+#define __UADD8 __uadd8
+#define __UQADD8 __uqadd8
+#define __UHADD8 __uhadd8
+#define __SSUB8 __ssub8
+#define __QSUB8 __qsub8
+#define __SHSUB8 __shsub8
+#define __USUB8 __usub8
+#define __UQSUB8 __uqsub8
+#define __UHSUB8 __uhsub8
+#define __SADD16 __sadd16
+#define __QADD16 __qadd16
+#define __SHADD16 __shadd16
+#define __UADD16 __uadd16
+#define __UQADD16 __uqadd16
+#define __UHADD16 __uhadd16
+#define __SSUB16 __ssub16
+#define __QSUB16 __qsub16
+#define __SHSUB16 __shsub16
+#define __USUB16 __usub16
+#define __UQSUB16 __uqsub16
+#define __UHSUB16 __uhsub16
+#define __SASX __sasx
+#define __QASX __qasx
+#define __SHASX __shasx
+#define __UASX __uasx
+#define __UQASX __uqasx
+#define __UHASX __uhasx
+#define __SSAX __ssax
+#define __QSAX __qsax
+#define __SHSAX __shsax
+#define __USAX __usax
+#define __UQSAX __uqsax
+#define __UHSAX __uhsax
+#define __USAD8 __usad8
+#define __USADA8 __usada8
+#define __SSAT16 __ssat16
+#define __USAT16 __usat16
+#define __UXTB16 __uxtb16
+#define __UXTAB16 __uxtab16
+#define __SXTB16 __sxtb16
+#define __SXTAB16 __sxtab16
+#define __SMUAD __smuad
+#define __SMUADX __smuadx
+#define __SMLAD __smlad
+#define __SMLADX __smladx
+#define __SMLALD __smlald
+#define __SMLALDX __smlaldx
+#define __SMUSD __smusd
+#define __SMUSDX __smusdx
+#define __SMLSD __smlsd
+#define __SMLSDX __smlsdx
+#define __SMLSLD __smlsld
+#define __SMLSLDX __smlsldx
+#define __SEL __sel
+#define __QADD __qadd
+#define __QSUB __qsub
+
+#define __PKHBT(ARG1,ARG2,ARG3) ( ((((uint32_t)(ARG1)) ) & 0x0000FFFFUL) | \
+ ((((uint32_t)(ARG2)) << (ARG3)) & 0xFFFF0000UL) )
+
+#define __PKHTB(ARG1,ARG2,ARG3) ( ((((uint32_t)(ARG1)) ) & 0xFFFF0000UL) | \
+ ((((uint32_t)(ARG2)) >> (ARG3)) & 0x0000FFFFUL) )
+
+#define __SMMLA(ARG1,ARG2,ARG3) ( (int32_t)((((int64_t)(ARG1) * (ARG2)) + \
+ ((int64_t)(ARG3) << 32) ) >> 32))
+
+/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/
+
+
+
+#elif defined ( __ICCARM__ ) /*------------------ ICC Compiler -------------------*/
+/* IAR iccarm specific functions */
+
+/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/
+#include <cmsis_iar.h>
+
+/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/
+
+
+
+#elif defined ( __TMS470__ ) /*---------------- TI CCS Compiler ------------------*/
+/* TI CCS specific functions */
+
+/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/
+#include <cmsis_ccs.h>
+
+/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/
+
+
+
+#elif defined ( __GNUC__ ) /*------------------ GNU Compiler ---------------------*/
+/* GNU gcc specific functions */
+
+/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SADD8(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("sadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QADD8(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("qadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHADD8(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("shadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UADD8(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("uadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQADD8(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("uqadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHADD8(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("uhadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SSUB8(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("ssub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QSUB8(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("qsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHSUB8(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("shsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USUB8(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("usub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQSUB8(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("uqsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHSUB8(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("uhsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SADD16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("sadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QADD16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("qadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHADD16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("shadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UADD16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("uadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQADD16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("uqadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHADD16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("uhadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SSUB16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("ssub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QSUB16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("qsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHSUB16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("shsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USUB16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("usub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQSUB16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("uqsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHSUB16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("uhsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SASX(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("sasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QASX(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("qasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHASX(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("shasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UASX(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("uasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQASX(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("uqasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHASX(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("uhasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SSAX(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("ssax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QSAX(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("qsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHSAX(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("shsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USAX(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("usax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQSAX(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("uqsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHSAX(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("uhsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USAD8(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("usad8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USADA8(uint32_t op1, uint32_t op2, uint32_t op3)
+{
+ uint32_t result;
+
+ __ASM volatile ("usada8 %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );
+ return(result);
+}
+
+#define __SSAT16(ARG1,ARG2) \
+({ \
+ uint32_t __RES, __ARG1 = (ARG1); \
+ __ASM ("ssat16 %0, %1, %2" : "=r" (__RES) : "I" (ARG2), "r" (__ARG1) ); \
+ __RES; \
+ })
+
+#define __USAT16(ARG1,ARG2) \
+({ \
+ uint32_t __RES, __ARG1 = (ARG1); \
+ __ASM ("usat16 %0, %1, %2" : "=r" (__RES) : "I" (ARG2), "r" (__ARG1) ); \
+ __RES; \
+ })
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UXTB16(uint32_t op1)
+{
+ uint32_t result;
+
+ __ASM volatile ("uxtb16 %0, %1" : "=r" (result) : "r" (op1));
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UXTAB16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("uxtab16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SXTB16(uint32_t op1)
+{
+ uint32_t result;
+
+ __ASM volatile ("sxtb16 %0, %1" : "=r" (result) : "r" (op1));
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SXTAB16(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("sxtab16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMUAD (uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("smuad %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMUADX (uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("smuadx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMLAD (uint32_t op1, uint32_t op2, uint32_t op3)
+{
+ uint32_t result;
+
+ __ASM volatile ("smlad %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMLADX (uint32_t op1, uint32_t op2, uint32_t op3)
+{
+ uint32_t result;
+
+ __ASM volatile ("smladx %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );
+ return(result);
+}
+
+#define __SMLALD(ARG1,ARG2,ARG3) \
+({ \
+ uint32_t __ARG1 = (ARG1), __ARG2 = (ARG2), __ARG3_H = (uint32_t)((uint64_t)(ARG3) >> 32), __ARG3_L = (uint32_t)((uint64_t)(ARG3) & 0xFFFFFFFFUL); \
+ __ASM volatile ("smlald %0, %1, %2, %3" : "=r" (__ARG3_L), "=r" (__ARG3_H) : "r" (__ARG1), "r" (__ARG2), "0" (__ARG3_L), "1" (__ARG3_H) ); \
+ (uint64_t)(((uint64_t)__ARG3_H << 32) | __ARG3_L); \
+ })
+
+#define __SMLALDX(ARG1,ARG2,ARG3) \
+({ \
+ uint32_t __ARG1 = (ARG1), __ARG2 = (ARG2), __ARG3_H = (uint32_t)((uint64_t)(ARG3) >> 32), __ARG3_L = (uint32_t)((uint64_t)(ARG3) & 0xFFFFFFFFUL); \
+ __ASM volatile ("smlaldx %0, %1, %2, %3" : "=r" (__ARG3_L), "=r" (__ARG3_H) : "r" (__ARG1), "r" (__ARG2), "0" (__ARG3_L), "1" (__ARG3_H) ); \
+ (uint64_t)(((uint64_t)__ARG3_H << 32) | __ARG3_L); \
+ })
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMUSD (uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("smusd %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMUSDX (uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("smusdx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMLSD (uint32_t op1, uint32_t op2, uint32_t op3)
+{
+ uint32_t result;
+
+ __ASM volatile ("smlsd %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMLSDX (uint32_t op1, uint32_t op2, uint32_t op3)
+{
+ uint32_t result;
+
+ __ASM volatile ("smlsdx %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );
+ return(result);
+}
+
+#define __SMLSLD(ARG1,ARG2,ARG3) \
+({ \
+ uint32_t __ARG1 = (ARG1), __ARG2 = (ARG2), __ARG3_H = (uint32_t)((ARG3) >> 32), __ARG3_L = (uint32_t)((ARG3) & 0xFFFFFFFFUL); \
+ __ASM volatile ("smlsld %0, %1, %2, %3" : "=r" (__ARG3_L), "=r" (__ARG3_H) : "r" (__ARG1), "r" (__ARG2), "0" (__ARG3_L), "1" (__ARG3_H) ); \
+ (uint64_t)(((uint64_t)__ARG3_H << 32) | __ARG3_L); \
+ })
+
+#define __SMLSLDX(ARG1,ARG2,ARG3) \
+({ \
+ uint32_t __ARG1 = (ARG1), __ARG2 = (ARG2), __ARG3_H = (uint32_t)((ARG3) >> 32), __ARG3_L = (uint32_t)((ARG3) & 0xFFFFFFFFUL); \
+ __ASM volatile ("smlsldx %0, %1, %2, %3" : "=r" (__ARG3_L), "=r" (__ARG3_H) : "r" (__ARG1), "r" (__ARG2), "0" (__ARG3_L), "1" (__ARG3_H) ); \
+ (uint64_t)(((uint64_t)__ARG3_H << 32) | __ARG3_L); \
+ })
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SEL (uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("sel %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QADD(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("qadd %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QSUB(uint32_t op1, uint32_t op2)
+{
+ uint32_t result;
+
+ __ASM volatile ("qsub %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+ return(result);
+}
+
+#define __PKHBT(ARG1,ARG2,ARG3) \
+({ \
+ uint32_t __RES, __ARG1 = (ARG1), __ARG2 = (ARG2); \
+ __ASM ("pkhbt %0, %1, %2, lsl %3" : "=r" (__RES) : "r" (__ARG1), "r" (__ARG2), "I" (ARG3) ); \
+ __RES; \
+ })
+
+#define __PKHTB(ARG1,ARG2,ARG3) \
+({ \
+ uint32_t __RES, __ARG1 = (ARG1), __ARG2 = (ARG2); \
+ if (ARG3 == 0) \
+ __ASM ("pkhtb %0, %1, %2" : "=r" (__RES) : "r" (__ARG1), "r" (__ARG2) ); \
+ else \
+ __ASM ("pkhtb %0, %1, %2, asr %3" : "=r" (__RES) : "r" (__ARG1), "r" (__ARG2), "I" (ARG3) ); \
+ __RES; \
+ })
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMMLA (int32_t op1, int32_t op2, int32_t op3)
+{
+ int32_t result;
+
+ __ASM volatile ("smmla %0, %1, %2, %3" : "=r" (result): "r" (op1), "r" (op2), "r" (op3) );
+ return(result);
+}
+
+/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/
+
+
+
+#elif defined ( __TASKING__ ) /*------------------ TASKING Compiler --------------*/
+/* TASKING carm specific functions */
+
+
+/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/
+/* not yet supported */
+/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/
+
+
+#endif
+
+/*@} end of group CMSIS_SIMD_intrinsics */
+
+
+#endif /* __CORE_CM4_SIMD_H */
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/src/modules/mathlib/CMSIS/Include/core_cmFunc.h b/src/modules/mathlib/CMSIS/Include/core_cmFunc.h new file mode 100644 index 000000000..139bc3c5e --- /dev/null +++ b/src/modules/mathlib/CMSIS/Include/core_cmFunc.h @@ -0,0 +1,636 @@ +/**************************************************************************//**
+ * @file core_cmFunc.h
+ * @brief CMSIS Cortex-M Core Function Access Header File
+ * @version V3.20
+ * @date 25. February 2013
+ *
+ * @note
+ *
+ ******************************************************************************/
+/* Copyright (c) 2009 - 2013 ARM LIMITED
+
+ All rights reserved.
+ Redistribution and use in source and binary forms, with or without
+ modification, are permitted provided that the following conditions are met:
+ - Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+ - 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.
+ - Neither the name of ARM 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 COPYRIGHT HOLDERS AND 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.
+ ---------------------------------------------------------------------------*/
+
+
+#ifndef __CORE_CMFUNC_H
+#define __CORE_CMFUNC_H
+
+
+/* ########################### Core Function Access ########################### */
+/** \ingroup CMSIS_Core_FunctionInterface
+ \defgroup CMSIS_Core_RegAccFunctions CMSIS Core Register Access Functions
+ @{
+ */
+
+#if defined ( __CC_ARM ) /*------------------RealView Compiler -----------------*/
+/* ARM armcc specific functions */
+
+#if (__ARMCC_VERSION < 400677)
+ #error "Please use ARM Compiler Toolchain V4.0.677 or later!"
+#endif
+
+/* intrinsic void __enable_irq(); */
+/* intrinsic void __disable_irq(); */
+
+/** \brief Get Control Register
+
+ This function returns the content of the Control Register.
+
+ \return Control Register value
+ */
+__STATIC_INLINE uint32_t __get_CONTROL(void)
+{
+ register uint32_t __regControl __ASM("control");
+ return(__regControl);
+}
+
+
+/** \brief Set Control Register
+
+ This function writes the given value to the Control Register.
+
+ \param [in] control Control Register value to set
+ */
+__STATIC_INLINE void __set_CONTROL(uint32_t control)
+{
+ register uint32_t __regControl __ASM("control");
+ __regControl = control;
+}
+
+
+/** \brief Get IPSR Register
+
+ This function returns the content of the IPSR Register.
+
+ \return IPSR Register value
+ */
+__STATIC_INLINE uint32_t __get_IPSR(void)
+{
+ register uint32_t __regIPSR __ASM("ipsr");
+ return(__regIPSR);
+}
+
+
+/** \brief Get APSR Register
+
+ This function returns the content of the APSR Register.
+
+ \return APSR Register value
+ */
+__STATIC_INLINE uint32_t __get_APSR(void)
+{
+ register uint32_t __regAPSR __ASM("apsr");
+ return(__regAPSR);
+}
+
+
+/** \brief Get xPSR Register
+
+ This function returns the content of the xPSR Register.
+
+ \return xPSR Register value
+ */
+__STATIC_INLINE uint32_t __get_xPSR(void)
+{
+ register uint32_t __regXPSR __ASM("xpsr");
+ return(__regXPSR);
+}
+
+
+/** \brief Get Process Stack Pointer
+
+ This function returns the current value of the Process Stack Pointer (PSP).
+
+ \return PSP Register value
+ */
+__STATIC_INLINE uint32_t __get_PSP(void)
+{
+ register uint32_t __regProcessStackPointer __ASM("psp");
+ return(__regProcessStackPointer);
+}
+
+
+/** \brief Set Process Stack Pointer
+
+ This function assigns the given value to the Process Stack Pointer (PSP).
+
+ \param [in] topOfProcStack Process Stack Pointer value to set
+ */
+__STATIC_INLINE void __set_PSP(uint32_t topOfProcStack)
+{
+ register uint32_t __regProcessStackPointer __ASM("psp");
+ __regProcessStackPointer = topOfProcStack;
+}
+
+
+/** \brief Get Main Stack Pointer
+
+ This function returns the current value of the Main Stack Pointer (MSP).
+
+ \return MSP Register value
+ */
+__STATIC_INLINE uint32_t __get_MSP(void)
+{
+ register uint32_t __regMainStackPointer __ASM("msp");
+ return(__regMainStackPointer);
+}
+
+
+/** \brief Set Main Stack Pointer
+
+ This function assigns the given value to the Main Stack Pointer (MSP).
+
+ \param [in] topOfMainStack Main Stack Pointer value to set
+ */
+__STATIC_INLINE void __set_MSP(uint32_t topOfMainStack)
+{
+ register uint32_t __regMainStackPointer __ASM("msp");
+ __regMainStackPointer = topOfMainStack;
+}
+
+
+/** \brief Get Priority Mask
+
+ This function returns the current state of the priority mask bit from the Priority Mask Register.
+
+ \return Priority Mask value
+ */
+__STATIC_INLINE uint32_t __get_PRIMASK(void)
+{
+ register uint32_t __regPriMask __ASM("primask");
+ return(__regPriMask);
+}
+
+
+/** \brief Set Priority Mask
+
+ This function assigns the given value to the Priority Mask Register.
+
+ \param [in] priMask Priority Mask
+ */
+__STATIC_INLINE void __set_PRIMASK(uint32_t priMask)
+{
+ register uint32_t __regPriMask __ASM("primask");
+ __regPriMask = (priMask);
+}
+
+
+#if (__CORTEX_M >= 0x03)
+
+/** \brief Enable FIQ
+
+ This function enables FIQ interrupts by clearing the F-bit in the CPSR.
+ Can only be executed in Privileged modes.
+ */
+#define __enable_fault_irq __enable_fiq
+
+
+/** \brief Disable FIQ
+
+ This function disables FIQ interrupts by setting the F-bit in the CPSR.
+ Can only be executed in Privileged modes.
+ */
+#define __disable_fault_irq __disable_fiq
+
+
+/** \brief Get Base Priority
+
+ This function returns the current value of the Base Priority register.
+
+ \return Base Priority register value
+ */
+__STATIC_INLINE uint32_t __get_BASEPRI(void)
+{
+ register uint32_t __regBasePri __ASM("basepri");
+ return(__regBasePri);
+}
+
+
+/** \brief Set Base Priority
+
+ This function assigns the given value to the Base Priority register.
+
+ \param [in] basePri Base Priority value to set
+ */
+__STATIC_INLINE void __set_BASEPRI(uint32_t basePri)
+{
+ register uint32_t __regBasePri __ASM("basepri");
+ __regBasePri = (basePri & 0xff);
+}
+
+
+/** \brief Get Fault Mask
+
+ This function returns the current value of the Fault Mask register.
+
+ \return Fault Mask register value
+ */
+__STATIC_INLINE uint32_t __get_FAULTMASK(void)
+{
+ register uint32_t __regFaultMask __ASM("faultmask");
+ return(__regFaultMask);
+}
+
+
+/** \brief Set Fault Mask
+
+ This function assigns the given value to the Fault Mask register.
+
+ \param [in] faultMask Fault Mask value to set
+ */
+__STATIC_INLINE void __set_FAULTMASK(uint32_t faultMask)
+{
+ register uint32_t __regFaultMask __ASM("faultmask");
+ __regFaultMask = (faultMask & (uint32_t)1);
+}
+
+#endif /* (__CORTEX_M >= 0x03) */
+
+
+#if (__CORTEX_M == 0x04)
+
+/** \brief Get FPSCR
+
+ This function returns the current value of the Floating Point Status/Control register.
+
+ \return Floating Point Status/Control register value
+ */
+__STATIC_INLINE uint32_t __get_FPSCR(void)
+{
+#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
+ register uint32_t __regfpscr __ASM("fpscr");
+ return(__regfpscr);
+#else
+ return(0);
+#endif
+}
+
+
+/** \brief Set FPSCR
+
+ This function assigns the given value to the Floating Point Status/Control register.
+
+ \param [in] fpscr Floating Point Status/Control value to set
+ */
+__STATIC_INLINE void __set_FPSCR(uint32_t fpscr)
+{
+#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
+ register uint32_t __regfpscr __ASM("fpscr");
+ __regfpscr = (fpscr);
+#endif
+}
+
+#endif /* (__CORTEX_M == 0x04) */
+
+
+#elif defined ( __ICCARM__ ) /*------------------ ICC Compiler -------------------*/
+/* IAR iccarm specific functions */
+
+#include <cmsis_iar.h>
+
+
+#elif defined ( __TMS470__ ) /*---------------- TI CCS Compiler ------------------*/
+/* TI CCS specific functions */
+
+#include <cmsis_ccs.h>
+
+
+#elif defined ( __GNUC__ ) /*------------------ GNU Compiler ---------------------*/
+/* GNU gcc specific functions */
+
+/** \brief Enable IRQ Interrupts
+
+ This function enables IRQ interrupts by clearing the I-bit in the CPSR.
+ Can only be executed in Privileged modes.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __enable_irq(void)
+{
+ __ASM volatile ("cpsie i" : : : "memory");
+}
+
+
+/** \brief Disable IRQ Interrupts
+
+ This function disables IRQ interrupts by setting the I-bit in the CPSR.
+ Can only be executed in Privileged modes.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __disable_irq(void)
+{
+ __ASM volatile ("cpsid i" : : : "memory");
+}
+
+
+/** \brief Get Control Register
+
+ This function returns the content of the Control Register.
+
+ \return Control Register value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_CONTROL(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, control" : "=r" (result) );
+ return(result);
+}
+
+
+/** \brief Set Control Register
+
+ This function writes the given value to the Control Register.
+
+ \param [in] control Control Register value to set
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_CONTROL(uint32_t control)
+{
+ __ASM volatile ("MSR control, %0" : : "r" (control) : "memory");
+}
+
+
+/** \brief Get IPSR Register
+
+ This function returns the content of the IPSR Register.
+
+ \return IPSR Register value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_IPSR(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, ipsr" : "=r" (result) );
+ return(result);
+}
+
+
+/** \brief Get APSR Register
+
+ This function returns the content of the APSR Register.
+
+ \return APSR Register value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_APSR(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, apsr" : "=r" (result) );
+ return(result);
+}
+
+
+/** \brief Get xPSR Register
+
+ This function returns the content of the xPSR Register.
+
+ \return xPSR Register value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_xPSR(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, xpsr" : "=r" (result) );
+ return(result);
+}
+
+
+/** \brief Get Process Stack Pointer
+
+ This function returns the current value of the Process Stack Pointer (PSP).
+
+ \return PSP Register value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_PSP(void)
+{
+ register uint32_t result;
+
+ __ASM volatile ("MRS %0, psp\n" : "=r" (result) );
+ return(result);
+}
+
+
+/** \brief Set Process Stack Pointer
+
+ This function assigns the given value to the Process Stack Pointer (PSP).
+
+ \param [in] topOfProcStack Process Stack Pointer value to set
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_PSP(uint32_t topOfProcStack)
+{
+ __ASM volatile ("MSR psp, %0\n" : : "r" (topOfProcStack) : "sp");
+}
+
+
+/** \brief Get Main Stack Pointer
+
+ This function returns the current value of the Main Stack Pointer (MSP).
+
+ \return MSP Register value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_MSP(void)
+{
+ register uint32_t result;
+
+ __ASM volatile ("MRS %0, msp\n" : "=r" (result) );
+ return(result);
+}
+
+
+/** \brief Set Main Stack Pointer
+
+ This function assigns the given value to the Main Stack Pointer (MSP).
+
+ \param [in] topOfMainStack Main Stack Pointer value to set
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_MSP(uint32_t topOfMainStack)
+{
+ __ASM volatile ("MSR msp, %0\n" : : "r" (topOfMainStack) : "sp");
+}
+
+
+/** \brief Get Priority Mask
+
+ This function returns the current state of the priority mask bit from the Priority Mask Register.
+
+ \return Priority Mask value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_PRIMASK(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, primask" : "=r" (result) );
+ return(result);
+}
+
+
+/** \brief Set Priority Mask
+
+ This function assigns the given value to the Priority Mask Register.
+
+ \param [in] priMask Priority Mask
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_PRIMASK(uint32_t priMask)
+{
+ __ASM volatile ("MSR primask, %0" : : "r" (priMask) : "memory");
+}
+
+
+#if (__CORTEX_M >= 0x03)
+
+/** \brief Enable FIQ
+
+ This function enables FIQ interrupts by clearing the F-bit in the CPSR.
+ Can only be executed in Privileged modes.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __enable_fault_irq(void)
+{
+ __ASM volatile ("cpsie f" : : : "memory");
+}
+
+
+/** \brief Disable FIQ
+
+ This function disables FIQ interrupts by setting the F-bit in the CPSR.
+ Can only be executed in Privileged modes.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __disable_fault_irq(void)
+{
+ __ASM volatile ("cpsid f" : : : "memory");
+}
+
+
+/** \brief Get Base Priority
+
+ This function returns the current value of the Base Priority register.
+
+ \return Base Priority register value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_BASEPRI(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, basepri_max" : "=r" (result) );
+ return(result);
+}
+
+
+/** \brief Set Base Priority
+
+ This function assigns the given value to the Base Priority register.
+
+ \param [in] basePri Base Priority value to set
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_BASEPRI(uint32_t value)
+{
+ __ASM volatile ("MSR basepri, %0" : : "r" (value) : "memory");
+}
+
+
+/** \brief Get Fault Mask
+
+ This function returns the current value of the Fault Mask register.
+
+ \return Fault Mask register value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_FAULTMASK(void)
+{
+ uint32_t result;
+
+ __ASM volatile ("MRS %0, faultmask" : "=r" (result) );
+ return(result);
+}
+
+
+/** \brief Set Fault Mask
+
+ This function assigns the given value to the Fault Mask register.
+
+ \param [in] faultMask Fault Mask value to set
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_FAULTMASK(uint32_t faultMask)
+{
+ __ASM volatile ("MSR faultmask, %0" : : "r" (faultMask) : "memory");
+}
+
+#endif /* (__CORTEX_M >= 0x03) */
+
+
+#if (__CORTEX_M == 0x04)
+
+/** \brief Get FPSCR
+
+ This function returns the current value of the Floating Point Status/Control register.
+
+ \return Floating Point Status/Control register value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_FPSCR(void)
+{
+#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
+ uint32_t result;
+
+ /* Empty asm statement works as a scheduling barrier */
+ __ASM volatile ("");
+ __ASM volatile ("VMRS %0, fpscr" : "=r" (result) );
+ __ASM volatile ("");
+ return(result);
+#else
+ return(0);
+#endif
+}
+
+
+/** \brief Set FPSCR
+
+ This function assigns the given value to the Floating Point Status/Control register.
+
+ \param [in] fpscr Floating Point Status/Control value to set
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_FPSCR(uint32_t fpscr)
+{
+#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
+ /* Empty asm statement works as a scheduling barrier */
+ __ASM volatile ("");
+ __ASM volatile ("VMSR fpscr, %0" : : "r" (fpscr) : "vfpcc");
+ __ASM volatile ("");
+#endif
+}
+
+#endif /* (__CORTEX_M == 0x04) */
+
+
+#elif defined ( __TASKING__ ) /*------------------ TASKING Compiler --------------*/
+/* TASKING carm specific functions */
+
+/*
+ * The CMSIS functions have been implemented as intrinsics in the compiler.
+ * Please use "carm -?i" to get an up to date list of all instrinsics,
+ * Including the CMSIS ones.
+ */
+
+#endif
+
+/*@} end of CMSIS_Core_RegAccFunctions */
+
+
+#endif /* __CORE_CMFUNC_H */
diff --git a/src/modules/mathlib/CMSIS/Include/core_cmInstr.h b/src/modules/mathlib/CMSIS/Include/core_cmInstr.h new file mode 100644 index 000000000..8946c2c49 --- /dev/null +++ b/src/modules/mathlib/CMSIS/Include/core_cmInstr.h @@ -0,0 +1,688 @@ +/**************************************************************************//**
+ * @file core_cmInstr.h
+ * @brief CMSIS Cortex-M Core Instruction Access Header File
+ * @version V3.20
+ * @date 05. March 2013
+ *
+ * @note
+ *
+ ******************************************************************************/
+/* Copyright (c) 2009 - 2013 ARM LIMITED
+
+ All rights reserved.
+ Redistribution and use in source and binary forms, with or without
+ modification, are permitted provided that the following conditions are met:
+ - Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+ - 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.
+ - Neither the name of ARM 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 COPYRIGHT HOLDERS AND 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.
+ ---------------------------------------------------------------------------*/
+
+
+#ifndef __CORE_CMINSTR_H
+#define __CORE_CMINSTR_H
+
+
+/* ########################## Core Instruction Access ######################### */
+/** \defgroup CMSIS_Core_InstructionInterface CMSIS Core Instruction Interface
+ Access to dedicated instructions
+ @{
+*/
+
+#if defined ( __CC_ARM ) /*------------------RealView Compiler -----------------*/
+/* ARM armcc specific functions */
+
+#if (__ARMCC_VERSION < 400677)
+ #error "Please use ARM Compiler Toolchain V4.0.677 or later!"
+#endif
+
+
+/** \brief No Operation
+
+ No Operation does nothing. This instruction can be used for code alignment purposes.
+ */
+#define __NOP __nop
+
+
+/** \brief Wait For Interrupt
+
+ Wait For Interrupt is a hint instruction that suspends execution
+ until one of a number of events occurs.
+ */
+#define __WFI __wfi
+
+
+/** \brief Wait For Event
+
+ Wait For Event is a hint instruction that permits the processor to enter
+ a low-power state until one of a number of events occurs.
+ */
+#define __WFE __wfe
+
+
+/** \brief Send Event
+
+ Send Event is a hint instruction. It causes an event to be signaled to the CPU.
+ */
+#define __SEV __sev
+
+
+/** \brief Instruction Synchronization Barrier
+
+ Instruction Synchronization Barrier flushes the pipeline in the processor,
+ so that all instructions following the ISB are fetched from cache or
+ memory, after the instruction has been completed.
+ */
+#define __ISB() __isb(0xF)
+
+
+/** \brief Data Synchronization Barrier
+
+ This function acts as a special kind of Data Memory Barrier.
+ It completes when all explicit memory accesses before this instruction complete.
+ */
+#define __DSB() __dsb(0xF)
+
+
+/** \brief Data Memory Barrier
+
+ This function ensures the apparent order of the explicit memory operations before
+ and after the instruction, without ensuring their completion.
+ */
+#define __DMB() __dmb(0xF)
+
+
+/** \brief Reverse byte order (32 bit)
+
+ This function reverses the byte order in integer value.
+
+ \param [in] value Value to reverse
+ \return Reversed value
+ */
+#define __REV __rev
+
+
+/** \brief Reverse byte order (16 bit)
+
+ This function reverses the byte order in two unsigned short values.
+
+ \param [in] value Value to reverse
+ \return Reversed value
+ */
+#ifndef __NO_EMBEDDED_ASM
+__attribute__((section(".rev16_text"))) __STATIC_INLINE __ASM uint32_t __REV16(uint32_t value)
+{
+ rev16 r0, r0
+ bx lr
+}
+#endif
+
+/** \brief Reverse byte order in signed short value
+
+ This function reverses the byte order in a signed short value with sign extension to integer.
+
+ \param [in] value Value to reverse
+ \return Reversed value
+ */
+#ifndef __NO_EMBEDDED_ASM
+__attribute__((section(".revsh_text"))) __STATIC_INLINE __ASM int32_t __REVSH(int32_t value)
+{
+ revsh r0, r0
+ bx lr
+}
+#endif
+
+
+/** \brief Rotate Right in unsigned value (32 bit)
+
+ This function Rotate Right (immediate) provides the value of the contents of a register rotated by a variable number of bits.
+
+ \param [in] value Value to rotate
+ \param [in] value Number of Bits to rotate
+ \return Rotated value
+ */
+#define __ROR __ror
+
+
+/** \brief Breakpoint
+
+ This function causes the processor to enter Debug state.
+ Debug tools can use this to investigate system state when the instruction at a particular address is reached.
+
+ \param [in] value is ignored by the processor.
+ If required, a debugger can use it to store additional information about the breakpoint.
+ */
+#define __BKPT(value) __breakpoint(value)
+
+
+#if (__CORTEX_M >= 0x03)
+
+/** \brief Reverse bit order of value
+
+ This function reverses the bit order of the given value.
+
+ \param [in] value Value to reverse
+ \return Reversed value
+ */
+#define __RBIT __rbit
+
+
+/** \brief LDR Exclusive (8 bit)
+
+ This function performs a exclusive LDR command for 8 bit value.
+
+ \param [in] ptr Pointer to data
+ \return value of type uint8_t at (*ptr)
+ */
+#define __LDREXB(ptr) ((uint8_t ) __ldrex(ptr))
+
+
+/** \brief LDR Exclusive (16 bit)
+
+ This function performs a exclusive LDR command for 16 bit values.
+
+ \param [in] ptr Pointer to data
+ \return value of type uint16_t at (*ptr)
+ */
+#define __LDREXH(ptr) ((uint16_t) __ldrex(ptr))
+
+
+/** \brief LDR Exclusive (32 bit)
+
+ This function performs a exclusive LDR command for 32 bit values.
+
+ \param [in] ptr Pointer to data
+ \return value of type uint32_t at (*ptr)
+ */
+#define __LDREXW(ptr) ((uint32_t ) __ldrex(ptr))
+
+
+/** \brief STR Exclusive (8 bit)
+
+ This function performs a exclusive STR command for 8 bit values.
+
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ \return 0 Function succeeded
+ \return 1 Function failed
+ */
+#define __STREXB(value, ptr) __strex(value, ptr)
+
+
+/** \brief STR Exclusive (16 bit)
+
+ This function performs a exclusive STR command for 16 bit values.
+
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ \return 0 Function succeeded
+ \return 1 Function failed
+ */
+#define __STREXH(value, ptr) __strex(value, ptr)
+
+
+/** \brief STR Exclusive (32 bit)
+
+ This function performs a exclusive STR command for 32 bit values.
+
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ \return 0 Function succeeded
+ \return 1 Function failed
+ */
+#define __STREXW(value, ptr) __strex(value, ptr)
+
+
+/** \brief Remove the exclusive lock
+
+ This function removes the exclusive lock which is created by LDREX.
+
+ */
+#define __CLREX __clrex
+
+
+/** \brief Signed Saturate
+
+ This function saturates a signed value.
+
+ \param [in] value Value to be saturated
+ \param [in] sat Bit position to saturate to (1..32)
+ \return Saturated value
+ */
+#define __SSAT __ssat
+
+
+/** \brief Unsigned Saturate
+
+ This function saturates an unsigned value.
+
+ \param [in] value Value to be saturated
+ \param [in] sat Bit position to saturate to (0..31)
+ \return Saturated value
+ */
+#define __USAT __usat
+
+
+/** \brief Count leading zeros
+
+ This function counts the number of leading zeros of a data value.
+
+ \param [in] value Value to count the leading zeros
+ \return number of leading zeros in value
+ */
+#define __CLZ __clz
+
+#endif /* (__CORTEX_M >= 0x03) */
+
+
+
+#elif defined ( __ICCARM__ ) /*------------------ ICC Compiler -------------------*/
+/* IAR iccarm specific functions */
+
+#include <cmsis_iar.h>
+
+
+#elif defined ( __TMS470__ ) /*---------------- TI CCS Compiler ------------------*/
+/* TI CCS specific functions */
+
+#include <cmsis_ccs.h>
+
+
+#elif defined ( __GNUC__ ) /*------------------ GNU Compiler ---------------------*/
+/* GNU gcc specific functions */
+
+/* Define macros for porting to both thumb1 and thumb2.
+ * For thumb1, use low register (r0-r7), specified by constrant "l"
+ * Otherwise, use general registers, specified by constrant "r" */
+#if defined (__thumb__) && !defined (__thumb2__)
+#define __CMSIS_GCC_OUT_REG(r) "=l" (r)
+#define __CMSIS_GCC_USE_REG(r) "l" (r)
+#else
+#define __CMSIS_GCC_OUT_REG(r) "=r" (r)
+#define __CMSIS_GCC_USE_REG(r) "r" (r)
+#endif
+
+/** \brief No Operation
+
+ No Operation does nothing. This instruction can be used for code alignment purposes.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __NOP(void)
+{
+ __ASM volatile ("nop");
+}
+
+
+/** \brief Wait For Interrupt
+
+ Wait For Interrupt is a hint instruction that suspends execution
+ until one of a number of events occurs.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __WFI(void)
+{
+ __ASM volatile ("wfi");
+}
+
+
+/** \brief Wait For Event
+
+ Wait For Event is a hint instruction that permits the processor to enter
+ a low-power state until one of a number of events occurs.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __WFE(void)
+{
+ __ASM volatile ("wfe");
+}
+
+
+/** \brief Send Event
+
+ Send Event is a hint instruction. It causes an event to be signaled to the CPU.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __SEV(void)
+{
+ __ASM volatile ("sev");
+}
+
+
+/** \brief Instruction Synchronization Barrier
+
+ Instruction Synchronization Barrier flushes the pipeline in the processor,
+ so that all instructions following the ISB are fetched from cache or
+ memory, after the instruction has been completed.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __ISB(void)
+{
+ __ASM volatile ("isb");
+}
+
+
+/** \brief Data Synchronization Barrier
+
+ This function acts as a special kind of Data Memory Barrier.
+ It completes when all explicit memory accesses before this instruction complete.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __DSB(void)
+{
+ __ASM volatile ("dsb");
+}
+
+
+/** \brief Data Memory Barrier
+
+ This function ensures the apparent order of the explicit memory operations before
+ and after the instruction, without ensuring their completion.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __DMB(void)
+{
+ __ASM volatile ("dmb");
+}
+
+
+/** \brief Reverse byte order (32 bit)
+
+ This function reverses the byte order in integer value.
+
+ \param [in] value Value to reverse
+ \return Reversed value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __REV(uint32_t value)
+{
+#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 5)
+ return __builtin_bswap32(value);
+#else
+ uint32_t result;
+
+ __ASM volatile ("rev %0, %1" : __CMSIS_GCC_OUT_REG (result) : __CMSIS_GCC_USE_REG (value) );
+ return(result);
+#endif
+}
+
+
+/** \brief Reverse byte order (16 bit)
+
+ This function reverses the byte order in two unsigned short values.
+
+ \param [in] value Value to reverse
+ \return Reversed value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __REV16(uint32_t value)
+{
+ uint32_t result;
+
+ __ASM volatile ("rev16 %0, %1" : __CMSIS_GCC_OUT_REG (result) : __CMSIS_GCC_USE_REG (value) );
+ return(result);
+}
+
+
+/** \brief Reverse byte order in signed short value
+
+ This function reverses the byte order in a signed short value with sign extension to integer.
+
+ \param [in] value Value to reverse
+ \return Reversed value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE int32_t __REVSH(int32_t value)
+{
+#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 8)
+ return (short)__builtin_bswap16(value);
+#else
+ uint32_t result;
+
+ __ASM volatile ("revsh %0, %1" : __CMSIS_GCC_OUT_REG (result) : __CMSIS_GCC_USE_REG (value) );
+ return(result);
+#endif
+}
+
+
+/** \brief Rotate Right in unsigned value (32 bit)
+
+ This function Rotate Right (immediate) provides the value of the contents of a register rotated by a variable number of bits.
+
+ \param [in] value Value to rotate
+ \param [in] value Number of Bits to rotate
+ \return Rotated value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __ROR(uint32_t op1, uint32_t op2)
+{
+ return (op1 >> op2) | (op1 << (32 - op2));
+}
+
+
+/** \brief Breakpoint
+
+ This function causes the processor to enter Debug state.
+ Debug tools can use this to investigate system state when the instruction at a particular address is reached.
+
+ \param [in] value is ignored by the processor.
+ If required, a debugger can use it to store additional information about the breakpoint.
+ */
+#define __BKPT(value) __ASM volatile ("bkpt "#value)
+
+
+#if (__CORTEX_M >= 0x03)
+
+/** \brief Reverse bit order of value
+
+ This function reverses the bit order of the given value.
+
+ \param [in] value Value to reverse
+ \return Reversed value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __RBIT(uint32_t value)
+{
+ uint32_t result;
+
+ __ASM volatile ("rbit %0, %1" : "=r" (result) : "r" (value) );
+ return(result);
+}
+
+
+/** \brief LDR Exclusive (8 bit)
+
+ This function performs a exclusive LDR command for 8 bit value.
+
+ \param [in] ptr Pointer to data
+ \return value of type uint8_t at (*ptr)
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint8_t __LDREXB(volatile uint8_t *addr)
+{
+ uint32_t result;
+
+#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 8)
+ __ASM volatile ("ldrexb %0, %1" : "=r" (result) : "Q" (*addr) );
+#else
+ /* Prior to GCC 4.8, "Q" will be expanded to [rx, #0] which is not
+ accepted by assembler. So has to use following less efficient pattern.
+ */
+ __ASM volatile ("ldrexb %0, [%1]" : "=r" (result) : "r" (addr) : "memory" );
+#endif
+ return(result);
+}
+
+
+/** \brief LDR Exclusive (16 bit)
+
+ This function performs a exclusive LDR command for 16 bit values.
+
+ \param [in] ptr Pointer to data
+ \return value of type uint16_t at (*ptr)
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint16_t __LDREXH(volatile uint16_t *addr)
+{
+ uint32_t result;
+
+#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 8)
+ __ASM volatile ("ldrexh %0, %1" : "=r" (result) : "Q" (*addr) );
+#else
+ /* Prior to GCC 4.8, "Q" will be expanded to [rx, #0] which is not
+ accepted by assembler. So has to use following less efficient pattern.
+ */
+ __ASM volatile ("ldrexh %0, [%1]" : "=r" (result) : "r" (addr) : "memory" );
+#endif
+ return(result);
+}
+
+
+/** \brief LDR Exclusive (32 bit)
+
+ This function performs a exclusive LDR command for 32 bit values.
+
+ \param [in] ptr Pointer to data
+ \return value of type uint32_t at (*ptr)
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __LDREXW(volatile uint32_t *addr)
+{
+ uint32_t result;
+
+ __ASM volatile ("ldrex %0, %1" : "=r" (result) : "Q" (*addr) );
+ return(result);
+}
+
+
+/** \brief STR Exclusive (8 bit)
+
+ This function performs a exclusive STR command for 8 bit values.
+
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ \return 0 Function succeeded
+ \return 1 Function failed
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __STREXB(uint8_t value, volatile uint8_t *addr)
+{
+ uint32_t result;
+
+ __ASM volatile ("strexb %0, %2, %1" : "=&r" (result), "=Q" (*addr) : "r" (value) );
+ return(result);
+}
+
+
+/** \brief STR Exclusive (16 bit)
+
+ This function performs a exclusive STR command for 16 bit values.
+
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ \return 0 Function succeeded
+ \return 1 Function failed
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __STREXH(uint16_t value, volatile uint16_t *addr)
+{
+ uint32_t result;
+
+ __ASM volatile ("strexh %0, %2, %1" : "=&r" (result), "=Q" (*addr) : "r" (value) );
+ return(result);
+}
+
+
+/** \brief STR Exclusive (32 bit)
+
+ This function performs a exclusive STR command for 32 bit values.
+
+ \param [in] value Value to store
+ \param [in] ptr Pointer to location
+ \return 0 Function succeeded
+ \return 1 Function failed
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __STREXW(uint32_t value, volatile uint32_t *addr)
+{
+ uint32_t result;
+
+ __ASM volatile ("strex %0, %2, %1" : "=&r" (result), "=Q" (*addr) : "r" (value) );
+ return(result);
+}
+
+
+/** \brief Remove the exclusive lock
+
+ This function removes the exclusive lock which is created by LDREX.
+
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __CLREX(void)
+{
+ __ASM volatile ("clrex" ::: "memory");
+}
+
+
+/** \brief Signed Saturate
+
+ This function saturates a signed value.
+
+ \param [in] value Value to be saturated
+ \param [in] sat Bit position to saturate to (1..32)
+ \return Saturated value
+ */
+#define __SSAT(ARG1,ARG2) \
+({ \
+ uint32_t __RES, __ARG1 = (ARG1); \
+ __ASM ("ssat %0, %1, %2" : "=r" (__RES) : "I" (ARG2), "r" (__ARG1) ); \
+ __RES; \
+ })
+
+
+/** \brief Unsigned Saturate
+
+ This function saturates an unsigned value.
+
+ \param [in] value Value to be saturated
+ \param [in] sat Bit position to saturate to (0..31)
+ \return Saturated value
+ */
+#define __USAT(ARG1,ARG2) \
+({ \
+ uint32_t __RES, __ARG1 = (ARG1); \
+ __ASM ("usat %0, %1, %2" : "=r" (__RES) : "I" (ARG2), "r" (__ARG1) ); \
+ __RES; \
+ })
+
+
+/** \brief Count leading zeros
+
+ This function counts the number of leading zeros of a data value.
+
+ \param [in] value Value to count the leading zeros
+ \return number of leading zeros in value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint8_t __CLZ(uint32_t value)
+{
+ uint32_t result;
+
+ __ASM volatile ("clz %0, %1" : "=r" (result) : "r" (value) );
+ return(result);
+}
+
+#endif /* (__CORTEX_M >= 0x03) */
+
+
+
+
+#elif defined ( __TASKING__ ) /*------------------ TASKING Compiler --------------*/
+/* TASKING carm specific functions */
+
+/*
+ * The CMSIS functions have been implemented as intrinsics in the compiler.
+ * Please use "carm -?i" to get an up to date list of all intrinsics,
+ * Including the CMSIS ones.
+ */
+
+#endif
+
+/*@}*/ /* end of group CMSIS_Core_InstructionInterface */
+
+#endif /* __CORE_CMINSTR_H */
diff --git a/src/modules/mathlib/CMSIS/libarm_cortexM3l_math.a b/src/modules/mathlib/CMSIS/libarm_cortexM3l_math.a Binary files differnew file mode 100644 index 000000000..6898bc27d --- /dev/null +++ b/src/modules/mathlib/CMSIS/libarm_cortexM3l_math.a diff --git a/src/modules/mathlib/CMSIS/libarm_cortexM4l_math.a b/src/modules/mathlib/CMSIS/libarm_cortexM4l_math.a Binary files differnew file mode 100755 index 000000000..a0185eaa9 --- /dev/null +++ b/src/modules/mathlib/CMSIS/libarm_cortexM4l_math.a diff --git a/src/modules/mathlib/CMSIS/libarm_cortexM4lf_math.a b/src/modules/mathlib/CMSIS/libarm_cortexM4lf_math.a Binary files differnew file mode 100755 index 000000000..94525528e --- /dev/null +++ b/src/modules/mathlib/CMSIS/libarm_cortexM4lf_math.a diff --git a/src/modules/mathlib/CMSIS/library.mk b/src/modules/mathlib/CMSIS/library.mk new file mode 100644 index 000000000..0cc1b559d --- /dev/null +++ b/src/modules/mathlib/CMSIS/library.mk @@ -0,0 +1,46 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# ARM CMSIS DSP library +# + +ifeq ($(CONFIG_ARCH),CORTEXM4F) +PREBUILT_LIB := libarm_cortexM4lf_math.a +else ifeq ($(CONFIG_ARCH),CORTEXM4) +PREBUILT_LIB := libarm_cortexM4l_math.a +else ifeq ($(CONFIG_ARCH),CORTEXM3) +PREBUILT_LIB := libarm_cortexM3l_math.a +else +$(error CONFIG_ARCH value '$(CONFIG_ARCH)' not supported by the DSP library) +endif diff --git a/src/modules/mathlib/CMSIS/license.txt b/src/modules/mathlib/CMSIS/license.txt new file mode 100644 index 000000000..31afac1ec --- /dev/null +++ b/src/modules/mathlib/CMSIS/license.txt @@ -0,0 +1,27 @@ +All pre-built libraries are guided by the following license:
+
+Copyright (C) 2009-2012 ARM Limited.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+ - Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+ - 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.
+ - Neither the name of ARM 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 COPYRIGHT HOLDERS AND 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.
diff --git a/src/modules/mathlib/math/Dcm.cpp b/src/modules/mathlib/math/Dcm.cpp new file mode 100644 index 000000000..f509f7081 --- /dev/null +++ b/src/modules/mathlib/math/Dcm.cpp @@ -0,0 +1,174 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Dcm.cpp + * + * math direction cosine matrix + */ + +#include <mathlib/math/test/test.hpp> + +#include "Dcm.hpp" +#include "Quaternion.hpp" +#include "EulerAngles.hpp" +#include "Vector3.hpp" + +namespace math +{ + +Dcm::Dcm() : + Matrix(Matrix::identity(3)) +{ +} + +Dcm::Dcm(float c00, float c01, float c02, + float c10, float c11, float c12, + float c20, float c21, float c22) : + Matrix(3, 3) +{ + Dcm &dcm = *this; + dcm(0, 0) = c00; + dcm(0, 1) = c01; + dcm(0, 2) = c02; + dcm(1, 0) = c10; + dcm(1, 1) = c11; + dcm(1, 2) = c12; + dcm(2, 0) = c20; + dcm(2, 1) = c21; + dcm(2, 2) = c22; +} + +Dcm::Dcm(const float data[3][3]) : + Matrix(3, 3) +{ + Dcm &dcm = *this; + /* set rotation matrix */ + for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) + dcm(i, j) = data[i][j]; +} + +Dcm::Dcm(const float *data) : + Matrix(3, 3, data) +{ +} + +Dcm::Dcm(const Quaternion &q) : + Matrix(3, 3) +{ + Dcm &dcm = *this; + double a = q.getA(); + double b = q.getB(); + double c = q.getC(); + double d = q.getD(); + double aSq = a * a; + double bSq = b * b; + double cSq = c * c; + double dSq = d * d; + dcm(0, 0) = aSq + bSq - cSq - dSq; + dcm(0, 1) = 2.0 * (b * c - a * d); + dcm(0, 2) = 2.0 * (a * c + b * d); + dcm(1, 0) = 2.0 * (b * c + a * d); + dcm(1, 1) = aSq - bSq + cSq - dSq; + dcm(1, 2) = 2.0 * (c * d - a * b); + dcm(2, 0) = 2.0 * (b * d - a * c); + dcm(2, 1) = 2.0 * (a * b + c * d); + dcm(2, 2) = aSq - bSq - cSq + dSq; +} + +Dcm::Dcm(const EulerAngles &euler) : + Matrix(3, 3) +{ + Dcm &dcm = *this; + double cosPhi = cos(euler.getPhi()); + double sinPhi = sin(euler.getPhi()); + double cosThe = cos(euler.getTheta()); + double sinThe = sin(euler.getTheta()); + double cosPsi = cos(euler.getPsi()); + double sinPsi = sin(euler.getPsi()); + + dcm(0, 0) = cosThe * cosPsi; + dcm(0, 1) = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi; + dcm(0, 2) = sinPhi * sinPsi + cosPhi * sinThe * cosPsi; + + dcm(1, 0) = cosThe * sinPsi; + dcm(1, 1) = cosPhi * cosPsi + sinPhi * sinThe * sinPsi; + dcm(1, 2) = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi; + + dcm(2, 0) = -sinThe; + dcm(2, 1) = sinPhi * cosThe; + dcm(2, 2) = cosPhi * cosThe; +} + +Dcm::Dcm(const Dcm &right) : + Matrix(right) +{ +} + +Dcm::~Dcm() +{ +} + +int __EXPORT dcmTest() +{ + printf("Test DCM\t\t: "); + // default ctor + ASSERT(matrixEqual(Dcm(), + Matrix::identity(3))); + // quaternion ctor + ASSERT(matrixEqual( + Dcm(Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f)), + Dcm(0.9362934f, -0.2750958f, 0.2183507f, + 0.2896295f, 0.9564251f, -0.0369570f, + -0.1986693f, 0.0978434f, 0.9751703f))); + // euler angle ctor + ASSERT(matrixEqual( + Dcm(EulerAngles(0.1f, 0.2f, 0.3f)), + Dcm(0.9362934f, -0.2750958f, 0.2183507f, + 0.2896295f, 0.9564251f, -0.0369570f, + -0.1986693f, 0.0978434f, 0.9751703f))); + // rotations + Vector3 vB(1, 2, 3); + ASSERT(vectorEqual(Vector3(-2.0f, 1.0f, 3.0f), + Dcm(EulerAngles(0.0f, 0.0f, M_PI_2_F))*vB)); + ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f), + Dcm(EulerAngles(0.0f, M_PI_2_F, 0.0f))*vB)); + ASSERT(vectorEqual(Vector3(1.0f, -3.0f, 2.0f), + Dcm(EulerAngles(M_PI_2_F, 0.0f, 0.0f))*vB)); + ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f), + Dcm(EulerAngles( + M_PI_2_F, M_PI_2_F, M_PI_2_F))*vB)); + printf("PASS\n"); + return 0; +} +} // namespace math diff --git a/src/modules/mathlib/math/Dcm.hpp b/src/modules/mathlib/math/Dcm.hpp new file mode 100644 index 000000000..df8970d3a --- /dev/null +++ b/src/modules/mathlib/math/Dcm.hpp @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Dcm.hpp + * + * math direction cosine matrix + */ + +//#pragma once + +#include "Vector.hpp" +#include "Matrix.hpp" + +namespace math +{ + +class Quaternion; +class EulerAngles; + +/** + * This is a Tait Bryan, Body 3-2-1 sequence. + * (yaw)-(pitch)-(roll) + * The Dcm transforms a vector in the body frame + * to the navigation frame, typically represented + * as C_nb. C_bn can be obtained through use + * of the transpose() method. + */ +class __EXPORT Dcm : public Matrix +{ +public: + /** + * default ctor + */ + Dcm(); + + /** + * scalar ctor + */ + Dcm(float c00, float c01, float c02, + float c10, float c11, float c12, + float c20, float c21, float c22); + + /** + * data ctor + */ + Dcm(const float *data); + + /** + * array ctor + */ + Dcm(const float data[3][3]); + + /** + * quaternion ctor + */ + Dcm(const Quaternion &q); + + /** + * euler angles ctor + */ + Dcm(const EulerAngles &euler); + + /** + * copy ctor (deep) + */ + Dcm(const Dcm &right); + + /** + * dtor + */ + virtual ~Dcm(); +}; + +int __EXPORT dcmTest(); + +} // math + diff --git a/src/modules/mathlib/math/EulerAngles.cpp b/src/modules/mathlib/math/EulerAngles.cpp new file mode 100644 index 000000000..e733d23bb --- /dev/null +++ b/src/modules/mathlib/math/EulerAngles.cpp @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Vector.cpp + * + * math vector + */ + +#include "test/test.hpp" + +#include "EulerAngles.hpp" +#include "Quaternion.hpp" +#include "Dcm.hpp" +#include "Vector3.hpp" + +namespace math +{ + +EulerAngles::EulerAngles() : + Vector(3) +{ + setPhi(0.0f); + setTheta(0.0f); + setPsi(0.0f); +} + +EulerAngles::EulerAngles(float phi, float theta, float psi) : + Vector(3) +{ + setPhi(phi); + setTheta(theta); + setPsi(psi); +} + +EulerAngles::EulerAngles(const Quaternion &q) : + Vector(3) +{ + (*this) = EulerAngles(Dcm(q)); +} + +EulerAngles::EulerAngles(const Dcm &dcm) : + Vector(3) +{ + setTheta(asinf(-dcm(2, 0))); + + if (fabsf(getTheta() - M_PI_2_F) < 1.0e-3f) { + setPhi(0.0f); + setPsi(atan2f(dcm(1, 2) - dcm(0, 1), + dcm(0, 2) + dcm(1, 1)) + getPhi()); + + } else if (fabsf(getTheta() + M_PI_2_F) < 1.0e-3f) { + setPhi(0.0f); + setPsi(atan2f(dcm(1, 2) - dcm(0, 1), + dcm(0, 2) + dcm(1, 1)) - getPhi()); + + } else { + setPhi(atan2f(dcm(2, 1), dcm(2, 2))); + setPsi(atan2f(dcm(1, 0), dcm(0, 0))); + } +} + +EulerAngles::~EulerAngles() +{ +} + +int __EXPORT eulerAnglesTest() +{ + printf("Test EulerAngles\t: "); + EulerAngles euler(0.1f, 0.2f, 0.3f); + + // test ctor + ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler)); + ASSERT(equal(euler.getPhi(), 0.1f)); + ASSERT(equal(euler.getTheta(), 0.2f)); + ASSERT(equal(euler.getPsi(), 0.3f)); + + // test dcm ctor + euler = Dcm(EulerAngles(0.1f, 0.2f, 0.3f)); + ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler)); + + // test quat ctor + euler = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f)); + ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler)); + + // test assignment + euler.setPhi(0.4f); + euler.setTheta(0.5f); + euler.setPsi(0.6f); + ASSERT(vectorEqual(Vector3(0.4f, 0.5f, 0.6f), euler)); + + printf("PASS\n"); + return 0; +} + +} // namespace math diff --git a/src/modules/mathlib/math/EulerAngles.hpp b/src/modules/mathlib/math/EulerAngles.hpp new file mode 100644 index 000000000..399eecfa7 --- /dev/null +++ b/src/modules/mathlib/math/EulerAngles.hpp @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Vector.h + * + * math vector + */ + +#pragma once + +#include "Vector.hpp" + +namespace math +{ + +class Quaternion; +class Dcm; + +class __EXPORT EulerAngles : public Vector +{ +public: + EulerAngles(); + EulerAngles(float phi, float theta, float psi); + EulerAngles(const Quaternion &q); + EulerAngles(const Dcm &dcm); + virtual ~EulerAngles(); + + // alias + void setPhi(float phi) { (*this)(0) = phi; } + void setTheta(float theta) { (*this)(1) = theta; } + void setPsi(float psi) { (*this)(2) = psi; } + + // const accessors + const float &getPhi() const { return (*this)(0); } + const float &getTheta() const { return (*this)(1); } + const float &getPsi() const { return (*this)(2); } + +}; + +int __EXPORT eulerAnglesTest(); + +} // math + diff --git a/src/modules/mathlib/math/Limits.cpp b/src/modules/mathlib/math/Limits.cpp new file mode 100644 index 000000000..d4c892d8a --- /dev/null +++ b/src/modules/mathlib/math/Limits.cpp @@ -0,0 +1,146 @@ +/**************************************************************************** + * + * 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 Limits.cpp + * + * Limiting / constrain helper functions + */ + + +#include <math.h> +#include <stdint.h> + +#include "Limits.hpp" + + +namespace math { + + +float __EXPORT min(float val1, float val2) +{ + return (val1 < val2) ? val1 : val2; +} + +int __EXPORT min(int val1, int val2) +{ + return (val1 < val2) ? val1 : val2; +} + +unsigned __EXPORT min(unsigned val1, unsigned val2) +{ + return (val1 < val2) ? val1 : val2; +} + +uint64_t __EXPORT min(uint64_t val1, uint64_t val2) +{ + return (val1 < val2) ? val1 : val2; +} + +double __EXPORT min(double val1, double val2) +{ + return (val1 < val2) ? val1 : val2; +} + +float __EXPORT max(float val1, float val2) +{ + return (val1 > val2) ? val1 : val2; +} + +int __EXPORT max(int val1, int val2) +{ + return (val1 > val2) ? val1 : val2; +} + +unsigned __EXPORT max(unsigned val1, unsigned val2) +{ + return (val1 > val2) ? val1 : val2; +} + +uint64_t __EXPORT max(uint64_t val1, uint64_t val2) +{ + return (val1 > val2) ? val1 : val2; +} + +double __EXPORT max(double val1, double val2) +{ + return (val1 > val2) ? val1 : val2; +} + + +float __EXPORT constrain(float val, float min, float max) +{ + return (val < min) ? min : ((val > max) ? max : val); +} + +int __EXPORT constrain(int val, int min, int max) +{ + return (val < min) ? min : ((val > max) ? max : val); +} + +unsigned __EXPORT constrain(unsigned val, unsigned min, unsigned max) +{ + return (val < min) ? min : ((val > max) ? max : val); +} + +uint64_t __EXPORT constrain(uint64_t val, uint64_t min, uint64_t max) +{ + return (val < min) ? min : ((val > max) ? max : val); +} + +double __EXPORT constrain(double val, double min, double max) +{ + return (val < min) ? min : ((val > max) ? max : val); +} + +float __EXPORT radians(float degrees) +{ + return (degrees / 180.0f) * M_PI_F; +} + +double __EXPORT radians(double degrees) +{ + return (degrees / 180.0) * M_PI; +} + +float __EXPORT degrees(float radians) +{ + return (radians / M_PI_F) * 180.0f; +} + +double __EXPORT degrees(double radians) +{ + return (radians / M_PI) * 180.0; +} + +}
\ No newline at end of file diff --git a/src/modules/mathlib/math/Limits.hpp b/src/modules/mathlib/math/Limits.hpp new file mode 100644 index 000000000..fb778dd66 --- /dev/null +++ b/src/modules/mathlib/math/Limits.hpp @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * 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 Limits.hpp + * + * Limiting / constrain helper functions + */ + +#pragma once + +#include <nuttx/config.h> +#include <stdint.h> + +namespace math { + + +float __EXPORT min(float val1, float val2); + +int __EXPORT min(int val1, int val2); + +unsigned __EXPORT min(unsigned val1, unsigned val2); + +uint64_t __EXPORT min(uint64_t val1, uint64_t val2); + +double __EXPORT min(double val1, double val2); + +float __EXPORT max(float val1, float val2); + +int __EXPORT max(int val1, int val2); + +unsigned __EXPORT max(unsigned val1, unsigned val2); + +uint64_t __EXPORT max(uint64_t val1, uint64_t val2); + +double __EXPORT max(double val1, double val2); + + +float __EXPORT constrain(float val, float min, float max); + +int __EXPORT constrain(int val, int min, int max); + +unsigned __EXPORT constrain(unsigned val, unsigned min, unsigned max); + +uint64_t __EXPORT constrain(uint64_t val, uint64_t min, uint64_t max); + +double __EXPORT constrain(double val, double min, double max); + +float __EXPORT radians(float degrees); + +double __EXPORT radians(double degrees); + +float __EXPORT degrees(float radians); + +double __EXPORT degrees(double radians); + +}
\ No newline at end of file diff --git a/src/modules/mathlib/math/Matrix.cpp b/src/modules/mathlib/math/Matrix.cpp new file mode 100644 index 000000000..ebd1aeda3 --- /dev/null +++ b/src/modules/mathlib/math/Matrix.cpp @@ -0,0 +1,193 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Matrix.cpp + * + * matrix code + */ + +#include "test/test.hpp" +#include <math.h> + +#include "Matrix.hpp" + +namespace math +{ + +static const float data_testA[] = { + 1, 2, 3, + 4, 5, 6 +}; +static Matrix testA(2, 3, data_testA); + +static const float data_testB[] = { + 0, 1, 3, + 7, -1, 2 +}; +static Matrix testB(2, 3, data_testB); + +static const float data_testC[] = { + 0, 1, + 2, 1, + 3, 2 +}; +static Matrix testC(3, 2, data_testC); + +static const float data_testD[] = { + 0, 1, 2, + 2, 1, 4, + 5, 2, 0 +}; +static Matrix testD(3, 3, data_testD); + +static const float data_testE[] = { + 1, -1, 2, + 0, 2, 3, + 2, -1, 1 +}; +static Matrix testE(3, 3, data_testE); + +static const float data_testF[] = { + 3.777e006f, 2.915e007f, 0.000e000f, + 2.938e007f, 2.267e008f, 0.000e000f, + 0.000e000f, 0.000e000f, 6.033e008f +}; +static Matrix testF(3, 3, data_testF); + +int __EXPORT matrixTest() +{ + matrixAddTest(); + matrixSubTest(); + matrixMultTest(); + matrixInvTest(); + matrixDivTest(); + return 0; +} + +int matrixAddTest() +{ + printf("Test Matrix Add\t\t: "); + Matrix r = testA + testB; + float data_test[] = { + 1.0f, 3.0f, 6.0f, + 11.0f, 4.0f, 8.0f + }; + ASSERT(matrixEqual(Matrix(2, 3, data_test), r)); + printf("PASS\n"); + return 0; +} + +int matrixSubTest() +{ + printf("Test Matrix Sub\t\t: "); + Matrix r = testA - testB; + float data_test[] = { + 1.0f, 1.0f, 0.0f, + -3.0f, 6.0f, 4.0f + }; + ASSERT(matrixEqual(Matrix(2, 3, data_test), r)); + printf("PASS\n"); + return 0; +} + +int matrixMultTest() +{ + printf("Test Matrix Mult\t: "); + Matrix r = testC * testB; + float data_test[] = { + 7.0f, -1.0f, 2.0f, + 7.0f, 1.0f, 8.0f, + 14.0f, 1.0f, 13.0f + }; + ASSERT(matrixEqual(Matrix(3, 3, data_test), r)); + printf("PASS\n"); + return 0; +} + +int matrixInvTest() +{ + printf("Test Matrix Inv\t\t: "); + Matrix origF = testF; + Matrix r = testF.inverse(); + float data_test[] = { + -0.0012518f, 0.0001610f, 0.0000000f, + 0.0001622f, -0.0000209f, 0.0000000f, + 0.0000000f, 0.0000000f, 1.6580e-9f + }; + ASSERT(matrixEqual(Matrix(3, 3, data_test), r)); + // make sure F in unchanged + ASSERT(matrixEqual(origF, testF)); + printf("PASS\n"); + return 0; +} + +int matrixDivTest() +{ + printf("Test Matrix Div\t\t: "); + Matrix r = testD / testE; + float data_test[] = { + 0.2222222f, 0.5555556f, -0.1111111f, + 0.0f, 1.0f, 1.0, + -4.1111111f, 1.2222222f, 4.5555556f + }; + ASSERT(matrixEqual(Matrix(3, 3, data_test), r)); + printf("PASS\n"); + return 0; +} + +bool matrixEqual(const Matrix &a, const Matrix &b, float eps) +{ + if (a.getRows() != b.getRows()) { + printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows()); + return false; + + } else if (a.getCols() != b.getCols()) { + printf("column number not equal a: %d, b:%d\n", a.getCols(), b.getCols()); + return false; + } + + bool ret = true; + + for (size_t i = 0; i < a.getRows(); i++) + for (size_t j = 0; j < a.getCols(); j++) { + if (!equal(a(i, j), b(i, j), eps)) { + printf("element mismatch (%d, %d)\n", i, j); + ret = false; + } + } + + return ret; +} + +} // namespace math diff --git a/src/modules/mathlib/math/Matrix.hpp b/src/modules/mathlib/math/Matrix.hpp new file mode 100644 index 000000000..f19db15ec --- /dev/null +++ b/src/modules/mathlib/math/Matrix.hpp @@ -0,0 +1,61 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Matrix.h + * + * matrix code + */ + +#pragma once + +#include <nuttx/config.h> + +#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) +#include "arm/Matrix.hpp" +#else +#include "generic/Matrix.hpp" +#endif + +namespace math +{ +class Matrix; +int matrixTest(); +int matrixAddTest(); +int matrixSubTest(); +int matrixMultTest(); +int matrixInvTest(); +int matrixDivTest(); +int matrixArmTest(); +bool matrixEqual(const Matrix &a, const Matrix &b, float eps = 1.0e-5f); +} // namespace math diff --git a/src/modules/mathlib/math/Quaternion.cpp b/src/modules/mathlib/math/Quaternion.cpp new file mode 100644 index 000000000..02fec4ca6 --- /dev/null +++ b/src/modules/mathlib/math/Quaternion.cpp @@ -0,0 +1,174 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Quaternion.cpp + * + * math vector + */ + +#include "test/test.hpp" + + +#include "Quaternion.hpp" +#include "Dcm.hpp" +#include "EulerAngles.hpp" + +namespace math +{ + +Quaternion::Quaternion() : + Vector(4) +{ + setA(1.0f); + setB(0.0f); + setC(0.0f); + setD(0.0f); +} + +Quaternion::Quaternion(float a, float b, + float c, float d) : + Vector(4) +{ + setA(a); + setB(b); + setC(c); + setD(d); +} + +Quaternion::Quaternion(const float *data) : + Vector(4, data) +{ +} + +Quaternion::Quaternion(const Vector &v) : + Vector(v) +{ +} + +Quaternion::Quaternion(const Dcm &dcm) : + Vector(4) +{ + // avoiding singularities by not using + // division equations + setA(0.5 * sqrt(1.0 + + double(dcm(0, 0) + dcm(1, 1) + dcm(2, 2)))); + setB(0.5 * sqrt(1.0 + + double(dcm(0, 0) - dcm(1, 1) - dcm(2, 2)))); + setC(0.5 * sqrt(1.0 + + double(-dcm(0, 0) + dcm(1, 1) - dcm(2, 2)))); + setD(0.5 * sqrt(1.0 + + double(-dcm(0, 0) - dcm(1, 1) + dcm(2, 2)))); +} + +Quaternion::Quaternion(const EulerAngles &euler) : + Vector(4) +{ + double cosPhi_2 = cos(double(euler.getPhi()) / 2.0); + double sinPhi_2 = sin(double(euler.getPhi()) / 2.0); + double cosTheta_2 = cos(double(euler.getTheta()) / 2.0); + double sinTheta_2 = sin(double(euler.getTheta()) / 2.0); + double cosPsi_2 = cos(double(euler.getPsi()) / 2.0); + double sinPsi_2 = sin(double(euler.getPsi()) / 2.0); + setA(cosPhi_2 * cosTheta_2 * cosPsi_2 + + sinPhi_2 * sinTheta_2 * sinPsi_2); + setB(sinPhi_2 * cosTheta_2 * cosPsi_2 - + cosPhi_2 * sinTheta_2 * sinPsi_2); + setC(cosPhi_2 * sinTheta_2 * cosPsi_2 + + sinPhi_2 * cosTheta_2 * sinPsi_2); + setD(cosPhi_2 * cosTheta_2 * sinPsi_2 - + sinPhi_2 * sinTheta_2 * cosPsi_2); +} + +Quaternion::Quaternion(const Quaternion &right) : + Vector(right) +{ +} + +Quaternion::~Quaternion() +{ +} + +Vector Quaternion::derivative(const Vector &w) +{ +#ifdef QUATERNION_ASSERT + ASSERT(w.getRows() == 3); +#endif + float dataQ[] = { + getA(), -getB(), -getC(), -getD(), + getB(), getA(), -getD(), getC(), + getC(), getD(), getA(), -getB(), + getD(), -getC(), getB(), getA() + }; + Vector v(4); + v(0) = 0.0f; + v(1) = w(0); + v(2) = w(1); + v(3) = w(2); + Matrix Q(4, 4, dataQ); + return Q * v * 0.5f; +} + +int __EXPORT quaternionTest() +{ + printf("Test Quaternion\t\t: "); + // test default ctor + Quaternion q; + ASSERT(equal(q.getA(), 1.0f)); + ASSERT(equal(q.getB(), 0.0f)); + ASSERT(equal(q.getC(), 0.0f)); + ASSERT(equal(q.getD(), 0.0f)); + // test float ctor + q = Quaternion(0.1825742f, 0.3651484f, 0.5477226f, 0.7302967f); + ASSERT(equal(q.getA(), 0.1825742f)); + ASSERT(equal(q.getB(), 0.3651484f)); + ASSERT(equal(q.getC(), 0.5477226f)); + ASSERT(equal(q.getD(), 0.7302967f)); + // test euler ctor + q = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f)); + ASSERT(vectorEqual(q, Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f))); + // test dcm ctor + q = Quaternion(Dcm()); + ASSERT(vectorEqual(q, Quaternion(1.0f, 0.0f, 0.0f, 0.0f))); + // TODO test derivative + // test accessors + q.setA(0.1f); + q.setB(0.2f); + q.setC(0.3f); + q.setD(0.4f); + ASSERT(vectorEqual(q, Quaternion(0.1f, 0.2f, 0.3f, 0.4f))); + printf("PASS\n"); + return 0; +} + +} // namespace math diff --git a/src/modules/mathlib/math/Quaternion.hpp b/src/modules/mathlib/math/Quaternion.hpp new file mode 100644 index 000000000..048a55d33 --- /dev/null +++ b/src/modules/mathlib/math/Quaternion.hpp @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Quaternion.hpp + * + * math quaternion lib + */ + +#pragma once + +#include "Vector.hpp" +#include "Matrix.hpp" + +namespace math +{ + +class Dcm; +class EulerAngles; + +class __EXPORT Quaternion : public Vector +{ +public: + + /** + * default ctor + */ + Quaternion(); + + /** + * ctor from floats + */ + Quaternion(float a, float b, float c, float d); + + /** + * ctor from data + */ + Quaternion(const float *data); + + /** + * ctor from Vector + */ + Quaternion(const Vector &v); + + /** + * ctor from EulerAngles + */ + Quaternion(const EulerAngles &euler); + + /** + * ctor from Dcm + */ + Quaternion(const Dcm &dcm); + + /** + * deep copy ctor + */ + Quaternion(const Quaternion &right); + + /** + * dtor + */ + virtual ~Quaternion(); + + /** + * derivative + */ + Vector derivative(const Vector &w); + + /** + * accessors + */ + void setA(float a) { (*this)(0) = a; } + void setB(float b) { (*this)(1) = b; } + void setC(float c) { (*this)(2) = c; } + void setD(float d) { (*this)(3) = d; } + const float &getA() const { return (*this)(0); } + const float &getB() const { return (*this)(1); } + const float &getC() const { return (*this)(2); } + const float &getD() const { return (*this)(3); } +}; + +int __EXPORT quaternionTest(); +} // math + diff --git a/src/modules/mathlib/math/Vector.cpp b/src/modules/mathlib/math/Vector.cpp new file mode 100644 index 000000000..35158a396 --- /dev/null +++ b/src/modules/mathlib/math/Vector.cpp @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Vector.cpp + * + * math vector + */ + +#include "test/test.hpp" + +#include "Vector.hpp" + +namespace math +{ + +static const float data_testA[] = {1, 3}; +static const float data_testB[] = {4, 1}; + +static Vector testA(2, data_testA); +static Vector testB(2, data_testB); + +int __EXPORT vectorTest() +{ + vectorAddTest(); + vectorSubTest(); + return 0; +} + +int vectorAddTest() +{ + printf("Test Vector Add\t\t: "); + Vector r = testA + testB; + float data_test[] = {5.0f, 4.0f}; + ASSERT(vectorEqual(Vector(2, data_test), r)); + printf("PASS\n"); + return 0; +} + +int vectorSubTest() +{ + printf("Test Vector Sub\t\t: "); + Vector r(2); + r = testA - testB; + float data_test[] = { -3.0f, 2.0f}; + ASSERT(vectorEqual(Vector(2, data_test), r)); + printf("PASS\n"); + return 0; +} + +bool vectorEqual(const Vector &a, const Vector &b, float eps) +{ + if (a.getRows() != b.getRows()) { + printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows()); + return false; + } + + bool ret = true; + + for (size_t i = 0; i < a.getRows(); i++) { + if (!equal(a(i), b(i), eps)) { + printf("element mismatch (%d)\n", i); + ret = false; + } + } + + return ret; +} + +} // namespace math diff --git a/src/modules/mathlib/math/Vector.hpp b/src/modules/mathlib/math/Vector.hpp new file mode 100644 index 000000000..73de793d5 --- /dev/null +++ b/src/modules/mathlib/math/Vector.hpp @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Vector.h + * + * math vector + */ + +#pragma once + +#include <nuttx/config.h> + +#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) +#include "arm/Vector.hpp" +#else +#include "generic/Vector.hpp" +#endif + +namespace math +{ +class Vector; +int __EXPORT vectorTest(); +int __EXPORT vectorAddTest(); +int __EXPORT vectorSubTest(); +bool vectorEqual(const Vector &a, const Vector &b, float eps = 1.0e-5f); +} // math diff --git a/src/modules/mathlib/math/Vector2f.cpp b/src/modules/mathlib/math/Vector2f.cpp new file mode 100644 index 000000000..68e741817 --- /dev/null +++ b/src/modules/mathlib/math/Vector2f.cpp @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * 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 Vector2f.cpp + * + * math vector + */ + +#include "test/test.hpp" + +#include "Vector2f.hpp" + +namespace math +{ + +Vector2f::Vector2f() : + Vector(2) +{ +} + +Vector2f::Vector2f(const Vector &right) : + Vector(right) +{ +#ifdef VECTOR_ASSERT + ASSERT(right.getRows() == 2); +#endif +} + +Vector2f::Vector2f(float x, float y) : + Vector(2) +{ + setX(x); + setY(y); +} + +Vector2f::Vector2f(const float *data) : + Vector(2, data) +{ +} + +Vector2f::~Vector2f() +{ +} + +float Vector2f::cross(const Vector2f &b) const +{ + const Vector2f &a = *this; + return a(0)*b(1) - a(1)*b(0); +} + +float Vector2f::operator %(const Vector2f &v) const +{ + return cross(v); +} + +float Vector2f::operator *(const Vector2f &v) const +{ + return dot(v); +} + +int __EXPORT vector2fTest() +{ + printf("Test Vector2f\t\t: "); + // test float ctor + Vector2f v(1, 2); + ASSERT(equal(v(0), 1)); + ASSERT(equal(v(1), 2)); + printf("PASS\n"); + return 0; +} + +} // namespace math diff --git a/src/modules/mathlib/math/Vector2f.hpp b/src/modules/mathlib/math/Vector2f.hpp new file mode 100644 index 000000000..ecd62e81c --- /dev/null +++ b/src/modules/mathlib/math/Vector2f.hpp @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * 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 Vector2f.hpp + * + * math 3 vector + */ + +#pragma once + +#include "Vector.hpp" + +namespace math +{ + +class __EXPORT Vector2f : + public Vector +{ +public: + Vector2f(); + Vector2f(const Vector &right); + Vector2f(float x, float y); + Vector2f(const float *data); + virtual ~Vector2f(); + float cross(const Vector2f &b) const; + float operator %(const Vector2f &v) const; + float operator *(const Vector2f &v) const; + inline Vector2f operator*(const float &right) const { + return Vector::operator*(right); + } + + /** + * accessors + */ + void setX(float x) { (*this)(0) = x; } + void setY(float y) { (*this)(1) = y; } + const float &getX() const { return (*this)(0); } + const float &getY() const { return (*this)(1); } +}; + +class __EXPORT Vector2 : + public Vector2f +{ +}; + +int __EXPORT vector2fTest(); +} // math + diff --git a/src/modules/mathlib/math/Vector3.cpp b/src/modules/mathlib/math/Vector3.cpp new file mode 100644 index 000000000..dcb85600e --- /dev/null +++ b/src/modules/mathlib/math/Vector3.cpp @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Vector3.cpp + * + * math vector + */ + +#include "test/test.hpp" + +#include "Vector3.hpp" + +namespace math +{ + +Vector3::Vector3() : + Vector(3) +{ +} + +Vector3::Vector3(const Vector &right) : + Vector(right) +{ +#ifdef VECTOR_ASSERT + ASSERT(right.getRows() == 3); +#endif +} + +Vector3::Vector3(float x, float y, float z) : + Vector(3) +{ + setX(x); + setY(y); + setZ(z); +} + +Vector3::Vector3(const float *data) : + Vector(3, data) +{ +} + +Vector3::~Vector3() +{ +} + +Vector3 Vector3::cross(const Vector3 &b) const +{ + const Vector3 &a = *this; + Vector3 result; + result(0) = a(1) * b(2) - a(2) * b(1); + result(1) = a(2) * b(0) - a(0) * b(2); + result(2) = a(0) * b(1) - a(1) * b(0); + return result; +} + +int __EXPORT vector3Test() +{ + printf("Test Vector3\t\t: "); + // test float ctor + Vector3 v(1, 2, 3); + ASSERT(equal(v(0), 1)); + ASSERT(equal(v(1), 2)); + ASSERT(equal(v(2), 3)); + printf("PASS\n"); + return 0; +} + +} // namespace math diff --git a/src/modules/mathlib/math/Vector3.hpp b/src/modules/mathlib/math/Vector3.hpp new file mode 100644 index 000000000..568d9669a --- /dev/null +++ b/src/modules/mathlib/math/Vector3.hpp @@ -0,0 +1,76 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Vector3.hpp + * + * math 3 vector + */ + +#pragma once + +#include "Vector.hpp" + +namespace math +{ + +class __EXPORT Vector3 : + public Vector +{ +public: + Vector3(); + Vector3(const Vector &right); + Vector3(float x, float y, float z); + Vector3(const float *data); + virtual ~Vector3(); + Vector3 cross(const Vector3 &b) const; + + /** + * accessors + */ + void setX(float x) { (*this)(0) = x; } + void setY(float y) { (*this)(1) = y; } + void setZ(float z) { (*this)(2) = z; } + const float &getX() const { return (*this)(0); } + const float &getY() const { return (*this)(1); } + const float &getZ() const { return (*this)(2); } +}; + +class __EXPORT Vector3f : + public Vector3 +{ +}; + +int __EXPORT vector3Test(); +} // math + diff --git a/src/modules/mathlib/math/arm/Matrix.cpp b/src/modules/mathlib/math/arm/Matrix.cpp new file mode 100644 index 000000000..21661622a --- /dev/null +++ b/src/modules/mathlib/math/arm/Matrix.cpp @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Matrix.cpp + * + * matrix code + */ + +#include "Matrix.hpp" diff --git a/src/modules/mathlib/math/arm/Matrix.hpp b/src/modules/mathlib/math/arm/Matrix.hpp new file mode 100644 index 000000000..715fd3a5e --- /dev/null +++ b/src/modules/mathlib/math/arm/Matrix.hpp @@ -0,0 +1,292 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Matrix.h + * + * matrix code + */ + +#pragma once + + +#include <inttypes.h> +#include <assert.h> +#include <stdlib.h> +#include <string.h> +#include <stdio.h> +#include <math.h> + +#include "../Vector.hpp" +#include "../Matrix.hpp" + +// arm specific +#include "../../CMSIS/Include/arm_math.h" + +namespace math +{ + +class __EXPORT Matrix +{ +public: + // constructor + Matrix(size_t rows, size_t cols) : + _matrix() { + arm_mat_init_f32(&_matrix, + rows, cols, + (float *)calloc(rows * cols, sizeof(float))); + } + Matrix(size_t rows, size_t cols, const float *data) : + _matrix() { + arm_mat_init_f32(&_matrix, + rows, cols, + (float *)malloc(rows * cols * sizeof(float))); + memcpy(getData(), data, getSize()); + } + // deconstructor + virtual ~Matrix() { + delete [] _matrix.pData; + } + // copy constructor (deep) + Matrix(const Matrix &right) : + _matrix() { + arm_mat_init_f32(&_matrix, + right.getRows(), right.getCols(), + (float *)malloc(right.getRows()* + right.getCols()*sizeof(float))); + memcpy(getData(), right.getData(), + getSize()); + } + // assignment + inline Matrix &operator=(const Matrix &right) { +#ifdef MATRIX_ASSERT + ASSERT(getRows() == right.getRows()); + ASSERT(getCols() == right.getCols()); +#endif + + if (this != &right) { + memcpy(getData(), right.getData(), + right.getSize()); + } + + return *this; + } + // element accessors + inline float &operator()(size_t i, size_t j) { +#ifdef MATRIX_ASSERT + ASSERT(i < getRows()); + ASSERT(j < getCols()); +#endif + return getData()[i * getCols() + j]; + } + inline const float &operator()(size_t i, size_t j) const { +#ifdef MATRIX_ASSERT + ASSERT(i < getRows()); + ASSERT(j < getCols()); +#endif + return getData()[i * getCols() + j]; + } + // output + inline void print() const { + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < getCols(); j++) { + float sig; + int exp; + float num = (*this)(i, j); + float2SigExp(num, sig, exp); + printf("%6.3fe%03.3d,", (double)sig, exp); + } + + printf("\n"); + } + } + // boolean ops + inline bool operator==(const Matrix &right) const { + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < getCols(); j++) { + if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f) + return false; + } + } + + return true; + } + // scalar ops + inline Matrix operator+(float right) const { + Matrix result(getRows(), getCols()); + arm_offset_f32((float *)getData(), right, + (float *)result.getData(), getRows()*getCols()); + return result; + } + inline Matrix operator-(float right) const { + Matrix result(getRows(), getCols()); + arm_offset_f32((float *)getData(), -right, + (float *)result.getData(), getRows()*getCols()); + return result; + } + inline Matrix operator*(float right) const { + Matrix result(getRows(), getCols()); + arm_mat_scale_f32(&_matrix, right, + &(result._matrix)); + return result; + } + inline Matrix operator/(float right) const { + Matrix result(getRows(), getCols()); + arm_mat_scale_f32(&_matrix, 1.0f / right, + &(result._matrix)); + return result; + } + // vector ops + inline Vector operator*(const Vector &right) const { +#ifdef MATRIX_ASSERT + ASSERT(getCols() == right.getRows()); +#endif + Matrix resultMat = (*this) * + Matrix(right.getRows(), 1, right.getData()); + return Vector(getRows(), resultMat.getData()); + } + // matrix ops + inline Matrix operator+(const Matrix &right) const { +#ifdef MATRIX_ASSERT + ASSERT(getRows() == right.getRows()); + ASSERT(getCols() == right.getCols()); +#endif + Matrix result(getRows(), getCols()); + arm_mat_add_f32(&_matrix, &(right._matrix), + &(result._matrix)); + return result; + } + inline Matrix operator-(const Matrix &right) const { +#ifdef MATRIX_ASSERT + ASSERT(getRows() == right.getRows()); + ASSERT(getCols() == right.getCols()); +#endif + Matrix result(getRows(), getCols()); + arm_mat_sub_f32(&_matrix, &(right._matrix), + &(result._matrix)); + return result; + } + inline Matrix operator*(const Matrix &right) const { +#ifdef MATRIX_ASSERT + ASSERT(getCols() == right.getRows()); +#endif + Matrix result(getRows(), right.getCols()); + arm_mat_mult_f32(&_matrix, &(right._matrix), + &(result._matrix)); + return result; + } + inline Matrix operator/(const Matrix &right) const { +#ifdef MATRIX_ASSERT + ASSERT(right.getRows() == right.getCols()); + ASSERT(getCols() == right.getCols()); +#endif + return (*this) * right.inverse(); + } + // other functions + inline Matrix transpose() const { + Matrix result(getCols(), getRows()); + arm_mat_trans_f32(&_matrix, &(result._matrix)); + return result; + } + inline void swapRows(size_t a, size_t b) { + if (a == b) return; + + for (size_t j = 0; j < getCols(); j++) { + float tmp = (*this)(a, j); + (*this)(a, j) = (*this)(b, j); + (*this)(b, j) = tmp; + } + } + inline void swapCols(size_t a, size_t b) { + if (a == b) return; + + for (size_t i = 0; i < getRows(); i++) { + float tmp = (*this)(i, a); + (*this)(i, a) = (*this)(i, b); + (*this)(i, b) = tmp; + } + } + /** + * inverse based on LU factorization with partial pivotting + */ + Matrix inverse() const { +#ifdef MATRIX_ASSERT + ASSERT(getRows() == getCols()); +#endif + Matrix result(getRows(), getCols()); + Matrix work = (*this); + arm_mat_inverse_f32(&(work._matrix), + &(result._matrix)); + return result; + } + inline void setAll(const float &val) { + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < getCols(); j++) { + (*this)(i, j) = val; + } + } + } + inline void set(const float *data) { + memcpy(getData(), data, getSize()); + } + inline size_t getRows() const { return _matrix.numRows; } + inline size_t getCols() const { return _matrix.numCols; } + inline static Matrix identity(size_t size) { + Matrix result(size, size); + + for (size_t i = 0; i < size; i++) { + result(i, i) = 1.0f; + } + + return result; + } + inline static Matrix zero(size_t size) { + Matrix result(size, size); + result.setAll(0.0f); + return result; + } + inline static Matrix zero(size_t m, size_t n) { + Matrix result(m, n); + result.setAll(0.0f); + return result; + } +protected: + inline size_t getSize() const { return sizeof(float) * getRows() * getCols(); } + inline float *getData() { return _matrix.pData; } + inline const float *getData() const { return _matrix.pData; } + inline void setData(float *data) { _matrix.pData = data; } +private: + arm_matrix_instance_f32 _matrix; +}; + +} // namespace math diff --git a/src/modules/mathlib/math/arm/Vector.cpp b/src/modules/mathlib/math/arm/Vector.cpp new file mode 100644 index 000000000..7ea6496bb --- /dev/null +++ b/src/modules/mathlib/math/arm/Vector.cpp @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Vector.cpp + * + * math vector + */ + +#include "Vector.hpp" diff --git a/src/modules/mathlib/math/arm/Vector.hpp b/src/modules/mathlib/math/arm/Vector.hpp new file mode 100644 index 000000000..4155800e8 --- /dev/null +++ b/src/modules/mathlib/math/arm/Vector.hpp @@ -0,0 +1,236 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Vector.h + * + * math vector + */ + +#pragma once + +#include <inttypes.h> +#include <assert.h> +#include <stdlib.h> +#include <string.h> +#include <stdio.h> +#include <math.h> + +#include "../Vector.hpp" +#include "../test/test.hpp" + +// arm specific +#include "../../CMSIS/Include/arm_math.h" + +namespace math +{ + +class __EXPORT Vector +{ +public: + // constructor + Vector(size_t rows) : + _rows(rows), + _data((float *)calloc(rows, sizeof(float))) { + } + Vector(size_t rows, const float *data) : + _rows(rows), + _data((float *)malloc(getSize())) { + memcpy(getData(), data, getSize()); + } + // deconstructor + virtual ~Vector() { + delete [] getData(); + } + // copy constructor (deep) + Vector(const Vector &right) : + _rows(right.getRows()), + _data((float *)malloc(getSize())) { + memcpy(getData(), right.getData(), + right.getSize()); + } + // assignment + inline Vector &operator=(const Vector &right) { +#ifdef VECTOR_ASSERT + ASSERT(getRows() == right.getRows()); +#endif + + if (this != &right) { + memcpy(getData(), right.getData(), + right.getSize()); + } + + return *this; + } + // element accessors + inline float &operator()(size_t i) { +#ifdef VECTOR_ASSERT + ASSERT(i < getRows()); +#endif + return getData()[i]; + } + inline const float &operator()(size_t i) const { +#ifdef VECTOR_ASSERT + ASSERT(i < getRows()); +#endif + return getData()[i]; + } + // output + inline void print() const { + for (size_t i = 0; i < getRows(); i++) { + float sig; + int exp; + float num = (*this)(i); + float2SigExp(num, sig, exp); + printf("%6.3fe%03.3d,", (double)sig, exp); + } + + printf("\n"); + } + // boolean ops + inline bool operator==(const Vector &right) const { + for (size_t i = 0; i < getRows(); i++) { + if (fabsf(((*this)(i) - right(i))) > 1e-30f) + return false; + } + + return true; + } + // scalar ops + inline Vector operator+(float right) const { + Vector result(getRows()); + arm_offset_f32((float *)getData(), + right, result.getData(), + getRows()); + return result; + } + inline Vector operator-(float right) const { + Vector result(getRows()); + arm_offset_f32((float *)getData(), + -right, result.getData(), + getRows()); + return result; + } + inline Vector operator*(float right) const { + Vector result(getRows()); + arm_scale_f32((float *)getData(), + right, result.getData(), + getRows()); + return result; + } + inline Vector operator/(float right) const { + Vector result(getRows()); + arm_scale_f32((float *)getData(), + 1.0f / right, result.getData(), + getRows()); + return result; + } + // vector ops + inline Vector operator+(const Vector &right) const { +#ifdef VECTOR_ASSERT + ASSERT(getRows() == right.getRows()); +#endif + Vector result(getRows()); + arm_add_f32((float *)getData(), + (float *)right.getData(), + result.getData(), + getRows()); + return result; + } + inline Vector operator-(const Vector &right) const { +#ifdef VECTOR_ASSERT + ASSERT(getRows() == right.getRows()); +#endif + Vector result(getRows()); + arm_sub_f32((float *)getData(), + (float *)right.getData(), + result.getData(), + getRows()); + return result; + } + inline Vector operator-(void) const { + Vector result(getRows()); + arm_negate_f32((float *)getData(), + result.getData(), + getRows()); + return result; + } + // other functions + inline float dot(const Vector &right) const { + float result = 0; + arm_dot_prod_f32((float *)getData(), + (float *)right.getData(), + getRows(), + &result); + return result; + } + inline float norm() const { + return sqrtf(dot(*this)); + } + inline float length() const { + return norm(); + } + inline Vector unit() const { + return (*this) / norm(); + } + inline Vector normalized() const { + return unit(); + } + inline void normalize() { + (*this) = (*this) / norm(); + } + inline static Vector zero(size_t rows) { + Vector result(rows); + // calloc returns zeroed memory + return result; + } + inline void setAll(const float &val) { + for (size_t i = 0; i < getRows(); i++) { + (*this)(i) = val; + } + } + inline void set(const float *data) { + memcpy(getData(), data, getSize()); + } + inline size_t getRows() const { return _rows; } + inline const float *getData() const { return _data; } +protected: + inline size_t getSize() const { return sizeof(float) * getRows(); } + inline float *getData() { return _data; } + inline void setData(float *data) { _data = data; } +private: + size_t _rows; + float *_data; +}; + +} // math diff --git a/src/modules/mathlib/math/generic/Matrix.cpp b/src/modules/mathlib/math/generic/Matrix.cpp new file mode 100644 index 000000000..21661622a --- /dev/null +++ b/src/modules/mathlib/math/generic/Matrix.cpp @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Matrix.cpp + * + * matrix code + */ + +#include "Matrix.hpp" diff --git a/src/modules/mathlib/math/generic/Matrix.hpp b/src/modules/mathlib/math/generic/Matrix.hpp new file mode 100644 index 000000000..5601a3447 --- /dev/null +++ b/src/modules/mathlib/math/generic/Matrix.hpp @@ -0,0 +1,437 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Matrix.h + * + * matrix code + */ + +#pragma once + + +#include <inttypes.h> +#include <assert.h> +#include <stdlib.h> +#include <string.h> +#include <stdio.h> +#include <math.h> + +#include "../Vector.hpp" +#include "../Matrix.hpp" + +namespace math +{ + +class __EXPORT Matrix +{ +public: + // constructor + Matrix(size_t rows, size_t cols) : + _rows(rows), + _cols(cols), + _data((float *)calloc(rows *cols, sizeof(float))) { + } + Matrix(size_t rows, size_t cols, const float *data) : + _rows(rows), + _cols(cols), + _data((float *)malloc(getSize())) { + memcpy(getData(), data, getSize()); + } + // deconstructor + virtual ~Matrix() { + delete [] getData(); + } + // copy constructor (deep) + Matrix(const Matrix &right) : + _rows(right.getRows()), + _cols(right.getCols()), + _data((float *)malloc(getSize())) { + memcpy(getData(), right.getData(), + right.getSize()); + } + // assignment + inline Matrix &operator=(const Matrix &right) { +#ifdef MATRIX_ASSERT + ASSERT(getRows() == right.getRows()); + ASSERT(getCols() == right.getCols()); +#endif + + if (this != &right) { + memcpy(getData(), right.getData(), + right.getSize()); + } + + return *this; + } + // element accessors + inline float &operator()(size_t i, size_t j) { +#ifdef MATRIX_ASSERT + ASSERT(i < getRows()); + ASSERT(j < getCols()); +#endif + return getData()[i * getCols() + j]; + } + inline const float &operator()(size_t i, size_t j) const { +#ifdef MATRIX_ASSERT + ASSERT(i < getRows()); + ASSERT(j < getCols()); +#endif + return getData()[i * getCols() + j]; + } + // output + inline void print() const { + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < getCols(); j++) { + float sig; + int exp; + float num = (*this)(i, j); + float2SigExp(num, sig, exp); + printf("%6.3fe%03.3d,", (double)sig, exp); + } + + printf("\n"); + } + } + // boolean ops + inline bool operator==(const Matrix &right) const { + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < getCols(); j++) { + if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f) + return false; + } + } + + return true; + } + // scalar ops + inline Matrix operator+(const float &right) const { + Matrix result(getRows(), getCols()); + + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < getCols(); j++) { + result(i, j) = (*this)(i, j) + right; + } + } + + return result; + } + inline Matrix operator-(const float &right) const { + Matrix result(getRows(), getCols()); + + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < getCols(); j++) { + result(i, j) = (*this)(i, j) - right; + } + } + + return result; + } + inline Matrix operator*(const float &right) const { + Matrix result(getRows(), getCols()); + + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < getCols(); j++) { + result(i, j) = (*this)(i, j) * right; + } + } + + return result; + } + inline Matrix operator/(const float &right) const { + Matrix result(getRows(), getCols()); + + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < getCols(); j++) { + result(i, j) = (*this)(i, j) / right; + } + } + + return result; + } + // vector ops + inline Vector operator*(const Vector &right) const { +#ifdef MATRIX_ASSERT + ASSERT(getCols() == right.getRows()); +#endif + Vector result(getRows()); + + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < getCols(); j++) { + result(i) += (*this)(i, j) * right(j); + } + } + + return result; + } + // matrix ops + inline Matrix operator+(const Matrix &right) const { +#ifdef MATRIX_ASSERT + ASSERT(getRows() == right.getRows()); + ASSERT(getCols() == right.getCols()); +#endif + Matrix result(getRows(), getCols()); + + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < getCols(); j++) { + result(i, j) = (*this)(i, j) + right(i, j); + } + } + + return result; + } + inline Matrix operator-(const Matrix &right) const { +#ifdef MATRIX_ASSERT + ASSERT(getRows() == right.getRows()); + ASSERT(getCols() == right.getCols()); +#endif + Matrix result(getRows(), getCols()); + + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < getCols(); j++) { + result(i, j) = (*this)(i, j) - right(i, j); + } + } + + return result; + } + inline Matrix operator*(const Matrix &right) const { +#ifdef MATRIX_ASSERT + ASSERT(getCols() == right.getRows()); +#endif + Matrix result(getRows(), right.getCols()); + + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < right.getCols(); j++) { + for (size_t k = 0; k < right.getRows(); k++) { + result(i, j) += (*this)(i, k) * right(k, j); + } + } + } + + return result; + } + inline Matrix operator/(const Matrix &right) const { +#ifdef MATRIX_ASSERT + ASSERT(right.getRows() == right.getCols()); + ASSERT(getCols() == right.getCols()); +#endif + return (*this) * right.inverse(); + } + // other functions + inline Matrix transpose() const { + Matrix result(getCols(), getRows()); + + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < getCols(); j++) { + result(j, i) = (*this)(i, j); + } + } + + return result; + } + inline void swapRows(size_t a, size_t b) { + if (a == b) return; + + for (size_t j = 0; j < getCols(); j++) { + float tmp = (*this)(a, j); + (*this)(a, j) = (*this)(b, j); + (*this)(b, j) = tmp; + } + } + inline void swapCols(size_t a, size_t b) { + if (a == b) return; + + for (size_t i = 0; i < getRows(); i++) { + float tmp = (*this)(i, a); + (*this)(i, a) = (*this)(i, b); + (*this)(i, b) = tmp; + } + } + /** + * inverse based on LU factorization with partial pivotting + */ + Matrix inverse() const { +#ifdef MATRIX_ASSERT + ASSERT(getRows() == getCols()); +#endif + size_t N = getRows(); + Matrix L = identity(N); + const Matrix &A = (*this); + Matrix U = A; + Matrix P = identity(N); + + //printf("A:\n"); A.print(); + + // for all diagonal elements + for (size_t n = 0; n < N; n++) { + + // if diagonal is zero, swap with row below + if (fabsf(U(n, n)) < 1e-8f) { + //printf("trying pivot for row %d\n",n); + for (size_t i = 0; i < N; i++) { + if (i == n) continue; + + //printf("\ttrying row %d\n",i); + if (fabsf(U(i, n)) > 1e-8f) { + //printf("swapped %d\n",i); + U.swapRows(i, n); + P.swapRows(i, n); + } + } + } + +#ifdef MATRIX_ASSERT + //printf("A:\n"); A.print(); + //printf("U:\n"); U.print(); + //printf("P:\n"); P.print(); + //fflush(stdout); + ASSERT(fabsf(U(n, n)) > 1e-8f); +#endif + + // failsafe, return zero matrix + if (fabsf(U(n, n)) < 1e-8f) { + return Matrix::zero(n); + } + + // for all rows below diagonal + for (size_t i = (n + 1); i < N; i++) { + L(i, n) = U(i, n) / U(n, n); + + // add i-th row and n-th row + // multiplied by: -a(i,n)/a(n,n) + for (size_t k = n; k < N; k++) { + U(i, k) -= L(i, n) * U(n, k); + } + } + } + + //printf("L:\n"); L.print(); + //printf("U:\n"); U.print(); + + // solve LY=P*I for Y by forward subst + Matrix Y = P; + + // for all columns of Y + for (size_t c = 0; c < N; c++) { + // for all rows of L + for (size_t i = 0; i < N; i++) { + // for all columns of L + for (size_t j = 0; j < i; j++) { + // for all existing y + // subtract the component they + // contribute to the solution + Y(i, c) -= L(i, j) * Y(j, c); + } + + // divide by the factor + // on current + // term to be solved + // Y(i,c) /= L(i,i); + // but L(i,i) = 1.0 + } + } + + //printf("Y:\n"); Y.print(); + + // solve Ux=y for x by back subst + Matrix X = Y; + + // for all columns of X + for (size_t c = 0; c < N; c++) { + // for all rows of U + for (size_t k = 0; k < N; k++) { + // have to go in reverse order + size_t i = N - 1 - k; + + // for all columns of U + for (size_t j = i + 1; j < N; j++) { + // for all existing x + // subtract the component they + // contribute to the solution + X(i, c) -= U(i, j) * X(j, c); + } + + // divide by the factor + // on current + // term to be solved + X(i, c) /= U(i, i); + } + } + + //printf("X:\n"); X.print(); + return X; + } + inline void setAll(const float &val) { + for (size_t i = 0; i < getRows(); i++) { + for (size_t j = 0; j < getCols(); j++) { + (*this)(i, j) = val; + } + } + } + inline void set(const float *data) { + memcpy(getData(), data, getSize()); + } + inline size_t getRows() const { return _rows; } + inline size_t getCols() const { return _cols; } + inline static Matrix identity(size_t size) { + Matrix result(size, size); + + for (size_t i = 0; i < size; i++) { + result(i, i) = 1.0f; + } + + return result; + } + inline static Matrix zero(size_t size) { + Matrix result(size, size); + result.setAll(0.0f); + return result; + } + inline static Matrix zero(size_t m, size_t n) { + Matrix result(m, n); + result.setAll(0.0f); + return result; + } +protected: + inline size_t getSize() const { return sizeof(float) * getRows() * getCols(); } + inline float *getData() { return _data; } + inline const float *getData() const { return _data; } + inline void setData(float *data) { _data = data; } +private: + size_t _rows; + size_t _cols; + float *_data; +}; + +} // namespace math diff --git a/src/modules/mathlib/math/generic/Vector.cpp b/src/modules/mathlib/math/generic/Vector.cpp new file mode 100644 index 000000000..7ea6496bb --- /dev/null +++ b/src/modules/mathlib/math/generic/Vector.cpp @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Vector.cpp + * + * math vector + */ + +#include "Vector.hpp" diff --git a/src/modules/mathlib/math/generic/Vector.hpp b/src/modules/mathlib/math/generic/Vector.hpp new file mode 100644 index 000000000..8cfdc676d --- /dev/null +++ b/src/modules/mathlib/math/generic/Vector.hpp @@ -0,0 +1,245 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Vector.h + * + * math vector + */ + +#pragma once + +#include <inttypes.h> +#include <assert.h> +#include <stdlib.h> +#include <string.h> +#include <stdio.h> +#include <math.h> + +#include "../Vector.hpp" + +namespace math +{ + +class __EXPORT Vector +{ +public: + // constructor + Vector(size_t rows) : + _rows(rows), + _data((float *)calloc(rows, sizeof(float))) { + } + Vector(size_t rows, const float *data) : + _rows(rows), + _data((float *)malloc(getSize())) { + memcpy(getData(), data, getSize()); + } + // deconstructor + virtual ~Vector() { + delete [] getData(); + } + // copy constructor (deep) + Vector(const Vector &right) : + _rows(right.getRows()), + _data((float *)malloc(getSize())) { + memcpy(getData(), right.getData(), + right.getSize()); + } + // assignment + inline Vector &operator=(const Vector &right) { +#ifdef VECTOR_ASSERT + ASSERT(getRows() == right.getRows()); +#endif + + if (this != &right) { + memcpy(getData(), right.getData(), + right.getSize()); + } + + return *this; + } + // element accessors + inline float &operator()(size_t i) { +#ifdef VECTOR_ASSERT + ASSERT(i < getRows()); +#endif + return getData()[i]; + } + inline const float &operator()(size_t i) const { +#ifdef VECTOR_ASSERT + ASSERT(i < getRows()); +#endif + return getData()[i]; + } + // output + inline void print() const { + for (size_t i = 0; i < getRows(); i++) { + float sig; + int exp; + float num = (*this)(i); + float2SigExp(num, sig, exp); + printf("%6.3fe%03.3d,", (double)sig, exp); + } + + printf("\n"); + } + // boolean ops + inline bool operator==(const Vector &right) const { + for (size_t i = 0; i < getRows(); i++) { + if (fabsf(((*this)(i) - right(i))) > 1e-30f) + return false; + } + + return true; + } + // scalar ops + inline Vector operator+(const float &right) const { + Vector result(getRows()); + + for (size_t i = 0; i < getRows(); i++) { + result(i) = (*this)(i) + right; + } + + return result; + } + inline Vector operator-(const float &right) const { + Vector result(getRows()); + + for (size_t i = 0; i < getRows(); i++) { + result(i) = (*this)(i) - right; + } + + return result; + } + inline Vector operator*(const float &right) const { + Vector result(getRows()); + + for (size_t i = 0; i < getRows(); i++) { + result(i) = (*this)(i) * right; + } + + return result; + } + inline Vector operator/(const float &right) const { + Vector result(getRows()); + + for (size_t i = 0; i < getRows(); i++) { + result(i) = (*this)(i) / right; + } + + return result; + } + // vector ops + inline Vector operator+(const Vector &right) const { +#ifdef VECTOR_ASSERT + ASSERT(getRows() == right.getRows()); +#endif + Vector result(getRows()); + + for (size_t i = 0; i < getRows(); i++) { + result(i) = (*this)(i) + right(i); + } + + return result; + } + inline Vector operator-(const Vector &right) const { +#ifdef VECTOR_ASSERT + ASSERT(getRows() == right.getRows()); +#endif + Vector result(getRows()); + + for (size_t i = 0; i < getRows(); i++) { + result(i) = (*this)(i) - right(i); + } + + return result; + } + inline Vector operator-(void) const { + Vector result(getRows()); + + for (size_t i = 0; i < getRows(); i++) { + result(i) = -((*this)(i)); + } + + return result; + } + // other functions + inline float dot(const Vector &right) const { + float result = 0; + + for (size_t i = 0; i < getRows(); i++) { + result += (*this)(i) * (*this)(i); + } + + return result; + } + inline float norm() const { + return sqrtf(dot(*this)); + } + inline float length() const { + return norm(); + } + inline Vector unit() const { + return (*this) / norm(); + } + inline Vector normalized() const { + return unit(); + } + inline void normalize() { + (*this) = (*this) / norm(); + } + inline static Vector zero(size_t rows) { + Vector result(rows); + // calloc returns zeroed memory + return result; + } + inline void setAll(const float &val) { + for (size_t i = 0; i < getRows(); i++) { + (*this)(i) = val; + } + } + inline void set(const float *data) { + memcpy(getData(), data, getSize()); + } + inline size_t getRows() const { return _rows; } +protected: + inline size_t getSize() const { return sizeof(float) * getRows(); } + inline float *getData() { return _data; } + inline const float *getData() const { return _data; } + inline void setData(float *data) { _data = data; } +private: + size_t _rows; + float *_data; +}; + +} // math diff --git a/src/modules/mathlib/math/nasa_rotation_def.pdf b/src/modules/mathlib/math/nasa_rotation_def.pdf Binary files differnew file mode 100644 index 000000000..eb67a4bfc --- /dev/null +++ b/src/modules/mathlib/math/nasa_rotation_def.pdf diff --git a/src/modules/mathlib/math/test/test.cpp b/src/modules/mathlib/math/test/test.cpp new file mode 100644 index 000000000..2fa2f7e7c --- /dev/null +++ b/src/modules/mathlib/math/test/test.cpp @@ -0,0 +1,94 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file test.cpp + * + * Test library code + */ + +#include <stdio.h> +#include <math.h> +#include <stdlib.h> + +#include "test.hpp" + +bool __EXPORT equal(float a, float b, float epsilon) +{ + float diff = fabsf(a - b); + + if (diff > epsilon) { + printf("not equal ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b)); + return false; + + } else return true; +} + +void __EXPORT float2SigExp( + const float &num, + float &sig, + int &exp) +{ + if (isnan(num) || isinf(num)) { + sig = 0.0f; + exp = -99; + return; + } + + if (fabsf(num) < 1.0e-38f) { + sig = 0; + exp = 0; + return; + } + + exp = log10f(fabsf(num)); + + if (exp > 0) { + exp = ceil(exp); + + } else { + exp = floor(exp); + } + + sig = num; + + // cheap power since it is integer + if (exp > 0) { + for (int i = 0; i < abs(exp); i++) sig /= 10; + + } else { + for (int i = 0; i < abs(exp); i++) sig *= 10; + } +} + + diff --git a/src/modules/mathlib/math/test/test.hpp b/src/modules/mathlib/math/test/test.hpp new file mode 100644 index 000000000..2027bb827 --- /dev/null +++ b/src/modules/mathlib/math/test/test.hpp @@ -0,0 +1,50 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file test.hpp + * + * Controller library code + */ + +#pragma once + +//#include <assert.h> +//#include <time.h> +//#include <stdlib.h> + +bool equal(float a, float b, float eps = 1e-5); +void float2SigExp( + const float &num, + float &sig, + int &exp); diff --git a/src/modules/mathlib/math/test_math.sce b/src/modules/mathlib/math/test_math.sce new file mode 100644 index 000000000..c3fba4729 --- /dev/null +++ b/src/modules/mathlib/math/test_math.sce @@ -0,0 +1,63 @@ +clc +clear +function out = float_truncate(in, digits) + out = round(in*10^digits) + out = out/10^digits +endfunction + +phi = 0.1 +theta = 0.2 +psi = 0.3 + +cosPhi = cos(phi) +cosPhi_2 = cos(phi/2) +sinPhi = sin(phi) +sinPhi_2 = sin(phi/2) + +cosTheta = cos(theta) +cosTheta_2 = cos(theta/2) +sinTheta = sin(theta) +sinTheta_2 = sin(theta/2) + +cosPsi = cos(psi) +cosPsi_2 = cos(psi/2) +sinPsi = sin(psi) +sinPsi_2 = sin(psi/2) + +C_nb = [cosTheta*cosPsi, -cosPhi*sinPsi + sinPhi*sinTheta*cosPsi, sinPhi*sinPsi + cosPhi*sinTheta*cosPsi; + cosTheta*sinPsi, cosPhi*cosPsi + sinPhi*sinTheta*sinPsi, -sinPhi*cosPsi + cosPhi*sinTheta*sinPsi; + -sinTheta, sinPhi*cosTheta, cosPhi*cosTheta] + +disp(C_nb) +//C_nb = float_truncate(C_nb,3) +//disp(C_nb) + +theta = asin(-C_nb(3,1)) +phi = atan(C_nb(3,2), C_nb(3,3)) +psi = atan(C_nb(2,1), C_nb(1,1)) +printf('phi %f\n', phi) +printf('theta %f\n', theta) +printf('psi %f\n', psi) + +q = [cosPhi_2*cosTheta_2*cosPsi_2 + sinPhi_2*sinTheta_2*sinPsi_2; + sinPhi_2*cosTheta_2*cosPsi_2 - cosPhi_2*sinTheta_2*sinPsi_2; + cosPhi_2*sinTheta_2*cosPsi_2 + sinPhi_2*cosTheta_2*sinPsi_2; + cosPhi_2*cosTheta_2*sinPsi_2 - sinPhi_2*sinTheta_2*cosPsi_2] + +//q = float_truncate(q,3) + +a = q(1) +b = q(2) +c = q(3) +d = q(4) +printf('q: %f %f %f %f\n', a, b, c, d) +a2 = a*a +b2 = b*b +c2 = c*c +d2 = d*d + +C2_nb = [a2 + b2 - c2 - d2, 2*(b*c - a*d), 2*(b*d + a*c); + 2*(b*c + a*d), a2 - b2 + c2 - d2, 2*(c*d - a*b); + 2*(b*d - a*c), 2*(c*d + a*b), a2 - b2 - c2 + d2] + +disp(C2_nb) diff --git a/src/modules/mathlib/mathlib.h b/src/modules/mathlib/mathlib.h new file mode 100644 index 000000000..40ffb22bc --- /dev/null +++ b/src/modules/mathlib/mathlib.h @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mathlib.h + * + * Common header for mathlib exports. + */ + +#ifdef __cplusplus + +#pragma once + +#include "math/Dcm.hpp" +#include "math/EulerAngles.hpp" +#include "math/Matrix.hpp" +#include "math/Quaternion.hpp" +#include "math/Vector.hpp" +#include "math/Vector3.hpp" +#include "math/Vector2f.hpp" +#include "math/Limits.hpp" + +#endif + +#ifdef CONFIG_ARCH_ARM + +#include "CMSIS/Include/arm_math.h" + +#endif
\ No newline at end of file diff --git a/src/modules/mathlib/module.mk b/src/modules/mathlib/module.mk new file mode 100644 index 000000000..2146a1413 --- /dev/null +++ b/src/modules/mathlib/module.mk @@ -0,0 +1,62 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Math library +# +SRCS = math/test/test.cpp \ + math/Vector.cpp \ + math/Vector2f.cpp \ + math/Vector3.cpp \ + math/EulerAngles.cpp \ + math/Quaternion.cpp \ + math/Dcm.cpp \ + math/Matrix.cpp \ + math/Limits.cpp + +# +# In order to include .config we first have to save off the +# current makefile name, since app.mk needs it. +# +APP_MAKEFILE := $(lastword $(MAKEFILE_LIST)) +-include $(TOPDIR)/.config + +ifeq ($(CONFIG_ARCH_CORTEXM4)$(CONFIG_ARCH_FPU),yy) +INCLUDE_DIRS += math/arm +SRCS += math/arm/Vector.cpp \ + math/arm/Matrix.cpp +else +#INCLUDE_DIRS += math/generic +#SRCS += math/generic/Vector.cpp \ +# math/generic/Matrix.cpp +endif diff --git a/src/modules/mavlink/.gitignore b/src/modules/mavlink/.gitignore new file mode 100644 index 000000000..a02827195 --- /dev/null +++ b/src/modules/mavlink/.gitignore @@ -0,0 +1,3 @@ +include +mavlink-* +pymavlink-* diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c new file mode 100644 index 000000000..7c1c4b175 --- /dev/null +++ b/src/modules/mavlink/mavlink.c @@ -0,0 +1,821 @@ +/**************************************************************************** + * + * 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 mavlink.c + * MAVLink 1.0 protocol implementation. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +#include <nuttx/config.h> +#include <unistd.h> +#include <pthread.h> +#include <stdio.h> +#include <math.h> +#include <stdbool.h> +#include <fcntl.h> +#include <mqueue.h> +#include <string.h> +#include "mavlink_bridge_header.h" +#include <drivers/drv_hrt.h> +#include <time.h> +#include <float.h> +#include <unistd.h> +#include <nuttx/sched.h> +#include <sys/prctl.h> +#include <termios.h> +#include <errno.h> +#include <stdlib.h> +#include <poll.h> + +#include <systemlib/param/param.h> +#include <systemlib/systemlib.h> +#include <systemlib/err.h> +#include <mavlink/mavlink_log.h> + +#include "waypoints.h" +#include "orb_topics.h" +#include "missionlib.h" +#include "mavlink_hil.h" +#include "util.h" +#include "waypoints.h" +#include "mavlink_parameters.h" + +/* define MAVLink specific parameters */ +PARAM_DEFINE_INT32(MAV_SYS_ID, 1); +PARAM_DEFINE_INT32(MAV_COMP_ID, 50); +PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING); + +__EXPORT int mavlink_main(int argc, char *argv[]); + +static int mavlink_thread_main(int argc, char *argv[]); + +/* thread state */ +volatile bool thread_should_exit = false; +static volatile bool thread_running = false; +static int mavlink_task; + +/* pthreads */ +static pthread_t receive_thread; +static pthread_t uorb_receive_thread; + +/* terminate MAVLink on user request - disabled by default */ +static bool mavlink_link_termination_allowed = false; + +mavlink_system_t mavlink_system = { + 100, + 50, + MAV_TYPE_FIXED_WING, + 0, + 0, + 0 +}; // System ID, 1-255, Component/Subsystem ID, 1-255 + +/* XXX not widely used */ +uint8_t chan = MAVLINK_COMM_0; + +/* XXX probably should be in a header... */ +extern pthread_t receive_start(int uart); + +/* Allocate storage space for waypoints */ +static mavlink_wpm_storage wpm_s; +mavlink_wpm_storage *wpm = &wpm_s; + +bool mavlink_hil_enabled = false; + +/* protocol interface */ +static int uart; +static int baudrate; +bool gcs_link = true; + +/* interface mode */ +static enum { + MAVLINK_INTERFACE_MODE_OFFBOARD, + MAVLINK_INTERFACE_MODE_ONBOARD +} mavlink_link_mode = MAVLINK_INTERFACE_MODE_OFFBOARD; + +static struct mavlink_logbuffer lb; + +static void mavlink_update_system(void); +static int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); +static void usage(void); +int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval); + + + +int +set_hil_on_off(bool hil_enabled) +{ + int ret = OK; + + /* Enable HIL */ + if (hil_enabled && !mavlink_hil_enabled) { + + mavlink_hil_enabled = true; + + /* ramp up some HIL-related subscriptions */ + unsigned hil_rate_interval; + + if (baudrate < 19200) { + /* 10 Hz */ + hil_rate_interval = 100; + + } else if (baudrate < 38400) { + /* 10 Hz */ + hil_rate_interval = 100; + + } else if (baudrate < 115200) { + /* 20 Hz */ + hil_rate_interval = 50; + + } else { + /* 200 Hz */ + hil_rate_interval = 5; + } + + orb_set_interval(mavlink_subs.spa_sub, hil_rate_interval); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval); + } + + if (!hil_enabled && mavlink_hil_enabled) { + mavlink_hil_enabled = false; + orb_set_interval(mavlink_subs.spa_sub, 200); + + } else { + ret = ERROR; + } + + return ret; +} + +void +get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) +{ + /* reset MAVLink mode bitfield */ + *mavlink_mode = 0; + + /* set mode flags independent of system state */ + + /* HIL */ + if (v_status.flag_hil_enabled) { + *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; + } + + /* manual input */ + if (v_status.flag_control_manual_enabled) { + *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + } + + /* attitude or rate control */ + if (v_status.flag_control_attitude_enabled || + v_status.flag_control_rates_enabled) { + *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; + } + + /* vector control */ + if (v_status.flag_control_velocity_enabled || + v_status.flag_control_position_enabled) { + *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; + } + + /* autonomous mode */ + if (v_status.state_machine == SYSTEM_STATE_AUTO) { + *mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED; + } + + /* set arming state */ + if (armed.armed) { + *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; + + } else { + *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; + } + + switch (v_status.state_machine) { + case SYSTEM_STATE_PREFLIGHT: + if (v_status.flag_preflight_gyro_calibration || + v_status.flag_preflight_mag_calibration || + v_status.flag_preflight_accel_calibration) { + *mavlink_state = MAV_STATE_CALIBRATING; + + } else { + *mavlink_state = MAV_STATE_UNINIT; + } + + break; + + case SYSTEM_STATE_STANDBY: + *mavlink_state = MAV_STATE_STANDBY; + break; + + case SYSTEM_STATE_GROUND_READY: + *mavlink_state = MAV_STATE_ACTIVE; + break; + + case SYSTEM_STATE_MANUAL: + *mavlink_state = MAV_STATE_ACTIVE; + break; + + case SYSTEM_STATE_STABILIZED: + *mavlink_state = MAV_STATE_ACTIVE; + break; + + case SYSTEM_STATE_AUTO: + *mavlink_state = MAV_STATE_ACTIVE; + break; + + case SYSTEM_STATE_MISSION_ABORT: + *mavlink_state = MAV_STATE_EMERGENCY; + break; + + case SYSTEM_STATE_EMCY_LANDING: + *mavlink_state = MAV_STATE_EMERGENCY; + break; + + case SYSTEM_STATE_EMCY_CUTOFF: + *mavlink_state = MAV_STATE_EMERGENCY; + break; + + case SYSTEM_STATE_GROUND_ERROR: + *mavlink_state = MAV_STATE_EMERGENCY; + break; + + case SYSTEM_STATE_REBOOT: + *mavlink_state = MAV_STATE_POWEROFF; + break; + } + +} + + +int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval) +{ + int ret = OK; + + switch (mavlink_msg_id) { + case MAVLINK_MSG_ID_SCALED_IMU: + /* sensor sub triggers scaled IMU */ + orb_set_interval(subs->sensor_sub, min_interval); + break; + + case MAVLINK_MSG_ID_HIGHRES_IMU: + /* sensor sub triggers highres IMU */ + orb_set_interval(subs->sensor_sub, min_interval); + break; + + case MAVLINK_MSG_ID_RAW_IMU: + /* sensor sub triggers RAW IMU */ + orb_set_interval(subs->sensor_sub, min_interval); + break; + + case MAVLINK_MSG_ID_ATTITUDE: + /* attitude sub triggers attitude */ + orb_set_interval(subs->att_sub, min_interval); + break; + + case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: + /* actuator_outputs triggers this message */ + orb_set_interval(subs->act_0_sub, min_interval); + orb_set_interval(subs->act_1_sub, min_interval); + orb_set_interval(subs->act_2_sub, min_interval); + orb_set_interval(subs->act_3_sub, min_interval); + orb_set_interval(subs->actuators_sub, min_interval); + break; + + case MAVLINK_MSG_ID_MANUAL_CONTROL: + /* manual_control_setpoint triggers this message */ + orb_set_interval(subs->man_control_sp_sub, min_interval); + break; + + case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT: + orb_set_interval(subs->debug_key_value, min_interval); + break; + + default: + /* not found */ + ret = ERROR; + break; + } + + return ret; +} + + +/**************************************************************************** + * MAVLink text message logger + ****************************************************************************/ + +static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); + +static const struct file_operations mavlink_fops = { + .ioctl = mavlink_dev_ioctl +}; + +static int +mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) +{ + static unsigned int total_counter = 0; + + switch (cmd) { + case (int)MAVLINK_IOC_SEND_TEXT_INFO: + case (int)MAVLINK_IOC_SEND_TEXT_CRITICAL: + case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: { + const char *txt = (const char *)arg; + struct mavlink_logmessage msg; + strncpy(msg.text, txt, sizeof(msg.text)); + mavlink_logbuffer_write(&lb, &msg); + total_counter++; + return OK; + } + + default: + return ENOTTY; + } +} + +#define MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED 0x10 + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb) +{ + /* process baud rate */ + int speed; + + switch (baud) { + case 0: speed = B0; break; + + case 50: speed = B50; break; + + case 75: speed = B75; break; + + case 110: speed = B110; break; + + case 134: speed = B134; break; + + case 150: speed = B150; break; + + case 200: speed = B200; break; + + case 300: speed = B300; break; + + case 600: speed = B600; break; + + case 1200: speed = B1200; break; + + case 1800: speed = B1800; break; + + case 2400: speed = B2400; break; + + case 4800: speed = B4800; break; + + case 9600: speed = B9600; break; + + case 19200: speed = B19200; break; + + case 38400: speed = B38400; break; + + case 57600: speed = B57600; break; + + case 115200: speed = B115200; break; + + case 230400: speed = B230400; break; + + case 460800: speed = B460800; break; + + case 921600: speed = B921600; break; + + default: + fprintf(stderr, "[mavlink] ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud); + return -EINVAL; + } + + /* open uart */ + printf("[mavlink] UART is %s, baudrate is %d\n", uart_name, baud); + uart = open(uart_name, O_RDWR | O_NOCTTY); + + /* Try to set baud rate */ + struct termios uart_config; + int termios_state; + *is_usb = false; + + /* make some wild guesses including that USB serial is indicated by either /dev/ttyACM0 or /dev/console */ + if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/console") != OK) { + /* Back up the original uart configuration to restore it after exit */ + if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { + fprintf(stderr, "[mavlink] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state); + close(uart); + return -1; + } + + /* Fill the struct for the new configuration */ + tcgetattr(uart, &uart_config); + + /* Clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; + + /* Set baud rate */ + if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { + fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); + close(uart); + return -1; + } + + + if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { + fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); + close(uart); + return -1; + } + + } else { + *is_usb = true; + } + + return uart; +} + +void +mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length) +{ + write(uart, ch, (size_t)(sizeof(uint8_t) * length)); +} + +/* + * Internal function to give access to the channel status for each channel + */ +extern mavlink_status_t *mavlink_get_channel_status(uint8_t channel) +{ + static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; + return &m_mavlink_status[channel]; +} + +/* + * Internal function to give access to the channel buffer for each channel + */ +extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel) +{ + static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS]; + return &m_mavlink_buffer[channel]; +} + +void mavlink_update_system(void) +{ + static bool initialized = false; + static param_t param_system_id; + static param_t param_component_id; + static param_t param_system_type; + + 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"); + initialized = true; + } + + /* update system and component id */ + int32_t system_id; + param_get(param_system_id, &system_id); + + if (system_id > 0 && system_id < 255) { + mavlink_system.sysid = system_id; + } + + int32_t component_id; + param_get(param_component_id, &component_id); + + if (component_id > 0 && component_id < 255) { + mavlink_system.compid = component_id; + } + + int32_t system_type; + param_get(param_system_type, &system_type); + + if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) { + mavlink_system.type = system_type; + } +} + +/** + * MAVLink Protocol main function. + */ +int mavlink_thread_main(int argc, char *argv[]) +{ + /* initialize mavlink text message buffering */ + mavlink_logbuffer_init(&lb, 5); + + int ch; + char *device_name = "/dev/ttyS1"; + baudrate = 57600; + + /* work around some stupidity in task_create's argv handling */ + argc -= 2; + argv += 2; + + while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) { + switch (ch) { + case 'b': + baudrate = strtoul(optarg, NULL, 10); + + if (baudrate == 0) + errx(1, "invalid baud rate '%s'", optarg); + + break; + + case 'd': + device_name = optarg; + break; + + case 'e': + mavlink_link_termination_allowed = true; + break; + + case 'o': + mavlink_link_mode = MAVLINK_INTERFACE_MODE_ONBOARD; + break; + + default: + usage(); + } + } + + struct termios uart_config_original; + + bool usb_uart; + + /* print welcome text */ + warnx("MAVLink v1.0 serial interface starting..."); + + /* inform about mode */ + warnx((mavlink_link_mode == MAVLINK_INTERFACE_MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE"); + + /* Flush stdout in case MAVLink is about to take it over */ + fflush(stdout); + + /* default values for arguments */ + uart = mavlink_open_uart(baudrate, device_name, &uart_config_original, &usb_uart); + + if (uart < 0) + err(1, "could not open %s", device_name); + + /* create the device node that's used for sending text log messages, etc. */ + register_driver(MAVLINK_LOG_DEVICE, &mavlink_fops, 0666, NULL); + + /* Initialize system properties */ + mavlink_update_system(); + + /* start the MAVLink receiver */ + receive_thread = receive_start(uart); + + /* start the ORB receiver */ + uorb_receive_thread = uorb_receive_start(); + + /* initialize waypoint manager */ + mavlink_wpm_init(wpm); + + /* all subscriptions are now active, set up initial guess about rate limits */ + if (baudrate >= 230400) { + /* 200 Hz / 5 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 20); + /* 50 Hz / 20 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 30); + /* 20 Hz / 50 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50); + /* 10 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 100); + /* 10 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100); + + } else if (baudrate >= 115200) { + /* 20 Hz / 50 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 50); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50); + /* 5 Hz / 200 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); + /* 5 Hz / 200 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 200); + /* 2 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); + + } else if (baudrate >= 57600) { + /* 10 Hz / 100 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 300); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 300); + /* 10 Hz / 100 ms ATTITUDE */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 200); + /* 5 Hz / 200 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200); + /* 5 Hz / 200 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500); + /* 2 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); + /* 2 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 500); + + } else { + /* very low baud rate, limit to 1 Hz / 1000 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 1000); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 1000); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 1000); + /* 1 Hz / 1000 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000); + /* 0.5 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000); + /* 0.1 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000); + } + + thread_running = true; + + /* arm counter to go off immediately */ + unsigned lowspeed_counter = 10; + + while (!thread_should_exit) { + + /* 1 Hz */ + if (lowspeed_counter == 10) { + mavlink_update_system(); + + /* translate the current system state to mavlink state and mode */ + uint8_t mavlink_state = 0; + uint8_t mavlink_mode = 0; + get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); + + /* send heartbeat */ + mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state); + + /* switch HIL mode if required */ + set_hil_on_off(v_status.flag_hil_enabled); + + /* send status (values already copied in the section above) */ + mavlink_msg_sys_status_send(chan, + v_status.onboard_control_sensors_present, + v_status.onboard_control_sensors_enabled, + v_status.onboard_control_sensors_health, + v_status.load, + v_status.voltage_battery * 1000.0f, + v_status.current_battery * 1000.0f, + v_status.battery_remaining, + v_status.drop_rate_comm, + v_status.errors_comm, + v_status.errors_count1, + v_status.errors_count2, + v_status.errors_count3, + v_status.errors_count4); + lowspeed_counter = 0; + } + + lowspeed_counter++; + + mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos); + + /* sleep quarter the time */ + usleep(25000); + + /* check if waypoint has been reached against the last positions */ + mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos); + + /* sleep quarter the time */ + usleep(25000); + + /* send parameters at 20 Hz (if queued for sending) */ + mavlink_pm_queued_send(); + mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos); + + /* sleep quarter the time */ + usleep(25000); + + mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos); + + if (baudrate > 57600) { + mavlink_pm_queued_send(); + } + + /* sleep 10 ms */ + usleep(10000); + + /* send one string at 10 Hz */ + if (!mavlink_logbuffer_is_empty(&lb)) { + struct mavlink_logmessage msg; + int lb_ret = mavlink_logbuffer_read(&lb, &msg); + + if (lb_ret == OK) { + mavlink_missionlib_send_gcs_string(msg.text); + } + } + + /* sleep 15 ms */ + usleep(15000); + } + + /* wait for threads to complete */ + pthread_join(receive_thread, NULL); + pthread_join(uorb_receive_thread, NULL); + + /* Reset the UART flags to original state */ + if (!usb_uart) + tcsetattr(uart, TCSANOW, &uart_config_original); + + thread_running = false; + + exit(0); +} + +static void +usage() +{ + fprintf(stderr, "usage: mavlink start [-d <devicename>] [-b <baud rate>]\n" + " mavlink stop\n" + " mavlink status\n"); + exit(1); +} + +int mavlink_main(int argc, char *argv[]) +{ + + if (argc < 2) { + warnx("missing command"); + usage(); + } + + if (!strcmp(argv[1], "start")) { + + /* this is not an error */ + if (thread_running) + errx(0, "mavlink already running\n"); + + thread_should_exit = false; + mavlink_task = task_spawn_cmd("mavlink", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + mavlink_thread_main, + (const char **)argv); + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + + while (thread_running) { + usleep(200000); + printf("."); + } + + warnx("terminated."); + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + errx(0, "running"); + + } else { + errx(1, "not running"); + } + } + + warnx("unrecognized command"); + usage(); + /* not getting here */ + return 0; +} + diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h new file mode 100644 index 000000000..149efda60 --- /dev/null +++ b/src/modules/mavlink/mavlink_bridge_header.h @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * 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 mavlink_bridge_header + * MAVLink bridge header for UART access. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +/* MAVLink adapter header */ +#ifndef MAVLINK_BRIDGE_HEADER_H +#define MAVLINK_BRIDGE_HEADER_H + +#define MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/* use efficient approach, see mavlink_helpers.h */ +#define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes + +#define MAVLINK_GET_CHANNEL_BUFFER mavlink_get_channel_buffer +#define MAVLINK_GET_CHANNEL_STATUS mavlink_get_channel_status + +#include <v1.0/mavlink_types.h> +#include <unistd.h> + + +/* Struct that stores the communication settings of this system. + you can also define / alter these settings elsewhere, as long + as they're included BEFORE mavlink.h. + So you can set the + + mavlink_system.sysid = 100; // System ID, 1-255 + mavlink_system.compid = 50; // Component/Subsystem ID, 1-255 + + Lines also in your main.c, e.g. by reading these parameter from EEPROM. + */ +extern mavlink_system_t mavlink_system; + +/** + * @brief Send multiple chars (uint8_t) over a comm channel + * + * @param chan MAVLink channel to use, usually MAVLINK_COMM_0 = UART0 + * @param ch Character to send + */ +extern void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length); + +extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan); +extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan); + +#include <v1.0/common/mavlink.h> + +#endif /* MAVLINK_BRIDGE_HEADER_H */ diff --git a/src/modules/mavlink/mavlink_hil.h b/src/modules/mavlink/mavlink_hil.h new file mode 100644 index 000000000..744ed7d94 --- /dev/null +++ b/src/modules/mavlink/mavlink_hil.h @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * 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 + * 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 mavlink_hil.h + * Hardware-in-the-loop simulation support. + */ + +#pragma once + +extern bool mavlink_hil_enabled; + +/** + * Enable / disable Hardware in the Loop simulation mode. + * + * @param hil_enabled The new HIL enable/disable state. + * @return OK if the HIL state changed, ERROR if the + * requested change could not be made or was + * redundant. + */ +extern int set_hil_on_off(bool hil_enabled); diff --git a/src/modules/mavlink/mavlink_log.c b/src/modules/mavlink/mavlink_log.c new file mode 100644 index 000000000..d9416a08b --- /dev/null +++ b/src/modules/mavlink/mavlink_log.c @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * 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 mavlink_log.c + * MAVLink text logging. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +#include <string.h> +#include <stdlib.h> +#include <stdarg.h> + +#include <mavlink/mavlink_log.h> + +void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size) +{ + lb->size = size; + lb->start = 0; + lb->count = 0; + lb->elems = (struct mavlink_logmessage *)calloc(lb->size, sizeof(struct mavlink_logmessage)); +} + +int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb) +{ + return lb->count == (int)lb->size; +} + +int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb) +{ + return lb->count == 0; +} + +void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem) +{ + int end = (lb->start + lb->count) % lb->size; + memcpy(&(lb->elems[end]), elem, sizeof(struct mavlink_logmessage)); + + if (mavlink_logbuffer_is_full(lb)) { + lb->start = (lb->start + 1) % lb->size; /* full, overwrite */ + + } else { + ++lb->count; + } +} + +int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem) +{ + if (!mavlink_logbuffer_is_empty(lb)) { + memcpy(elem, &(lb->elems[lb->start]), sizeof(struct mavlink_logmessage)); + lb->start = (lb->start + 1) % lb->size; + --lb->count; + return 0; + + } else { + return 1; + } +} + +void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...) +{ + va_list ap; + va_start(ap, fmt); + int end = (lb->start + lb->count) % lb->size; + lb->elems[end].severity = severity; + vsnprintf(lb->elems[end].text, sizeof(lb->elems[0].text), fmt, ap); + va_end(ap); + + /* increase count */ + if (mavlink_logbuffer_is_full(lb)) { + lb->start = (lb->start + 1) % lb->size; /* full, overwrite */ + + } else { + ++lb->count; + } +} + +__EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...) +{ + va_list ap; + va_start(ap, fmt); + char text[MAVLINK_LOG_MAXLEN + 1]; + vsnprintf(text, sizeof(text), fmt, ap); + va_end(ap); + ioctl(_fd, severity, (unsigned long)&text[0]); +} diff --git a/src/modules/mavlink/mavlink_parameters.c b/src/modules/mavlink/mavlink_parameters.c new file mode 100644 index 000000000..18ca7a854 --- /dev/null +++ b/src/modules/mavlink/mavlink_parameters.c @@ -0,0 +1,230 @@ +/**************************************************************************** + * + * 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 mavlink_parameters.c + * MAVLink parameter protocol implementation (BSD-relicensed). + * + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +#include "mavlink_bridge_header.h" +#include "mavlink_parameters.h" +#include <uORB/uORB.h> +#include "math.h" /* isinf / isnan checks */ +#include <assert.h> +#include <stdio.h> +#include <fcntl.h> +#include <unistd.h> +#include <stdbool.h> +#include <string.h> +#include <errno.h> +#include <systemlib/param/param.h> +#include <systemlib/err.h> +#include <sys/stat.h> + +extern mavlink_system_t mavlink_system; + +extern int mavlink_missionlib_send_message(mavlink_message_t *msg); +extern int mavlink_missionlib_send_gcs_string(const char *string); + +/** + * If the queue index is not at 0, the queue sending + * logic will send parameters from the current index + * to len - 1, the end of the param list. + */ +static unsigned int mavlink_param_queue_index = 0; + +/** + * Callback for param interface. + */ +void mavlink_pm_callback(void *arg, param_t param); + +void mavlink_pm_callback(void *arg, param_t param) +{ + mavlink_pm_send_param(param); + usleep(*(unsigned int *)arg); +} + +void mavlink_pm_send_all_params(unsigned int delay) +{ + unsigned int dbuf = delay; + param_foreach(&mavlink_pm_callback, &dbuf, false); +} + +int mavlink_pm_queued_send() +{ + if (mavlink_param_queue_index < param_count()) { + mavlink_pm_send_param(param_for_index(mavlink_param_queue_index)); + mavlink_param_queue_index++; + return 0; + + } else { + return 1; + } +} + +void mavlink_pm_start_queued_send() +{ + mavlink_param_queue_index = 0; +} + +int mavlink_pm_send_param_for_index(uint16_t index) +{ + return mavlink_pm_send_param(param_for_index(index)); +} + +int mavlink_pm_send_param_for_name(const char *name) +{ + return mavlink_pm_send_param(param_find(name)); +} + +int 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]; + float val_buf; + static mavlink_message_t tx_msg; + + /* query parameter type */ + param_type_t type = param_type(param); + /* copy parameter name */ + strncpy((char *)name_buf, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + + /* + * Map onboard parameter type to MAVLink type, + * endianess matches (both little endian) + */ + uint8_t mavlink_type; + + if (type == PARAM_TYPE_INT32) { + mavlink_type = MAVLINK_TYPE_INT32_T; + + } else if (type == PARAM_TYPE_FLOAT) { + mavlink_type = MAVLINK_TYPE_FLOAT; + + } else { + mavlink_type = MAVLINK_TYPE_FLOAT; + } + + /* + * get param value, since MAVLink encodes float and int params in the same + * space during transmission, copy param onto float val_buf + */ + + int ret; + + if ((ret = param_get(param, &val_buf)) != OK) { + return ret; + } + + mavlink_msg_param_value_pack_chan(mavlink_system.sysid, + mavlink_system.compid, + MAVLINK_COMM_0, + &tx_msg, + name_buf, + val_buf, + mavlink_type, + param_count(), + param_get_index(param)); + ret = mavlink_missionlib_send_message(&tx_msg); + return ret; +} + +void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg) +{ + switch (msg->msgid) { + case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { + /* Start sending parameters */ + mavlink_pm_start_queued_send(); + mavlink_missionlib_send_gcs_string("[mavlink pm] sending list"); + } break; + + case MAVLINK_MSG_ID_PARAM_SET: { + + /* Handle parameter setting */ + + if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) { + mavlink_param_set_t mavlink_param_set; + mavlink_msg_param_set_decode(msg, &mavlink_param_set); + + if (mavlink_param_set.target_system == mavlink_system.sysid && ((mavlink_param_set.target_component == mavlink_system.compid) || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) { + /* local name buffer to enforce null-terminated string */ + char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; + strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + /* enforce null termination */ + name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; + /* attempt to find parameter, set and send it */ + param_t param = param_find(name); + + if (param == PARAM_INVALID) { + char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; + sprintf(buf, "[mavlink pm] unknown: %s", name); + mavlink_missionlib_send_gcs_string(buf); + + } else { + /* set and send parameter */ + param_set(param, &(mavlink_param_set.param_value)); + mavlink_pm_send_param(param); + } + } + } + } break; + + case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { + mavlink_param_request_read_t mavlink_param_request_read; + mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read); + + if (mavlink_param_request_read.target_system == mavlink_system.sysid && ((mavlink_param_request_read.target_component == mavlink_system.compid) || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) { + /* when no index is given, loop through string ids and compare them */ + if (mavlink_param_request_read.param_index == -1) { + /* local name buffer to enforce null-terminated string */ + char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; + strncpy(name, mavlink_param_request_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + /* enforce null termination */ + name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; + /* attempt to find parameter and send it */ + mavlink_pm_send_param_for_name(name); + + } else { + /* when index is >= 0, send this parameter again */ + mavlink_pm_send_param_for_index(mavlink_param_request_read.param_index); + } + } + + } break; + } +} diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h new file mode 100644 index 000000000..b1e38bcc8 --- /dev/null +++ b/src/modules/mavlink/mavlink_parameters.h @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * 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 mavlink_parameters.h + * MAVLink parameter protocol definitions (BSD-relicensed). + * + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +/* This assumes you have the mavlink headers on your include path + or in the same folder as this source file */ + + +#include <v1.0/mavlink_types.h> +#include <stdbool.h> +#include <systemlib/param/param.h> + +/** + * Handle parameter related messages. + */ +void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); + +/** + * Send all parameters at once. + * + * This function blocks until all parameters have been sent. + * it delays each parameter by the passed amount of microseconds. + * + * @param delay The delay in us between sending all parameters. + */ +void mavlink_pm_send_all_params(unsigned int delay); + +/** + * Send one parameter. + * + * @param param The parameter id to send. + * @return zero on success, nonzero on failure. + */ +int mavlink_pm_send_param(param_t param); + +/** + * Send one parameter identified by index. + * + * @param index The index of the parameter to send. + * @return zero on success, nonzero else. + */ +int mavlink_pm_send_param_for_index(uint16_t index); + +/** + * Send one parameter identified by name. + * + * @param name The index of the parameter to send. + * @return zero on success, nonzero else. + */ +int mavlink_pm_send_param_for_name(const char *name); + +/** + * Send a queue of parameters, one parameter per function call. + * + * @return zero on success, nonzero on failure + */ +int mavlink_pm_queued_send(void); + +/** + * Start sending the parameter queue. + * + * This function will not directly send parameters, but instead + * activate the sending of one parameter on each call of + * mavlink_pm_queued_send(). + * @see mavlink_pm_queued_send() + */ +void mavlink_pm_start_queued_send(void); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp new file mode 100644 index 000000000..01bbabd46 --- /dev/null +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -0,0 +1,671 @@ +/**************************************************************************** + * + * 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 mavlink_receiver.c + * MAVLink protocol message receive and dispatch + * + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +/* XXX trim includes */ +#include <nuttx/config.h> +#include <unistd.h> +#include <pthread.h> +#include <stdio.h> +#include <math.h> +#include <stdbool.h> +#include <fcntl.h> +#include <mqueue.h> +#include <string.h> +#include <drivers/drv_hrt.h> +#include <time.h> +#include <float.h> +#include <unistd.h> +#include <nuttx/sched.h> +#include <sys/prctl.h> +#include <termios.h> +#include <errno.h> +#include <stdlib.h> +#include <poll.h> + +#include <mathlib/mathlib.h> + +#include <systemlib/param/param.h> +#include <systemlib/systemlib.h> +#include <systemlib/err.h> +#include <systemlib/airspeed.h> +#include <mavlink/mavlink_log.h> + +__BEGIN_DECLS + +#include "mavlink_bridge_header.h" +#include "waypoints.h" +#include "orb_topics.h" +#include "missionlib.h" +#include "mavlink_hil.h" +#include "mavlink_parameters.h" +#include "util.h" + +extern bool gcs_link; + +__END_DECLS + +/* XXX should be in a header somewhere */ +extern "C" pthread_t receive_start(int uart); + +static void handle_message(mavlink_message_t *msg); +static void *receive_thread(void *arg); + +static mavlink_status_t status; +static struct vehicle_vicon_position_s vicon_position; +static struct vehicle_command_s vcmd; +static struct offboard_control_setpoint_s offboard_control_sp; + +struct vehicle_global_position_s hil_global_pos; +struct vehicle_attitude_s hil_attitude; +struct vehicle_gps_position_s hil_gps; +struct sensor_combined_s hil_sensors; +static orb_advert_t pub_hil_global_pos = -1; +static orb_advert_t pub_hil_attitude = -1; +static orb_advert_t pub_hil_gps = -1; +static orb_advert_t pub_hil_sensors = -1; +static orb_advert_t pub_hil_airspeed = -1; + +static orb_advert_t cmd_pub = -1; +static orb_advert_t flow_pub = -1; + +static orb_advert_t offboard_control_sp_pub = -1; +static orb_advert_t vicon_position_pub = -1; +static orb_advert_t telemetry_status_pub = -1; + +static void +handle_message(mavlink_message_t *msg) +{ + if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) { + + mavlink_command_long_t cmd_mavlink; + mavlink_msg_command_long_decode(msg, &cmd_mavlink); + + if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) + || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { + //check for MAVLINK terminate command + if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { + /* This is the link shutdown command, terminate mavlink */ + printf("[mavlink] Terminating .. \n"); + fflush(stdout); + usleep(50000); + + /* terminate other threads and this thread */ + thread_should_exit = true; + + } else { + + /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ + vcmd.param1 = cmd_mavlink.param1; + vcmd.param2 = cmd_mavlink.param2; + vcmd.param3 = cmd_mavlink.param3; + vcmd.param4 = cmd_mavlink.param4; + vcmd.param5 = cmd_mavlink.param5; + vcmd.param6 = cmd_mavlink.param6; + vcmd.param7 = cmd_mavlink.param7; + vcmd.command = cmd_mavlink.command; + vcmd.target_system = cmd_mavlink.target_system; + vcmd.target_component = cmd_mavlink.target_component; + vcmd.source_system = msg->sysid; + vcmd.source_component = msg->compid; + vcmd.confirmation = cmd_mavlink.confirmation; + + /* check if topic is advertised */ + if (cmd_pub <= 0) { + cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + } else { + /* publish */ + orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); + } + } + } + } + + if (msg->msgid == MAVLINK_MSG_ID_OPTICAL_FLOW) { + mavlink_optical_flow_t flow; + mavlink_msg_optical_flow_decode(msg, &flow); + + struct optical_flow_s f; + + f.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; + f.flow_comp_y_m = flow.flow_comp_m_y; + f.ground_distance_m = flow.ground_distance; + f.quality = flow.quality; + f.sensor_id = flow.sensor_id; + + /* check if topic is advertised */ + if (flow_pub <= 0) { + flow_pub = orb_advertise(ORB_ID(optical_flow), &f); + + } else { + /* publish */ + orb_publish(ORB_ID(optical_flow), flow_pub, &f); + } + } + + if (msg->msgid == MAVLINK_MSG_ID_SET_MODE) { + /* Set mode on request */ + mavlink_set_mode_t new_mode; + mavlink_msg_set_mode_decode(msg, &new_mode); + + /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ + vcmd.param1 = new_mode.base_mode; + vcmd.param2 = new_mode.custom_mode; + vcmd.param3 = 0; + vcmd.param4 = 0; + vcmd.param5 = 0; + vcmd.param6 = 0; + vcmd.param7 = 0; + vcmd.command = MAV_CMD_DO_SET_MODE; + vcmd.target_system = new_mode.target_system; + vcmd.target_component = MAV_COMP_ID_ALL; + vcmd.source_system = msg->sysid; + vcmd.source_component = msg->compid; + vcmd.confirmation = 1; + + /* check if topic is advertised */ + if (cmd_pub <= 0) { + cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + + } else { + /* create command */ + orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); + } + } + + /* Handle Vicon position estimates */ + if (msg->msgid == MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE) { + mavlink_vicon_position_estimate_t pos; + mavlink_msg_vicon_position_estimate_decode(msg, &pos); + + vicon_position.timestamp = hrt_absolute_time(); + + vicon_position.x = pos.x; + vicon_position.y = pos.y; + vicon_position.z = pos.z; + + vicon_position.roll = pos.roll; + vicon_position.pitch = pos.pitch; + vicon_position.yaw = pos.yaw; + + if (vicon_position_pub <= 0) { + vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position); + + } else { + orb_publish(ORB_ID(vehicle_vicon_position), vicon_position_pub, &vicon_position); + } + } + + /* Handle quadrotor motor setpoints */ + + if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) { + mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint; + mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint); + + if (mavlink_system.sysid < 4) { + + /* switch to a receiving link mode */ + gcs_link = false; + + /* + * rate control mode - defined by MAVLink + */ + + uint8_t ml_mode = 0; + bool ml_armed = false; + + switch (quad_motors_setpoint.mode) { + case 0: + ml_armed = false; + break; + + case 1: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; + ml_armed = true; + + break; + + case 2: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; + ml_armed = true; + + break; + + case 3: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; + break; + + case 4: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; + break; + } + + offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX; + + if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) { + ml_armed = false; + } + + offboard_control_sp.armed = ml_armed; + offboard_control_sp.mode = static_cast<enum OFFBOARD_CONTROL_MODE>(ml_mode); + + offboard_control_sp.timestamp = hrt_absolute_time(); + + /* check if topic has to be advertised */ + if (offboard_control_sp_pub <= 0) { + offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); + + } else { + /* Publish */ + orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp); + } + } + } + + /* handle status updates of the radio */ + if (msg->msgid == MAVLINK_MSG_ID_RADIO_STATUS) { + + struct telemetry_status_s tstatus; + + mavlink_radio_status_t rstatus; + mavlink_msg_radio_status_decode(msg, &rstatus); + + /* publish telemetry status topic */ + tstatus.timestamp = hrt_absolute_time(); + tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO; + tstatus.rssi = rstatus.rssi; + tstatus.remote_rssi = rstatus.remrssi; + tstatus.txbuf = rstatus.txbuf; + tstatus.noise = rstatus.noise; + tstatus.remote_noise = rstatus.remnoise; + tstatus.rxerrors = rstatus.rxerrors; + tstatus.fixed = rstatus.fixed; + + if (telemetry_status_pub == 0) { + telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); + + } else { + orb_publish(ORB_ID(telemetry_status), telemetry_status_pub, &tstatus); + } + } + + /* + * Only decode hil messages in HIL mode. + * + * 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 + */ + + if (mavlink_hil_enabled) { + + uint64_t timestamp = hrt_absolute_time(); + + if (msg->msgid == MAVLINK_MSG_ID_HIL_SENSOR) { + + mavlink_hil_sensor_t imu; + mavlink_msg_hil_sensor_decode(msg, &imu); + + /* packet counter */ + static uint16_t hil_counter = 0; + static uint16_t hil_frames = 0; + static uint64_t old_timestamp = 0; + + /* sensors general */ + hil_sensors.timestamp = hrt_absolute_time(); + + /* hil gyro */ + static const float mrad2rad = 1.0e-3f; + hil_sensors.gyro_counter = hil_counter; + hil_sensors.gyro_raw[0] = imu.xgyro / mrad2rad; + hil_sensors.gyro_raw[1] = imu.ygyro / mrad2rad; + hil_sensors.gyro_raw[2] = imu.zgyro / mrad2rad; + hil_sensors.gyro_rad_s[0] = imu.xgyro; + hil_sensors.gyro_rad_s[1] = imu.ygyro; + hil_sensors.gyro_rad_s[2] = imu.zgyro; + + /* accelerometer */ + hil_sensors.accelerometer_counter = hil_counter; + static const float mg2ms2 = 9.8f / 1000.0f; + hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2; + hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2; + hil_sensors.accelerometer_raw[2] = imu.zacc / mg2ms2; + hil_sensors.accelerometer_m_s2[0] = imu.xacc; + hil_sensors.accelerometer_m_s2[1] = imu.yacc; + hil_sensors.accelerometer_m_s2[2] = imu.zacc; + hil_sensors.accelerometer_mode = 0; // TODO what is this? + hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16 + + /* adc */ + hil_sensors.adc_voltage_v[0] = 0; + hil_sensors.adc_voltage_v[1] = 0; + hil_sensors.adc_voltage_v[2] = 0; + + /* magnetometer */ + float mga2ga = 1.0e-3f; + hil_sensors.magnetometer_counter = hil_counter; + hil_sensors.magnetometer_raw[0] = imu.xmag / mga2ga; + hil_sensors.magnetometer_raw[1] = imu.ymag / mga2ga; + hil_sensors.magnetometer_raw[2] = imu.zmag / mga2ga; + hil_sensors.magnetometer_ga[0] = imu.xmag; + hil_sensors.magnetometer_ga[1] = imu.ymag; + hil_sensors.magnetometer_ga[2] = imu.zmag; + hil_sensors.magnetometer_range_ga = 32.7f; // int16 + hil_sensors.magnetometer_mode = 0; // TODO what is this + hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f; + + /* baro */ + hil_sensors.baro_pres_mbar = imu.abs_pressure; + hil_sensors.baro_alt_meter = imu.pressure_alt; + hil_sensors.baro_temp_celcius = imu.temperature; + + hil_sensors.gyro_counter = hil_counter; + hil_sensors.magnetometer_counter = hil_counter; + hil_sensors.accelerometer_counter = hil_counter; + + /* differential pressure */ + hil_sensors.differential_pressure_pa = imu.diff_pressure; + hil_sensors.differential_pressure_counter = hil_counter; + + /* airspeed from differential pressure, ambient pressure and temp */ + struct airspeed_s airspeed; + airspeed.timestamp = hrt_absolute_time(); + + float ias = calc_indicated_airspeed(imu.diff_pressure); + // XXX need to fix this + float tas = ias; + + airspeed.indicated_airspeed_m_s = ias; + airspeed.true_airspeed_m_s = tas; + + if (pub_hil_airspeed < 0) { + pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed); + } else { + orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed); + } + //warnx("SENSOR: IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s); + + /* publish */ + if (pub_hil_sensors > 0) { + orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors); + } else { + pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); + } + + // increment counters + hil_counter++; + hil_frames++; + + // output + if ((timestamp - old_timestamp) > 10000000) { + printf("receiving hil sensor at %d hz\n", hil_frames/10); + old_timestamp = timestamp; + hil_frames = 0; + } + } + + if (msg->msgid == MAVLINK_MSG_ID_HIL_GPS) { + + mavlink_hil_gps_t gps; + mavlink_msg_hil_gps_decode(msg, &gps); + + /* gps */ + hil_gps.timestamp_position = gps.time_usec; + hil_gps.time_gps_usec = gps.time_usec; + hil_gps.lat = gps.lat; + hil_gps.lon = gps.lon; + hil_gps.alt = gps.alt; + hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m + hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m + hil_gps.s_variance_m_s = 5.0f; + hil_gps.p_variance_m = hil_gps.eph_m * hil_gps.eph_m; + hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s + + /* gps.cog is in degrees 0..360 * 100, heading is -PI..+PI */ + float heading_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f; + /* go back to -PI..PI */ + if (heading_rad > M_PI_F) + heading_rad -= 2.0f * M_PI_F; + hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m + hil_gps.vel_e_m_s = gps.ve * 1e-2f; // from cm to m + hil_gps.vel_d_m_s = gps.vd * 1e-2f; // from cm to m + hil_gps.vel_ned_valid = true; + /* COG (course over ground) is spec'ed as -PI..+PI */ + hil_gps.cog_rad = heading_rad; + hil_gps.fix_type = gps.fix_type; + hil_gps.satellites_visible = gps.satellites_visible; + + /* publish GPS measurement data */ + if (pub_hil_gps > 0) { + orb_publish(ORB_ID(vehicle_gps_position), pub_hil_gps, &hil_gps); + } else { + pub_hil_gps = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); + } + + } + + if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE_QUATERNION) { + + mavlink_hil_state_quaternion_t hil_state; + mavlink_msg_hil_state_quaternion_decode(msg, &hil_state); + + struct airspeed_s airspeed; + airspeed.timestamp = hrt_absolute_time(); + airspeed.indicated_airspeed_m_s = hil_state.ind_airspeed * 1e-2f; + airspeed.true_airspeed_m_s = hil_state.true_airspeed * 1e-2f; + + if (pub_hil_airspeed < 0) { + pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed); + } else { + orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed); + } + + hil_global_pos.lat = hil_state.lat; + hil_global_pos.lon = hil_state.lon; + hil_global_pos.alt = hil_state.alt / 1000.0f; + hil_global_pos.vx = hil_state.vx / 100.0f; + hil_global_pos.vy = hil_state.vy / 100.0f; + hil_global_pos.vz = hil_state.vz / 100.0f; + + /* set timestamp and notify processes (broadcast) */ + hil_global_pos.timestamp = hrt_absolute_time(); + + if (pub_hil_global_pos > 0) { + orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos); + } else { + pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); + } + + /* Calculate Rotation Matrix */ + math::Quaternion q(hil_state.attitude_quaternion); + math::Dcm C_nb(q); + math::EulerAngles euler(C_nb); + + /* set rotation matrix */ + for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) + hil_attitude.R[i][j] = C_nb(i, j); + + hil_attitude.R_valid = true; + + /* set quaternion */ + hil_attitude.q[0] = q(0); + hil_attitude.q[1] = q(1); + hil_attitude.q[2] = q(2); + hil_attitude.q[3] = q(3); + hil_attitude.q_valid = true; + + hil_attitude.roll = euler.getPhi(); + hil_attitude.pitch = euler.getTheta(); + hil_attitude.yaw = euler.getPsi(); + hil_attitude.rollspeed = hil_state.rollspeed; + hil_attitude.pitchspeed = hil_state.pitchspeed; + hil_attitude.yawspeed = hil_state.yawspeed; + + /* set timestamp and notify processes (broadcast) */ + hil_attitude.timestamp = hrt_absolute_time(); + + if (pub_hil_attitude > 0) { + orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude); + } else { + pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); + } + } + + if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) { + mavlink_manual_control_t man; + mavlink_msg_manual_control_decode(msg, &man); + + struct rc_channels_s rc_hil; + memset(&rc_hil, 0, sizeof(rc_hil)); + static orb_advert_t rc_pub = 0; + + rc_hil.timestamp = hrt_absolute_time(); + rc_hil.chan_count = 4; + + rc_hil.chan[0].scaled = man.x / 1000.0f; + rc_hil.chan[1].scaled = man.y / 1000.0f; + rc_hil.chan[2].scaled = man.r / 1000.0f; + rc_hil.chan[3].scaled = man.z / 1000.0f; + + struct manual_control_setpoint_s mc; + static orb_advert_t mc_pub = 0; + + int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + + /* 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, &mc); + + mc.timestamp = rc_hil.timestamp; + mc.roll = man.x / 1000.0f; + mc.pitch = man.y / 1000.0f; + mc.yaw = man.r / 1000.0f; + mc.throttle = man.z / 1000.0f; + + /* fake RC channels with manual control input from simulator */ + + + if (rc_pub == 0) { + rc_pub = orb_advertise(ORB_ID(rc_channels), &rc_hil); + + } else { + orb_publish(ORB_ID(rc_channels), rc_pub, &rc_hil); + } + + if (mc_pub == 0) { + mc_pub = orb_advertise(ORB_ID(manual_control_setpoint), &mc); + + } else { + orb_publish(ORB_ID(manual_control_setpoint), mc_pub, &mc); + } + } + } +} + + +/** + * Receive data from UART. + */ +static void * +receive_thread(void *arg) +{ + int uart_fd = *((int *)arg); + + const int timeout = 1000; + uint8_t buf[32]; + + mavlink_message_t msg; + + prctl(PR_SET_NAME, "mavlink uart rcv", getpid()); + + while (!thread_should_exit) { + + struct pollfd fds[1]; + fds[0].fd = uart_fd; + fds[0].events = POLLIN; + + if (poll(fds, 1, timeout) > 0) { + /* non-blocking read. read may return negative values */ + ssize_t nread = read(uart_fd, buf, sizeof(buf)); + + /* if read failed, this loop won't execute */ + for (ssize_t i = 0; i < nread; i++) { + if (mavlink_parse_char(chan, buf[i], &msg, &status)) { + /* handle generic messages and commands */ + handle_message(&msg); + + /* Handle packet with waypoint component */ + mavlink_wpm_message_handler(&msg, &global_pos, &local_pos); + + /* Handle packet with parameter component */ + mavlink_pm_message_handler(MAVLINK_COMM_0, &msg); + } + } + } + } + + return NULL; +} + +pthread_t +receive_start(int uart) +{ + pthread_attr_t receiveloop_attr; + pthread_attr_init(&receiveloop_attr); + + // set to non-blocking read + int flags = fcntl(uart, F_GETFL, 0); + fcntl(uart, F_SETFL, flags | O_NONBLOCK); + + struct sched_param param; + param.sched_priority = SCHED_PRIORITY_MAX - 40; + (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); + + pthread_attr_setstacksize(&receiveloop_attr, 3000); + + pthread_t thread; + pthread_create(&thread, &receiveloop_attr, receive_thread, &uart); + return thread; +} diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c new file mode 100644 index 000000000..be88b8794 --- /dev/null +++ b/src/modules/mavlink/missionlib.c @@ -0,0 +1,372 @@ +/**************************************************************************** + * + * 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 + * 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 missionlib.h + * MAVLink missionlib components + */ + +// XXX trim includes +#include <nuttx/config.h> +#include <unistd.h> +#include <pthread.h> +#include <stdio.h> +#include <math.h> +#include <stdbool.h> +#include <fcntl.h> +#include <mqueue.h> +#include <string.h> +#include "mavlink_bridge_header.h" +#include <drivers/drv_hrt.h> +#include <time.h> +#include <float.h> +#include <unistd.h> +#include <nuttx/sched.h> +#include <sys/prctl.h> +#include <termios.h> +#include <errno.h> +#include <stdlib.h> +#include <poll.h> + +#include <systemlib/err.h> +#include <systemlib/param/param.h> +#include <systemlib/systemlib.h> +#include <mavlink/mavlink_log.h> + +#include "waypoints.h" +#include "orb_topics.h" +#include "missionlib.h" +#include "mavlink_hil.h" +#include "util.h" +#include "waypoints.h" +#include "mavlink_parameters.h" + +static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN]; +static uint64_t loiter_start_time; + +static bool set_special_fields(float param1, float param2, float param3, float param4, uint16_t command, + struct vehicle_global_position_setpoint_s *sp); + +int +mavlink_missionlib_send_message(mavlink_message_t *msg) +{ + uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg); + + mavlink_send_uart_bytes(chan, missionlib_msg_buf, len); + return 0; +} + +int +mavlink_missionlib_send_gcs_string(const char *string) +{ + const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN; + mavlink_statustext_t statustext; + int i = 0; + + while (i < len - 1) { + statustext.text[i] = string[i]; + + if (string[i] == '\0') + break; + + i++; + } + + if (i > 1) { + /* Enforce null termination */ + statustext.text[i] = '\0'; + mavlink_message_t msg; + + mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext); + return mavlink_missionlib_send_message(&msg); + + } else { + return 1; + } +} + +/** + * Get system time since boot in microseconds + * + * @return the system time since boot in microseconds + */ +uint64_t mavlink_missionlib_get_system_timestamp() +{ + return hrt_absolute_time(); +} + +/** + * Set special vehicle setpoint fields based on current mission item. + * + * @return true if the mission item could be interpreted + * successfully, it return false on failure. + */ +bool set_special_fields(float param1, float param2, float param3, float param4, uint16_t command, + struct vehicle_global_position_setpoint_s *sp) +{ + switch (command) { + case MAV_CMD_NAV_LOITER_UNLIM: + sp->nav_cmd = NAV_CMD_LOITER_UNLIMITED; + break; + case MAV_CMD_NAV_LOITER_TIME: + sp->nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + loiter_start_time = hrt_absolute_time(); + break; + // case MAV_CMD_NAV_LOITER_TURNS: + // sp->nav_cmd = NAV_CMD_LOITER_TURN_COUNT; + // break; + case MAV_CMD_NAV_WAYPOINT: + sp->nav_cmd = NAV_CMD_WAYPOINT; + break; + case MAV_CMD_NAV_RETURN_TO_LAUNCH: + sp->nav_cmd = NAV_CMD_RETURN_TO_LAUNCH; + break; + case MAV_CMD_NAV_LAND: + sp->nav_cmd = NAV_CMD_LAND; + break; + case MAV_CMD_NAV_TAKEOFF: + sp->nav_cmd = NAV_CMD_TAKEOFF; + break; + default: + /* abort */ + return false; + } + + sp->loiter_radius = param3; + sp->loiter_direction = (param3 >= 0) ? 1 : -1; + + sp->param1 = param1; + sp->param1 = param2; + sp->param1 = param3; + sp->param1 = param4; +} + +/** + * This callback is executed each time a waypoint changes. + * + * It publishes the vehicle_global_position_setpoint_s or the + * vehicle_local_position_setpoint_s topic, depending on the type of waypoint + */ +void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, + float param2, float param3, float param4, float param5_lat_x, + float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command) +{ + static orb_advert_t global_position_setpoint_pub = -1; + static orb_advert_t global_position_set_triplet_pub = -1; + static orb_advert_t local_position_setpoint_pub = -1; + static unsigned last_waypoint_index = -1; + char buf[50] = {0}; + + // XXX include check if WP is supported, jump to next if not + + /* Update controller setpoints */ + if (frame == (int)MAV_FRAME_GLOBAL) { + /* global, absolute waypoint */ + struct vehicle_global_position_setpoint_s sp; + sp.lat = param5_lat_x * 1e7f; + sp.lon = param6_lon_y * 1e7f; + sp.altitude = param7_alt_z; + sp.altitude_is_relative = false; + sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; + set_special_fields(param1, param2, param3, param4, command, &sp); + + /* Initialize setpoint publication if necessary */ + if (global_position_setpoint_pub < 0) { + global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp); + + } else { + orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp); + } + + + /* fill triplet: previous, current, next waypoint */ + struct vehicle_global_position_set_triplet_s triplet; + + /* current waypoint is same as sp */ + memcpy(&(triplet.current), &sp, sizeof(sp)); + + /* + * Check if previous WP (in mission, not in execution order) + * is available and identify correct index + */ + int last_setpoint_index = -1; + bool last_setpoint_valid = false; + + /* at first waypoint, but cycled once through mission */ + if (index == 0 && last_waypoint_index > 0) { + last_setpoint_index = last_waypoint_index; + } else { + last_setpoint_index = index - 1; + } + + while (last_setpoint_index >= 0) { + + if (wpm->waypoints[last_setpoint_index].frame == (int)MAV_FRAME_GLOBAL && + (wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT || + wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS || + wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TIME || + wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_UNLIM)) { + last_setpoint_valid = true; + break; + } + + last_setpoint_index--; + } + + /* + * Check if next WP (in mission, not in execution order) + * is available and identify correct index + */ + int next_setpoint_index = -1; + bool next_setpoint_valid = false; + + /* at last waypoint, try to re-loop through mission as default */ + if (index == (wpm->size - 1) && wpm->size > 1) { + next_setpoint_index = 0; + } else if (wpm->size > 1) { + next_setpoint_index = index + 1; + } + + while (next_setpoint_index < wpm->size - 1) { + + if (wpm->waypoints[next_setpoint_index].frame == (int)MAV_FRAME_GLOBAL && (wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT || + wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS || + wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TIME || + wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_UNLIM)) { + next_setpoint_valid = true; + break; + } + + next_setpoint_index++; + } + + /* populate last and next */ + + triplet.previous_valid = false; + triplet.next_valid = false; + + if (last_setpoint_valid) { + triplet.previous_valid = true; + struct vehicle_global_position_setpoint_s sp; + sp.lat = wpm->waypoints[last_setpoint_index].x * 1e7f; + sp.lon = wpm->waypoints[last_setpoint_index].y * 1e7f; + sp.altitude = wpm->waypoints[last_setpoint_index].z; + sp.altitude_is_relative = false; + sp.yaw = (wpm->waypoints[last_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F; + set_special_fields(wpm->waypoints[last_setpoint_index].param1, + wpm->waypoints[last_setpoint_index].param2, + wpm->waypoints[last_setpoint_index].param3, + wpm->waypoints[last_setpoint_index].param4, + wpm->waypoints[last_setpoint_index].command, &sp); + memcpy(&(triplet.previous), &sp, sizeof(sp)); + } + + if (next_setpoint_valid) { + triplet.next_valid = true; + struct vehicle_global_position_setpoint_s sp; + sp.lat = wpm->waypoints[next_setpoint_index].x * 1e7f; + sp.lon = wpm->waypoints[next_setpoint_index].y * 1e7f; + sp.altitude = wpm->waypoints[next_setpoint_index].z; + sp.altitude_is_relative = false; + sp.yaw = (wpm->waypoints[next_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F; + set_special_fields(wpm->waypoints[next_setpoint_index].param1, + wpm->waypoints[next_setpoint_index].param2, + wpm->waypoints[next_setpoint_index].param3, + wpm->waypoints[next_setpoint_index].param4, + wpm->waypoints[next_setpoint_index].command, &sp); + memcpy(&(triplet.next), &sp, sizeof(sp)); + } + + /* Initialize triplet publication if necessary */ + if (global_position_set_triplet_pub < 0) { + global_position_set_triplet_pub = orb_advertise(ORB_ID(vehicle_global_position_set_triplet), &triplet); + + } else { + orb_publish(ORB_ID(vehicle_global_position_set_triplet), global_position_set_triplet_pub, &triplet); + } + + sprintf(buf, "[mp] WP#%i lat: % 3.6f/lon % 3.6f/alt % 4.6f/hdg %3.4f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); + + } else if (frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { + /* global, relative alt (in relation to HOME) waypoint */ + struct vehicle_global_position_setpoint_s sp; + sp.lat = param5_lat_x * 1e7f; + sp.lon = param6_lon_y * 1e7f; + sp.altitude = param7_alt_z; + sp.altitude_is_relative = true; + sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; + set_special_fields(param1, param2, param3, param4, command, &sp); + + /* Initialize publication if necessary */ + if (global_position_setpoint_pub < 0) { + global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp); + + } else { + orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp); + } + + + + sprintf(buf, "[mp] WP#%i (lat: %f/lon %f/rel alt %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); + + } else if (frame == (int)MAV_FRAME_LOCAL_ENU || frame == (int)MAV_FRAME_LOCAL_NED) { + /* local, absolute waypoint */ + struct vehicle_local_position_setpoint_s sp; + sp.x = param5_lat_x; + sp.y = param6_lon_y; + sp.z = param7_alt_z; + sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; + + /* Initialize publication if necessary */ + if (local_position_setpoint_pub < 0) { + local_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &sp); + + } else { + orb_publish(ORB_ID(vehicle_local_position_setpoint), local_position_setpoint_pub, &sp); + } + + sprintf(buf, "[mp] WP#%i (x: %f/y %f/z %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); + } else { + warnx("non-navigation WP, ignoring"); + mavlink_missionlib_send_gcs_string("[mp] Unknown waypoint type, ignoring."); + return; + } + + /* only set this for known waypoint types (non-navigation types would have returned earlier) */ + last_waypoint_index = index; + + mavlink_missionlib_send_gcs_string(buf); + printf("%s\n", buf); + //printf("[mavlink mp] new setpoint\n");//: frame: %d, lat: %d, lon: %d, alt: %d, yaw: %d\n", frame, param5_lat_x*1000, param6_lon_y*1000, param7_alt_z*1000, param4*1000); +} diff --git a/src/modules/mavlink/missionlib.h b/src/modules/mavlink/missionlib.h new file mode 100644 index 000000000..c7d8f90c5 --- /dev/null +++ b/src/modules/mavlink/missionlib.h @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * 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 + * 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 missionlib.h + * MAVLink mission helper library + */ + +#pragma once + +#include "mavlink_bridge_header.h" + +//extern void mavlink_wpm_send_message(mavlink_message_t *msg); +//extern void mavlink_wpm_send_gcs_string(const char *string); +//extern uint64_t mavlink_wpm_get_system_timestamp(void); +extern int mavlink_missionlib_send_message(mavlink_message_t *msg); +extern int mavlink_missionlib_send_gcs_string(const char *string); +extern uint64_t mavlink_missionlib_get_system_timestamp(void); +extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, + float param2, float param3, float param4, float param5_lat_x, + float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command); diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk new file mode 100644 index 000000000..bfccb2d38 --- /dev/null +++ b/src/modules/mavlink/module.mk @@ -0,0 +1,47 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# MAVLink protocol to uORB interface process +# + +MODULE_COMMAND = mavlink +SRCS += mavlink.c \ + missionlib.c \ + mavlink_parameters.c \ + mavlink_log.c \ + mavlink_receiver.cpp \ + orb_listener.c \ + waypoints.c + +INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c new file mode 100644 index 000000000..edb8761b8 --- /dev/null +++ b/src/modules/mavlink/orb_listener.c @@ -0,0 +1,796 @@ +/**************************************************************************** + * + * 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 orb_listener.c + * Monitors ORB topics and sends update messages as appropriate. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +// XXX trim includes +#include <nuttx/config.h> +#include <stdio.h> +#include <math.h> +#include <stdbool.h> +#include <fcntl.h> +#include <string.h> +#include "mavlink_bridge_header.h" +#include <drivers/drv_hrt.h> +#include <time.h> +#include <float.h> +#include <unistd.h> +#include <sys/prctl.h> +#include <stdlib.h> +#include <poll.h> + +#include <mavlink/mavlink_log.h> + +#include "waypoints.h" +#include "orb_topics.h" +#include "missionlib.h" +#include "mavlink_hil.h" +#include "util.h" + +extern bool gcs_link; + +struct vehicle_global_position_s global_pos; +struct vehicle_local_position_s local_pos; +struct vehicle_status_s v_status; +struct rc_channels_s rc; +struct rc_input_values rc_raw; +struct actuator_armed_s armed; +struct actuator_controls_effective_s actuators_0; +struct vehicle_attitude_s att; + +struct mavlink_subscriptions mavlink_subs; + +static int status_sub; +static int rc_sub; + +static unsigned int sensors_raw_counter; +static unsigned int attitude_counter; +static unsigned int gps_counter; + +/* + * Last sensor loop time + * some outputs are better timestamped + * with this "global" reference. + */ +static uint64_t last_sensor_timestamp; + +static void *uorb_receive_thread(void *arg); + +struct listener { + void (*callback)(const struct listener *l); + int *subp; + uintptr_t arg; +}; + +static void l_sensor_combined(const struct listener *l); +static void l_vehicle_attitude(const struct listener *l); +static void l_vehicle_gps_position(const struct listener *l); +static void l_vehicle_status(const struct listener *l); +static void l_rc_channels(const struct listener *l); +static void l_input_rc(const struct listener *l); +static void l_global_position(const struct listener *l); +static void l_local_position(const struct listener *l); +static void l_global_position_setpoint(const struct listener *l); +static void l_local_position_setpoint(const struct listener *l); +static void l_attitude_setpoint(const struct listener *l); +static void l_actuator_outputs(const struct listener *l); +static void l_actuator_armed(const struct listener *l); +static void l_manual_control_setpoint(const struct listener *l); +static void l_vehicle_attitude_controls(const struct listener *l); +static void l_debug_key_value(const struct listener *l); +static void l_optical_flow(const struct listener *l); +static void l_vehicle_rates_setpoint(const struct listener *l); +static void l_home(const struct listener *l); +static void l_airspeed(const struct listener *l); + +static const struct listener listeners[] = { + {l_sensor_combined, &mavlink_subs.sensor_sub, 0}, + {l_vehicle_attitude, &mavlink_subs.att_sub, 0}, + {l_vehicle_gps_position, &mavlink_subs.gps_sub, 0}, + {l_vehicle_status, &status_sub, 0}, + {l_rc_channels, &rc_sub, 0}, + {l_input_rc, &mavlink_subs.input_rc_sub, 0}, + {l_global_position, &mavlink_subs.global_pos_sub, 0}, + {l_local_position, &mavlink_subs.local_pos_sub, 0}, + {l_global_position_setpoint, &mavlink_subs.spg_sub, 0}, + {l_local_position_setpoint, &mavlink_subs.spl_sub, 0}, + {l_attitude_setpoint, &mavlink_subs.spa_sub, 0}, + {l_actuator_outputs, &mavlink_subs.act_0_sub, 0}, + {l_actuator_outputs, &mavlink_subs.act_1_sub, 1}, + {l_actuator_outputs, &mavlink_subs.act_2_sub, 2}, + {l_actuator_outputs, &mavlink_subs.act_3_sub, 3}, + {l_actuator_armed, &mavlink_subs.armed_sub, 0}, + {l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0}, + {l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0}, + {l_debug_key_value, &mavlink_subs.debug_key_value, 0}, + {l_optical_flow, &mavlink_subs.optical_flow, 0}, + {l_vehicle_rates_setpoint, &mavlink_subs.rates_setpoint_sub, 0}, + {l_home, &mavlink_subs.home_sub, 0}, + {l_airspeed, &mavlink_subs.airspeed_sub, 0}, +}; + +static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]); + +void +l_sensor_combined(const struct listener *l) +{ + struct sensor_combined_s raw; + + /* copy sensors raw data into local buffer */ + orb_copy(ORB_ID(sensor_combined), mavlink_subs.sensor_sub, &raw); + + last_sensor_timestamp = raw.timestamp; + + /* mark individual fields as changed */ + uint16_t fields_updated = 0; + static unsigned accel_counter = 0; + static unsigned gyro_counter = 0; + static unsigned mag_counter = 0; + static unsigned baro_counter = 0; + + if (accel_counter != raw.accelerometer_counter) { + /* mark first three dimensions as changed */ + fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); + accel_counter = raw.accelerometer_counter; + } + + if (gyro_counter != raw.gyro_counter) { + /* mark second group dimensions as changed */ + fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); + gyro_counter = raw.gyro_counter; + } + + if (mag_counter != raw.magnetometer_counter) { + /* mark third group dimensions as changed */ + fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); + mag_counter = raw.magnetometer_counter; + } + + if (baro_counter != raw.baro_counter) { + /* mark last group dimensions as changed */ + fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); + baro_counter = raw.baro_counter; + } + + if (gcs_link) + mavlink_msg_highres_imu_send(MAVLINK_COMM_0, last_sensor_timestamp, + raw.accelerometer_m_s2[0], raw.accelerometer_m_s2[1], + raw.accelerometer_m_s2[2], raw.gyro_rad_s[0], + raw.gyro_rad_s[1], raw.gyro_rad_s[2], + raw.magnetometer_ga[0], + raw.magnetometer_ga[1], raw.magnetometer_ga[2], + raw.baro_pres_mbar, raw.differential_pressure_pa, + raw.baro_alt_meter, raw.baro_temp_celcius, + fields_updated); + + sensors_raw_counter++; +} + +void +l_vehicle_attitude(const struct listener *l) +{ + /* copy attitude data into local buffer */ + orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att); + + if (gcs_link) + /* send sensor values */ + mavlink_msg_attitude_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + att.roll, + att.pitch, + att.yaw, + att.rollspeed, + att.pitchspeed, + att.yawspeed); + + attitude_counter++; +} + +void +l_vehicle_gps_position(const struct listener *l) +{ + struct vehicle_gps_position_s gps; + + /* copy gps data into local buffer */ + orb_copy(ORB_ID(vehicle_gps_position), mavlink_subs.gps_sub, &gps); + + /* GPS COG is 0..2PI in degrees * 1e2 */ + float cog_deg = gps.cog_rad; + if (cog_deg > M_PI_F) + cog_deg -= 2.0f * M_PI_F; + cog_deg *= M_RAD_TO_DEG_F; + + + /* GPS position */ + mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, + gps.timestamp_position, + gps.fix_type, + gps.lat, + gps.lon, + gps.alt, + gps.eph_m * 1e2f, // from m to cm + gps.epv_m * 1e2f, // from m to cm + gps.vel_m_s * 1e2f, // from m/s to cm/s + cog_deg * 1e2f, // from rad to deg * 100 + gps.satellites_visible); + + /* update SAT info every 10 seconds */ + if (gps.satellite_info_available && (gps_counter % 50 == 0)) { + mavlink_msg_gps_status_send(MAVLINK_COMM_0, + gps.satellites_visible, + gps.satellite_prn, + gps.satellite_used, + gps.satellite_elevation, + gps.satellite_azimuth, + gps.satellite_snr); + } + + gps_counter++; +} + +void +l_vehicle_status(const struct listener *l) +{ + /* immediately communicate state changes back to user */ + orb_copy(ORB_ID(vehicle_status), status_sub, &v_status); + orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); + + /* enable or disable HIL */ + set_hil_on_off(v_status.flag_hil_enabled); + + /* translate the current syste state to mavlink state and mode */ + uint8_t mavlink_state = 0; + uint8_t mavlink_mode = 0; + get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); + + /* send heartbeat */ + mavlink_msg_heartbeat_send(chan, + mavlink_system.type, + MAV_AUTOPILOT_PX4, + mavlink_mode, + v_status.state_machine, + mavlink_state); +} + +void +l_rc_channels(const struct listener *l) +{ + /* copy rc channels into local buffer */ + orb_copy(ORB_ID(rc_channels), rc_sub, &rc); + // XXX Add RC channels scaled message here +} + +void +l_input_rc(const struct listener *l) +{ + /* copy rc channels into local buffer */ + orb_copy(ORB_ID(input_rc), mavlink_subs.input_rc_sub, &rc_raw); + + if (gcs_link) + /* Channels are sent in MAVLink main loop at a fixed interval */ + mavlink_msg_rc_channels_raw_send(chan, + rc_raw.timestamp / 1000, + 0, + (rc_raw.channel_count > 0) ? rc_raw.values[0] : UINT16_MAX, + (rc_raw.channel_count > 1) ? rc_raw.values[1] : UINT16_MAX, + (rc_raw.channel_count > 2) ? rc_raw.values[2] : UINT16_MAX, + (rc_raw.channel_count > 3) ? rc_raw.values[3] : UINT16_MAX, + (rc_raw.channel_count > 4) ? rc_raw.values[4] : UINT16_MAX, + (rc_raw.channel_count > 5) ? rc_raw.values[5] : UINT16_MAX, + (rc_raw.channel_count > 6) ? rc_raw.values[6] : UINT16_MAX, + (rc_raw.channel_count > 7) ? rc_raw.values[7] : UINT16_MAX, + 255); +} + +void +l_global_position(const struct listener *l) +{ + /* copy global position data into local buffer */ + orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos); + + uint64_t timestamp = global_pos.timestamp; + int32_t lat = global_pos.lat; + int32_t lon = global_pos.lon; + int32_t alt = (int32_t)(global_pos.alt * 1000); + int32_t relative_alt = (int32_t)(global_pos.relative_alt * 1000.0f); + int16_t vx = (int16_t)(global_pos.vx * 100.0f); + int16_t vy = (int16_t)(global_pos.vy * 100.0f); + int16_t vz = (int16_t)(global_pos.vz * 100.0f); + + /* heading in degrees * 10, from 0 to 36.000) */ + uint16_t hdg = (global_pos.hdg / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f); + + mavlink_msg_global_position_int_send(MAVLINK_COMM_0, + timestamp / 1000, + lat, + lon, + alt, + relative_alt, + vx, + vy, + vz, + hdg); +} + +void +l_local_position(const struct listener *l) +{ + /* copy local position data into local buffer */ + orb_copy(ORB_ID(vehicle_local_position), mavlink_subs.local_pos_sub, &local_pos); + + if (gcs_link) + mavlink_msg_local_position_ned_send(MAVLINK_COMM_0, + local_pos.timestamp / 1000, + local_pos.x, + local_pos.y, + local_pos.z, + local_pos.vx, + local_pos.vy, + local_pos.vz); +} + +void +l_global_position_setpoint(const struct listener *l) +{ + struct vehicle_global_position_setpoint_s global_sp; + + /* copy local position data into local buffer */ + orb_copy(ORB_ID(vehicle_global_position_setpoint), mavlink_subs.spg_sub, &global_sp); + + uint8_t coordinate_frame = MAV_FRAME_GLOBAL; + + if (global_sp.altitude_is_relative) + coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; + + if (gcs_link) + mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0, + coordinate_frame, + global_sp.lat, + global_sp.lon, + global_sp.altitude, + global_sp.yaw); +} + +void +l_local_position_setpoint(const struct listener *l) +{ + struct vehicle_local_position_setpoint_s local_sp; + + /* copy local position data into local buffer */ + orb_copy(ORB_ID(vehicle_local_position_setpoint), mavlink_subs.spl_sub, &local_sp); + + if (gcs_link) + mavlink_msg_local_position_setpoint_send(MAVLINK_COMM_0, + MAV_FRAME_LOCAL_NED, + local_sp.x, + local_sp.y, + local_sp.z, + local_sp.yaw); +} + +void +l_attitude_setpoint(const struct listener *l) +{ + struct vehicle_attitude_setpoint_s att_sp; + + /* copy local position data into local buffer */ + orb_copy(ORB_ID(vehicle_attitude_setpoint), mavlink_subs.spa_sub, &att_sp); + + if (gcs_link) + mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, + att_sp.timestamp / 1000, + att_sp.roll_body, + att_sp.pitch_body, + att_sp.yaw_body, + att_sp.thrust); +} + +void +l_vehicle_rates_setpoint(const struct listener *l) +{ + struct vehicle_rates_setpoint_s rates_sp; + + /* copy local position data into local buffer */ + orb_copy(ORB_ID(vehicle_rates_setpoint), mavlink_subs.rates_setpoint_sub, &rates_sp); + + if (gcs_link) + mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(MAVLINK_COMM_0, + rates_sp.timestamp / 1000, + rates_sp.roll, + rates_sp.pitch, + rates_sp.yaw, + rates_sp.thrust); +} + +void +l_actuator_outputs(const struct listener *l) +{ + struct actuator_outputs_s act_outputs; + + orb_id_t ids[] = { + ORB_ID(actuator_outputs_0), + ORB_ID(actuator_outputs_1), + ORB_ID(actuator_outputs_2), + ORB_ID(actuator_outputs_3) + }; + + /* copy actuator data into local buffer */ + orb_copy(ids[l->arg], *l->subp, &act_outputs); + + if (gcs_link) { + mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, + l->arg /* port number */, + act_outputs.output[0], + act_outputs.output[1], + act_outputs.output[2], + act_outputs.output[3], + act_outputs.output[4], + act_outputs.output[5], + act_outputs.output[6], + act_outputs.output[7]); + + /* only send in HIL mode */ + if (mavlink_hil_enabled && armed.armed) { + + /* translate the current syste state to mavlink state and mode */ + uint8_t mavlink_state = 0; + uint8_t mavlink_mode = 0; + get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); + + /* HIL message as per MAVLink spec */ + + /* scale / assign outputs depending on system type */ + + if (mavlink_system.type == MAV_TYPE_QUADROTOR) { + mavlink_msg_hil_controls_send(chan, + hrt_absolute_time(), + ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, + -1, + -1, + -1, + -1, + mavlink_mode, + 0); + + } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) { + mavlink_msg_hil_controls_send(chan, + hrt_absolute_time(), + ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, + -1, + -1, + mavlink_mode, + 0); + + } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) { + mavlink_msg_hil_controls_send(chan, + hrt_absolute_time(), + ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f, + mavlink_mode, + 0); + + } else { + mavlink_msg_hil_controls_send(chan, + hrt_absolute_time(), + (act_outputs.output[0] - 1500.0f) / 500.0f, + (act_outputs.output[1] - 1500.0f) / 500.0f, + (act_outputs.output[2] - 1500.0f) / 500.0f, + (act_outputs.output[3] - 1000.0f) / 1000.0f, + (act_outputs.output[4] - 1500.0f) / 500.0f, + (act_outputs.output[5] - 1500.0f) / 500.0f, + (act_outputs.output[6] - 1500.0f) / 500.0f, + (act_outputs.output[7] - 1500.0f) / 500.0f, + mavlink_mode, + 0); + } + } + } +} + +void +l_actuator_armed(const struct listener *l) +{ + orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); +} + +void +l_manual_control_setpoint(const struct listener *l) +{ + struct manual_control_setpoint_s man_control; + + /* copy manual control data into local buffer */ + orb_copy(ORB_ID(manual_control_setpoint), mavlink_subs.man_control_sp_sub, &man_control); + + if (gcs_link) + mavlink_msg_manual_control_send(MAVLINK_COMM_0, + mavlink_system.sysid, + man_control.roll * 1000, + man_control.pitch * 1000, + man_control.yaw * 1000, + man_control.throttle * 1000, + 0); +} + +void +l_vehicle_attitude_controls(const struct listener *l) +{ + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_sub, &actuators_0); + + if (gcs_link) { + /* send, add spaces so that string buffer is at least 10 chars long */ + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "eff ctrl0 ", + actuators_0.control_effective[0]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "eff ctrl1 ", + actuators_0.control_effective[1]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "eff ctrl2 ", + actuators_0.control_effective[2]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "eff ctrl3 ", + actuators_0.control_effective[3]); + } +} + +void +l_debug_key_value(const struct listener *l) +{ + struct debug_key_value_s debug; + + orb_copy(ORB_ID(debug_key_value), mavlink_subs.debug_key_value, &debug); + + /* Enforce null termination */ + debug.key[sizeof(debug.key) - 1] = '\0'; + + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + debug.key, + debug.value); +} + +void +l_optical_flow(const struct listener *l) +{ + struct optical_flow_s flow; + + orb_copy(ORB_ID(optical_flow), mavlink_subs.optical_flow, &flow); + + mavlink_msg_optical_flow_send(MAVLINK_COMM_0, flow.timestamp, flow.sensor_id, flow.flow_raw_x, flow.flow_raw_y, + flow.flow_comp_x_m, flow.flow_comp_y_m, flow.quality, flow.ground_distance_m); +} + +void +l_home(const struct listener *l) +{ + struct home_position_s home; + + orb_copy(ORB_ID(home_position), mavlink_subs.home_sub, &home); + + mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, home.lat, home.lon, home.alt); +} + +void +l_airspeed(const struct listener *l) +{ + struct airspeed_s airspeed; + + orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed); + + float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); + float throttle = actuators_0.control_effective[3] * (UINT16_MAX - 1); + float alt = global_pos.alt; + float climb = global_pos.vz; + + mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, + ((att.yaw + M_PI_F) / M_PI_F) * 180.0f, throttle, alt, climb); +} + +static void * +uorb_receive_thread(void *arg) +{ + /* Set thread name */ + prctl(PR_SET_NAME, "mavlink orb rcv", getpid()); + + /* + * set up poll to block for new data, + * wait for a maximum of 1000 ms (1 second) + */ + const int timeout = 1000; + + /* + * Initialise listener array. + * + * Might want to invoke each listener once to set initial state. + */ + struct pollfd fds[n_listeners]; + + for (unsigned i = 0; i < n_listeners; i++) { + fds[i].fd = *listeners[i].subp; + fds[i].events = POLLIN; + + /* Invoke callback to set initial state */ + //listeners[i].callback(&listener[i]); + } + + while (!thread_should_exit) { + + int poll_ret = poll(fds, n_listeners, timeout); + + /* handle the poll result */ + if (poll_ret == 0) { + mavlink_missionlib_send_gcs_string("[mavlink] No telemetry data for 1 s"); + + } else if (poll_ret < 0) { + mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data"); + + } else { + + for (unsigned i = 0; i < n_listeners; i++) { + if (fds[i].revents & POLLIN) + listeners[i].callback(&listeners[i]); + } + } + } + + return NULL; +} + +pthread_t +uorb_receive_start(void) +{ + /* --- SENSORS RAW VALUE --- */ + mavlink_subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + /* rate limit set externally based on interface speed, set a basic default here */ + orb_set_interval(mavlink_subs.sensor_sub, 200); /* 5Hz updates */ + + /* --- ATTITUDE VALUE --- */ + mavlink_subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + /* rate limit set externally based on interface speed, set a basic default here */ + orb_set_interval(mavlink_subs.att_sub, 200); /* 5Hz updates */ + + /* --- GPS VALUE --- */ + mavlink_subs.gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + orb_set_interval(mavlink_subs.gps_sub, 200); /* 5Hz updates */ + + /* --- HOME POSITION --- */ + mavlink_subs.home_sub = orb_subscribe(ORB_ID(home_position)); + orb_set_interval(mavlink_subs.home_sub, 1000); /* 1Hz updates */ + + /* --- SYSTEM STATE --- */ + status_sub = orb_subscribe(ORB_ID(vehicle_status)); + orb_set_interval(status_sub, 300); /* max 3.33 Hz updates */ + + /* --- RC CHANNELS VALUE --- */ + rc_sub = orb_subscribe(ORB_ID(rc_channels)); + orb_set_interval(rc_sub, 100); /* 10Hz updates */ + + /* --- RC RAW VALUE --- */ + mavlink_subs.input_rc_sub = orb_subscribe(ORB_ID(input_rc)); + orb_set_interval(mavlink_subs.input_rc_sub, 100); + + /* --- GLOBAL POS VALUE --- */ + mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + orb_set_interval(mavlink_subs.global_pos_sub, 100); /* 10 Hz active updates */ + + /* --- LOCAL POS VALUE --- */ + mavlink_subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + orb_set_interval(mavlink_subs.local_pos_sub, 1000); /* 1Hz active updates */ + + /* --- GLOBAL SETPOINT VALUE --- */ + mavlink_subs.spg_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); + orb_set_interval(mavlink_subs.spg_sub, 2000); /* 0.5 Hz updates */ + + /* --- LOCAL SETPOINT VALUE --- */ + mavlink_subs.spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); + orb_set_interval(mavlink_subs.spl_sub, 2000); /* 0.5 Hz updates */ + + /* --- ATTITUDE SETPOINT VALUE --- */ + mavlink_subs.spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + orb_set_interval(mavlink_subs.spa_sub, 2000); /* 0.5 Hz updates */ + + /* --- RATES SETPOINT VALUE --- */ + mavlink_subs.rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + orb_set_interval(mavlink_subs.rates_setpoint_sub, 2000); /* 0.5 Hz updates */ + + /* --- ACTUATOR OUTPUTS --- */ + mavlink_subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0)); + mavlink_subs.act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1)); + mavlink_subs.act_2_sub = orb_subscribe(ORB_ID(actuator_outputs_2)); + mavlink_subs.act_3_sub = orb_subscribe(ORB_ID(actuator_outputs_3)); + /* rate limits set externally based on interface speed, set a basic default here */ + orb_set_interval(mavlink_subs.act_0_sub, 100); /* 10Hz updates */ + orb_set_interval(mavlink_subs.act_1_sub, 100); /* 10Hz updates */ + orb_set_interval(mavlink_subs.act_2_sub, 100); /* 10Hz updates */ + orb_set_interval(mavlink_subs.act_3_sub, 100); /* 10Hz updates */ + + /* --- ACTUATOR ARMED VALUE --- */ + mavlink_subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(mavlink_subs.armed_sub, 100); /* 10Hz updates */ + + /* --- MAPPED MANUAL CONTROL INPUTS --- */ + mavlink_subs.man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + /* rate limits set externally based on interface speed, set a basic default here */ + orb_set_interval(mavlink_subs.man_control_sp_sub, 100); /* 10Hz updates */ + + /* --- ACTUATOR CONTROL VALUE --- */ + mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); + orb_set_interval(mavlink_subs.actuators_sub, 100); /* 10Hz updates */ + + /* --- DEBUG VALUE OUTPUT --- */ + mavlink_subs.debug_key_value = orb_subscribe(ORB_ID(debug_key_value)); + orb_set_interval(mavlink_subs.debug_key_value, 100); /* 10Hz updates */ + + /* --- FLOW SENSOR --- */ + mavlink_subs.optical_flow = orb_subscribe(ORB_ID(optical_flow)); + orb_set_interval(mavlink_subs.optical_flow, 200); /* 5Hz updates */ + + /* --- AIRSPEED / VFR / HUD --- */ + mavlink_subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + orb_set_interval(mavlink_subs.airspeed_sub, 200); /* 5Hz updates */ + + /* start the listener loop */ + pthread_attr_t uorb_attr; + pthread_attr_init(&uorb_attr); + + /* Set stack size, needs less than 2k */ + pthread_attr_setstacksize(&uorb_attr, 2048); + + pthread_t thread; + pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL); + return thread; +} diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h new file mode 100644 index 000000000..73e278dc6 --- /dev/null +++ b/src/modules/mavlink/orb_topics.h @@ -0,0 +1,110 @@ +/**************************************************************************** + * + * Copyright (C) 2008-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 + * 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 orb_topics.h + * Common sets of topics subscribed to or published by the MAVLink driver, + * and structures maintained by those subscriptions. + */ +#pragma once + +#include <uORB/uORB.h> +#include <uORB/topics/sensor_combined.h> +#include <uORB/topics/rc_channels.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_gps_position.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/home_position.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/offboard_control_setpoint.h> +#include <uORB/topics/vehicle_command.h> +#include <uORB/topics/vehicle_local_position_setpoint.h> +#include <uORB/topics/vehicle_vicon_position.h> +#include <uORB/topics/vehicle_global_position_setpoint.h> +#include <uORB/topics/vehicle_global_position_set_triplet.h> +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/optical_flow.h> +#include <uORB/topics/actuator_outputs.h> +#include <uORB/topics/actuator_controls_effective.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/telemetry_status.h> +#include <uORB/topics/debug_key_value.h> +#include <uORB/topics/airspeed.h> +#include <drivers/drv_rc_input.h> + +struct mavlink_subscriptions { + int sensor_sub; + int att_sub; + int global_pos_sub; + int act_0_sub; + int act_1_sub; + int act_2_sub; + int act_3_sub; + int gps_sub; + int man_control_sp_sub; + int armed_sub; + int actuators_sub; + int local_pos_sub; + int spa_sub; + int spl_sub; + int spg_sub; + int debug_key_value; + int input_rc_sub; + int optical_flow; + int rates_setpoint_sub; + int home_sub; + int airspeed_sub; +}; + +extern struct mavlink_subscriptions mavlink_subs; + +/** Global position */ +extern struct vehicle_global_position_s global_pos; + +/** Local position */ +extern struct vehicle_local_position_s local_pos; + +/** Vehicle status */ +extern struct vehicle_status_s v_status; + +/** RC channels */ +extern struct rc_channels_s rc; + +/** Actuator armed state */ +extern struct actuator_armed_s armed; + +/** Worker thread starter */ +extern pthread_t uorb_receive_start(void); diff --git a/src/modules/mavlink/util.h b/src/modules/mavlink/util.h new file mode 100644 index 000000000..a4ff06a88 --- /dev/null +++ b/src/modules/mavlink/util.h @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * 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 + * 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 util.h + * Utility and helper functions and data. + */ + +#pragma once + +/** MAVLink communications channel */ +extern uint8_t chan; + +/** Shutdown marker */ +extern volatile bool thread_should_exit; + +/** Waypoint storage */ +extern mavlink_wpm_storage *wpm; + +/** + * Translate the custom state into standard mavlink modes and state. + */ +extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode); diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c new file mode 100644 index 000000000..405046750 --- /dev/null +++ b/src/modules/mavlink/waypoints.c @@ -0,0 +1,1161 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @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 + * 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 waypoints.c + * MAVLink waypoint protocol implementation (BSD-relicensed). + */ + +#include <math.h> +#include <sys/prctl.h> +#include <unistd.h> +#include <stdio.h> + +#include "mavlink_bridge_header.h" +#include "missionlib.h" +#include "waypoints.h" +#include "util.h" + +#ifndef FM_PI +#define FM_PI 3.1415926535897932384626433832795f +#endif + +bool debug = false; +bool verbose = false; + + +#define MAVLINK_WPM_NO_PRINTF + +uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; + +void mavlink_wpm_init(mavlink_wpm_storage *state) +{ + // Set all waypoints to zero + + // Set count to zero + state->size = 0; + state->max_size = MAVLINK_WPM_MAX_WP_COUNT; + state->current_state = MAVLINK_WPM_STATE_IDLE; + state->current_partner_sysid = 0; + state->current_partner_compid = 0; + state->timestamp_lastaction = 0; + state->timestamp_last_send_setpoint = 0; + state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; + state->delay_setpoint = MAVLINK_WPM_SETPOINT_DELAY_DEFAULT; + state->idle = false; ///< indicates if the system is following the waypoints or is waiting + state->current_active_wp_id = -1; ///< id of current waypoint + state->yaw_reached = false; ///< boolean for yaw attitude reached + state->pos_reached = false; ///< boolean for position reached + state->timestamp_lastoutside_orbit = 0;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value + state->timestamp_firstinside_orbit = 0;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value + +} + +/* + * @brief Sends an waypoint ack message + */ +void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) +{ + mavlink_message_t msg; + mavlink_mission_ack_t wpa; + + wpa.target_system = wpm->current_partner_sysid; + wpa.target_component = wpm->current_partner_compid; + wpa.type = type; + + mavlink_msg_mission_ack_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpa); + mavlink_missionlib_send_message(&msg); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + + if (MAVLINK_WPM_TEXT_FEEDBACK) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("Sent waypoint ACK"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system); + +#endif + mavlink_missionlib_send_gcs_string("Sent waypoint ACK"); + } +} + +/* + * @brief Broadcasts the new target waypoint and directs the MAV to fly there + * + * This function broadcasts its new active waypoint sequence number and + * sends a message to the controller, advising it to fly to the coordinates + * of the waypoint with a given orientation + * + * @param seq The waypoint sequence number the MAV should fly to. + */ +void mavlink_wpm_send_waypoint_current(uint16_t seq) +{ + if (seq < wpm->size) { + mavlink_mission_item_t *cur = &(wpm->waypoints[seq]); + + mavlink_message_t msg; + mavlink_mission_current_t wpc; + + wpc.seq = cur->seq; + + mavlink_msg_mission_current_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc); + mavlink_missionlib_send_message(&msg); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Set current waypoint\n"); //// printf("Broadcasted new current waypoint %u\n", wpc.seq); + + } else { + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds\n"); + } +} + +/* + * @brief Directs the MAV to fly to a position + * + * Sends a message to the controller, advising it to fly to the coordinates + * of the waypoint with a given orientation + * + * @param seq The waypoint sequence number the MAV should fly to. + */ +void mavlink_wpm_send_setpoint(uint16_t seq) +{ + if (seq < wpm->size) { + mavlink_mission_item_t *cur = &(wpm->waypoints[seq]); + mavlink_missionlib_current_waypoint_changed(cur->seq, cur->param1, + cur->param2, cur->param3, cur->param4, cur->x, + cur->y, cur->z, cur->frame, cur->command); + + wpm->timestamp_last_send_setpoint = mavlink_missionlib_get_system_timestamp(); + + } else { + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index out of bounds\n"); //// if (verbose) // printf("ERROR: index out of bounds\n"); + } +} + +void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count) +{ + mavlink_message_t msg; + mavlink_mission_count_t wpc; + + wpc.target_system = wpm->current_partner_sysid; + wpc.target_component = wpm->current_partner_compid; + wpc.count = count; + + mavlink_msg_mission_count_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc); + mavlink_missionlib_send_message(&msg); + + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint count"); //// if (verbose) // printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); +} + +void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) +{ + if (seq < wpm->size) { + mavlink_message_t msg; + mavlink_mission_item_t *wp = &(wpm->waypoints[seq]); + wp->target_system = wpm->current_partner_sysid; + wp->target_component = wpm->current_partner_compid; + mavlink_msg_mission_item_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, wp); + mavlink_missionlib_send_message(&msg); + + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint"); //// if (verbose) // printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + + } else { + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index out of bounds\n"); + } +} + +void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq) +{ + if (seq < wpm->max_size) { + mavlink_message_t msg; + mavlink_mission_request_t wpr; + wpr.target_system = wpm->current_partner_sysid; + wpr.target_component = wpm->current_partner_compid; + wpr.seq = seq; + mavlink_msg_mission_request_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpr); + mavlink_missionlib_send_message(&msg); + + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint request"); //// if (verbose) // printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + + } else { + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity\n"); + } +} + +/* + * @brief emits a message that a waypoint reached + * + * This function broadcasts a message that a waypoint is reached. + * + * @param seq The waypoint sequence number the MAV has reached. + */ +void mavlink_wpm_send_waypoint_reached(uint16_t seq) +{ + mavlink_message_t msg; + mavlink_mission_item_reached_t wp_reached; + + wp_reached.seq = seq; + + mavlink_msg_mission_item_reached_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wp_reached); + mavlink_missionlib_send_message(&msg); + + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint reached message"); //// if (verbose) // printf("Sent waypoint %u reached message\n", wp_reached.seq); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); +} + +//float mavlink_wpm_distance_to_segment(uint16_t seq, float x, float y, float z) +//{ +// if (seq < wpm->size) +// { +// mavlink_mission_item_t *cur = waypoints->at(seq); +// +// const PxVector3 A(cur->x, cur->y, cur->z); +// const PxVector3 C(x, y, z); +// +// // seq not the second last waypoint +// if ((uint16_t)(seq+1) < wpm->size) +// { +// mavlink_mission_item_t *next = waypoints->at(seq+1); +// const PxVector3 B(next->x, next->y, next->z); +// const float r = (B-A).dot(C-A) / (B-A).lengthSquared(); +// if (r >= 0 && r <= 1) +// { +// const PxVector3 P(A + r*(B-A)); +// return (P-C).length(); +// } +// else if (r < 0.f) +// { +// return (C-A).length(); +// } +// else +// { +// return (C-B).length(); +// } +// } +// else +// { +// return (C-A).length(); +// } +// } +// else +// { +// // if (verbose) // printf("ERROR: index out of bounds\n"); +// } +// return -1.f; +//} + +/* + * Calculate distance in global frame. + * + * The distance calculation is based on the WGS84 geoid (GPS) + */ +float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt) +{ + //TODO: implement for z once altidude contoller is implemented + + static uint16_t counter; + +// if(counter % 10 == 0) printf(" x = %.10f, y = %.10f\n", x, y); + if (seq < wpm->size) { + mavlink_mission_item_t *cur = &(wpm->waypoints[seq]); + + double current_x_rad = cur->x / 180.0 * M_PI; + double current_y_rad = cur->y / 180.0 * M_PI; + double x_rad = lat / 180.0 * M_PI; + double y_rad = lon / 180.0 * M_PI; + + double d_lat = x_rad - current_x_rad; + double d_lon = y_rad - current_y_rad; + + double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0f) * cos(current_x_rad) * cos(x_rad); + double c = 2 * atan2(sqrt(a), sqrt(1 - a)); + + const double radius_earth = 6371000.0; + + return radius_earth * c; + + } else { + return -1.0f; + } + + counter++; + +} + +/* + * Calculate distance in local frame (NED) + */ +float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float z) +{ + if (seq < wpm->size) { + mavlink_mission_item_t *cur = &(wpm->waypoints[seq]); + + float dx = (cur->x - x); + float dy = (cur->y - y); + float dz = (cur->z - z); + + return sqrt(dx * dx + dy * dy + dz * dz); + + } else { + return -1.0f; + } +} + +void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos) +{ + static uint16_t counter; + + if (wpm->current_active_wp_id < wpm->size) { + + float orbit; + if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT) { + + orbit = wpm->waypoints[wpm->current_active_wp_id].param2; + + } else if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS || + wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME || + wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM) { + + orbit = wpm->waypoints[wpm->current_active_wp_id].param3; + } else { + + // XXX set default orbit via param + orbit = 15.0f; + } + + int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame; + float dist = -1.0f; + + if (coordinate_frame == (int)MAV_FRAME_GLOBAL) { + dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt); + + } else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { + dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, global_pos->lat, global_pos->lon, global_pos->relative_alt); + + } else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) { + dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z); + + } else if (coordinate_frame == (int)MAV_FRAME_MISSION) { + /* Check if conditions of mission item are satisfied */ + // XXX TODO + } + + if (dist >= 0.f && dist <= orbit /*&& wpm->yaw_reached*/) { //TODO implement yaw + + wpm->pos_reached = true; + + } + +// else +// { +// if(counter % 100 == 0) +// printf("Setpoint not reached yet: %0.4f, orbit: %.4f, coordinate frame: %d\n",dist, orbit, coordinate_frame); +// } + } + + //check if the current waypoint was reached + if (wpm->pos_reached /*wpm->yaw_reached &&*/ && !wpm->idle) { + if (wpm->current_active_wp_id < wpm->size) { + mavlink_mission_item_t *cur_wp = &(wpm->waypoints[wpm->current_active_wp_id]); + + if (wpm->timestamp_firstinside_orbit == 0) { + // Announce that last waypoint was reached + mavlink_wpm_send_waypoint_reached(cur_wp->seq); + wpm->timestamp_firstinside_orbit = now; + } + + // check if the MAV was long enough inside the waypoint orbit + //if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000)) + + bool time_elapsed = false; + + if (cur_wp->command == (int)MAV_CMD_NAV_LOITER_TIME) { + if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) { + time_elapsed = true; + } + } else if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) { + time_elapsed = true; + } else if (cur_wp->command == (int)MAV_CMD_NAV_TAKEOFF) { + time_elapsed = true; + } + + if (time_elapsed) { + if (cur_wp->autocontinue) { + cur_wp->current = 0; + + /* only accept supported navigation waypoints, skip unknown ones */ + do { + + if (wpm->current_active_wp_id == wpm->size - 1 && wpm->size > 1) { + /* the last waypoint was reached, if auto continue is + * activated restart the waypoint list from the beginning + */ + wpm->current_active_wp_id = 0; + + } else { + if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size) + wpm->current_active_wp_id++; + } + + } while (!(wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT || + wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS || + wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME || + wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM)); + + // Fly to next waypoint + wpm->timestamp_firstinside_orbit = 0; + mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id); + mavlink_wpm_send_setpoint(wpm->current_active_wp_id); + wpm->waypoints[wpm->current_active_wp_id].current = true; + wpm->pos_reached = false; + wpm->yaw_reached = false; + printf("Set new waypoint (%u)\n", wpm->current_active_wp_id); + } + } + } + + } else { + wpm->timestamp_lastoutside_orbit = now; + } + + counter++; +} + + +int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, struct vehicle_local_position_s *local_position) +{ + /* check for timed-out operations */ + if (now - wpm->timestamp_lastaction > wpm->timeout && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { + +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("Operation timeout switching -> IDLE"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm->current_state); + +#endif + wpm->current_state = MAVLINK_WPM_STATE_IDLE; + wpm->current_count = 0; + wpm->current_partner_sysid = 0; + wpm->current_partner_compid = 0; + wpm->current_wp_id = -1; + + if (wpm->size == 0) { + wpm->current_active_wp_id = -1; + } + } + + // Do NOT continously send the current WP, since we're not loosing messages + // if (now - wpm->timestamp_last_send_setpoint > wpm->delay_setpoint && wpm->current_active_wp_id < wpm->size) { + // mavlink_wpm_send_setpoint(wpm->current_active_wp_id); + // } + + check_waypoints_reached(now, global_position , local_position); + + return OK; +} + + +void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehicle_global_position_s *global_pos , struct vehicle_local_position_s *local_pos) +{ + uint64_t now = mavlink_missionlib_get_system_timestamp(); + + switch (msg->msgid) { +// case MAVLINK_MSG_ID_ATTITUDE: +// { +// if(msg->sysid == mavlink_system.sysid && wpm->current_active_wp_id < wpm->size) +// { +// mavlink_mission_item_t *wp = &(wpm->waypoints[wpm->current_active_wp_id]); +// if(wp->frame == MAV_FRAME_LOCAL_ENU || wp->frame == MAV_FRAME_LOCAL_NED) +// { +// mavlink_attitude_t att; +// mavlink_msg_attitude_decode(msg, &att); +// float yaw_tolerance = wpm->accept_range_yaw; +// //compare current yaw +// if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*FM_PI) +// { +// if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4) +// wpm->yaw_reached = true; +// } +// else if(att.yaw - yaw_tolerance < 0.0f) +// { +// float lowerBound = 360.0f + att.yaw - yaw_tolerance; +// if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance) +// wpm->yaw_reached = true; +// } +// else +// { +// float upperBound = att.yaw + yaw_tolerance - 2.f*FM_PI; +// if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound) +// wpm->yaw_reached = true; +// } +// } +// } +// break; +// } +// +// case MAVLINK_MSG_ID_LOCAL_POSITION_NED: +// { +// if(msg->sysid == mavlink_system.sysid && wpm->current_active_wp_id < wpm->size) +// { +// mavlink_mission_item_t *wp = &(wpm->waypoints[wpm->current_active_wp_id]); +// +// if(wp->frame == MAV_FRAME_LOCAL_ENU || MAV_FRAME_LOCAL_NED) +// { +// mavlink_local_position_ned_t pos; +// mavlink_msg_local_position_ned_decode(msg, &pos); +// //// if (debug) // printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z); +// +// wpm->pos_reached = false; +// +// // compare current position (given in message) with current waypoint +// float orbit = wp->param1; +// +// float dist; +//// if (wp->param2 == 0) +//// { +//// // FIXME segment distance +//// //dist = mavlink_wpm_distance_to_segment(current_active_wp_id, pos.x, pos.y, pos.z); +//// } +//// else +//// { +// dist = mavlink_wpm_distance_to_point(wpm->current_active_wp_id, pos.x, pos.y, pos.z); +//// } +// +// if (dist >= 0.f && dist <= orbit && wpm->yaw_reached) +// { +// wpm->pos_reached = true; +// } +// } +// } +// break; +// } + +// case MAVLINK_MSG_ID_CMD: // special action from ground station +// { +// mavlink_cmd_t action; +// mavlink_msg_cmd_decode(msg, &action); +// if(action.target == mavlink_system.sysid) +// { +// // if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl; +// switch (action.action) +// { +// // case MAV_ACTION_LAUNCH: +// // // if (verbose) std::cerr << "Launch received" << std::endl; +// // current_active_wp_id = 0; +// // if (wpm->size>0) +// // { +// // setActive(waypoints[current_active_wp_id]); +// // } +// // else +// // // if (verbose) std::cerr << "No launch, waypointList empty" << std::endl; +// // break; +// +// // case MAV_ACTION_CONTINUE: +// // // if (verbose) std::c +// // err << "Continue received" << std::endl; +// // idle = false; +// // setActive(waypoints[current_active_wp_id]); +// // break; +// +// // case MAV_ACTION_HALT: +// // // if (verbose) std::cerr << "Halt received" << std::endl; +// // idle = true; +// // break; +// +// // default: +// // // if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl; +// // break; +// } +// } +// break; +// } + + case MAVLINK_MSG_ID_MISSION_ACK: { + mavlink_mission_ack_t wpa; + mavlink_msg_mission_ack_decode(msg, &wpa); + + if ((msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid) && (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/)) { + wpm->timestamp_lastaction = now; + + if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { + if (wpm->current_wp_id == wpm->size - 1) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Received ACK after having sent last waypoint, going to state MAVLINK_WPM_STATE_IDLE\n"); + +#endif + wpm->current_state = MAVLINK_WPM_STATE_IDLE; + wpm->current_wp_id = 0; + } + } + + } else { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); + +#endif + } + + break; + } + + case MAVLINK_MSG_ID_MISSION_SET_CURRENT: { + mavlink_mission_set_current_t wpc; + mavlink_msg_mission_set_current_decode(msg, &wpc); + + if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) { + wpm->timestamp_lastaction = now; + + if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { + if (wpc.seq < wpm->size) { + // if (verbose) // printf("Received MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT\n"); + wpm->current_active_wp_id = wpc.seq; + uint32_t i; + + for (i = 0; i < wpm->size; i++) { + if (i == wpm->current_active_wp_id) { + wpm->waypoints[i].current = true; + + } else { + wpm->waypoints[i].current = false; + } + } + +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("NEW WP SET"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("New current waypoint %u\n", wpm->current_active_wp_id); + +#endif + wpm->yaw_reached = false; + wpm->pos_reached = false; + mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id); + mavlink_wpm_send_setpoint(wpm->current_active_wp_id); + wpm->timestamp_firstinside_orbit = 0; + + } else { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT: Index out of bounds\n"); + +#endif + } + + } else { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE NOT IN IDLE STATE\n"); + +#endif + } + + } else { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); + +#endif + } + + break; + } + + case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { + mavlink_mission_request_list_t wprl; + mavlink_msg_mission_request_list_decode(msg, &wprl); + + if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) { + wpm->timestamp_lastaction = now; + + if (wpm->current_state == MAVLINK_WPM_STATE_IDLE || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { + if (wpm->size > 0) { + //if (verbose && wpm->current_state == MAVLINK_WPM_STATE_IDLE) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u changing state to MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); +// if (verbose && wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST again from %u staying in state MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); + wpm->current_state = MAVLINK_WPM_STATE_SENDLIST; + wpm->current_wp_id = 0; + wpm->current_partner_sysid = msg->sysid; + wpm->current_partner_compid = msg->compid; + + } else { + // if (verbose) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid); + } + + wpm->current_count = wpm->size; + mavlink_wpm_send_waypoint_count(msg->sysid, msg->compid, wpm->current_count); + + } else { + // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST because i'm doing something else already (state=%i).\n", wpm->current_state); + } + } else { + // if (verbose) // printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT MISMATCH\n"); + } + + break; + } + + case MAVLINK_MSG_ID_MISSION_REQUEST: { + mavlink_mission_request_t wpr; + mavlink_msg_mission_request_decode(msg, &wpr); + + if (msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid && wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) { + wpm->timestamp_lastaction = now; + + //ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) + if ((wpm->current_state == MAVLINK_WPM_STATE_SENDLIST && wpr.seq == 0) || (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && (wpr.seq == wpm->current_wp_id || wpr.seq == wpm->current_wp_id + 1) && wpr.seq < wpm->size)) { + if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("GOT WP REQ, state -> SEND"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); + +#endif + } + + if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm->current_wp_id + 1) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("GOT 2nd WP REQ"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); + +#endif + } + + if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm->current_wp_id) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("GOT 2nd WP REQ"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); + +#endif + } + + wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; + wpm->current_wp_id = wpr.seq; + mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, wpr.seq); + + } else { + // if (verbose) + { + if (!(wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS)) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).\n", wpm->current_state); + +#endif + break; + + } else if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { + if (wpr.seq != 0) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq); + +#endif + } + + } else if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { + if (wpr.seq != wpm->current_wp_id && wpr.seq != wpm->current_wp_id + 1) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, wpm->current_wp_id, wpm->current_wp_id + 1); + +#endif + + } else if (wpr.seq >= wpm->size) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq); + +#endif + } + + } else { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: ?"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST - FIXME: missed error description\n"); + +#endif + } + } + } + + } else { + //we we're target but already communicating with someone else + if ((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid)) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, wpm->current_partner_sysid); + +#endif + + } else { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); + +#endif + } + + } + + break; + } + + case MAVLINK_MSG_ID_MISSION_COUNT: { + mavlink_mission_count_t wpc; + mavlink_msg_mission_count_decode(msg, &wpc); + + if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) { + wpm->timestamp_lastaction = now; + + if (wpm->current_state == MAVLINK_WPM_STATE_IDLE || (wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wpm->current_wp_id == 0)) { +// printf("wpc count in: %d\n",wpc.count); +// printf("Comp id: %d\n",msg->compid); +// printf("Current partner sysid: %d\n",wpm->current_partner_sysid); + + if (wpc.count > 0) { + if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("WP CMD OK: state -> GETLIST"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid); + +#endif + } + + if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u\n", wpc.count, msg->sysid); + +#endif + } + + wpm->current_state = MAVLINK_WPM_STATE_GETLIST; + wpm->current_wp_id = 0; + wpm->current_partner_sysid = msg->sysid; + wpm->current_partner_compid = msg->compid; + wpm->current_count = wpc.count; + +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("CLR RCV BUF: READY"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("clearing receive buffer and readying for receiving waypoints\n"); + +#endif + wpm->rcv_size = 0; + //while(waypoints_receive_buffer->size() > 0) +// { +// delete waypoints_receive_buffer->back(); +// waypoints_receive_buffer->pop_back(); +// } + + mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); + + } else if (wpc.count == 0) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("COUNT 0"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE\n"); + +#endif + wpm->rcv_size = 0; + //while(waypoints_receive_buffer->size() > 0) +// { +// delete waypoints->back(); +// waypoints->pop_back(); +// } + wpm->current_active_wp_id = -1; + wpm->yaw_reached = false; + wpm->pos_reached = false; + break; + + } else { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("IGN WP CMD"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Ignoring MAVLINK_MSG_ID_MISSION_ITEM_COUNT from %u with count of %u\n", msg->sysid, wpc.count); + +#endif + } + + } else { + if (!(wpm->current_state == MAVLINK_WPM_STATE_IDLE || wpm->current_state == MAVLINK_WPM_STATE_GETLIST)) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm doing something else already (state=%i).\n", wpm->current_state); + +#endif + + } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wpm->current_wp_id != 0) { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.\n", wpm->current_wp_id); + +#endif + + } else { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: ?"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT - FIXME: missed error description\n"); + +#endif + } + } + + } else { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); +#else + + if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); + +#endif + } + + } + break; + + case MAVLINK_MSG_ID_MISSION_ITEM: { + mavlink_mission_item_t wp; + mavlink_msg_mission_item_decode(msg, &wp); + + mavlink_missionlib_send_gcs_string("GOT WP"); +// printf("sysid=%d, current_partner_sysid=%d\n", msg->sysid, wpm->current_partner_sysid); +// printf("compid=%d, current_partner_compid=%d\n", msg->compid, wpm->current_partner_compid); + +// if((msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid) && (wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_wpm_comp_id*/)) + if (wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_wpm_comp_id) { + + wpm->timestamp_lastaction = now; + +// printf("wpm->current_state=%u, wp.seq = %d, wpm->current_wp_id=%d\n", wpm->current_state, wp.seq, wpm->current_wp_id); + +// wpm->current_state = MAVLINK_WPM_STATE_GETLIST;//removeme debug XXX TODO + + //ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids + if ((wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wp.seq == 0) || (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm->current_wp_id && wp.seq < wpm->current_count)) { + //mavlink_missionlib_send_gcs_string("DEBUG 2"); + +// if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid); +// if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm->current_wp_id) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u\n", wp.seq, msg->sysid); +// if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm->current_wp_id) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u (again) from %u\n", wp.seq, msg->sysid); +// + wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; + mavlink_mission_item_t *newwp = &(wpm->rcv_waypoints[wp.seq]); + memcpy(newwp, &wp, sizeof(mavlink_mission_item_t)); +// printf("WP seq: %d\n",wp.seq); + wpm->current_wp_id = wp.seq + 1; + + // if (verbose) // printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4); +// printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4); + +// printf ("wpm->current_wp_id =%d, wpm->current_count=%d\n\n", wpm->current_wp_id, wpm->current_count); + if (wpm->current_wp_id == wpm->current_count && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { + mavlink_missionlib_send_gcs_string("GOT ALL WPS"); + // if (verbose) // printf("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm->current_count); + + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, 0); + + if (wpm->current_active_wp_id > wpm->rcv_size - 1) { + wpm->current_active_wp_id = wpm->rcv_size - 1; + } + + // switch the waypoints list + // FIXME CHECK!!! + uint32_t i; + + for (i = 0; i < wpm->current_count; ++i) { + wpm->waypoints[i] = wpm->rcv_waypoints[i]; + } + + wpm->size = wpm->current_count; + + //get the new current waypoint + + for (i = 0; i < wpm->size; i++) { + if (wpm->waypoints[i].current == 1) { + wpm->current_active_wp_id = i; + //// if (verbose) // printf("New current waypoint %u\n", current_active_wp_id); + wpm->yaw_reached = false; + wpm->pos_reached = false; + mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id); + mavlink_wpm_send_setpoint(wpm->current_active_wp_id); + wpm->timestamp_firstinside_orbit = 0; + break; + } + } + + if (i == wpm->size) { + wpm->current_active_wp_id = -1; + wpm->yaw_reached = false; + wpm->pos_reached = false; + wpm->timestamp_firstinside_orbit = 0; + } + + wpm->current_state = MAVLINK_WPM_STATE_IDLE; + + } else { + mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); + } + + } else { + if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { + //we're done receiving waypoints, answer with ack. + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, 0); + printf("Received MAVLINK_MSG_ID_MISSION_ITEM while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n"); + } + + // if (verbose) + { + if (!(wpm->current_state == MAVLINK_WPM_STATE_GETLIST || wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)) { +// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u because i'm doing something else already (state=%i).\n", wp.seq, wpm->current_state); + break; + + } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { + if (!(wp.seq == 0)) { +// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.\n", wp.seq); + } else { +// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); + } + } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { + if (!(wp.seq == wpm->current_wp_id)) { +// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm->current_wp_id); + mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); + + } else if (!(wp.seq < wpm->current_count)) { +// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.\n", wp.seq); + } else { +// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); + } + } else { +// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); + } + } + } + } else { + //we we're target but already communicating with someone else + if ((wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid) && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { + // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, wpm->current_partner_sysid); + } else if (wp.target_system == mavlink_system.sysid /* && wp.target_component == mavlink_wpm_comp_id*/) { + // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid); + } + } + + break; + } + + case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: { + mavlink_mission_clear_all_t wpca; + mavlink_msg_mission_clear_all_decode(msg, &wpca); + + if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state == MAVLINK_WPM_STATE_IDLE) { + wpm->timestamp_lastaction = now; + + // if (verbose) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid); + // Delete all waypoints + wpm->size = 0; + wpm->current_active_wp_id = -1; + wpm->yaw_reached = false; + wpm->pos_reached = false; + + } else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { + // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm->current_state); + } + + break; + } + + default: { + // if (debug) // printf("Waypoint: received message of unknown type"); + break; + } + } + + check_waypoints_reached(now, global_pos, local_pos); +} diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h new file mode 100644 index 000000000..96a0ecd30 --- /dev/null +++ b/src/modules/mavlink/waypoints.h @@ -0,0 +1,131 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @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 + * 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 waypoints.h + * MAVLink waypoint protocol definition (BSD-relicensed). + */ + +#ifndef WAYPOINTS_H_ +#define WAYPOINTS_H_ + +/* This assumes you have the mavlink headers on your include path + or in the same folder as this source file */ + +#include <v1.0/mavlink_types.h> + +// #ifndef MAVLINK_SEND_UART_BYTES +// #define MAVLINK_SEND_UART_BYTES(chan, buffer, len) mavlink_send_uart_bytes(chan, buffer, len) +// #endif +//extern mavlink_system_t mavlink_system; +#include "mavlink_bridge_header.h" +#include <stdbool.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_local_position.h> + +// FIXME XXX - TO BE MOVED TO XML +enum MAVLINK_WPM_STATES { + MAVLINK_WPM_STATE_IDLE = 0, + MAVLINK_WPM_STATE_SENDLIST, + MAVLINK_WPM_STATE_SENDLIST_SENDWPS, + MAVLINK_WPM_STATE_GETLIST, + MAVLINK_WPM_STATE_GETLIST_GETWPS, + MAVLINK_WPM_STATE_GETLIST_GOTALL, + MAVLINK_WPM_STATE_ENUM_END +}; + +enum MAVLINK_WPM_CODES { + MAVLINK_WPM_CODE_OK = 0, + MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, + MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, + MAVLINK_WPM_CODE_ENUM_END +}; + + +/* WAYPOINT MANAGER - MISSION LIB */ + +#define MAVLINK_WPM_MAX_WP_COUNT 15 +#define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates +#ifndef MAVLINK_WPM_TEXT_FEEDBACK +#define MAVLINK_WPM_TEXT_FEEDBACK 0 ///< Report back status information as text +#endif +#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication timeout in useconds +#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint +#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000 + + +struct mavlink_wpm_storage { + mavlink_mission_item_t waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Currently active waypoints +#ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE + mavlink_mission_item_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Receive buffer for next waypoints +#endif + uint16_t size; + uint16_t max_size; + uint16_t rcv_size; + enum MAVLINK_WPM_STATES current_state; + int16_t current_wp_id; ///< Waypoint in current transmission + int16_t current_active_wp_id; ///< Waypoint the system is currently heading towards + uint16_t current_count; + uint8_t current_partner_sysid; + uint8_t current_partner_compid; + uint64_t timestamp_lastaction; + uint64_t timestamp_last_send_setpoint; + uint64_t timestamp_firstinside_orbit; + uint64_t timestamp_lastoutside_orbit; + uint32_t timeout; + uint32_t delay_setpoint; + float accept_range_yaw; + float accept_range_distance; + bool yaw_reached; + bool pos_reached; + bool idle; +}; + +typedef struct mavlink_wpm_storage mavlink_wpm_storage; + +void mavlink_wpm_init(mavlink_wpm_storage *state); +int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, + struct vehicle_local_position_s *local_pos); +void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehicle_global_position_s *global_pos , + struct vehicle_local_position_s *local_pos); + +extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, + float param2, float param3, float param4, float param5_lat_x, + float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command); + +#endif /* WAYPOINTS_H_ */ diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c new file mode 100644 index 000000000..20fb11b2c --- /dev/null +++ b/src/modules/mavlink_onboard/mavlink.c @@ -0,0 +1,528 @@ +/**************************************************************************** + * + * 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 mavlink.c + * MAVLink 1.0 protocol implementation. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +#include <nuttx/config.h> +#include <unistd.h> +#include <pthread.h> +#include <stdio.h> +#include <math.h> +#include <stdbool.h> +#include <fcntl.h> +#include <mqueue.h> +#include <string.h> +#include "mavlink_bridge_header.h" +#include <drivers/drv_hrt.h> +#include <time.h> +#include <float.h> +#include <unistd.h> +#include <nuttx/sched.h> +#include <sys/prctl.h> +#include <termios.h> +#include <errno.h> +#include <stdlib.h> +#include <poll.h> + +#include <systemlib/param/param.h> +#include <systemlib/systemlib.h> +#include <systemlib/err.h> + +#include "orb_topics.h" +#include "util.h" + +__EXPORT int mavlink_onboard_main(int argc, char *argv[]); + +static int mavlink_thread_main(int argc, char *argv[]); + +/* thread state */ +volatile bool thread_should_exit = false; +static volatile bool thread_running = false; +static int mavlink_task; + +/* pthreads */ +static pthread_t receive_thread; + +/* terminate MAVLink on user request - disabled by default */ +static bool mavlink_link_termination_allowed = false; + +mavlink_system_t mavlink_system = { + 100, + 50, + MAV_TYPE_QUADROTOR, + 0, + 0, + 0 +}; // System ID, 1-255, Component/Subsystem ID, 1-255 + +/* XXX not widely used */ +uint8_t chan = MAVLINK_COMM_0; + +/* XXX probably should be in a header... */ +extern pthread_t receive_start(int uart); + +bool mavlink_hil_enabled = false; + +/* protocol interface */ +static int uart; +static int baudrate; +bool gcs_link = true; + +/* interface mode */ +static enum { + MAVLINK_INTERFACE_MODE_OFFBOARD, + MAVLINK_INTERFACE_MODE_ONBOARD +} mavlink_link_mode = MAVLINK_INTERFACE_MODE_OFFBOARD; + +static void mavlink_update_system(void); +static int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); +static void usage(void); + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb) +{ + /* process baud rate */ + int speed; + + switch (baud) { + case 0: speed = B0; break; + + case 50: speed = B50; break; + + case 75: speed = B75; break; + + case 110: speed = B110; break; + + case 134: speed = B134; break; + + case 150: speed = B150; break; + + case 200: speed = B200; break; + + case 300: speed = B300; break; + + case 600: speed = B600; break; + + case 1200: speed = B1200; break; + + case 1800: speed = B1800; break; + + case 2400: speed = B2400; break; + + case 4800: speed = B4800; break; + + case 9600: speed = B9600; break; + + case 19200: speed = B19200; break; + + case 38400: speed = B38400; break; + + case 57600: speed = B57600; break; + + case 115200: speed = B115200; break; + + case 230400: speed = B230400; break; + + case 460800: speed = B460800; break; + + case 921600: speed = B921600; break; + + default: + fprintf(stderr, "[mavlink] ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud); + return -EINVAL; + } + + /* open uart */ + printf("[mavlink] UART is %s, baudrate is %d\n", uart_name, baud); + uart = open(uart_name, O_RDWR | O_NOCTTY); + + /* Try to set baud rate */ + struct termios uart_config; + int termios_state; + *is_usb = false; + + if (strcmp(uart_name, "/dev/ttyACM0") != OK) { + /* Back up the original uart configuration to restore it after exit */ + if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { + fprintf(stderr, "[mavlink] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state); + close(uart); + return -1; + } + + /* Fill the struct for the new configuration */ + tcgetattr(uart, &uart_config); + + /* Clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; + + /* Set baud rate */ + if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { + fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); + close(uart); + return -1; + } + + + if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { + fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); + close(uart); + return -1; + } + + } else { + *is_usb = true; + } + + return uart; +} + +void +mavlink_send_uart_bytes(mavlink_channel_t channel, uint8_t *ch, int length) +{ + write(uart, ch, (size_t)(sizeof(uint8_t) * length)); +} + +/* + * Internal function to give access to the channel status for each channel + */ +mavlink_status_t* mavlink_get_channel_status(uint8_t channel) +{ + static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; + return &m_mavlink_status[channel]; +} + +/* + * Internal function to give access to the channel buffer for each channel + */ +mavlink_message_t* mavlink_get_channel_buffer(uint8_t channel) +{ + static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS]; + return &m_mavlink_buffer[channel]; +} + +void mavlink_update_system(void) +{ + static bool initialized = false; + param_t param_system_id; + param_t param_component_id; + param_t param_system_type; + + 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"); + } + + /* update system and component id */ + int32_t system_id; + param_get(param_system_id, &system_id); + if (system_id > 0 && system_id < 255) { + mavlink_system.sysid = system_id; + } + + int32_t component_id; + param_get(param_component_id, &component_id); + if (component_id > 0 && component_id < 255) { + mavlink_system.compid = component_id; + } + + int32_t system_type; + param_get(param_system_type, &system_type); + if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) { + mavlink_system.type = system_type; + } +} + +void +get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed, + uint8_t *mavlink_state, uint8_t *mavlink_mode) +{ + /* reset MAVLink mode bitfield */ + *mavlink_mode = 0; + + /* set mode flags independent of system state */ + if (v_status->flag_control_manual_enabled) { + *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + } + + if (v_status->flag_hil_enabled) { + *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; + } + + /* set arming state */ + if (armed->armed) { + *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; + } else { + *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; + } + + switch (v_status->state_machine) { + case SYSTEM_STATE_PREFLIGHT: + if (v_status->flag_preflight_gyro_calibration || + v_status->flag_preflight_mag_calibration || + v_status->flag_preflight_accel_calibration) { + *mavlink_state = MAV_STATE_CALIBRATING; + } else { + *mavlink_state = MAV_STATE_UNINIT; + } + break; + + case SYSTEM_STATE_STANDBY: + *mavlink_state = MAV_STATE_STANDBY; + break; + + case SYSTEM_STATE_GROUND_READY: + *mavlink_state = MAV_STATE_ACTIVE; + break; + + case SYSTEM_STATE_MANUAL: + *mavlink_state = MAV_STATE_ACTIVE; + *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + break; + + case SYSTEM_STATE_STABILIZED: + *mavlink_state = MAV_STATE_ACTIVE; + *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; + break; + + case SYSTEM_STATE_AUTO: + *mavlink_state = MAV_STATE_ACTIVE; + *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; + break; + + case SYSTEM_STATE_MISSION_ABORT: + *mavlink_state = MAV_STATE_EMERGENCY; + break; + + case SYSTEM_STATE_EMCY_LANDING: + *mavlink_state = MAV_STATE_EMERGENCY; + break; + + case SYSTEM_STATE_EMCY_CUTOFF: + *mavlink_state = MAV_STATE_EMERGENCY; + break; + + case SYSTEM_STATE_GROUND_ERROR: + *mavlink_state = MAV_STATE_EMERGENCY; + break; + + case SYSTEM_STATE_REBOOT: + *mavlink_state = MAV_STATE_POWEROFF; + break; + } + +} + +/** + * MAVLink Protocol main function. + */ +int mavlink_thread_main(int argc, char *argv[]) +{ + int ch; + char *device_name = "/dev/ttyS1"; + baudrate = 57600; + + struct vehicle_status_s v_status; + struct actuator_armed_s armed; + + /* work around some stupidity in task_create's argv handling */ + argc -= 2; + argv += 2; + + while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) { + switch (ch) { + case 'b': + baudrate = strtoul(optarg, NULL, 10); + if (baudrate == 0) + errx(1, "invalid baud rate '%s'", optarg); + break; + + case 'd': + device_name = optarg; + break; + + case 'e': + mavlink_link_termination_allowed = true; + break; + + case 'o': + mavlink_link_mode = MAVLINK_INTERFACE_MODE_ONBOARD; + break; + + default: + usage(); + } + } + + struct termios uart_config_original; + bool usb_uart; + + /* print welcome text */ + warnx("MAVLink v1.0 serial interface starting..."); + + /* inform about mode */ + warnx((mavlink_link_mode == MAVLINK_INTERFACE_MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE"); + + /* Flush stdout in case MAVLink is about to take it over */ + fflush(stdout); + + /* default values for arguments */ + uart = mavlink_open_uart(baudrate, device_name, &uart_config_original, &usb_uart); + if (uart < 0) + err(1, "could not open %s", device_name); + + /* Initialize system properties */ + mavlink_update_system(); + + /* start the MAVLink receiver */ + receive_thread = receive_start(uart); + + thread_running = true; + + /* arm counter to go off immediately */ + unsigned lowspeed_counter = 10; + + while (!thread_should_exit) { + + /* 1 Hz */ + if (lowspeed_counter == 10) { + mavlink_update_system(); + + /* translate the current system state to mavlink state and mode */ + uint8_t mavlink_state = 0; + uint8_t mavlink_mode = 0; + get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode); + + /* send heartbeat */ + mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state); + + /* send status (values already copied in the section above) */ + mavlink_msg_sys_status_send(chan, + v_status.onboard_control_sensors_present, + v_status.onboard_control_sensors_enabled, + v_status.onboard_control_sensors_health, + v_status.load, + v_status.voltage_battery * 1000.0f, + v_status.current_battery * 1000.0f, + v_status.battery_remaining, + v_status.drop_rate_comm, + v_status.errors_comm, + v_status.errors_count1, + v_status.errors_count2, + v_status.errors_count3, + v_status.errors_count4); + lowspeed_counter = 0; + } + lowspeed_counter++; + + /* sleep 1000 ms */ + usleep(1000000); + } + + /* wait for threads to complete */ + pthread_join(receive_thread, NULL); + + /* Reset the UART flags to original state */ + if (!usb_uart) + tcsetattr(uart, TCSANOW, &uart_config_original); + + thread_running = false; + + exit(0); +} + +static void +usage() +{ + fprintf(stderr, "usage: mavlink start [-d <devicename>] [-b <baud rate>]\n" + " mavlink stop\n" + " mavlink status\n"); + exit(1); +} + +int mavlink_onboard_main(int argc, char *argv[]) +{ + + if (argc < 2) { + warnx("missing command"); + usage(); + } + + if (!strcmp(argv[1], "start")) { + + /* this is not an error */ + if (thread_running) + errx(0, "mavlink already running\n"); + + thread_should_exit = false; + mavlink_task = task_spawn_cmd("mavlink_onboard", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + mavlink_thread_main, + (const char**)argv); + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + while (thread_running) { + usleep(200000); + } + warnx("terminated."); + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + errx(0, "running"); + } else { + errx(1, "not running"); + } + } + + warnx("unrecognized command"); + usage(); + /* not getting here */ + return 0; +} + diff --git a/src/modules/mavlink_onboard/mavlink_bridge_header.h b/src/modules/mavlink_onboard/mavlink_bridge_header.h new file mode 100644 index 000000000..b72bbb2b1 --- /dev/null +++ b/src/modules/mavlink_onboard/mavlink_bridge_header.h @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * 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 mavlink_bridge_header + * MAVLink bridge header for UART access. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +/* MAVLink adapter header */ +#ifndef MAVLINK_BRIDGE_HEADER_H +#define MAVLINK_BRIDGE_HEADER_H + +#define MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/* use efficient approach, see mavlink_helpers.h */ +#define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes + +#define MAVLINK_GET_CHANNEL_BUFFER mavlink_get_channel_buffer +#define MAVLINK_GET_CHANNEL_STATUS mavlink_get_channel_status + +#include <v1.0/mavlink_types.h> +#include <unistd.h> + + +/* Struct that stores the communication settings of this system. + you can also define / alter these settings elsewhere, as long + as they're included BEFORE mavlink.h. + So you can set the + + mavlink_system.sysid = 100; // System ID, 1-255 + mavlink_system.compid = 50; // Component/Subsystem ID, 1-255 + + Lines also in your main.c, e.g. by reading these parameter from EEPROM. + */ +extern mavlink_system_t mavlink_system; + +/** + * @brief Send multiple chars (uint8_t) over a comm channel + * + * @param chan MAVLink channel to use, usually MAVLINK_COMM_0 = UART0 + * @param ch Character to send + */ +extern void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t *ch, int length); + +mavlink_status_t* mavlink_get_channel_status(uint8_t chan); +mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan); + +#include <v1.0/common/mavlink.h> + +#endif /* MAVLINK_BRIDGE_HEADER_H */ diff --git a/src/modules/mavlink_onboard/mavlink_receiver.c b/src/modules/mavlink_onboard/mavlink_receiver.c new file mode 100644 index 000000000..68d49c24b --- /dev/null +++ b/src/modules/mavlink_onboard/mavlink_receiver.c @@ -0,0 +1,330 @@ +/**************************************************************************** + * + * 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 mavlink_receiver.c + * MAVLink protocol message receive and dispatch + * + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +/* XXX trim includes */ +#include <nuttx/config.h> +#include <unistd.h> +#include <pthread.h> +#include <stdio.h> +#include <math.h> +#include <stdbool.h> +#include <fcntl.h> +#include <mqueue.h> +#include <string.h> +#include "mavlink_bridge_header.h" +#include <drivers/drv_hrt.h> +#include <time.h> +#include <float.h> +#include <unistd.h> +#include <nuttx/sched.h> +#include <sys/prctl.h> +#include <termios.h> +#include <errno.h> +#include <stdlib.h> +#include <poll.h> + +#include <systemlib/param/param.h> +#include <systemlib/systemlib.h> + +#include "util.h" +#include "orb_topics.h" + +/* XXX should be in a header somewhere */ +pthread_t receive_start(int uart); + +static void handle_message(mavlink_message_t *msg); +static void *receive_thread(void *arg); + +static mavlink_status_t status; +static struct vehicle_vicon_position_s vicon_position; +static struct vehicle_command_s vcmd; +static struct offboard_control_setpoint_s offboard_control_sp; + +struct vehicle_global_position_s hil_global_pos; +struct vehicle_attitude_s hil_attitude; +orb_advert_t pub_hil_global_pos = -1; +orb_advert_t pub_hil_attitude = -1; + +static orb_advert_t cmd_pub = -1; +static orb_advert_t flow_pub = -1; + +static orb_advert_t offboard_control_sp_pub = -1; +static orb_advert_t vicon_position_pub = -1; + +extern bool gcs_link; + +static void +handle_message(mavlink_message_t *msg) +{ + if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) { + + mavlink_command_long_t cmd_mavlink; + mavlink_msg_command_long_decode(msg, &cmd_mavlink); + + if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) + || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { + //check for MAVLINK terminate command + if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { + /* This is the link shutdown command, terminate mavlink */ + printf("[mavlink] Terminating .. \n"); + fflush(stdout); + usleep(50000); + + /* terminate other threads and this thread */ + thread_should_exit = true; + + } else { + + /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ + vcmd.param1 = cmd_mavlink.param1; + vcmd.param2 = cmd_mavlink.param2; + vcmd.param3 = cmd_mavlink.param3; + vcmd.param4 = cmd_mavlink.param4; + vcmd.param5 = cmd_mavlink.param5; + vcmd.param6 = cmd_mavlink.param6; + vcmd.param7 = cmd_mavlink.param7; + vcmd.command = cmd_mavlink.command; + vcmd.target_system = cmd_mavlink.target_system; + vcmd.target_component = cmd_mavlink.target_component; + vcmd.source_system = msg->sysid; + vcmd.source_component = msg->compid; + vcmd.confirmation = cmd_mavlink.confirmation; + + /* check if topic is advertised */ + if (cmd_pub <= 0) { + cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + } + /* publish */ + orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); + } + } + } + + if (msg->msgid == MAVLINK_MSG_ID_OPTICAL_FLOW) { + mavlink_optical_flow_t flow; + mavlink_msg_optical_flow_decode(msg, &flow); + + struct optical_flow_s f; + + f.timestamp = hrt_absolute_time(); + f.flow_raw_x = flow.flow_x; + f.flow_raw_y = flow.flow_y; + f.flow_comp_x_m = flow.flow_comp_m_x; + f.flow_comp_y_m = flow.flow_comp_m_y; + f.ground_distance_m = flow.ground_distance; + f.quality = flow.quality; + f.sensor_id = flow.sensor_id; + + /* check if topic is advertised */ + if (flow_pub <= 0) { + flow_pub = orb_advertise(ORB_ID(optical_flow), &f); + } else { + /* publish */ + orb_publish(ORB_ID(optical_flow), flow_pub, &f); + } + + } + + if (msg->msgid == MAVLINK_MSG_ID_SET_MODE) { + /* Set mode on request */ + mavlink_set_mode_t new_mode; + mavlink_msg_set_mode_decode(msg, &new_mode); + + /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ + vcmd.param1 = new_mode.base_mode; + vcmd.param2 = new_mode.custom_mode; + vcmd.param3 = 0; + vcmd.param4 = 0; + vcmd.param5 = 0; + vcmd.param6 = 0; + vcmd.param7 = 0; + vcmd.command = MAV_CMD_DO_SET_MODE; + vcmd.target_system = new_mode.target_system; + vcmd.target_component = MAV_COMP_ID_ALL; + vcmd.source_system = msg->sysid; + vcmd.source_component = msg->compid; + vcmd.confirmation = 1; + + /* check if topic is advertised */ + if (cmd_pub <= 0) { + cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + } else { + /* create command */ + orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); + } + } + + /* Handle Vicon position estimates */ + if (msg->msgid == MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE) { + mavlink_vicon_position_estimate_t pos; + mavlink_msg_vicon_position_estimate_decode(msg, &pos); + + vicon_position.x = pos.x; + vicon_position.y = pos.y; + vicon_position.z = pos.z; + + if (vicon_position_pub <= 0) { + vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position); + } else { + orb_publish(ORB_ID(vehicle_vicon_position), vicon_position_pub, &vicon_position); + } + } + + /* Handle quadrotor motor setpoints */ + + if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) { + mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint; + mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint); + + if (mavlink_system.sysid < 4) { + + /* switch to a receiving link mode */ + gcs_link = false; + + /* + * rate control mode - defined by MAVLink + */ + + uint8_t ml_mode = 0; + bool ml_armed = false; + + switch (quad_motors_setpoint.mode) { + case 0: + ml_armed = false; + break; + case 1: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; + ml_armed = true; + + break; + case 2: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; + ml_armed = true; + + break; + case 3: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; + break; + case 4: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; + break; + } + + offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid-1] / (float)INT16_MAX; + offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid-1] / (float)INT16_MAX; + offboard_control_sp.p3= (float)quad_motors_setpoint.yaw[mavlink_system.sysid-1] / (float)INT16_MAX; + offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid-1]/(float)UINT16_MAX; + + if (quad_motors_setpoint.thrust[mavlink_system.sysid-1] == 0) { + ml_armed = false; + } + + offboard_control_sp.armed = ml_armed; + offboard_control_sp.mode = ml_mode; + + offboard_control_sp.timestamp = hrt_absolute_time(); + + /* check if topic has to be advertised */ + if (offboard_control_sp_pub <= 0) { + offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); + } else { + /* Publish */ + orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp); + } + } + } + +} + + +/** + * Receive data from UART. + */ +static void * +receive_thread(void *arg) +{ + int uart_fd = *((int*)arg); + + const int timeout = 1000; + uint8_t ch; + + mavlink_message_t msg; + + prctl(PR_SET_NAME, "mavlink offb rcv", getpid()); + + while (!thread_should_exit) { + + struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } }; + + if (poll(fds, 1, timeout) > 0) { + /* non-blocking read until buffer is empty */ + int nread = 0; + + do { + nread = read(uart_fd, &ch, 1); + + if (mavlink_parse_char(chan, ch, &msg, &status)) { //parse the char + /* handle generic messages and commands */ + handle_message(&msg); + } + } while (nread > 0); + } + } + + return NULL; +} + +pthread_t +receive_start(int uart) +{ + pthread_attr_t receiveloop_attr; + pthread_attr_init(&receiveloop_attr); + + struct sched_param param; + param.sched_priority = SCHED_PRIORITY_MAX - 40; + (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); + + pthread_attr_setstacksize(&receiveloop_attr, 2048); + + pthread_t thread; + pthread_create(&thread, &receiveloop_attr, receive_thread, &uart); + return thread; +}
\ No newline at end of file diff --git a/src/modules/mavlink_onboard/module.mk b/src/modules/mavlink_onboard/module.mk new file mode 100644 index 000000000..a7a4980fa --- /dev/null +++ b/src/modules/mavlink_onboard/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# MAVLink protocol to uORB interface process (XXX hack for onboard use) +# + +MODULE_COMMAND = mavlink_onboard +SRCS = mavlink.c \ + mavlink_receiver.c + +INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h new file mode 100644 index 000000000..f18f56243 --- /dev/null +++ b/src/modules/mavlink_onboard/orb_topics.h @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * Copyright (C) 2008-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 + * 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 orb_topics.h + * Common sets of topics subscribed to or published by the MAVLink driver, + * and structures maintained by those subscriptions. + */ +#pragma once + +#include <uORB/uORB.h> +#include <uORB/topics/sensor_combined.h> +#include <uORB/topics/rc_channels.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_status.h> +#include <uORB/topics/offboard_control_setpoint.h> +#include <uORB/topics/vehicle_command.h> +#include <uORB/topics/vehicle_local_position_setpoint.h> +#include <uORB/topics/vehicle_vicon_position.h> +#include <uORB/topics/vehicle_global_position_setpoint.h> +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/optical_flow.h> +#include <uORB/topics/actuator_outputs.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/debug_key_value.h> +#include <drivers/drv_rc_input.h> + +struct mavlink_subscriptions { + int sensor_sub; + int att_sub; + int global_pos_sub; + int act_0_sub; + int act_1_sub; + int act_2_sub; + int act_3_sub; + int gps_sub; + int man_control_sp_sub; + int armed_sub; + int actuators_sub; + int local_pos_sub; + int spa_sub; + int spl_sub; + int spg_sub; + int debug_key_value; + int input_rc_sub; +}; + +extern struct mavlink_subscriptions mavlink_subs; + +/** Global position */ +extern struct vehicle_global_position_s global_pos; + +/** Local position */ +extern struct vehicle_local_position_s local_pos; + +/** Vehicle status */ +// extern struct vehicle_status_s v_status; + +/** RC channels */ +extern struct rc_channels_s rc; + +/** Actuator armed state */ +// extern struct actuator_armed_s armed; + +/** Worker thread starter */ +extern pthread_t uorb_receive_start(void); diff --git a/src/modules/mavlink_onboard/util.h b/src/modules/mavlink_onboard/util.h new file mode 100644 index 000000000..38a4db372 --- /dev/null +++ b/src/modules/mavlink_onboard/util.h @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * 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 + * 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 util.h + * Utility and helper functions and data. + */ + +#pragma once + +#include "orb_topics.h" + +/** MAVLink communications channel */ +extern uint8_t chan; + +/** Shutdown marker */ +extern volatile bool thread_should_exit; + +/** + * Translate the custom state into standard mavlink modes and state. + */ +extern void get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed, + uint8_t *mavlink_state, uint8_t *mavlink_mode); diff --git a/src/modules/multirotor_att_control/module.mk b/src/modules/multirotor_att_control/module.mk new file mode 100755 index 000000000..7569e1c7e --- /dev/null +++ b/src/modules/multirotor_att_control/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Build multirotor attitude controller +# + +MODULE_COMMAND = multirotor_att_control + +SRCS = multirotor_att_control_main.c \ + multirotor_attitude_control.c \ + multirotor_rate_control.c diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c new file mode 100644 index 000000000..99f25cfe9 --- /dev/null +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -0,0 +1,485 @@ +/**************************************************************************** + * + * 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 multirotor_att_control_main.c + * + * Implementation of multirotor attitude control main loop. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +#include <nuttx/config.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <stdbool.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> +#include <getopt.h> +#include <time.h> +#include <math.h> +#include <poll.h> +#include <sys/prctl.h> +#include <drivers/drv_hrt.h> +#include <uORB/uORB.h> +#include <drivers/drv_gyro.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/offboard_control_setpoint.h> +#include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/sensor_combined.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/parameter_update.h> + +#include <systemlib/perf_counter.h> +#include <systemlib/systemlib.h> +#include <systemlib/param/param.h> + +#include "multirotor_attitude_control.h" +#include "multirotor_rate_control.h" + +PARAM_DEFINE_FLOAT(MC_RCLOSS_THR, 0.0f); // This defines the throttle when the RC signal is lost. + +__EXPORT int multirotor_att_control_main(int argc, char *argv[]); + +static bool thread_should_exit; +static int mc_task; +static bool motor_test_mode = false; + +static orb_advert_t actuator_pub; + +static struct vehicle_status_s state; + +static int +mc_thread_main(int argc, char *argv[]) +{ + /* declare and safely initialize all structs */ + memset(&state, 0, sizeof(state)); + 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 manual_control_setpoint_s manual; + memset(&manual, 0, sizeof(manual)); + struct sensor_combined_s raw; + memset(&raw, 0, sizeof(raw)); + struct offboard_control_setpoint_s offboard_sp; + memset(&offboard_sp, 0, sizeof(offboard_sp)); + struct vehicle_rates_setpoint_s rates_sp; + memset(&rates_sp, 0, sizeof(rates_sp)); + + struct actuator_controls_s actuators; + memset(&actuators, 0, sizeof(actuators)); + + /* subscribe to attitude, motor setpoints and system state */ + int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int param_sub = orb_subscribe(ORB_ID(parameter_update)); + int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); + int state_sub = orb_subscribe(ORB_ID(vehicle_status)); + int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + + /* + * Do not rate-limit the loop to prevent aliasing + * if rate-limiting would be desired later, the line below would + * enable it. + * + * rate-limit the attitude subscription to 200Hz to pace our loop + * orb_set_interval(att_sub, 5); + */ + struct pollfd fds[2] = { + { .fd = att_sub, .events = POLLIN }, + { .fd = param_sub, .events = POLLIN } + }; + + /* publish actuator controls */ + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { + actuators.control[i] = 0.0f; + } + + actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); + orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + orb_advert_t rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + + /* register the perf counter */ + perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control_runtime"); + perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "multirotor_att_control_interval"); + perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "multirotor_att_control_err"); + + /* welcome user */ + printf("[multirotor_att_control] starting\n"); + + /* store last control mode to detect mode switches */ + bool flag_control_manual_enabled = false; + bool flag_control_attitude_enabled = false; + bool flag_system_armed = false; + + /* store if yaw position or yaw speed has been changed */ + bool control_yaw_position = true; + + /* store if we stopped a yaw movement */ + bool first_time_after_yaw_speed_control = true; + + /* prepare the handle for the failsafe throttle */ + param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR"); + float failsafe_throttle = 0.0f; + + + 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(mc_err_perf); + + } else if (ret == 0) { + /* no return value, ignore */ + } else { + + /* only update parameters if they changed */ + if (fds[1].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 */ + // XXX no params here yet + } + + /* only run controller if attitude changed */ + if (fds[0].revents & POLLIN) { + + perf_begin(mc_loop_perf); + + /* get a local copy of system state */ + bool updated; + orb_check(state_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_status), state_sub, &state); + } + + /* get a local copy of manual setpoint */ + orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); + /* get a local copy of attitude */ + orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); + /* get a local copy of attitude setpoint */ + orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp); + /* get a local copy of rates setpoint */ + orb_check(setpoint_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(offboard_control_setpoint), setpoint_sub, &offboard_sp); + } + + /* get a local copy of the current sensor values */ + orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); + + + /** STEP 1: Define which input is the dominating control input */ + if (state.flag_control_offboard_enabled) { + /* offboard inputs */ + if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { + rates_sp.roll = offboard_sp.p1; + rates_sp.pitch = offboard_sp.p2; + rates_sp.yaw = offboard_sp.p3; + rates_sp.thrust = offboard_sp.p4; +// printf("thrust_rate=%8.4f\n",offboard_sp.p4); + rates_sp.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); + + } else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { + att_sp.roll_body = offboard_sp.p1; + att_sp.pitch_body = offboard_sp.p2; + att_sp.yaw_body = offboard_sp.p3; + att_sp.thrust = offboard_sp.p4; +// printf("thrust_att=%8.4f\n",offboard_sp.p4); + att_sp.timestamp = hrt_absolute_time(); + /* STEP 2: publish the result to the vehicle actuators */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + } + + + } else if (state.flag_control_manual_enabled) { + + if (state.flag_control_attitude_enabled) { + + /* initialize to current yaw if switching to manual or att control */ + if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || + state.flag_control_manual_enabled != flag_control_manual_enabled || + state.flag_system_armed != flag_system_armed) { + att_sp.yaw_body = att.yaw; + } + + static bool rc_loss_first_time = true; + + /* if the RC signal is lost, try to stay level and go slowly back down to ground */ + if (state.rc_signal_lost) { + /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ + param_get(failsafe_throttle_handle, &failsafe_throttle); + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + + /* + * Only go to failsafe throttle if last known throttle was + * high enough to create some lift to make hovering state likely. + * + * This is to prevent that someone landing, but not disarming his + * multicopter (throttle = 0) does not make it jump up in the air + * if shutting down his remote. + */ + if (isfinite(manual.throttle) && manual.throttle > 0.2f) { + att_sp.thrust = failsafe_throttle; + + } else { + att_sp.thrust = 0.0f; + } + + /* keep current yaw, do not attempt to go to north orientation, + * since if the pilot regains RC control, he will be lost regarding + * the current orientation. + */ + if (rc_loss_first_time) + att_sp.yaw_body = att.yaw; + + rc_loss_first_time = false; + + } else { + rc_loss_first_time = true; + + att_sp.roll_body = manual.roll; + att_sp.pitch_body = manual.pitch; + + /* set attitude if arming */ + if (!flag_control_attitude_enabled && state.flag_system_armed) { + att_sp.yaw_body = att.yaw; + } + + /* act if stabilization is active or if the (nonsense) direct pass through mode is set */ + if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS || + state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { + + if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) { + rates_sp.yaw = manual.yaw; + control_yaw_position = false; + + } else { + /* + * This mode SHOULD be the default mode, which is: + * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS + * + * However, we fall back to this setting for all other (nonsense) + * settings as well. + */ + + /* only move setpoint if manual input is != 0 */ + if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { + rates_sp.yaw = manual.yaw; + control_yaw_position = false; + first_time_after_yaw_speed_control = true; + + } else { + if (first_time_after_yaw_speed_control) { + att_sp.yaw_body = att.yaw; + first_time_after_yaw_speed_control = false; + } + + control_yaw_position = true; + } + } + } + + att_sp.thrust = manual.throttle; + att_sp.timestamp = hrt_absolute_time(); + } + + /* STEP 2: publish the controller output */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + + if (motor_test_mode) { + printf("testmode"); + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + att_sp.yaw_body = 0.0f; + att_sp.thrust = 0.1f; + att_sp.timestamp = hrt_absolute_time(); + /* STEP 2: publish the result to the vehicle actuators */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + } + + } else { + /* manual rate inputs, from RC control or joystick */ + if (state.flag_control_rates_enabled && + state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) { + rates_sp.roll = manual.roll; + + rates_sp.pitch = manual.pitch; + rates_sp.yaw = manual.yaw; + rates_sp.thrust = manual.throttle; + rates_sp.timestamp = hrt_absolute_time(); + } + } + + } + + /** STEP 3: Identify the controller setup to run and set up the inputs correctly */ + if (state.flag_control_attitude_enabled) { + multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position); + + orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); + } + + /* measure in what intervals the controller runs */ + perf_count(mc_interval_perf); + + float gyro[3]; + + /* get current rate setpoint */ + bool rates_sp_valid = false; + orb_check(rates_sp_sub, &rates_sp_valid); + + if (rates_sp_valid) { + orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp); + } + + /* apply controller */ + gyro[0] = att.rollspeed; + gyro[1] = att.pitchspeed; + gyro[2] = att.yawspeed; + + multirotor_control_rates(&rates_sp, gyro, &actuators); + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + + /* update state */ + flag_control_attitude_enabled = state.flag_control_attitude_enabled; + flag_control_manual_enabled = state.flag_control_manual_enabled; + flag_system_armed = state.flag_system_armed; + + perf_end(mc_loop_perf); + } /* end of poll call for attitude updates */ + } /* end of poll return value check */ + } + + printf("[multirotor att control] stopping, disarming motors.\n"); + + /* 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(state_sub); + close(manual_sub); + close(actuator_pub); + close(att_sp_pub); + + perf_print_counter(mc_loop_perf); + perf_free(mc_loop_perf); + + fflush(stdout); + exit(0); +} + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + + fprintf(stderr, "usage: multirotor_att_control [-m <mode>] [-t] {start|status|stop}\n"); + fprintf(stderr, " <mode> is 'rates' or 'attitude'\n"); + fprintf(stderr, " -t enables motor test mode with 10%% thrust\n"); + exit(1); +} + +int multirotor_att_control_main(int argc, char *argv[]) +{ + int ch; + unsigned int optioncount = 0; + + while ((ch = getopt(argc, argv, "tm:")) != EOF) { + switch (ch) { + case 't': + motor_test_mode = true; + optioncount += 1; + break; + + case ':': + usage("missing parameter"); + break; + + default: + fprintf(stderr, "option: -%c\n", ch); + usage("unrecognized option"); + break; + } + } + + argc -= optioncount; + //argv += optioncount; + + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1 + optioncount], "start")) { + + thread_should_exit = false; + mc_task = task_spawn_cmd("multirotor_att_control", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 15, + 2048, + mc_thread_main, + NULL); + exit(0); + } + + if (!strcmp(argv[1 + optioncount], "stop")) { + thread_should_exit = true; + exit(0); + } + + usage("unrecognized command"); + exit(1); +} diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c new file mode 100644 index 000000000..8f19c6a4b --- /dev/null +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -0,0 +1,251 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler <thomasgubler@student.ethz.ch> + * Julian Oes <joes@student.ethz.ch> + * Laurens Mackay <mackayl@student.ethz.ch> + * Tobias Naegeli <naegelit@student.ethz.ch> + * Martin Rutschmann <rutmarti@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 multirotor_attitude_control.c + * + * Implementation of attitude controller for multirotors. + * + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Laurens Mackay <mackayl@student.ethz.ch> + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Martin Rutschmann <rutmarti@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +#include "multirotor_attitude_control.h" +#include <stdio.h> +#include <stdlib.h> +#include <stdio.h> +#include <stdint.h> +#include <stdbool.h> +#include <float.h> +#include <math.h> +#include <systemlib/pid/pid.h> +#include <systemlib/param/param.h> +#include <drivers/drv_hrt.h> + +PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f); +PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f); +PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f); +//PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f); +//PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f); + +PARAM_DEFINE_FLOAT(MC_ATT_P, 0.2f); +PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_ATT_D, 0.05f); +//PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f); +//PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f); + +//PARAM_DEFINE_FLOAT(MC_ATT_XOFF, 0.0f); +//PARAM_DEFINE_FLOAT(MC_ATT_YOFF, 0.0f); + +struct mc_att_control_params { + float yaw_p; + float yaw_i; + float yaw_d; + //float yaw_awu; + //float yaw_lim; + + float att_p; + float att_i; + float att_d; + //float att_awu; + //float att_lim; + + //float att_xoff; + //float att_yoff; +}; + +struct mc_att_control_param_handles { + param_t yaw_p; + param_t yaw_i; + param_t yaw_d; + //param_t yaw_awu; + //param_t yaw_lim; + + param_t att_p; + param_t att_i; + param_t att_d; + //param_t att_awu; + //param_t att_lim; + + //param_t att_xoff; + //param_t att_yoff; +}; + +/** + * Initialize all parameter handles and values + * + */ +static int parameters_init(struct mc_att_control_param_handles *h); + +/** + * Update all parameters + * + */ +static int parameters_update(const struct mc_att_control_param_handles *h, struct mc_att_control_params *p); + + +static int parameters_init(struct mc_att_control_param_handles *h) +{ + /* PID parameters */ + h->yaw_p = param_find("MC_YAWPOS_P"); + h->yaw_i = param_find("MC_YAWPOS_I"); + h->yaw_d = param_find("MC_YAWPOS_D"); + //h->yaw_awu = param_find("MC_YAWPOS_AWU"); + //h->yaw_lim = param_find("MC_YAWPOS_LIM"); + + h->att_p = param_find("MC_ATT_P"); + h->att_i = param_find("MC_ATT_I"); + h->att_d = param_find("MC_ATT_D"); + //h->att_awu = param_find("MC_ATT_AWU"); + //h->att_lim = param_find("MC_ATT_LIM"); + + //h->att_xoff = param_find("MC_ATT_XOFF"); + //h->att_yoff = param_find("MC_ATT_YOFF"); + + return OK; +} + +static int parameters_update(const struct mc_att_control_param_handles *h, struct mc_att_control_params *p) +{ + param_get(h->yaw_p, &(p->yaw_p)); + param_get(h->yaw_i, &(p->yaw_i)); + param_get(h->yaw_d, &(p->yaw_d)); + //param_get(h->yaw_awu, &(p->yaw_awu)); + //param_get(h->yaw_lim, &(p->yaw_lim)); + + param_get(h->att_p, &(p->att_p)); + param_get(h->att_i, &(p->att_i)); + param_get(h->att_d, &(p->att_d)); + //param_get(h->att_awu, &(p->att_awu)); + //param_get(h->att_lim, &(p->att_lim)); + + //param_get(h->att_xoff, &(p->att_xoff)); + //param_get(h->att_yoff, &(p->att_yoff)); + + return OK; +} + +void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, + const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position) +{ + static uint64_t last_run = 0; + static uint64_t last_input = 0; + float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; + last_run = hrt_absolute_time(); + + if (last_input != att_sp->timestamp) { + last_input = att_sp->timestamp; + } + + static int motor_skip_counter = 0; + + static PID_t pitch_controller; + static PID_t roll_controller; + + static struct mc_att_control_params p; + static struct mc_att_control_param_handles h; + + static bool initialized = false; + + static float yaw_error; + + /* initialize the pid controllers when the function is called for the first time */ + if (initialized == false) { + parameters_init(&h); + parameters_update(&h, &p); + + pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET, 0.0f); + pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET, 0.0f); + + initialized = true; + } + + /* load new parameters with lower rate */ + if (motor_skip_counter % 500 == 0) { + /* update parameters from storage */ + parameters_update(&h, &p); + + /* apply parameters */ + pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); + pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); + } + + /* reset integral if on ground */ + if (att_sp->thrust < 0.1f) { + pid_reset_integral(&pitch_controller); + pid_reset_integral(&roll_controller); + } + + + /* calculate current control outputs */ + + /* control pitch (forward) output */ + rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body , + att->pitch, att->pitchspeed, deltaT); + + /* control roll (left/right) output */ + rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body , + att->roll, att->rollspeed, deltaT); + + if (control_yaw_position) { + /* control yaw rate */ + + /* positive error: rotate to right, negative error, rotate to left (NED frame) */ + // yaw_error = _wrap_pi(att_sp->yaw_body - att->yaw); + + yaw_error = att_sp->yaw_body - att->yaw; + + if (yaw_error > M_PI_F) { + yaw_error -= M_TWOPI_F; + + } else if (yaw_error < -M_PI_F) { + yaw_error += M_TWOPI_F; + } + + rates_sp->yaw = p.yaw_p * (yaw_error) - (p.yaw_d * att->yawspeed); + } + + rates_sp->thrust = att_sp->thrust; + + motor_skip_counter++; +} diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.h b/src/modules/multirotor_att_control/multirotor_attitude_control.h new file mode 100644 index 000000000..e78f45c47 --- /dev/null +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.h @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler <thomasgubler@student.ethz.ch> + * Julian Oes <joes@student.ethz.ch> + * Laurens Mackay <mackayl@student.ethz.ch> + * Tobias Naegeli <naegelit@student.ethz.ch> + * Martin Rutschmann <rutmarti@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 multirotor_attitude_control.h + * + * Definition of attitude controller for multirotors. + * + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Laurens Mackay <mackayl@student.ethz.ch> + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Martin Rutschmann <rutmarti@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +#ifndef MULTIROTOR_ATTITUDE_CONTROL_H_ +#define MULTIROTOR_ATTITUDE_CONTROL_H_ + +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/actuator_controls.h> + +void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, + const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position); + +#endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */ diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c new file mode 100644 index 000000000..e58d357d5 --- /dev/null +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -0,0 +1,225 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. + * Author: Tobias Naegeli <naegelit@student.ethz.ch> + * Lorenz Meier <lm@inf.ethz.ch> + * Anton Babushkin <anton.babushkin@me.com> + * 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 + * 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 multirotor_rate_control.c + * + * Implementation of rate controller for multirotors. + * + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + * @author Julian Oes <joes@student.ethz.ch> + */ + +#include "multirotor_rate_control.h" +#include <stdio.h> +#include <stdlib.h> +#include <stdio.h> +#include <stdint.h> +#include <stdbool.h> +#include <float.h> +#include <math.h> +#include <systemlib/pid/pid.h> +#include <systemlib/param/param.h> +#include <systemlib/err.h> +#include <drivers/drv_hrt.h> + +PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.0f); /* same on Flamewheel */ +PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); +//PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.0f); +//PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 1.0f); + +PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.0f); /* 0.15 F405 Flamewheel */ +PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.0f); +PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f); +//PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f); +//PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 1.0f); /**< roughly < 500 deg/s limit */ + +struct mc_rate_control_params { + + float yawrate_p; + float yawrate_d; + float yawrate_i; + //float yawrate_awu; + //float yawrate_lim; + + float attrate_p; + float attrate_d; + float attrate_i; + //float attrate_awu; + //float attrate_lim; + + float rate_lim; +}; + +struct mc_rate_control_param_handles { + + param_t yawrate_p; + param_t yawrate_i; + param_t yawrate_d; + //param_t yawrate_awu; + //param_t yawrate_lim; + + param_t attrate_p; + param_t attrate_i; + param_t attrate_d; + //param_t attrate_awu; + //param_t attrate_lim; +}; + +/** + * Initialize all parameter handles and values + * + */ +static int parameters_init(struct mc_rate_control_param_handles *h); + +/** + * Update all parameters + * + */ +static int parameters_update(const struct mc_rate_control_param_handles *h, struct mc_rate_control_params *p); + + +static int parameters_init(struct mc_rate_control_param_handles *h) +{ + /* PID parameters */ + h->yawrate_p = param_find("MC_YAWRATE_P"); + h->yawrate_i = param_find("MC_YAWRATE_I"); + h->yawrate_d = param_find("MC_YAWRATE_D"); + //h->yawrate_awu = param_find("MC_YAWRATE_AWU"); + //h->yawrate_lim = param_find("MC_YAWRATE_LIM"); + + h->attrate_p = param_find("MC_ATTRATE_P"); + h->attrate_i = param_find("MC_ATTRATE_I"); + h->attrate_d = param_find("MC_ATTRATE_D"); + //h->attrate_awu = param_find("MC_ATTRATE_AWU"); + //h->attrate_lim = param_find("MC_ATTRATE_LIM"); + + return OK; +} + +static int parameters_update(const struct mc_rate_control_param_handles *h, struct mc_rate_control_params *p) +{ + param_get(h->yawrate_p, &(p->yawrate_p)); + param_get(h->yawrate_i, &(p->yawrate_i)); + param_get(h->yawrate_d, &(p->yawrate_d)); + //param_get(h->yawrate_awu, &(p->yawrate_awu)); + //param_get(h->yawrate_lim, &(p->yawrate_lim)); + + param_get(h->attrate_p, &(p->attrate_p)); + param_get(h->attrate_i, &(p->attrate_i)); + param_get(h->attrate_d, &(p->attrate_d)); + //param_get(h->attrate_awu, &(p->attrate_awu)); + //param_get(h->attrate_lim, &(p->attrate_lim)); + + return OK; +} + +void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, + const float rates[], struct actuator_controls_s *actuators) +{ + static uint64_t last_run = 0; + const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; + static uint64_t last_input = 0; + + if (last_input != rate_sp->timestamp) { + last_input = rate_sp->timestamp; + } + + last_run = hrt_absolute_time(); + + static int motor_skip_counter = 0; + + static PID_t pitch_rate_controller; + static PID_t roll_rate_controller; + + static struct mc_rate_control_params p; + static struct mc_rate_control_param_handles h; + + static bool initialized = false; + + /* initialize the pid controllers when the function is called for the first time */ + if (initialized == false) { + parameters_init(&h); + parameters_update(&h, &p); + initialized = true; + + pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); + pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); + + } + + /* load new parameters with lower rate */ + if (motor_skip_counter % 2500 == 0) { + /* update parameters from storage */ + parameters_update(&h, &p); + pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f); + pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f); + } + + /* reset integral if on ground */ + if (rate_sp->thrust < 0.01f) { + pid_reset_integral(&pitch_rate_controller); + pid_reset_integral(&roll_rate_controller); + } + + /* control pitch (forward) output */ + float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , + rates[1], 0.0f, deltaT); + + /* control roll (left/right) output */ + float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , + rates[0], 0.0f, deltaT); + + /* control yaw rate */ //XXX use library here + float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]); + + /* increase resilience to faulty control inputs */ + if (!isfinite(yaw_rate_control)) { + yaw_rate_control = 0.0f; + warnx("rej. NaN ctrl yaw"); + } + + actuators->control[0] = roll_control; + actuators->control[1] = pitch_control; + actuators->control[2] = yaw_rate_control; + actuators->control[3] = rate_sp->thrust; + + motor_skip_counter++; +} diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.h b/src/modules/multirotor_att_control/multirotor_rate_control.h new file mode 100644 index 000000000..362b5ed86 --- /dev/null +++ b/src/modules/multirotor_att_control/multirotor_rate_control.h @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler <thomasgubler@student.ethz.ch> + * Julian Oes <joes@student.ethz.ch> + * Laurens Mackay <mackayl@student.ethz.ch> + * Tobias Naegeli <naegelit@student.ethz.ch> + * Martin Rutschmann <rutmarti@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 multirotor_attitude_control.h + * + * Definition of rate controller for multirotors. + * + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Laurens Mackay <mackayl@student.ethz.ch> + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Martin Rutschmann <rutmarti@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +#ifndef MULTIROTOR_RATE_CONTROL_H_ +#define MULTIROTOR_RATE_CONTROL_H_ + +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/actuator_controls.h> + +void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, + const float rates[], struct actuator_controls_s *actuators); + +#endif /* MULTIROTOR_RATE_CONTROL_H_ */ diff --git a/src/modules/multirotor_pos_control/module.mk b/src/modules/multirotor_pos_control/module.mk new file mode 100644 index 000000000..d04847745 --- /dev/null +++ b/src/modules/multirotor_pos_control/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Build multirotor position control +# + +MODULE_COMMAND = multirotor_pos_control + +SRCS = multirotor_pos_control.c \ + multirotor_pos_control_params.c diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c new file mode 100644 index 000000000..f39d11438 --- /dev/null +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -0,0 +1,238 @@ +/**************************************************************************** + * + * Copyright (C) 2008-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 multirotor_pos_control.c + * + * Skeleton for multirotor position controller + */ + +#include <nuttx/config.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <stdbool.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> +#include <termios.h> +#include <time.h> +#include <sys/prctl.h> +#include <drivers/drv_hrt.h> +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/vehicle_local_position_setpoint.h> +#include <uORB/topics/vehicle_vicon_position.h> +#include <systemlib/systemlib.h> + +#include "multirotor_pos_control_params.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 */ + +__EXPORT int multirotor_pos_control_main(int argc, char *argv[]); + +/** + * Mainloop of position controller. + */ +static int multirotor_pos_control_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); + fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\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_spawn_cmd(). + */ +int multirotor_pos_control_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + printf("multirotor pos control already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + deamon_task = task_spawn_cmd("multirotor pos control", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 60, + 4096, + multirotor_pos_control_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) { + printf("\tmultirotor pos control app is running\n"); + } else { + printf("\tmultirotor pos control app not started\n"); + } + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +static int +multirotor_pos_control_thread_main(int argc, char *argv[]) +{ + /* welcome user */ + printf("[multirotor pos control] Control started, taking over position control\n"); + + /* structures */ + struct vehicle_status_s state; + struct vehicle_attitude_s att; + //struct vehicle_global_position_setpoint_s global_pos_sp; + struct vehicle_local_position_setpoint_s local_pos_sp; + struct vehicle_vicon_position_s local_pos; + struct manual_control_setpoint_s manual; + struct vehicle_attitude_setpoint_s att_sp; + + /* subscribe to attitude, motor setpoints and system state */ + int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int state_sub = orb_subscribe(ORB_ID(vehicle_status)); + int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + int local_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); + //int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); + int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); + + /* publish attitude setpoint */ + orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + + thread_running = true; + + int loopcounter = 0; + + struct multirotor_position_control_params p; + struct multirotor_position_control_param_handles h; + parameters_init(&h); + parameters_update(&h, &p); + + + while (1) { + /* get a local copy of the vehicle state */ + orb_copy(ORB_ID(vehicle_status), state_sub, &state); + /* get a local copy of manual setpoint */ + orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); + /* get a local copy of attitude */ + orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); + /* get a local copy of local position */ + orb_copy(ORB_ID(vehicle_vicon_position), local_pos_sub, &local_pos); + /* get a local copy of local position setpoint */ + orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); + + if (loopcounter == 500) { + parameters_update(&h, &p); + loopcounter = 0; + } + + // if (state.state_machine == SYSTEM_STATE_AUTO) { + + // XXX IMPLEMENT POSITION CONTROL HERE + + float dT = 1.0f / 50.0f; + + float x_setpoint = 0.0f; + + // XXX enable switching between Vicon and local position estimate + /* local pos is the Vicon position */ + + // XXX just an example, lacks rotation around world-body transformation + att_sp.pitch_body = (local_pos.x - x_setpoint) * p.p; + att_sp.roll_body = 0.0f; + att_sp.yaw_body = 0.0f; + att_sp.thrust = 0.3f; + att_sp.timestamp = hrt_absolute_time(); + + /* publish new attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + // } else if (state.state_machine == SYSTEM_STATE_STABILIZED) { + /* set setpoint to current position */ + // XXX select pos reset channel on remote + /* reset setpoint to current position (position hold) */ + // if (1 == 2) { + // local_pos_sp.x = local_pos.x; + // local_pos_sp.y = local_pos.y; + // local_pos_sp.z = local_pos.z; + // local_pos_sp.yaw = att.yaw; + // } + // } + + /* run at approximately 50 Hz */ + usleep(20000); + loopcounter++; + + } + + printf("[multirotor pos control] ending now...\n"); + + thread_running = false; + + fflush(stdout); + return 0; +} + diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c new file mode 100644 index 000000000..6b73dc405 --- /dev/null +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: 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 multirotor_position_control_params.c + * + * Parameters for EKF filter + */ + +#include "multirotor_pos_control_params.h" + +/* Extended Kalman Filter covariances */ + +/* controller parameters */ +PARAM_DEFINE_FLOAT(MC_POS_P, 0.2f); + +int parameters_init(struct multirotor_position_control_param_handles *h) +{ + /* PID parameters */ + h->p = param_find("MC_POS_P"); + + return OK; +} + +int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p) +{ + param_get(h->p, &(p->p)); + + return OK; +} diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h new file mode 100644 index 000000000..274f6c22a --- /dev/null +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: 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 multirotor_position_control_params.h + * + * Parameters for position controller + */ + +#include <systemlib/param/param.h> + +struct multirotor_position_control_params { + float p; + float i; + float d; +}; + +struct multirotor_position_control_param_handles { + param_t p; + param_t i; + param_t d; +}; + +/** + * Initialize all parameter handles and values + * + */ +int parameters_init(struct multirotor_position_control_param_handles *h); + +/** + * Update all parameters + * + */ +int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p); diff --git a/src/modules/multirotor_pos_control/position_control.c b/src/modules/multirotor_pos_control/position_control.c new file mode 100644 index 000000000..9c53a5bf6 --- /dev/null +++ b/src/modules/multirotor_pos_control/position_control.c @@ -0,0 +1,235 @@ +// /**************************************************************************** +// * +// * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. +// * Author: @author Lorenz Meier <lm@inf.ethz.ch> +// * @author Laurens Mackay <mackayl@student.ethz.ch> +// * @author Tobias Naegeli <naegelit@student.ethz.ch> +// * @author Martin Rutschmann <rutmarti@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 multirotor_position_control.c +// * Implementation of the position control for a multirotor VTOL +// */ + +// #include <stdio.h> +// #include <stdlib.h> +// #include <stdio.h> +// #include <stdint.h> +// #include <math.h> +// #include <stdbool.h> +// #include <float.h> +// #include <systemlib/pid/pid.h> + +// #include "multirotor_position_control.h" + +// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual, +// const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos, +// const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp) +// { +// static PID_t distance_controller; + +// static int read_ret; +// static global_data_position_t position_estimated; + +// static uint16_t counter; + +// static bool initialized; +// static uint16_t pm_counter; + +// static float lat_next; +// static float lon_next; + +// static float pitch_current; + +// static float thrust_total; + + +// if (initialized == false) { + +// pid_init(&distance_controller, +// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P], +// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I], +// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D], +// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU], +// PID_MODE_DERIVATIV_CALC, 150);//150 + +// // pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM]; +// // pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM]; + +// thrust_total = 0.0f; + +// /* Position initialization */ +// /* Wait for new position estimate */ +// do { +// read_ret = read_lock_position(&position_estimated); +// } while (read_ret != 0); + +// lat_next = position_estimated.lat; +// lon_next = position_estimated.lon; + +// /* attitude initialization */ +// global_data_lock(&global_data_attitude->access_conf); +// pitch_current = global_data_attitude->pitch; +// global_data_unlock(&global_data_attitude->access_conf); + +// initialized = true; +// } + +// /* load new parameters with 10Hz */ +// if (counter % 50 == 0) { +// if (global_data_trylock(&global_data_parameter_storage->access_conf) == 0) { +// /* check whether new parameters are available */ +// if (global_data_parameter_storage->counter > pm_counter) { +// pid_set_parameters(&distance_controller, +// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P], +// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I], +// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D], +// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]); + +// // +// // pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM]; +// // pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM]; + +// pm_counter = global_data_parameter_storage->counter; +// printf("Position controller changed pid parameters\n"); +// } +// } + +// global_data_unlock(&global_data_parameter_storage->access_conf); +// } + + +// /* Wait for new position estimate */ +// do { +// read_ret = read_lock_position(&position_estimated); +// } while (read_ret != 0); + +// /* Get next waypoint */ //TODO: add local copy + +// if (0 == global_data_trylock(&global_data_position_setpoint->access_conf)) { +// lat_next = global_data_position_setpoint->x; +// lon_next = global_data_position_setpoint->y; +// global_data_unlock(&global_data_position_setpoint->access_conf); +// } + +// /* Get distance to waypoint */ +// float distance_to_waypoint = get_distance_to_next_waypoint(position_estimated.lat , position_estimated.lon, lat_next, lon_next); +// // if(counter % 5 == 0) +// // printf("distance_to_waypoint: %.4f\n", distance_to_waypoint); + +// /* Get bearing to waypoint (direction on earth surface to next waypoint) */ +// float bearing = get_bearing_to_next_waypoint(position_estimated.lat, position_estimated.lon, lat_next, lon_next); + +// if (counter % 5 == 0) +// printf("bearing: %.4f\n", bearing); + +// /* Calculate speed in direction of bearing (needed for controller) */ +// float speed_norm = sqrtf(position_estimated.vx * position_estimated.vx + position_estimated.vy * position_estimated.vy); +// // if(counter % 5 == 0) +// // printf("speed_norm: %.4f\n", speed_norm); +// float speed_to_waypoint = 0; //(position_estimated.vx * cosf(bearing) + position_estimated.vy * sinf(bearing))/speed_norm; //FIXME, TODO: re-enable this once we have a full estimate of the speed, then we can do a PID for the distance controller + +// /* Control Thrust in bearing direction */ +// float horizontal_thrust = -pid_calculate(&distance_controller, 0, distance_to_waypoint, speed_to_waypoint, +// CONTROL_PID_POSITION_INTERVAL); //TODO: maybe this "-" sign is an error somewhere else + +// // if(counter % 5 == 0) +// // printf("horizontal thrust: %.4f\n", horizontal_thrust); + +// /* Get total thrust (from remote for now) */ +// if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) { +// thrust_total = (float)global_data_rc_channels->chan[THROTTLE].scale; //TODO: how should we use the RC_CHANNELS_FUNCTION enum? +// global_data_unlock(&global_data_rc_channels->access_conf); +// } + +// const float max_gas = 500.0f; +// thrust_total *= max_gas / 20000.0f; //TODO: check this +// thrust_total += max_gas / 2.0f; + + +// if (horizontal_thrust > thrust_total) { +// horizontal_thrust = thrust_total; + +// } else if (horizontal_thrust < -thrust_total) { +// horizontal_thrust = -thrust_total; +// } + + + +// //TODO: maybe we want to add a speed controller later... + +// /* Calclulate thrust in east and north direction */ +// float thrust_north = cosf(bearing) * horizontal_thrust; +// float thrust_east = sinf(bearing) * horizontal_thrust; + +// if (counter % 10 == 0) { +// printf("thrust north: %.4f\n", thrust_north); +// printf("thrust east: %.4f\n", thrust_east); +// fflush(stdout); +// } + +// /* Get current attitude */ +// if (0 == global_data_trylock(&global_data_attitude->access_conf)) { +// pitch_current = global_data_attitude->pitch; +// global_data_unlock(&global_data_attitude->access_conf); +// } + +// /* Get desired pitch & roll */ +// float pitch_desired = 0.0f; +// float roll_desired = 0.0f; + +// if (thrust_total != 0) { +// float pitch_fraction = -thrust_north / thrust_total; +// float roll_fraction = thrust_east / (cosf(pitch_current) * thrust_total); + +// if (roll_fraction < -1) { +// roll_fraction = -1; + +// } else if (roll_fraction > 1) { +// roll_fraction = 1; +// } + +// // if(counter % 5 == 0) +// // { +// // printf("pitch_fraction: %.4f, roll_fraction: %.4f\n",pitch_fraction, roll_fraction); +// // fflush(stdout); +// // } + +// pitch_desired = asinf(pitch_fraction); +// roll_desired = asinf(roll_fraction); +// } + +// att_sp.roll = roll_desired; +// att_sp.pitch = pitch_desired; + +// counter++; +// } diff --git a/src/modules/multirotor_pos_control/position_control.h b/src/modules/multirotor_pos_control/position_control.h new file mode 100644 index 000000000..2144ebc34 --- /dev/null +++ b/src/modules/multirotor_pos_control/position_control.h @@ -0,0 +1,50 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * @author Laurens Mackay <mackayl@student.ethz.ch> + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Martin Rutschmann <rutmarti@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 multirotor_position_control.h + * Definition of the position control for a multirotor VTOL + */ + +// #ifndef POSITION_CONTROL_H_ +// #define POSITION_CONTROL_H_ + +// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual, +// const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos, +// const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp); + +// #endif /* POSITION_CONTROL_H_ */ diff --git a/src/modules/position_estimator/module.mk b/src/modules/position_estimator/module.mk new file mode 100644 index 000000000..f64095d9d --- /dev/null +++ b/src/modules/position_estimator/module.mk @@ -0,0 +1,44 @@ +############################################################################ +# +# 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 new file mode 100644 index 000000000..e84945299 --- /dev/null +++ b/src/modules/position_estimator/position_estimator_main.c @@ -0,0 +1,423 @@ +/**************************************************************************** + * + * 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_mc/codegen/kalman_dlqe1.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe1.c new file mode 100755 index 000000000..977565b8e --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe1.c @@ -0,0 +1,58 @@ +/* + * 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 new file mode 100755 index 000000000..2f5330563 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe1.h @@ -0,0 +1,30 @@ +/* + * 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 new file mode 100755 index 000000000..6627f76e9 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.c @@ -0,0 +1,31 @@ +/* + * 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 new file mode 100755 index 000000000..a77eb5712 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.h @@ -0,0 +1,30 @@ +/* + * 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 new file mode 100755 index 000000000..a65536f79 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.c @@ -0,0 +1,31 @@ +/* + * 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 new file mode 100755 index 000000000..100c7f76c --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.h @@ -0,0 +1,30 @@ +/* + * 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 new file mode 100755 index 000000000..d4b2c2d61 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_types.h @@ -0,0 +1,16 @@ +/* + * 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 new file mode 100755 index 000000000..11b999064 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe2.c @@ -0,0 +1,119 @@ +/* + * 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 new file mode 100755 index 000000000..30170ae22 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe2.h @@ -0,0 +1,32 @@ +/* + * 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 new file mode 100755 index 000000000..de5a1d8aa --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.c @@ -0,0 +1,31 @@ +/* + * 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 new file mode 100755 index 000000000..3d507ff31 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.h @@ -0,0 +1,32 @@ +/* + * 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 new file mode 100755 index 000000000..0757c878c --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.c @@ -0,0 +1,31 @@ +/* + * 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 new file mode 100755 index 000000000..23995020b --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.h @@ -0,0 +1,32 @@ +/* + * 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 new file mode 100755 index 000000000..f7a04d908 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_types.h @@ -0,0 +1,16 @@ +/* + * 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 new file mode 100755 index 000000000..9efe2ea7a --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe3.c @@ -0,0 +1,137 @@ +/*
+ * 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 new file mode 100755 index 000000000..9bbffbbb3 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe3.h @@ -0,0 +1,33 @@ +/*
+ * 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 new file mode 100755 index 000000000..8f2275c13 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.c @@ -0,0 +1,32 @@ +/*
+ * 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 new file mode 100755 index 000000000..952eb7b89 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.h @@ -0,0 +1,38 @@ +/*
+ * 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 new file mode 100755 index 000000000..b87d604c4 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.c @@ -0,0 +1,47 @@ +/*
+ * 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 new file mode 100755 index 000000000..9dee90f9e --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.h @@ -0,0 +1,33 @@ +/*
+ * 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 new file mode 100755 index 000000000..b00858205 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.c @@ -0,0 +1,31 @@ +/*
+ * 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 new file mode 100755 index 000000000..69cc85c76 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.h @@ -0,0 +1,33 @@ +/*
+ * 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 new file mode 100755 index 000000000..f36ea4557 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_types.h @@ -0,0 +1,16 @@ +/*
+ * 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 new file mode 100755 index 000000000..5139848bc --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.c @@ -0,0 +1,136 @@ +/* + * 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 new file mode 100755 index 000000000..205c8eb4e --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.h @@ -0,0 +1,31 @@ +/* + * 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 new file mode 100755 index 000000000..4c535618a --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.c @@ -0,0 +1,157 @@ +/* + * 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 new file mode 100755 index 000000000..94cbe2ce6 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.h @@ -0,0 +1,31 @@ +/* + * 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 new file mode 100755 index 000000000..aa89f8a9d --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.c @@ -0,0 +1,31 @@ +/* + * 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 new file mode 100755 index 000000000..8d358a9a3 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.h @@ -0,0 +1,31 @@ +/* + * 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 new file mode 100755 index 000000000..20ed2edbb --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.c @@ -0,0 +1,31 @@ +/* + * 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 new file mode 100755 index 000000000..5eb5807a0 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.h @@ -0,0 +1,31 @@ +/* + * 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 new file mode 100755 index 000000000..43e5f016c --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_types.h @@ -0,0 +1,16 @@ +/* + * 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 new file mode 100755 index 000000000..5bd87c390 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.c @@ -0,0 +1,31 @@ +/* + * 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 new file mode 100755 index 000000000..44bce472f --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.h @@ -0,0 +1,31 @@ +/* + * 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 new file mode 100755 index 000000000..41e11936f --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.c @@ -0,0 +1,31 @@ +/* + * 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 new file mode 100755 index 000000000..e84ea01bc --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.h @@ -0,0 +1,31 @@ +/* + * 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 new file mode 100755 index 000000000..4b473f56f --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_types.h @@ -0,0 +1,16 @@ +/* + * 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 new file mode 100755 index 000000000..51aef7b76 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/randn.c @@ -0,0 +1,524 @@ +/*
+ * 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 new file mode 100755 index 000000000..8a2aa9277 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/randn.h @@ -0,0 +1,33 @@ +/*
+ * 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 new file mode 100755 index 000000000..c6fa7884e --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/rtGetInf.c @@ -0,0 +1,139 @@ +/* + * 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 new file mode 100755 index 000000000..e7b2a2d1c --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/rtGetInf.h @@ -0,0 +1,23 @@ +/* + * 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 new file mode 100755 index 000000000..552770149 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/rtGetNaN.c @@ -0,0 +1,96 @@ +/* + * 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 new file mode 100755 index 000000000..5acdd9790 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/rtGetNaN.h @@ -0,0 +1,21 @@ +/* + * 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 new file mode 100755 index 000000000..de121c4a0 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/rt_nonfinite.c @@ -0,0 +1,87 @@ +/* + * 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 new file mode 100755 index 000000000..3bbcfd087 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/rt_nonfinite.h @@ -0,0 +1,53 @@ +/* + * 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 new file mode 100755 index 000000000..8916e8572 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/rtwtypes.h @@ -0,0 +1,159 @@ +/* + * 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 new file mode 100755 index 000000000..ff939d029 --- /dev/null +++ b/src/modules/position_estimator_mc/kalman_dlqe1.m @@ -0,0 +1,3 @@ +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 new file mode 100755 index 000000000..2a50164ef --- /dev/null +++ b/src/modules/position_estimator_mc/kalman_dlqe2.m @@ -0,0 +1,9 @@ +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 new file mode 100755 index 000000000..4c6421b7f --- /dev/null +++ b/src/modules/position_estimator_mc/kalman_dlqe3.m @@ -0,0 +1,17 @@ +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 new file mode 100644 index 000000000..40b135ea4 --- /dev/null +++ b/src/modules/position_estimator_mc/module.mk @@ -0,0 +1,60 @@ +############################################################################ +# +# 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 new file mode 100755 index 000000000..144ff8c7c --- /dev/null +++ b/src/modules/position_estimator_mc/positionKalmanFilter1D.m @@ -0,0 +1,19 @@ +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 new file mode 100755 index 000000000..f94cce1fb --- /dev/null +++ b/src/modules/position_estimator_mc/positionKalmanFilter1D_dT.m @@ -0,0 +1,26 @@ +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 new file mode 100755 index 000000000..984bd1329 --- /dev/null +++ b/src/modules/position_estimator_mc/position_estimator_mc_main.c @@ -0,0 +1,510 @@ +/**************************************************************************** + * + * 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]))){ + orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est); + } + } + } /* 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/position_estimator_mc/position_estimator_mc_params.c b/src/modules/position_estimator_mc/position_estimator_mc_params.c new file mode 100755 index 000000000..72e5bc73b --- /dev/null +++ b/src/modules/position_estimator_mc/position_estimator_mc_params.c @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * 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> + * + * 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_mc_params.c + * + * Parameters for position_estimator_mc + */ + +#include "position_estimator_mc_params.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); + +int parameters_init(struct position_estimator_mc_param_handles *h) +{ + 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) +{ + 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; +} diff --git a/src/modules/position_estimator_mc/position_estimator_mc_params.h b/src/modules/position_estimator_mc/position_estimator_mc_params.h new file mode 100755 index 000000000..c4c95f7b4 --- /dev/null +++ b/src/modules/position_estimator_mc/position_estimator_mc_params.h @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * 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> + * + * 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_mc_params.h + * + * Parameters for Position Estimator + */ + +#include <systemlib/param/param.h> + +struct position_estimator_mc_params { + float addNoise; + float sigma; + float R; + int baro; /* consider barometer */ +}; + +struct position_estimator_mc_param_handles { + param_t addNoise; + param_t sigma; + param_t r; + param_t baro_param_handle; +}; + +/** + * Initialize all parameter handles and values + * + */ +int parameters_init(struct position_estimator_mc_param_handles *h); + +/** + * Update all parameters + * + */ +int parameters_update(const struct position_estimator_mc_param_handles *h, struct position_estimator_mc_params *p); diff --git a/src/modules/px4iofirmware/adc.c b/src/modules/px4iofirmware/adc.c new file mode 100644 index 000000000..81566eb2a --- /dev/null +++ b/src/modules/px4iofirmware/adc.c @@ -0,0 +1,172 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file adc.c + * + * Simple ADC support for PX4IO on STM32. + */ +#include <nuttx/config.h> +#include <stdint.h> + +#include <nuttx/arch.h> +#include <arch/stm32/chip.h> +#include <stm32.h> + +#include <drivers/drv_hrt.h> +#include <systemlib/perf_counter.h> + +#define DEBUG +#include "px4io.h" + +/* + * Register accessors. + * For now, no reason not to just use ADC1. + */ +#define REG(_reg) (*(volatile uint32_t *)(STM32_ADC1_BASE + _reg)) + +#define rSR REG(STM32_ADC_SR_OFFSET) +#define rCR1 REG(STM32_ADC_CR1_OFFSET) +#define rCR2 REG(STM32_ADC_CR2_OFFSET) +#define rSMPR1 REG(STM32_ADC_SMPR1_OFFSET) +#define rSMPR2 REG(STM32_ADC_SMPR2_OFFSET) +#define rJOFR1 REG(STM32_ADC_JOFR1_OFFSET) +#define rJOFR2 REG(STM32_ADC_JOFR2_OFFSET) +#define rJOFR3 REG(STM32_ADC_JOFR3_OFFSET) +#define rJOFR4 REG(STM32_ADC_JOFR4_OFFSET) +#define rHTR REG(STM32_ADC_HTR_OFFSET) +#define rLTR REG(STM32_ADC_LTR_OFFSET) +#define rSQR1 REG(STM32_ADC_SQR1_OFFSET) +#define rSQR2 REG(STM32_ADC_SQR2_OFFSET) +#define rSQR3 REG(STM32_ADC_SQR3_OFFSET) +#define rJSQR REG(STM32_ADC_JSQR_OFFSET) +#define rJDR1 REG(STM32_ADC_JDR1_OFFSET) +#define rJDR2 REG(STM32_ADC_JDR2_OFFSET) +#define rJDR3 REG(STM32_ADC_JDR3_OFFSET) +#define rJDR4 REG(STM32_ADC_JDR4_OFFSET) +#define rDR REG(STM32_ADC_DR_OFFSET) + +perf_counter_t adc_perf; + +int +adc_init(void) +{ + adc_perf = perf_alloc(PC_ELAPSED, "adc"); + + /* do calibration if supported */ +#ifdef ADC_CR2_CAL + rCR2 |= ADC_CR2_RSTCAL; + up_udelay(1); + + if (rCR2 & ADC_CR2_RSTCAL) + return -1; + + rCR2 |= ADC_CR2_CAL; + up_udelay(100); + + if (rCR2 & ADC_CR2_CAL) + return -1; + +#endif + + /* arbitrarily configure all channels for 55 cycle sample time */ + rSMPR1 = 0b00000011011011011011011011011011; + rSMPR2 = 0b00011011011011011011011011011011; + + /* XXX for F2/4, might want to select 12-bit mode? */ + rCR1 = 0; + + /* enable the temperature sensor / Vrefint channel if supported*/ + rCR2 = +#ifdef ADC_CR2_TSVREFE + /* enable the temperature sensor in CR2 */ + ADC_CR2_TSVREFE | +#endif + 0; + +#ifdef ADC_CCR_TSVREFE + /* enable temperature sensor in CCR */ + rCCR = ADC_CCR_TSVREFE; +#endif + + /* configure for a single-channel sequence */ + rSQR1 = 0; + rSQR2 = 0; + rSQR3 = 0; /* will be updated with the channel each tick */ + + /* power-cycle the ADC and turn it on */ + rCR2 &= ~ADC_CR2_ADON; + up_udelay(10); + rCR2 |= ADC_CR2_ADON; + up_udelay(10); + rCR2 |= ADC_CR2_ADON; + up_udelay(10); + + return 0; +} + +/* + return one measurement, or 0xffff on error + */ +uint16_t +adc_measure(unsigned channel) +{ + perf_begin(adc_perf); + + /* clear any previous EOC */ + if (rSR & ADC_SR_EOC) + rSR &= ~ADC_SR_EOC; + + /* run a single conversion right now - should take about 60 cycles (a few microseconds) max */ + rSQR3 = channel; + rCR2 |= ADC_CR2_ADON; + + /* wait for the conversion to complete */ + hrt_abstime now = hrt_absolute_time(); + + while (!(rSR & ADC_SR_EOC)) { + + /* never spin forever - this will give a bogus result though */ + if (hrt_elapsed_time(&now) > 100) { + debug("adc timeout"); + perf_end(adc_perf); + return 0xffff; + } + } + + /* read the result and clear EOC */ + uint16_t result = rDR; + + perf_end(adc_perf); + return result; +} diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c new file mode 100644 index 000000000..3cf9ca149 --- /dev/null +++ b/src/modules/px4iofirmware/controls.c @@ -0,0 +1,331 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file controls.c + * + * R/C inputs and servo outputs. + */ + +#include <nuttx/config.h> +#include <stdbool.h> + +#include <drivers/drv_hrt.h> +#include <systemlib/perf_counter.h> +#include <systemlib/ppm_decode.h> + +#include "px4io.h" + +#define RC_FAILSAFE_TIMEOUT 2000000 /**< two seconds failsafe timeout */ +#define RC_CHANNEL_HIGH_THRESH 5000 +#define RC_CHANNEL_LOW_THRESH -5000 + +static bool ppm_input(uint16_t *values, uint16_t *num_values); + +static perf_counter_t c_gather_dsm; +static perf_counter_t c_gather_sbus; +static perf_counter_t c_gather_ppm; + +void +controls_init(void) +{ + /* DSM input */ + dsm_init("/dev/ttyS0"); + + /* S.bus input */ + sbus_init("/dev/ttyS2"); + + /* default to a 1:1 input map, all enabled */ + for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) { + unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; + + r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = 0; + r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MIN] = 1000; + r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_CENTER] = 1500; + r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MAX] = 2000; + r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_DEADZONE] = 30; + r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_ASSIGNMENT] = i; + r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + } + + c_gather_dsm = perf_alloc(PC_ELAPSED, "c_gather_dsm"); + c_gather_sbus = perf_alloc(PC_ELAPSED, "c_gather_sbus"); + c_gather_ppm = perf_alloc(PC_ELAPSED, "c_gather_ppm"); +} + +void +controls_tick() { + + /* + * Gather R/C control inputs from supported sources. + * + * Note that if you're silly enough to connect more than + * one control input source, they're going to fight each + * other. Don't do that. + */ + + perf_begin(c_gather_dsm); + bool dsm_updated = dsm_input(r_raw_rc_values, &r_raw_rc_count); + if (dsm_updated) + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; + perf_end(c_gather_dsm); + + perf_begin(c_gather_sbus); + bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count); + if (sbus_updated) + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; + perf_end(c_gather_sbus); + + /* + * XXX each S.bus frame will cause a PPM decoder interrupt + * storm (lots of edges). It might be sensible to actually + * disable the PPM decoder completely if we have S.bus signal. + */ + perf_begin(c_gather_ppm); + bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count); + if (ppm_updated) + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; + perf_end(c_gather_ppm); + + ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS); + + /* + * In some cases we may have received a frame, but input has still + * been lost. + */ + bool rc_input_lost = false; + + /* + * If we received a new frame from any of the RC sources, process it. + */ + if (dsm_updated || sbus_updated || ppm_updated) { + + /* update RC-received timestamp */ + system_state.rc_channels_timestamp = hrt_absolute_time(); + + /* record a bitmask of channels assigned */ + unsigned assigned_channels = 0; + + /* 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; + + /* and update the scaled/mapped version */ + unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; + ASSERT(mapped < MAX_CONTROL_CHANNELS); + + /* 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; + + r_rc_values[mapped] = SIGNED_TO_REG(scaled); + assigned_channels |= (1 << mapped); + } + } + + /* set un-assigned controls to zero */ + for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) { + if (!(assigned_channels & (1 << i))) + r_rc_values[i] = 0; + } + + /* + * If we got an update with zero channels, treat it as + * a loss of input. + * + * This might happen if a protocol-based receiver returns an update + * that contains no channels that we have mapped. + */ + if (assigned_channels == 0) { + rc_input_lost = true; + } else { + /* set RC OK flag */ + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; + } + + /* + * Export the valid channel bitmap + */ + r_rc_valid = assigned_channels; + } + + /* + * If we haven't seen any new control data in 200ms, assume we + * have lost input. + */ + if (hrt_elapsed_time(&system_state.rc_channels_timestamp) > 200000) { + rc_input_lost = true; + + /* clear the input-kind flags here */ + r_status_flags &= ~( + PX4IO_P_STATUS_FLAGS_RC_PPM | + PX4IO_P_STATUS_FLAGS_RC_DSM | + PX4IO_P_STATUS_FLAGS_RC_SBUS); + } + + /* + * Handle losing RC input + */ + 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); + + /* Set the RC_LOST alarm */ + r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST; + + /* Mark the arrays as empty */ + r_raw_rc_count = 0; + r_rc_valid = 0; + } + + /* + * Check for manual override. + * + * The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag must be set, and we + * must have R/C input. + * 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)) { + + bool override = false; + + /* + * Check mapped channel 5 (can be any remote channel, + * depends on RC_MAP_OVER parameter); + * If the value is 'high' then the pilot has + * requested override. + * + */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) > RC_CHANNEL_HIGH_THRESH)) + override = true; + + if (override) { + + r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; + + /* mix new RC input control values to servos */ + if (dsm_updated || sbus_updated || ppm_updated) + mixer_tick(); + + } else { + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE; + } + } +} + +static bool +ppm_input(uint16_t *values, uint16_t *num_values) +{ + bool result = false; + + /* avoid racing with PPM updates */ + irqstate_t state = irqsave(); + + /* + * If we have received a new PPM frame within the last 200ms, accept it + * and then invalidate it. + */ + if (hrt_elapsed_time(&ppm_last_valid_decode) < 200000) { + + /* PPM data exists, copy it */ + *num_values = ppm_decoded_channels; + if (*num_values > MAX_CONTROL_CHANNELS) + *num_values = MAX_CONTROL_CHANNELS; + + for (unsigned i = 0; i < *num_values; i++) + values[i] = ppm_buffer[i]; + + /* clear validity */ + ppm_last_valid_decode = 0; + + /* good if we got any channels */ + result = (*num_values > 0); + } + + irqrestore(state); + + return result; +} diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c new file mode 100644 index 000000000..ea35e5513 --- /dev/null +++ b/src/modules/px4iofirmware/dsm.c @@ -0,0 +1,356 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file dsm.c + * + * Serial protocol decoder for the Spektrum DSM* family of protocols. + * + * Decodes into the global PPM buffer and updates accordingly. + */ + +#include <nuttx/config.h> + +#include <fcntl.h> +#include <unistd.h> +#include <termios.h> + +#include <drivers/drv_hrt.h> + +#define DEBUG + +#include "px4io.h" + +#define DSM_FRAME_SIZE 16 +#define DSM_FRAME_CHANNELS 7 + +static int dsm_fd = -1; + +static hrt_abstime last_rx_time; +static hrt_abstime last_frame_time; + +static uint8_t frame[DSM_FRAME_SIZE]; + +static unsigned partial_frame_count; +static unsigned channel_shift; + +unsigned dsm_frame_drops; + +static bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value); +static void dsm_guess_format(bool reset); +static bool dsm_decode(hrt_abstime now, uint16_t *values, uint16_t *num_values); + +int +dsm_init(const char *device) +{ + if (dsm_fd < 0) + dsm_fd = open(device, O_RDONLY | O_NONBLOCK); + + if (dsm_fd >= 0) { + struct termios t; + + /* 115200bps, no parity, one stop bit */ + tcgetattr(dsm_fd, &t); + cfsetspeed(&t, 115200); + t.c_cflag &= ~(CSTOPB | PARENB); + tcsetattr(dsm_fd, TCSANOW, &t); + + /* initialise the decoder */ + partial_frame_count = 0; + last_rx_time = hrt_absolute_time(); + + /* reset the format detector */ + dsm_guess_format(true); + + debug("DSM: ready"); + + } else { + debug("DSM: open failed"); + } + + return dsm_fd; +} + +bool +dsm_input(uint16_t *values, uint16_t *num_values) +{ + ssize_t ret; + hrt_abstime now; + + /* + * The DSM* protocol doesn't provide any explicit framing, + * so we detect frame boundaries by the inter-frame delay. + * + * The minimum frame spacing is 11ms; with 16 bytes at 115200bps + * frame transmission time is ~1.4ms. + * + * We expect to only be called when bytes arrive for processing, + * and if an interval of more than 5ms passes between calls, + * the first byte we read will be the first byte of a frame. + * + * In the case where byte(s) are dropped from a frame, this also + * provides a degree of protection. Of course, it would be better + * if we didn't drop bytes... + */ + now = hrt_absolute_time(); + + if ((now - last_rx_time) > 5000) { + if (partial_frame_count > 0) { + dsm_frame_drops++; + partial_frame_count = 0; + } + } + + /* + * Fetch bytes, but no more than we would need to complete + * the current frame. + */ + ret = read(dsm_fd, &frame[partial_frame_count], DSM_FRAME_SIZE - partial_frame_count); + + /* if the read failed for any reason, just give up here */ + if (ret < 1) + return false; + + last_rx_time = now; + + /* + * Add bytes to the current frame + */ + partial_frame_count += ret; + + /* + * If we don't have a full frame, return + */ + if (partial_frame_count < DSM_FRAME_SIZE) + return false; + + /* + * Great, it looks like we might have a frame. Go ahead and + * decode it. + */ + partial_frame_count = 0; + return dsm_decode(now, values, num_values); +} + +static bool +dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value) +{ + + if (raw == 0xffff) + return false; + + *channel = (raw >> shift) & 0xf; + + uint16_t data_mask = (1 << shift) - 1; + *value = raw & data_mask; + + //debug("DSM: %d 0x%04x -> %d %d", shift, raw, *channel, *value); + + return true; +} + +static void +dsm_guess_format(bool reset) +{ + static uint32_t cs10; + static uint32_t cs11; + static unsigned samples; + + /* reset the 10/11 bit sniffed channel masks */ + if (reset) { + cs10 = 0; + cs11 = 0; + samples = 0; + channel_shift = 0; + return; + } + + /* scan the channels in the current frame in both 10- and 11-bit mode */ + for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) { + + uint8_t *dp = &frame[2 + (2 * i)]; + uint16_t raw = (dp[0] << 8) | dp[1]; + unsigned channel, value; + + /* if the channel decodes, remember the assigned number */ + if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31)) + cs10 |= (1 << channel); + + if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31)) + cs11 |= (1 << channel); + + /* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-frame format */ + } + + /* wait until we have seen plenty of frames - 2 should normally be enough */ + if (samples++ < 5) + return; + + /* + * Iterate the set of sensible sniffed channel sets and see whether + * decoding in 10 or 11-bit mode has yielded anything we recognise. + * + * XXX Note that due to what seem to be bugs in the DSM2 high-resolution + * stream, we may want to sniff for longer in some cases when we think we + * are talking to a DSM2 receiver in high-resolution mode (so that we can + * reject it, ideally). + * See e.g. http://git.openpilot.org/cru/OPReview-116 for a discussion + * of this issue. + */ + static uint32_t masks[] = { + 0x3f, /* 6 channels (DX6) */ + 0x7f, /* 7 channels (DX7) */ + 0xff, /* 8 channels (DX8) */ + 0x1ff, /* 9 channels (DX9, etc.) */ + 0x3ff, /* 10 channels (DX10) */ + 0x3fff /* 18 channels (DX10) */ + }; + unsigned votes10 = 0; + unsigned votes11 = 0; + + for (unsigned i = 0; i < (sizeof(masks) / sizeof(masks[0])); i++) { + + if (cs10 == masks[i]) + votes10++; + + if (cs11 == masks[i]) + votes11++; + } + + if ((votes11 == 1) && (votes10 == 0)) { + channel_shift = 11; + debug("DSM: 11-bit format"); + return; + } + + if ((votes10 == 1) && (votes11 == 0)) { + channel_shift = 10; + debug("DSM: 10-bit format"); + return; + } + + /* call ourselves to reset our state ... we have to try again */ + debug("DSM: format detect fail, 10: 0x%08x %d 11: 0x%08x %d", cs10, votes10, cs11, votes11); + dsm_guess_format(true); +} + +static bool +dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) +{ + + /* + debug("DSM frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x", + frame[0], frame[1], frame[2], frame[3], frame[4], frame[5], frame[6], frame[7], + frame[8], frame[9], frame[10], frame[11], frame[12], frame[13], frame[14], frame[15]); + */ + /* + * If we have lost signal for at least a second, reset the + * format guessing heuristic. + */ + if (((frame_time - last_frame_time) > 1000000) && (channel_shift != 0)) + dsm_guess_format(true); + + /* we have received something we think is a frame */ + last_frame_time = frame_time; + + /* if we don't know the frame format, update the guessing state machine */ + if (channel_shift == 0) { + dsm_guess_format(false); + return false; + } + + /* + * The encoding of the first two bytes is uncertain, so we're + * going to ignore them for now. + * + * Each channel is a 16-bit unsigned value containing either a 10- + * or 11-bit channel value and a 4-bit channel number, shifted + * either 10 or 11 bits. The MSB may also be set to indicate the + * second frame in variants of the protocol where more than + * seven channels are being transmitted. + */ + + for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) { + + uint8_t *dp = &frame[2 + (2 * i)]; + uint16_t raw = (dp[0] << 8) | dp[1]; + unsigned channel, value; + + if (!dsm_decode_channel(raw, channel_shift, &channel, &value)) + continue; + + /* ignore channels out of range */ + if (channel >= PX4IO_INPUT_CHANNELS) + continue; + + /* update the decoded channel count */ + if (channel >= *num_values) + *num_values = channel + 1; + + /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ + if (channel_shift == 11) + value /= 2; + + value += 998; + + /* + * Store the decoded channel into the R/C input buffer, taking into + * account the different ideas about channel assignement that we have. + * + * Specifically, the first four channels in rc_channel_data are roll, pitch, thrust, yaw, + * but the first four channels from the DSM receiver are thrust, roll, pitch, yaw. + */ + switch (channel) { + case 0: + channel = 2; + break; + + case 1: + channel = 0; + break; + + case 2: + channel = 1; + + default: + break; + } + + values[channel] = value; + } + + /* + * XXX Note that we may be in failsafe here; we need to work out how to detect that. + */ + return true; +} diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c new file mode 100644 index 000000000..4485daa5b --- /dev/null +++ b/src/modules/px4iofirmware/i2c.c @@ -0,0 +1,340 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file i2c.c + * + * I2C communication for the PX4IO module. + */ + +#include <stdint.h> + +#include <nuttx/arch.h> +#include <arch/board/board.h> +#include <stm32_i2c.h> +#include <stm32_dma.h> + +//#define DEBUG +#include "px4io.h" + +/* + * I2C register definitions. + */ +#define I2C_BASE STM32_I2C1_BASE + +#define REG(_reg) (*(volatile uint32_t *)(I2C_BASE + _reg)) + +#define rCR1 REG(STM32_I2C_CR1_OFFSET) +#define rCR2 REG(STM32_I2C_CR2_OFFSET) +#define rOAR1 REG(STM32_I2C_OAR1_OFFSET) +#define rOAR2 REG(STM32_I2C_OAR2_OFFSET) +#define rDR REG(STM32_I2C_DR_OFFSET) +#define rSR1 REG(STM32_I2C_SR1_OFFSET) +#define rSR2 REG(STM32_I2C_SR2_OFFSET) +#define rCCR REG(STM32_I2C_CCR_OFFSET) +#define rTRISE REG(STM32_I2C_TRISE_OFFSET) + +static int i2c_interrupt(int irq, void *context); +static void i2c_rx_setup(void); +static void i2c_tx_setup(void); +static void i2c_rx_complete(void); +static void i2c_tx_complete(void); + +static DMA_HANDLE rx_dma; +static DMA_HANDLE tx_dma; + +static uint8_t rx_buf[68]; +static unsigned rx_len; + +static const uint8_t junk_buf[] = { 0xff, 0xff, 0xff, 0xff }; + +static const uint8_t *tx_buf = junk_buf; +static unsigned tx_len = sizeof(junk_buf); +unsigned tx_count; + +static uint8_t selected_page; +static uint8_t selected_offset; + +enum { + DIR_NONE = 0, + DIR_TX = 1, + DIR_RX = 2 +} direction; + +void +i2c_init(void) +{ + debug("i2c init"); + + /* allocate DMA handles and initialise DMA */ + rx_dma = stm32_dmachannel(DMACHAN_I2C1_RX); + i2c_rx_setup(); + tx_dma = stm32_dmachannel(DMACHAN_I2C1_TX); + i2c_tx_setup(); + + /* enable the i2c block clock and reset it */ + modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_I2C1EN); + modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_I2C1RST); + modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_I2C1RST, 0); + + /* configure the i2c GPIOs */ + stm32_configgpio(GPIO_I2C1_SCL); + stm32_configgpio(GPIO_I2C1_SDA); + + /* soft-reset the block */ + rCR1 |= I2C_CR1_SWRST; + rCR1 = 0; + + /* set for DMA operation */ + rCR2 |= I2C_CR2_ITEVFEN |I2C_CR2_ITERREN | I2C_CR2_DMAEN; + + /* set the frequency value in CR2 */ + rCR2 &= ~I2C_CR2_FREQ_MASK; + rCR2 |= STM32_PCLK1_FREQUENCY / 1000000; + + /* set divisor and risetime for fast mode */ + uint16_t result = STM32_PCLK1_FREQUENCY / (400000 * 25); + if (result < 1) + result = 1; + result = 3; + rCCR &= ~I2C_CCR_CCR_MASK; + rCCR |= I2C_CCR_DUTY | I2C_CCR_FS | result; + rTRISE = (uint16_t)((((STM32_PCLK1_FREQUENCY / 1000000) * 300) / 1000) + 1); + + /* set our device address */ + rOAR1 = 0x1a << 1; + + /* enable event interrupts */ + irq_attach(STM32_IRQ_I2C1EV, i2c_interrupt); + irq_attach(STM32_IRQ_I2C1ER, i2c_interrupt); + up_enable_irq(STM32_IRQ_I2C1EV); + up_enable_irq(STM32_IRQ_I2C1ER); + + /* and enable the I2C port */ + rCR1 |= I2C_CR1_ACK | I2C_CR1_PE; + +#ifdef DEBUG + i2c_dump(); +#endif +} + + +/* + reset the I2C bus + used to recover from lockups + */ +void i2c_reset(void) +{ + rCR1 |= I2C_CR1_SWRST; + rCR1 = 0; + + /* set for DMA operation */ + rCR2 |= I2C_CR2_ITEVFEN |I2C_CR2_ITERREN | I2C_CR2_DMAEN; + + /* set the frequency value in CR2 */ + rCR2 &= ~I2C_CR2_FREQ_MASK; + rCR2 |= STM32_PCLK1_FREQUENCY / 1000000; + + /* set divisor and risetime for fast mode */ + uint16_t result = STM32_PCLK1_FREQUENCY / (400000 * 25); + if (result < 1) + result = 1; + result = 3; + rCCR &= ~I2C_CCR_CCR_MASK; + rCCR |= I2C_CCR_DUTY | I2C_CCR_FS | result; + rTRISE = (uint16_t)((((STM32_PCLK1_FREQUENCY / 1000000) * 300) / 1000) + 1); + + /* set our device address */ + rOAR1 = 0x1a << 1; + + /* and enable the I2C port */ + rCR1 |= I2C_CR1_ACK | I2C_CR1_PE; +} + +static int +i2c_interrupt(int irq, FAR void *context) +{ + uint16_t sr1 = rSR1; + + if (sr1 & (I2C_SR1_STOPF | I2C_SR1_AF | I2C_SR1_ADDR)) { + + if (sr1 & I2C_SR1_STOPF) { + /* write to CR1 to clear STOPF */ + (void)rSR1; /* as recommended, re-read SR1 */ + rCR1 |= I2C_CR1_PE; + } + + /* DMA never stops, so we should do that now */ + switch (direction) { + case DIR_TX: + i2c_tx_complete(); + break; + case DIR_RX: + i2c_rx_complete(); + break; + default: + /* not currently transferring - must be a new txn */ + break; + } + direction = DIR_NONE; + } + + if (sr1 & I2C_SR1_ADDR) { + + /* clear ADDR to ack our selection and get direction */ + (void)rSR1; /* as recommended, re-read SR1 */ + uint16_t sr2 = rSR2; + + if (sr2 & I2C_SR2_TRA) { + /* we are the transmitter */ + + direction = DIR_TX; + + } else { + /* we are the receiver */ + + direction = DIR_RX; + } + } + + /* clear any errors that might need it (this handles AF as well */ + if (sr1 & I2C_SR1_ERRORMASK) + rSR1 = 0; + + return 0; +} + +static void +i2c_rx_setup(void) +{ + /* + * Note that we configure DMA in circular mode; this means that a too-long + * transfer will overwrite the buffer, but that avoids us having to deal with + * bailing out of a transaction while the master is still babbling at us. + */ + rx_len = 0; + stm32_dmasetup(rx_dma, (uintptr_t)&rDR, (uintptr_t)&rx_buf[0], sizeof(rx_buf), + DMA_CCR_CIRC | + DMA_CCR_MINC | + DMA_CCR_PSIZE_32BITS | + DMA_CCR_MSIZE_8BITS | + DMA_CCR_PRIMED); + + stm32_dmastart(rx_dma, NULL, NULL, false); +} + +static void +i2c_rx_complete(void) +{ + rx_len = sizeof(rx_buf) - stm32_dmaresidual(rx_dma); + stm32_dmastop(rx_dma); + + if (rx_len >= 2) { + selected_page = rx_buf[0]; + selected_offset = rx_buf[1]; + + /* work out how many registers are being written */ + unsigned count = (rx_len - 2) / 2; + if (count > 0) { + registers_set(selected_page, selected_offset, (const uint16_t *)&rx_buf[2], count); + } else { + /* no registers written, must be an address cycle */ + uint16_t *regs; + unsigned reg_count; + + /* work out which registers are being addressed */ + int ret = registers_get(selected_page, selected_offset, ®s, ®_count); + if (ret == 0) { + tx_buf = (uint8_t *)regs; + tx_len = reg_count * 2; + } else { + tx_buf = junk_buf; + tx_len = sizeof(junk_buf); + } + + /* disable interrupts while reconfiguring DMA for the selected registers */ + irqstate_t flags = irqsave(); + + stm32_dmastop(tx_dma); + i2c_tx_setup(); + + irqrestore(flags); + } + } + + /* prepare for the next transaction */ + i2c_rx_setup(); +} + +static void +i2c_tx_setup(void) +{ + /* + * Note that we configure DMA in circular mode; this means that a too-long + * transfer will copy the buffer more than once, but that avoids us having + * to deal with bailing out of a transaction while the master is still + * babbling at us. + */ + stm32_dmasetup(tx_dma, (uintptr_t)&rDR, (uintptr_t)&tx_buf[0], tx_len, + DMA_CCR_DIR | + DMA_CCR_CIRC | + DMA_CCR_MINC | + DMA_CCR_PSIZE_8BITS | + DMA_CCR_MSIZE_8BITS | + DMA_CCR_PRIMED); + + stm32_dmastart(tx_dma, NULL, NULL, false); +} + +static void +i2c_tx_complete(void) +{ + tx_count = tx_len - stm32_dmaresidual(tx_dma); + stm32_dmastop(tx_dma); + + /* for debug purposes, save the length of the last transmit as seen by the DMA */ + + /* leave tx_buf/tx_len alone, so that a retry will see the same data */ + + /* prepare for the next transaction */ + i2c_tx_setup(); +} + +void +i2c_dump(void) +{ + debug("CR1 0x%08x CR2 0x%08x", rCR1, rCR2); + debug("OAR1 0x%08x OAR2 0x%08x", rOAR1, rOAR2); + debug("CCR 0x%08x TRISE 0x%08x", rCCR, rTRISE); + debug("SR1 0x%08x SR2 0x%08x", rSR1, rSR2); +} diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp new file mode 100644 index 000000000..a2193b526 --- /dev/null +++ b/src/modules/px4iofirmware/mixer.cpp @@ -0,0 +1,370 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mixer.cpp + * + * Control channel input/output mixer and failsafe. + */ + +#include <nuttx/config.h> +#include <syslog.h> + +#include <sys/types.h> +#include <stdbool.h> +#include <string.h> + +#include <drivers/drv_pwm_output.h> +#include <drivers/drv_hrt.h> + +#include <systemlib/mixer/mixer.h> + +extern "C" { +//#define DEBUG +#include "px4io.h" +} + +/* + * Maximum interval in us before FMU signal is considered lost + */ +#define FMU_INPUT_DROP_LIMIT_US 200000 + +/* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */ +#define ROLL 0 +#define PITCH 1 +#define YAW 2 +#define THROTTLE 3 +#define OVERRIDE 4 + +/* current servo arm/disarm state */ +static bool mixer_servos_armed = false; + +/* selected control values and count for mixing */ +enum mixer_source { + MIX_NONE, + MIX_FMU, + MIX_OVERRIDE, + MIX_FAILSAFE +}; +static mixer_source source; + +static int mixer_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &control); + +static MixerGroup mixer_group(mixer_callback, 0); + +/* Set the failsafe values of all mixed channels (based on zero throttle, controls centered) */ +static void mixer_set_failsafe(); + +void +mixer_tick(void) +{ + /* check that we are receiving fresh data from the FMU */ + if (hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { + + /* too long without FMU input, time to go to failsafe */ + if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { + isr_debug(1, "AP RX timeout"); + } + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM); + r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST; + + } else { + r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; + } + + /* default to failsafe mixing */ + source = MIX_FAILSAFE; + + /* + * Decide which set of controls we're using. + */ + + /* do not mix if mixer is invalid or if RAW_PWM mode is on and FMU is good */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) && + !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { + + /* don't actually mix anything - we already have raw PWM values or + not a valid mixer. */ + source = MIX_NONE; + + } else { + + if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { + + /* mix from FMU controls */ + source = MIX_FMU; + } + + if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { + + /* if allowed, mix from RC inputs directly */ + source = MIX_OVERRIDE; + } + } + + /* + * Set failsafe status flag depending on mixing source + */ + if (source == MIX_FAILSAFE) { + r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE; + } else { + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE); + } + + /* + * Run the mixers. + */ + if (source == MIX_FAILSAFE) { + + /* copy failsafe values to the servo outputs */ + for (unsigned i = 0; i < IO_SERVO_COUNT; i++) { + r_page_servos[i] = r_page_servo_failsafe[i]; + + /* safe actuators for FMU feedback */ + r_page_actuators[i] = (r_page_servos[i] - 1500) / 600.0f; + } + + + } else if (source != MIX_NONE) { + + float outputs[IO_SERVO_COUNT]; + unsigned mixed; + + /* mix */ + mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT); + + /* scale to PWM and update the servo outputs as required */ + for (unsigned i = 0; i < mixed; i++) { + + /* save actuator values for FMU readback */ + r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); + + /* scale to servo output */ + r_page_servos[i] = (outputs[i] * 600.0f) + 1500; + + } + for (unsigned i = mixed; i < IO_SERVO_COUNT; i++) + r_page_servos[i] = 0; + } + + /* + * Decide whether the servos should be armed right now. + * + * We must be armed, and we must have a PWM source; either raw from + * FMU or from the mixer. + * + * XXX correct behaviour for failsafe may require an additional case + * here. + */ + bool should_arm = ( + /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && + /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && + /* there is valid input via direct PWM or mixer */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) && + /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) && + /* FMU is available or FMU is not available but override is an option */ + ((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) )) + ); + + if (should_arm && !mixer_servos_armed) { + /* need to arm, but not armed */ + up_pwm_servo_arm(true); + mixer_servos_armed = true; + + } else if (!should_arm && mixer_servos_armed) { + /* armed but need to disarm */ + up_pwm_servo_arm(false); + mixer_servos_armed = false; + } + + if (mixer_servos_armed) { + /* update the servo outputs. */ + for (unsigned i = 0; i < IO_SERVO_COUNT; i++) + up_pwm_servo_set(i, r_page_servos[i]); + } +} + +static int +mixer_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &control) +{ + if (control_group != 0) + return -1; + + switch (source) { + case MIX_FMU: + if (control_index < PX4IO_CONTROL_CHANNELS) { + control = REG_TO_FLOAT(r_page_controls[control_index]); + break; + } + return -1; + + case MIX_OVERRIDE: + if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << control_index)) { + control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); + break; + } + return -1; + + case MIX_FAILSAFE: + case MIX_NONE: + control = 0.0f; + return -1; + } + + return 0; +} + +/* + * XXX error handling here should be more aggressive; currently it is + * possible to get STATUS_FLAGS_MIXER_OK set even though the mixer has + * not loaded faithfully. + */ + +static char mixer_text[256]; /* large enough for one mixer */ +static unsigned mixer_text_length = 0; + +void +mixer_handle_text(const void *buffer, size_t length) +{ + /* do not allow a mixer change while fully armed */ + if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && + /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + return; + } + + px4io_mixdata *msg = (px4io_mixdata *)buffer; + + isr_debug(2, "mix txt %u", length); + + if (length < sizeof(px4io_mixdata)) + return; + + unsigned text_length = length - sizeof(px4io_mixdata); + + switch (msg->action) { + case F2I_MIXER_ACTION_RESET: + isr_debug(2, "reset"); + + /* FIRST mark the mixer as invalid */ + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; + /* THEN actually delete it */ + mixer_group.reset(); + mixer_text_length = 0; + + /* FALLTHROUGH */ + case F2I_MIXER_ACTION_APPEND: + isr_debug(2, "append %d", length); + + /* check for overflow - this would be really fatal */ + if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) { + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; + return; + } + + /* append mixer text and nul-terminate */ + memcpy(&mixer_text[mixer_text_length], msg->text, text_length); + mixer_text_length += text_length; + mixer_text[mixer_text_length] = '\0'; + isr_debug(2, "buflen %u", mixer_text_length); + + /* process the text buffer, adding new mixers as their descriptions can be parsed */ + unsigned resid = mixer_text_length; + mixer_group.load_from_buf(&mixer_text[0], resid); + + /* if anything was parsed */ + if (resid != mixer_text_length) { + + /* only set mixer ok if no residual is left over */ + if (resid == 0) { + r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK; + } else { + /* not yet reached the end of the mixer, set as not ok */ + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; + } + + isr_debug(2, "used %u", mixer_text_length - resid); + + /* copy any leftover text to the base of the buffer for re-use */ + if (resid > 0) + memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid); + + mixer_text_length = resid; + + /* update failsafe values */ + mixer_set_failsafe(); + } + + break; + } +} + +static void +mixer_set_failsafe() +{ + /* + * Check if a custom failsafe value has been written, + * or if the mixer is not ok and bail out. + */ + if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) || + !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) + return; + + /* set failsafe defaults to the values for all inputs = 0 */ + float outputs[IO_SERVO_COUNT]; + unsigned mixed; + + /* mix */ + mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT); + + /* scale to PWM and update the servo outputs as required */ + for (unsigned i = 0; i < mixed; i++) { + + /* scale to servo output */ + r_page_servo_failsafe[i] = (outputs[i] * 600.0f) + 1500; + + } + + /* disable the rest of the outputs */ + for (unsigned i = mixed; i < IO_SERVO_COUNT; i++) + r_page_servo_failsafe[i] = 0; + +} diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk new file mode 100644 index 000000000..6379366f4 --- /dev/null +++ b/src/modules/px4iofirmware/module.mk @@ -0,0 +1,19 @@ + + +SRCS = adc.c \ + controls.c \ + dsm.c \ + i2c.c \ + px4io.c \ + registers.c \ + safety.c \ + sbus.c \ + ../systemlib/up_cxxinitialize.c \ + ../systemlib/hx_stream.c \ + ../systemlib/perf_counter.c \ + mixer.cpp \ + ../systemlib/mixer/mixer.cpp \ + ../systemlib/mixer/mixer_group.cpp \ + ../systemlib/mixer/mixer_multirotor.cpp \ + ../systemlib/mixer/mixer_simple.cpp \ +
\ No newline at end of file diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h new file mode 100644 index 000000000..674f9dddd --- /dev/null +++ b/src/modules/px4iofirmware/protocol.h @@ -0,0 +1,204 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +/** + * @file protocol.h + * + * PX4IO I2C interface protocol. + * + * Communication is performed via writes to and reads from 16-bit virtual + * registers organised into pages of 255 registers each. + * + * The first two bytes of each write select a page and offset address + * respectively. Subsequent reads and writes increment the offset within + * the page. + * + * Most pages are readable or writable but not both. + * + * Note that some pages may permit offset values greater than 255, which + * can only be achieved by long writes. The offset does not wrap. + * + * Writes to unimplemented registers are ignored. Reads from unimplemented + * registers return undefined values. + * + * As convention, values that would be floating point in other parts of + * the PX4 system are expressed as signed integer values scaled by 10000, + * e.g. control values range from -10000..10000. Use the REG_TO_SIGNED and + * SIGNED_TO_REG macros to convert between register representation and + * the signed version, and REG_TO_FLOAT/FLOAT_TO_REG to convert to float. + * + * Note that the implementation of readable pages prefers registers within + * readable pages to be densely packed. Page numbers do not need to be + * packed. + */ + +#define PX4IO_CONTROL_CHANNELS 8 +#define PX4IO_INPUT_CHANNELS 12 +#define PX4IO_RELAY_CHANNELS 4 + +/* Per C, this is safe for all 2's complement systems */ +#define REG_TO_SIGNED(_reg) ((int16_t)(_reg)) +#define SIGNED_TO_REG(_signed) ((uint16_t)(_signed)) + +#define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f) +#define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f)) + +/* static configuration page */ +#define PX4IO_PAGE_CONFIG 0 +#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers TBD */ +#define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers TBD */ +#define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */ +#define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */ +#define PX4IO_P_CONFIG_CONTROL_COUNT 4 /* hardcoded max control count supported */ +#define PX4IO_P_CONFIG_ACTUATOR_COUNT 5 /* hardcoded max actuator output count */ +#define PX4IO_P_CONFIG_RC_INPUT_COUNT 6 /* hardcoded max R/C input count supported */ +#define PX4IO_P_CONFIG_ADC_INPUT_COUNT 7 /* hardcoded max ADC inputs */ +#define PX4IO_P_CONFIG_RELAY_COUNT 8 /* hardcoded # of relay outputs */ + +/* dynamic status page */ +#define PX4IO_PAGE_STATUS 1 +#define PX4IO_P_STATUS_FREEMEM 0 +#define PX4IO_P_STATUS_CPULOAD 1 + +#define PX4IO_P_STATUS_FLAGS 2 /* monitoring flags */ +#define PX4IO_P_STATUS_FLAGS_ARMED (1 << 0) /* arm-ok and locally armed */ +#define PX4IO_P_STATUS_FLAGS_OVERRIDE (1 << 1) /* in manual override */ +#define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 2) /* RC input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 3) /* PPM input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_DSM (1 << 4) /* DSM input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_SBUS (1 << 5) /* SBUS input is valid */ +#define PX4IO_P_STATUS_FLAGS_FMU_OK (1 << 6) /* controls from FMU are valid */ +#define PX4IO_P_STATUS_FLAGS_RAW_PWM (1 << 7) /* raw PWM from FMU is bypassing the mixer */ +#define PX4IO_P_STATUS_FLAGS_MIXER_OK (1 << 8) /* mixer is OK */ +#define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */ +#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */ +#define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */ + +#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ +#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* VBatt is very close to regulator dropout */ +#define PX4IO_P_STATUS_ALARMS_TEMPERATURE (1 << 1) /* board temperature is high */ +#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT (1 << 2) /* servo current limit was exceeded */ +#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT (1 << 3) /* accessory current limit was exceeded */ +#define PX4IO_P_STATUS_ALARMS_FMU_LOST (1 << 4) /* timed out waiting for controls from FMU */ +#define PX4IO_P_STATUS_ALARMS_RC_LOST (1 << 5) /* timed out waiting for RC input */ +#define PX4IO_P_STATUS_ALARMS_PWM_ERROR (1 << 6) /* PWM configuration or output was bad */ + +#define PX4IO_P_STATUS_VBATT 4 /* battery voltage in mV */ +#define PX4IO_P_STATUS_IBATT 5 /* battery current (raw ADC) */ + +/* array of post-mix actuator outputs, -10000..10000 */ +#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* array of PWM servo output values, microseconds */ +#define PX4IO_PAGE_SERVOS 3 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* array of raw RC input values, microseconds */ +#define PX4IO_PAGE_RAW_RC_INPUT 4 +#define PX4IO_P_RAW_RC_COUNT 0 /* number of valid channels */ +#define PX4IO_P_RAW_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT channels from here */ + +/* array of scaled RC input values, -10000..10000 */ +#define PX4IO_PAGE_RC_INPUT 5 +#define PX4IO_P_RC_VALID 0 /* bitmask of valid controls */ +#define PX4IO_P_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT controls from here */ + +/* array of raw ADC values */ +#define PX4IO_PAGE_RAW_ADC_INPUT 6 /* 0..CONFIG_ADC_INPUT_COUNT-1 */ + +/* PWM servo information */ +#define PX4IO_PAGE_PWM_INFO 7 +#define PX4IO_RATE_MAP_BASE 0 /* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */ + +/* setup page */ +#define PX4IO_PAGE_SETUP 100 +#define PX4IO_P_SETUP_FEATURES 0 + +#define PX4IO_P_SETUP_ARMING 1 /* arming controls */ +#define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */ +#define PX4IO_P_SETUP_ARMING_FMU_ARMED (1 << 1) /* FMU is already armed */ +#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 2) /* OK to switch to manual override via override RC channel */ +#define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM (1 << 3) /* use custom failsafe values, not 0 values of mixer */ +#define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 4) /* OK to try in-air restart */ + +#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ +#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ +#define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */ +#define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */ +#define PX4IO_P_SETUP_VBATT_SCALE 6 /* battery voltage correction factor (float) */ +#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ + +/* autopilot control values, -10000..10000 */ +#define PX4IO_PAGE_CONTROLS 101 /* 0..CONFIG_CONTROL_COUNT */ + +/* raw text load to the mixer parser - ignores offset */ +#define PX4IO_PAGE_MIXERLOAD 102 + +/* R/C channel config */ +#define PX4IO_PAGE_RC_CONFIG 103 /* R/C input configuration */ +#define PX4IO_P_RC_CONFIG_MIN 0 /* lowest input value */ +#define PX4IO_P_RC_CONFIG_CENTER 1 /* center input value */ +#define PX4IO_P_RC_CONFIG_MAX 2 /* highest input value */ +#define PX4IO_P_RC_CONFIG_DEADZONE 3 /* band around center that is ignored */ +#define PX4IO_P_RC_CONFIG_ASSIGNMENT 4 /* mapped input value */ +#define PX4IO_P_RC_CONFIG_OPTIONS 5 /* channel options bitmask */ +#define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED (1 << 0) +#define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE (1 << 1) +#define PX4IO_P_RC_CONFIG_STRIDE 6 /* spacing between channel config data */ + +/* PWM output - overrides mixer */ +#define PX4IO_PAGE_DIRECT_PWM 104 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* PWM failsafe values - zero disables the output */ +#define PX4IO_PAGE_FAILSAFE_PWM 105 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + +/** + * As-needed mixer data upload. + * + * This message adds text to the mixer text buffer; the text + * buffer is drained as the definitions are consumed. + */ +#pragma pack(push, 1) +struct px4io_mixdata { + uint16_t f2i_mixer_magic; +#define F2I_MIXER_MAGIC 0x6d74 + + uint8_t action; +#define F2I_MIXER_ACTION_RESET 0 +#define F2I_MIXER_ACTION_APPEND 1 + + char text[0]; /* actual text size may vary */ +}; +#pragma pack(pop) + diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c new file mode 100644 index 000000000..bc8dfc116 --- /dev/null +++ b/src/modules/px4iofirmware/px4io.c @@ -0,0 +1,237 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4io.c + * Top-level logic for the PX4IO module. + */ + +#include <nuttx/config.h> + +#include <stdio.h> // required for task_create +#include <stdbool.h> +#include <stdlib.h> +#include <errno.h> +#include <string.h> +#include <poll.h> +#include <signal.h> + +#include <drivers/drv_pwm_output.h> +#include <drivers/drv_hrt.h> + +#include <systemlib/perf_counter.h> + +#include <stm32_uart.h> + +#define DEBUG +#include "px4io.h" + +__EXPORT int user_start(int argc, char *argv[]); + +extern void up_cxxinitialize(void); + +struct sys_state_s system_state; + +static struct hrt_call serial_dma_call; + +#ifdef CONFIG_STM32_I2C1 +/* store i2c reset count XXX this should be a register, together with other error counters */ +volatile uint32_t i2c_loop_resets = 0; +#endif + +/* + * a set of debug buffers to allow us to send debug information from ISRs + */ + +static volatile uint32_t msg_counter; +static volatile uint32_t last_msg_counter; +static volatile uint8_t msg_next_out, msg_next_in; + +/* + * WARNING: too large buffers here consume the memory required + * for mixer handling. Do not allocate more than 80 bytes for + * output. + */ +#define NUM_MSG 2 +static char msg[NUM_MSG][40]; + +/* + * add a debug message to be printed on the console + */ +void +isr_debug(uint8_t level, const char *fmt, ...) +{ + if (level > r_page_setup[PX4IO_P_SETUP_SET_DEBUG]) { + return; + } + va_list ap; + va_start(ap, fmt); + vsnprintf(msg[msg_next_in], sizeof(msg[0]), fmt, ap); + va_end(ap); + msg_next_in = (msg_next_in+1) % NUM_MSG; + msg_counter++; +} + +/* + * show all pending debug messages + */ +static void +show_debug_messages(void) +{ + if (msg_counter != last_msg_counter) { + uint32_t n = msg_counter - last_msg_counter; + if (n > NUM_MSG) n = NUM_MSG; + last_msg_counter = msg_counter; + while (n--) { + debug("%s", msg[msg_next_out]); + msg_next_out = (msg_next_out+1) % NUM_MSG; + } + } +} + +int +user_start(int argc, char *argv[]) +{ + /* run C++ ctors before we go any further */ + up_cxxinitialize(); + + /* reset all to zero */ + memset(&system_state, 0, sizeof(system_state)); + + /* configure the high-resolution time/callout interface */ + hrt_init(); + + /* + * Poll at 1ms intervals for received bytes that have not triggered + * a DMA event. + */ +#ifdef CONFIG_ARCH_DMA + hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); +#endif + + /* print some startup info */ + lowsyslog("\nPX4IO: starting\n"); + + /* default all the LEDs to off while we start */ + LED_AMBER(false); + LED_BLUE(false); + LED_SAFETY(false); + + /* turn on servo power */ + POWER_SERVO(true); + + /* start the safety switch handler */ + safety_init(); + + /* configure the first 8 PWM outputs (i.e. all of them) */ + up_pwm_servo_init(0xff); + + /* initialise the control inputs */ + controls_init(); + +#ifdef CONFIG_STM32_I2C1 + /* start the i2c handler */ + i2c_init(); +#endif + + /* add a performance counter for mixing */ + perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); + + /* add a performance counter for controls */ + perf_counter_t controls_perf = perf_alloc(PC_ELAPSED, "controls"); + + /* and one for measuring the loop rate */ + perf_counter_t loop_perf = perf_alloc(PC_INTERVAL, "loop"); + + struct mallinfo minfo = mallinfo(); + lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks); + +#if 0 + /* not enough memory, lock down */ + if (minfo.mxordblk < 500) { + lowsyslog("ERR: not enough MEM"); + bool phase = false; + + if (phase) { + LED_AMBER(true); + LED_BLUE(false); + } else { + LED_AMBER(false); + LED_BLUE(true); + } + + phase = !phase; + usleep(300000); + } +#endif + + /* + * Run everything in a tight loop. + */ + + uint64_t last_debug_time = 0; + for (;;) { + + /* track the rate at which the loop is running */ + perf_count(loop_perf); + + /* kick the mixer */ + perf_begin(mixer_perf); + mixer_tick(); + perf_end(mixer_perf); + + /* kick the control inputs */ + perf_begin(controls_perf); + controls_tick(); + perf_end(controls_perf); + + /* check for debug activity */ + show_debug_messages(); + + /* post debug state at ~1Hz */ + if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) { + + struct mallinfo minfo = mallinfo(); + + isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u m=%u", + (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG], + (unsigned)r_status_flags, + (unsigned)r_setup_arming, + (unsigned)r_setup_features, + (unsigned)i2c_loop_resets, + (unsigned)minfo.mxordblk); + last_debug_time = hrt_absolute_time(); + } + } +} + diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h new file mode 100644 index 000000000..272cdb7bf --- /dev/null +++ b/src/modules/px4iofirmware/px4io.h @@ -0,0 +1,199 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4io.h + * + * General defines and structures for the PX4IO module firmware. + */ + +#include <nuttx/config.h> + +#include <stdbool.h> +#include <stdint.h> + +#include <drivers/boards/px4io/px4io_internal.h> + +#include "protocol.h" + +/* + * Constants and limits. + */ +#define MAX_CONTROL_CHANNELS 12 +#define IO_SERVO_COUNT 8 + +/* + * Debug logging + */ + +#ifdef DEBUG +# include <debug.h> +# define debug(fmt, args...) lowsyslog(fmt "\n", ##args) +#else +# define debug(fmt, args...) do {} while(0) +#endif + +/* + * Registers. + */ +extern uint16_t r_page_status[]; /* PX4IO_PAGE_STATUS */ +extern uint16_t r_page_actuators[]; /* PX4IO_PAGE_ACTUATORS */ +extern uint16_t r_page_servos[]; /* PX4IO_PAGE_SERVOS */ +extern uint16_t r_page_raw_rc_input[]; /* PX4IO_PAGE_RAW_RC_INPUT */ +extern uint16_t r_page_rc_input[]; /* PX4IO_PAGE_RC_INPUT */ +extern uint16_t r_page_adc[]; /* PX4IO_PAGE_RAW_ADC_INPUT */ + +extern volatile uint16_t r_page_setup[]; /* PX4IO_PAGE_SETUP */ +extern volatile uint16_t r_page_controls[]; /* PX4IO_PAGE_CONTROLS */ +extern uint16_t r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */ +extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */ + +/* + * Register aliases. + * + * Handy aliases for registers that are widely used. + */ +#define r_status_flags r_page_status[PX4IO_P_STATUS_FLAGS] +#define r_status_alarms r_page_status[PX4IO_P_STATUS_ALARMS] + +#define r_raw_rc_count r_page_raw_rc_input[PX4IO_P_RAW_RC_COUNT] +#define r_raw_rc_values (&r_page_raw_rc_input[PX4IO_P_RAW_RC_BASE]) +#define r_rc_valid r_page_rc_input[PX4IO_P_RC_VALID] +#define r_rc_values (&r_page_rc_input[PX4IO_P_RAW_RC_BASE]) + +#define r_setup_features r_page_setup[PX4IO_P_SETUP_FEATURES] +#define r_setup_arming r_page_setup[PX4IO_P_SETUP_ARMING] +#define r_setup_pwm_rates r_page_setup[PX4IO_P_SETUP_PWM_RATES] +#define r_setup_pwm_defaultrate r_page_setup[PX4IO_P_SETUP_PWM_DEFAULTRATE] +#define r_setup_pwm_altrate r_page_setup[PX4IO_P_SETUP_PWM_ALTRATE] +#define r_setup_relays r_page_setup[PX4IO_P_SETUP_RELAYS] + +#define r_control_values (&r_page_controls[0]) + +/* + * System state structure. + */ +struct sys_state_s { + + volatile uint64_t rc_channels_timestamp; + + /** + * Last FMU receive time, in microseconds since system boot + */ + volatile uint64_t fmu_data_received_time; + +}; + +extern struct sys_state_s system_state; + +/* + * GPIO handling. + */ +#define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED1, !(_s)) +#define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s)) +#define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s)) + +#define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s)) +#ifdef GPIO_ACC1_PWR_EN + #define POWER_ACC1(_s) stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s)) +#endif +#ifdef GPIO_ACC2_PWR_EN + #define POWER_ACC2(_s) stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s)) +#endif +#ifdef GPIO_RELAY1_EN + #define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s)) +#endif +#ifdef GPIO_RELAY2_EN + #define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s)) +#endif + +#define OVERCURRENT_ACC (!stm32_gpioread(GPIO_ACC_OC_DETECT)) +#define OVERCURRENT_SERVO (!stm32_gpioread(GPIO_SERVO_OC_DETECT)) +#define BUTTON_SAFETY stm32_gpioread(GPIO_BTN_SAFETY) + +#define ADC_VBATT 4 +#define ADC_IN5 5 +#define ADC_CHANNEL_COUNT 2 + +/* + * Mixer + */ +extern void mixer_tick(void); +extern void mixer_handle_text(const void *buffer, size_t length); + +/** + * Safety switch/LED. + */ +extern void safety_init(void); + +#ifdef CONFIG_STM32_I2C1 +/** + * FMU communications + */ +extern void i2c_init(void); +#endif + +/** + * Register space + */ +extern void registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); +extern int registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values); + +/** + * Sensors/misc inputs + */ +extern int adc_init(void); +extern uint16_t adc_measure(unsigned channel); + +/** + * R/C receiver handling. + * + * Input functions return true when they receive an update from the RC controller. + */ +extern void controls_init(void); +extern void controls_tick(void); +extern int dsm_init(const char *device); +extern bool dsm_input(uint16_t *values, uint16_t *num_values); +extern int sbus_init(const char *device); +extern bool sbus_input(uint16_t *values, uint16_t *num_values); + +/** global debug level for isr_debug() */ +extern volatile uint8_t debug_level; + +/* send a debug message to the console */ +extern void isr_debug(uint8_t level, const char *fmt, ...); + +#ifdef CONFIG_STM32_I2C1 +void i2c_dump(void); +void i2c_reset(void); +#endif diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c new file mode 100644 index 000000000..df7d6dcd3 --- /dev/null +++ b/src/modules/px4iofirmware/registers.c @@ -0,0 +1,647 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file registers.c + * + * Implementation of the PX4IO register space. + */ + +#include <nuttx/config.h> + +#include <stdbool.h> +#include <stdlib.h> +#include <string.h> + +#include <drivers/drv_hrt.h> + +#include "px4io.h" +#include "protocol.h" + +static int registers_set_one(uint8_t page, uint8_t offset, uint16_t value); +static void pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate); + +/** + * PAGE 0 + * + * Static configuration parameters. + */ +static const uint16_t r_page_config[] = { + [PX4IO_P_CONFIG_PROTOCOL_VERSION] = 1, /* XXX hardcoded magic number */ + [PX4IO_P_CONFIG_SOFTWARE_VERSION] = 1, /* XXX hardcoded magic number */ + [PX4IO_P_CONFIG_BOOTLOADER_VERSION] = 3, /* XXX hardcoded magic number */ + [PX4IO_P_CONFIG_MAX_TRANSFER] = 64, /* XXX hardcoded magic number */ + [PX4IO_P_CONFIG_CONTROL_COUNT] = PX4IO_CONTROL_CHANNELS, + [PX4IO_P_CONFIG_ACTUATOR_COUNT] = IO_SERVO_COUNT, + [PX4IO_P_CONFIG_RC_INPUT_COUNT] = MAX_CONTROL_CHANNELS, + [PX4IO_P_CONFIG_ADC_INPUT_COUNT] = ADC_CHANNEL_COUNT, + [PX4IO_P_CONFIG_RELAY_COUNT] = PX4IO_RELAY_CHANNELS, +}; + +/** + * PAGE 1 + * + * Status values. + */ +uint16_t r_page_status[] = { + [PX4IO_P_STATUS_FREEMEM] = 0, + [PX4IO_P_STATUS_CPULOAD] = 0, + [PX4IO_P_STATUS_FLAGS] = 0, + [PX4IO_P_STATUS_ALARMS] = 0, + [PX4IO_P_STATUS_VBATT] = 0, + [PX4IO_P_STATUS_IBATT] = 0 +}; + +/** + * PAGE 2 + * + * Post-mixed actuator values. + */ +uint16_t r_page_actuators[IO_SERVO_COUNT]; + +/** + * PAGE 3 + * + * Servo PWM values + */ +uint16_t r_page_servos[IO_SERVO_COUNT]; + +/** + * PAGE 4 + * + * Raw RC input + */ +uint16_t r_page_raw_rc_input[] = +{ + [PX4IO_P_RAW_RC_COUNT] = 0, + [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + MAX_CONTROL_CHANNELS)] = 0 +}; + +/** + * PAGE 5 + * + * Scaled/routed RC input + */ +uint16_t r_page_rc_input[] = { + [PX4IO_P_RC_VALID] = 0, + [PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + MAX_CONTROL_CHANNELS)] = 0 +}; + +/** + * Scratch page; used for registers that are constructed as-read. + * + * PAGE 6 Raw ADC input. + * PAGE 7 PWM rate maps. + */ +uint16_t r_page_scratch[32]; + +/** + * PAGE 100 + * + * Setup registers + */ +volatile uint16_t r_page_setup[] = +{ + [PX4IO_P_SETUP_FEATURES] = 0, + [PX4IO_P_SETUP_ARMING] = 0, + [PX4IO_P_SETUP_PWM_RATES] = 0, + [PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50, + [PX4IO_P_SETUP_PWM_ALTRATE] = 200, + [PX4IO_P_SETUP_RELAYS] = 0, + [PX4IO_P_SETUP_VBATT_SCALE] = 10000, + [PX4IO_P_SETUP_SET_DEBUG] = 0, +}; + +#define PX4IO_P_SETUP_FEATURES_VALID (0) +#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \ + PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \ + PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \ + PX4IO_P_SETUP_ARMING_IO_ARM_OK) +#define PX4IO_P_SETUP_RATES_VALID ((1 << IO_SERVO_COUNT) - 1) +#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) + +/** + * PAGE 101 + * + * Control values from the FMU. + */ +volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS]; + +/* + * PAGE 102 does not have a buffer. + */ + +/** + * PAGE 103 + * + * R/C channel input configuration. + */ +uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE]; + +/* valid options */ +#define PX4IO_P_RC_CONFIG_OPTIONS_VALID (PX4IO_P_RC_CONFIG_OPTIONS_REVERSE | PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) + +/* + * PAGE 104 uses r_page_servos. + */ + +/** + * PAGE 105 + * + * Failsafe servo PWM values + * + * Disable pulses as default. + */ +uint16_t r_page_servo_failsafe[IO_SERVO_COUNT] = { 0 }; + +void +registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) +{ + + switch (page) { + + /* handle bulk controls input */ + case PX4IO_PAGE_CONTROLS: + + /* copy channel data */ + while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) { + + /* XXX range-check value? */ + r_page_controls[offset] = *values; + + offset++; + num_values--; + values++; + } + + system_state.fmu_data_received_time = hrt_absolute_time(); + r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PWM; + + break; + + /* handle raw PWM input */ + case PX4IO_PAGE_DIRECT_PWM: + + /* copy channel data */ + while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) { + + /* XXX range-check value? */ + r_page_servos[offset] = *values; + + offset++; + num_values--; + values++; + } + + system_state.fmu_data_received_time = hrt_absolute_time(); + r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM; + + break; + + /* handle setup for servo failsafe values */ + case PX4IO_PAGE_FAILSAFE_PWM: + + /* copy channel data */ + while ((offset < IO_SERVO_COUNT) && (num_values > 0)) { + + /* XXX range-check value? */ + r_page_servo_failsafe[offset] = *values; + + /* flag the failsafe values as custom */ + r_setup_arming |= PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM; + + offset++; + num_values--; + values++; + } + break; + + /* handle text going to the mixer parser */ + case PX4IO_PAGE_MIXERLOAD: + mixer_handle_text(values, num_values * sizeof(*values)); + break; + + default: + /* avoid offset wrap */ + if ((offset + num_values) > 255) + num_values = 255 - offset; + + /* iterate individual registers, set each in turn */ + while (num_values--) { + if (registers_set_one(page, offset, *values)) + break; + offset++; + values++; + } + } +} + +static int +registers_set_one(uint8_t page, uint8_t offset, uint16_t value) +{ + switch (page) { + + case PX4IO_PAGE_STATUS: + switch (offset) { + case PX4IO_P_STATUS_ALARMS: + /* clear bits being written */ + r_status_alarms &= ~value; + break; + + case PX4IO_P_STATUS_FLAGS: + /* + * Allow FMU override of arming state (to allow in-air restores), + * but only if the arming state is not in sync on the IO side. + */ + if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { + r_status_flags = value; + } + break; + + default: + /* just ignore writes to other registers in this page */ + break; + } + break; + + case PX4IO_PAGE_SETUP: + switch (offset) { + case PX4IO_P_SETUP_FEATURES: + + value &= PX4IO_P_SETUP_FEATURES_VALID; + r_setup_features = value; + + /* no implemented feature selection at this point */ + + break; + + case PX4IO_P_SETUP_ARMING: + + value &= PX4IO_P_SETUP_ARMING_VALID; + + /* + * Update arming state - disarm if no longer OK. + * This builds on the requirement that the FMU driver + * asks about the FMU arming state on initialization, + * so that an in-air reset of FMU can not lead to a + * lockup of the IO arming state. + */ + if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; + } + + r_setup_arming = value; + + break; + + case PX4IO_P_SETUP_PWM_RATES: + value &= PX4IO_P_SETUP_RATES_VALID; + pwm_configure_rates(value, r_setup_pwm_defaultrate, r_setup_pwm_altrate); + break; + + case PX4IO_P_SETUP_PWM_DEFAULTRATE: + if (value < 50) + value = 50; + if (value > 400) + value = 400; + pwm_configure_rates(r_setup_pwm_rates, value, r_setup_pwm_altrate); + break; + + case PX4IO_P_SETUP_PWM_ALTRATE: + if (value < 50) + value = 50; + if (value > 400) + value = 400; + pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value); + break; + + case PX4IO_P_SETUP_RELAYS: + value &= PX4IO_P_SETUP_RELAYS_VALID; + r_setup_relays = value; + POWER_RELAY1(value & (1 << 0) ? 1 : 0); + POWER_RELAY2(value & (1 << 1) ? 1 : 0); + POWER_ACC1(value & (1 << 2) ? 1 : 0); + POWER_ACC2(value & (1 << 3) ? 1 : 0); + break; + + case PX4IO_P_SETUP_SET_DEBUG: + r_page_setup[PX4IO_P_SETUP_SET_DEBUG] = value; + isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]); + break; + + default: + return -1; + } + break; + + case PX4IO_PAGE_RC_CONFIG: { + + /* do not allow a RC config change while fully armed */ + if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && + /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + break; + } + + unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE; + unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE; + uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE]; + + if (channel >= MAX_CONTROL_CHANNELS) + return -1; + + /* disable the channel until we have a chance to sanity-check it */ + conf[PX4IO_P_RC_CONFIG_OPTIONS] &= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + + switch (index) { + + case PX4IO_P_RC_CONFIG_MIN: + case PX4IO_P_RC_CONFIG_CENTER: + case PX4IO_P_RC_CONFIG_MAX: + case PX4IO_P_RC_CONFIG_DEADZONE: + case PX4IO_P_RC_CONFIG_ASSIGNMENT: + conf[index] = value; + break; + + case PX4IO_P_RC_CONFIG_OPTIONS: + value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID; + r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; + + /* set all options except the enabled option */ + conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + + /* should the channel be enabled? */ + /* this option is normally set last */ + if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { + uint8_t count = 0; + + /* assert min..center..max ordering */ + if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) { + count++; + } + if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) { + count++; + } + if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) { + count++; + } + if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) { + count++; + } + /* assert deadzone is sane */ + if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) { + count++; + } + if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) { + count++; + } + + /* sanity checks pass, enable channel */ + if (count) { + isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1)); + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK; + } else { + conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + } + } + break; + /* inner switch: case PX4IO_P_RC_CONFIG_OPTIONS */ + + } + break; + /* case PX4IO_RC_PAGE_CONFIG */ + } + + default: + return -1; + } + return 0; +} + +uint8_t last_page; +uint8_t last_offset; + +int +registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values) +{ +#define SELECT_PAGE(_page_name) \ + do { \ + *values = &_page_name[0]; \ + *num_values = sizeof(_page_name) / sizeof(_page_name[0]); \ + } while(0) + + switch (page) { + + /* + * Handle pages that are updated dynamically at read time. + */ + case PX4IO_PAGE_STATUS: + /* PX4IO_P_STATUS_FREEMEM */ + { + struct mallinfo minfo = mallinfo(); + r_page_status[PX4IO_P_STATUS_FREEMEM] = minfo.fordblks; + } + + /* XXX PX4IO_P_STATUS_CPULOAD */ + + /* PX4IO_P_STATUS_FLAGS maintained externally */ + + /* PX4IO_P_STATUS_ALARMS maintained externally */ + + /* PX4IO_P_STATUS_VBATT */ + { + /* + * Coefficients here derived by measurement of the 5-16V + * range on one unit: + * + * V counts + * 5 1001 + * 6 1219 + * 7 1436 + * 8 1653 + * 9 1870 + * 10 2086 + * 11 2303 + * 12 2522 + * 13 2738 + * 14 2956 + * 15 3172 + * 16 3389 + * + * slope = 0.0046067 + * intercept = 0.3863 + * + * Intercept corrected for best results @ 12V. + */ + unsigned counts = adc_measure(ADC_VBATT); + if (counts != 0xffff) { + unsigned mV = (4150 + (counts * 46)) / 10 - 200; + unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000; + + r_page_status[PX4IO_P_STATUS_VBATT] = corrected; + } + } + + /* PX4IO_P_STATUS_IBATT */ + { + /* + note that we have no idea what sort of + current sensor is attached, so we just + return the raw 12 bit ADC value and let the + FMU sort it out, with user selectable + configuration for their sensor + */ + unsigned counts = adc_measure(ADC_IN5); + if (counts != 0xffff) { + r_page_status[PX4IO_P_STATUS_IBATT] = counts; + } + } + + SELECT_PAGE(r_page_status); + break; + + case PX4IO_PAGE_RAW_ADC_INPUT: + memset(r_page_scratch, 0, sizeof(r_page_scratch)); + r_page_scratch[0] = adc_measure(ADC_VBATT); + r_page_scratch[1] = adc_measure(ADC_IN5); + + SELECT_PAGE(r_page_scratch); + break; + + case PX4IO_PAGE_PWM_INFO: + memset(r_page_scratch, 0, sizeof(r_page_scratch)); + for (unsigned i = 0; i < IO_SERVO_COUNT; i++) + r_page_scratch[PX4IO_RATE_MAP_BASE + i] = up_pwm_servo_get_rate_group(i); + + SELECT_PAGE(r_page_scratch); + break; + + /* + * Pages that are just a straight read of the register state. + */ + + /* status pages */ + case PX4IO_PAGE_CONFIG: + SELECT_PAGE(r_page_config); + break; + case PX4IO_PAGE_ACTUATORS: + SELECT_PAGE(r_page_actuators); + break; + case PX4IO_PAGE_SERVOS: + SELECT_PAGE(r_page_servos); + break; + case PX4IO_PAGE_RAW_RC_INPUT: + SELECT_PAGE(r_page_raw_rc_input); + break; + case PX4IO_PAGE_RC_INPUT: + SELECT_PAGE(r_page_rc_input); + break; + + /* readback of input pages */ + case PX4IO_PAGE_SETUP: + SELECT_PAGE(r_page_setup); + break; + case PX4IO_PAGE_CONTROLS: + SELECT_PAGE(r_page_controls); + break; + case PX4IO_PAGE_RC_CONFIG: + SELECT_PAGE(r_page_rc_input_config); + break; + case PX4IO_PAGE_DIRECT_PWM: + SELECT_PAGE(r_page_servos); + break; + case PX4IO_PAGE_FAILSAFE_PWM: + SELECT_PAGE(r_page_servo_failsafe); + break; + + default: + return -1; + } + +#undef SELECT_PAGE +#undef COPY_PAGE + +last_page = page; +last_offset = offset; + + /* if the offset is at or beyond the end of the page, we have no data */ + if (offset >= *num_values) + return -1; + + /* correct the data pointer and count for the offset */ + *values += offset; + *num_values -= offset; + + return 0; +} + +/* + * Helper function to handle changes to the PWM rate control registers. + */ +static void +pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate) +{ + for (unsigned pass = 0; pass < 2; pass++) { + for (unsigned group = 0; group < IO_SERVO_COUNT; group++) { + + /* get the channel mask for this rate group */ + uint32_t mask = up_pwm_servo_get_rate_group(group); + if (mask == 0) + continue; + + /* all channels in the group must be either default or alt-rate */ + uint32_t alt = map & mask; + + if (pass == 0) { + /* preflight */ + if ((alt != 0) && (alt != mask)) { + /* not a legal map, bail with an alarm */ + r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR; + return; + } + } else { + /* set it - errors here are unexpected */ + if (alt != 0) { + if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_altrate) != OK) + r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR; + } else { + if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_defaultrate) != OK) + r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR; + } + } + } + } + r_setup_pwm_rates = map; + r_setup_pwm_defaultrate = defaultrate; + r_setup_pwm_altrate = altrate; +} diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c new file mode 100644 index 000000000..4dbecc274 --- /dev/null +++ b/src/modules/px4iofirmware/safety.c @@ -0,0 +1,195 @@ +/**************************************************************************** + * + * 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 safety.c + * Safety button logic. + */ + +#include <nuttx/config.h> + +#include <stdbool.h> + +#include <drivers/drv_hrt.h> + +#include "px4io.h" + +static struct hrt_call arming_call; +static struct hrt_call heartbeat_call; +static struct hrt_call failsafe_call; + +/* + * Count the number of times in a row that we see the arming button + * held down. + */ +static unsigned counter = 0; + +/* + * Define the various LED flash sequences for each system state. + */ +#define LED_PATTERN_FMU_OK_TO_ARM 0x0003 /**< slow blinking */ +#define LED_PATTERN_FMU_REFUSE_TO_ARM 0x5555 /**< fast blinking */ +#define LED_PATTERN_IO_ARMED 0x5050 /**< long off, then double blink */ +#define LED_PATTERN_FMU_ARMED 0x5500 /**< long off, then quad blink */ +#define LED_PATTERN_IO_FMU_ARMED 0xffff /**< constantly on */ + +static unsigned blink_counter = 0; + +/* + * IMPORTANT: The arming state machine critically + * depends on using the same threshold + * for arming and disarming. Since disarming + * is quite deadly for the system, a similar + * length can be justified. + */ +#define ARM_COUNTER_THRESHOLD 10 + +static bool safety_button_pressed; + +static void safety_check_button(void *arg); +static void heartbeat_blink(void *arg); +static void failsafe_blink(void *arg); + +void +safety_init(void) +{ + /* arrange for the button handler to be called at 10Hz */ + hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL); + + /* arrange for the heartbeat handler to be called at 4Hz */ + hrt_call_every(&heartbeat_call, 1000, 250000, heartbeat_blink, NULL); + + /* arrange for the failsafe blinker to be called at 8Hz */ + hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL); +} + +static void +safety_check_button(void *arg) +{ + /* + * Debounce the safety button, change state if it has been held for long enough. + * + */ + safety_button_pressed = BUTTON_SAFETY; + + /* + * Keep pressed for a while to arm. + * + * Note that the counting sequence has to be same length + * for arming / disarming in order to end up as proper + * state machine, keep ARM_COUNTER_THRESHOLD the same + * length in all cases of the if/else struct below. + */ + if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && + (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) { + + if (counter < ARM_COUNTER_THRESHOLD) { + counter++; + + } else if (counter == ARM_COUNTER_THRESHOLD) { + /* switch to armed state */ + r_status_flags |= PX4IO_P_STATUS_FLAGS_ARMED; + counter++; + } + + } else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + + if (counter < ARM_COUNTER_THRESHOLD) { + counter++; + + } else if (counter == ARM_COUNTER_THRESHOLD) { + /* change to disarmed state and notify the FMU */ + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; + counter++; + } + + } else { + counter = 0; + } + + /* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */ + uint16_t pattern = LED_PATTERN_FMU_REFUSE_TO_ARM; + + if (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) { + if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) { + pattern = LED_PATTERN_IO_FMU_ARMED; + + } else { + pattern = LED_PATTERN_IO_ARMED; + } + + } else if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) { + pattern = LED_PATTERN_FMU_ARMED; + } else if (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) { + pattern = LED_PATTERN_FMU_OK_TO_ARM; + + } + + /* Turn the LED on if we have a 1 at the current bit position */ + LED_SAFETY(pattern & (1 << blink_counter++)); + + if (blink_counter > 15) { + blink_counter = 0; + } +} + +static void +heartbeat_blink(void *arg) +{ + static bool heartbeat = false; + + /* XXX add flags here that need to be frobbed by various loops */ + + LED_BLUE(heartbeat = !heartbeat); +} + +static void +failsafe_blink(void *arg) +{ + /* indicate that a serious initialisation error occured */ + if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)) { + LED_AMBER(true); + return; + } + + static bool failsafe = false; + + /* blink the failsafe LED if we don't have FMU input */ + if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { + failsafe = !failsafe; + } else { + failsafe = false; + } + + LED_AMBER(failsafe); +}
\ No newline at end of file diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c new file mode 100644 index 000000000..073ddeaba --- /dev/null +++ b/src/modules/px4iofirmware/sbus.c @@ -0,0 +1,255 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file sbus.c + * + * Serial protocol decoder for the Futaba S.bus protocol. + */ + +#include <nuttx/config.h> + +#include <fcntl.h> +#include <unistd.h> +#include <termios.h> + +#include <systemlib/ppm_decode.h> + +#include <drivers/drv_hrt.h> + +#define DEBUG +#include "px4io.h" +#include "protocol.h" +#include "debug.h" + +#define SBUS_FRAME_SIZE 25 +#define SBUS_INPUT_CHANNELS 16 + +static int sbus_fd = -1; + +static hrt_abstime last_rx_time; +static hrt_abstime last_frame_time; + +static uint8_t frame[SBUS_FRAME_SIZE]; + +static unsigned partial_frame_count; + +unsigned sbus_frame_drops; + +static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values); + +int +sbus_init(const char *device) +{ + if (sbus_fd < 0) + sbus_fd = open(device, O_RDONLY | O_NONBLOCK); + + if (sbus_fd >= 0) { + struct termios t; + + /* 100000bps, even parity, two stop bits */ + tcgetattr(sbus_fd, &t); + cfsetspeed(&t, 100000); + t.c_cflag |= (CSTOPB | PARENB); + tcsetattr(sbus_fd, TCSANOW, &t); + + /* initialise the decoder */ + partial_frame_count = 0; + last_rx_time = hrt_absolute_time(); + + debug("S.Bus: ready"); + + } else { + debug("S.Bus: open failed"); + } + + return sbus_fd; +} + +bool +sbus_input(uint16_t *values, uint16_t *num_values) +{ + ssize_t ret; + hrt_abstime now; + + /* + * The S.bus protocol doesn't provide reliable framing, + * so we detect frame boundaries by the inter-frame delay. + * + * The minimum frame spacing is 7ms; with 25 bytes at 100000bps + * frame transmission time is ~2ms. + * + * We expect to only be called when bytes arrive for processing, + * and if an interval of more than 3ms passes between calls, + * the first byte we read will be the first byte of a frame. + * + * In the case where byte(s) are dropped from a frame, this also + * provides a degree of protection. Of course, it would be better + * if we didn't drop bytes... + */ + now = hrt_absolute_time(); + + if ((now - last_rx_time) > 3000) { + if (partial_frame_count > 0) { + sbus_frame_drops++; + partial_frame_count = 0; + } + } + + /* + * Fetch bytes, but no more than we would need to complete + * the current frame. + */ + ret = read(sbus_fd, &frame[partial_frame_count], SBUS_FRAME_SIZE - partial_frame_count); + + /* if the read failed for any reason, just give up here */ + if (ret < 1) + return false; + + last_rx_time = now; + + /* + * Add bytes to the current frame + */ + partial_frame_count += ret; + + /* + * If we don't have a full frame, return + */ + if (partial_frame_count < SBUS_FRAME_SIZE) + return false; + + /* + * Great, it looks like we might have a frame. Go ahead and + * decode it. + */ + partial_frame_count = 0; + return sbus_decode(now, values, num_values); +} + +/* + * S.bus decoder matrix. + * + * Each channel value can come from up to 3 input bytes. Each row in the + * matrix describes up to three bytes, and each entry gives: + * + * - byte offset in the data portion of the frame + * - right shift applied to the data byte + * - mask for the data byte + * - left shift applied to the result into the channel value + */ +struct sbus_bit_pick { + uint8_t byte; + uint8_t rshift; + uint8_t mask; + uint8_t lshift; +}; +static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { + /* 0 */ { { 0, 0, 0xff, 0}, { 1, 0, 0x07, 8}, { 0, 0, 0x00, 0} }, + /* 1 */ { { 1, 3, 0x1f, 0}, { 2, 0, 0x3f, 5}, { 0, 0, 0x00, 0} }, + /* 2 */ { { 2, 6, 0x03, 0}, { 3, 0, 0xff, 2}, { 4, 0, 0x01, 10} }, + /* 3 */ { { 4, 1, 0x7f, 0}, { 5, 0, 0x0f, 7}, { 0, 0, 0x00, 0} }, + /* 4 */ { { 5, 4, 0x0f, 0}, { 6, 0, 0x7f, 4}, { 0, 0, 0x00, 0} }, + /* 5 */ { { 6, 7, 0x01, 0}, { 7, 0, 0xff, 1}, { 8, 0, 0x03, 9} }, + /* 6 */ { { 8, 2, 0x3f, 0}, { 9, 0, 0x1f, 6}, { 0, 0, 0x00, 0} }, + /* 7 */ { { 9, 5, 0x07, 0}, {10, 0, 0xff, 3}, { 0, 0, 0x00, 0} }, + /* 8 */ { {11, 0, 0xff, 0}, {12, 0, 0x07, 8}, { 0, 0, 0x00, 0} }, + /* 9 */ { {12, 3, 0x1f, 0}, {13, 0, 0x3f, 5}, { 0, 0, 0x00, 0} }, + /* 10 */ { {13, 6, 0x03, 0}, {14, 0, 0xff, 2}, {15, 0, 0x01, 10} }, + /* 11 */ { {15, 1, 0x7f, 0}, {16, 0, 0x0f, 7}, { 0, 0, 0x00, 0} }, + /* 12 */ { {16, 4, 0x0f, 0}, {17, 0, 0x7f, 4}, { 0, 0, 0x00, 0} }, + /* 13 */ { {17, 7, 0x01, 0}, {18, 0, 0xff, 1}, {19, 0, 0x03, 9} }, + /* 14 */ { {19, 2, 0x3f, 0}, {20, 0, 0x1f, 6}, { 0, 0, 0x00, 0} }, + /* 15 */ { {20, 5, 0x07, 0}, {21, 0, 0xff, 3}, { 0, 0, 0x00, 0} } +}; + +static bool +sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) +{ + /* check frame boundary markers to avoid out-of-sync cases */ + if ((frame[0] != 0x0f) || (frame[24] != 0x00)) { + sbus_frame_drops++; + return false; + } + + /* if the failsafe or connection lost bit is set, we consider the frame invalid */ + if ((frame[23] & (1 << 2)) && /* signal lost */ + (frame[23] & (1 << 3))) { /* failsafe */ + + /* actively announce signal loss */ + *values = 0; + return false; + } + + /* we have received something we think is a frame */ + last_frame_time = frame_time; + + unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ? + SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS; + + /* use the decoder matrix to extract channel data */ + for (unsigned channel = 0; channel < chancount; channel++) { + unsigned value = 0; + + for (unsigned pick = 0; pick < 3; pick++) { + const struct sbus_bit_pick *decode = &sbus_decoder[channel][pick]; + + if (decode->mask != 0) { + unsigned piece = frame[1 + decode->byte]; + piece >>= decode->rshift; + piece &= decode->mask; + piece <<= decode->lshift; + + value |= piece; + } + } + + /* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ + values[channel] = (value / 2) + 998; + } + + /* decode switch channels if data fields are wide enough */ + if (PX4IO_INPUT_CHANNELS > 17 && chancount > 15) { + chancount = 18; + + /* channel 17 (index 16) */ + values[16] = (frame[23] & (1 << 0)) * 1000 + 998; + /* channel 18 (index 17) */ + values[17] = (frame[23] & (1 << 1)) * 1000 + 998; + } + + /* note the number of channels decoded */ + *num_values = chancount; + + return true; +} diff --git a/src/modules/sdlog/module.mk b/src/modules/sdlog/module.mk new file mode 100644 index 000000000..89da2d827 --- /dev/null +++ b/src/modules/sdlog/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# 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 new file mode 100644 index 000000000..c22523bf2 --- /dev/null +++ b/src/modules/sdlog/sdlog.c @@ -0,0 +1,840 @@ +/**************************************************************************** + * + * 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 new file mode 100644 index 000000000..d7c8a4759 --- /dev/null +++ b/src/modules/sdlog/sdlog_ringbuffer.c @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * 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 new file mode 100644 index 000000000..b65916459 --- /dev/null +++ b/src/modules/sdlog/sdlog_ringbuffer.h @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * 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/logbuffer.c b/src/modules/sdlog2/logbuffer.c new file mode 100644 index 000000000..b3243f7b5 --- /dev/null +++ b/src/modules/sdlog2/logbuffer.c @@ -0,0 +1,134 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * 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 + * 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 logbuffer.c + * + * Ring FIFO buffer for binary log data. + * + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#include <string.h> +#include <stdlib.h> + +#include "logbuffer.h" + +int logbuffer_init(struct logbuffer_s *lb, int size) +{ + lb->size = size; + lb->write_ptr = 0; + lb->read_ptr = 0; + lb->data = malloc(lb->size); + return (lb->data == 0) ? ERROR : OK; +} + +int logbuffer_count(struct logbuffer_s *lb) +{ + int n = lb->write_ptr - lb->read_ptr; + + if (n < 0) { + n += lb->size; + } + + return n; +} + +int logbuffer_is_empty(struct logbuffer_s *lb) +{ + return lb->read_ptr == lb->write_ptr; +} + +bool logbuffer_write(struct logbuffer_s *lb, void *ptr, int size) +{ + // bytes available to write + int available = lb->read_ptr - lb->write_ptr - 1; + + if (available < 0) + available += lb->size; + + if (size > available) { + // buffer overflow + return false; + } + + char *c = (char *) ptr; + int n = lb->size - lb->write_ptr; // bytes to end of the buffer + + if (n < size) { + // message goes over end of the buffer + memcpy(&(lb->data[lb->write_ptr]), c, n); + lb->write_ptr = 0; + + } else { + n = 0; + } + + // now: n = bytes already written + int p = size - n; // number of bytes to write + memcpy(&(lb->data[lb->write_ptr]), &(c[n]), p); + lb->write_ptr = (lb->write_ptr + p) % lb->size; + return true; +} + +int logbuffer_get_ptr(struct logbuffer_s *lb, void **ptr, bool *is_part) +{ + // bytes available to read + int available = lb->write_ptr - lb->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 = lb->size - lb->read_ptr; + *is_part = lb->write_ptr > 0; + } + + *ptr = &(lb->data[lb->read_ptr]); + return n; +} + +void logbuffer_mark_read(struct logbuffer_s *lb, int n) +{ + lb->read_ptr = (lb->read_ptr + n) % lb->size; +} diff --git a/src/modules/sdlog2/logbuffer.h b/src/modules/sdlog2/logbuffer.h new file mode 100644 index 000000000..3a5e3a29f --- /dev/null +++ b/src/modules/sdlog2/logbuffer.h @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * 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 + * 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 logbuffer.h + * + * Ring FIFO buffer for binary log data. + * + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#ifndef SDLOG2_RINGBUFFER_H_ +#define SDLOG2_RINGBUFFER_H_ + +#include <stdbool.h> + +struct logbuffer_s { + // pointers and size are in bytes + int write_ptr; + int read_ptr; + int size; + char *data; +}; + +int logbuffer_init(struct logbuffer_s *lb, int size); + +int logbuffer_count(struct logbuffer_s *lb); + +int logbuffer_is_empty(struct logbuffer_s *lb); + +bool logbuffer_write(struct logbuffer_s *lb, void *ptr, int size); + +int logbuffer_get_ptr(struct logbuffer_s *lb, void **ptr, bool *is_part); + +void logbuffer_mark_read(struct logbuffer_s *lb, int n); + +#endif diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk new file mode 100644 index 000000000..f53129688 --- /dev/null +++ b/src/modules/sdlog2/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# sdlog2 Application +# + +MODULE_COMMAND = sdlog2 +# The main thread only buffers to RAM, needs a high priority +MODULE_PRIORITY = "SCHED_PRIORITY_MAX-30" + +SRCS = sdlog2.c \ + logbuffer.c diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c new file mode 100644 index 000000000..3e6b20472 --- /dev/null +++ b/src/modules/sdlog2/sdlog2.c @@ -0,0 +1,1272 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> + * 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 + * 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 sdlog2.c + * + * Simple SD logger for flight data. Buffers new sensor values and + * does the heavy SD I/O in a low-priority worker thread. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#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/vehicle_status.h> +#include <uORB/topics/sensor_combined.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/vehicle_rates_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_local_position_setpoint.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 <uORB/topics/rc_channels.h> +#include <uORB/topics/esc_status.h> + +#include <systemlib/systemlib.h> + +#include <mavlink/mavlink_log.h> + +#include "logbuffer.h" +#include "sdlog2_format.h" +#include "sdlog2_messages.h" + +#define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \ + log_msgs_written++; \ + } else { \ + log_msgs_skipped++; \ + /*printf("skip\n");*/ \ + } + +#define LOG_ORB_SUBSCRIBE(_var, _topic) subs.##_var##_sub = orb_subscribe(ORB_ID(##_topic##)); \ + fds[fdsc_count].fd = subs.##_var##_sub; \ + fds[fdsc_count].events = POLLIN; \ + fdsc_count++; + + +//#define SDLOG2_DEBUG + +static bool main_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 bool logwriter_should_exit = false; /**< Logwriter thread exit flag */ +static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log folders */ +static const int MAX_NO_LOGFILE = 999; /**< Maximum number of log files */ +static const int LOG_BUFFER_SIZE_DEFAULT = 8192; +static const int MAX_WRITE_CHUNK = 512; +static const int MIN_BYTES_TO_WRITE = 512; + +static const char *mountpoint = "/fs/microsd"; +static int mavlink_fd = -1; +struct logbuffer_s lb; + +/* mutex / condition to synchronize threads */ +static pthread_mutex_t logbuffer_mutex; +static pthread_cond_t logbuffer_cond; + +static char folder_path[64]; + +/* statistics counters */ +static unsigned long log_bytes_written = 0; +static uint64_t start_time = 0; +static unsigned long log_msgs_written = 0; +static unsigned long log_msgs_skipped = 0; + +/* current state of logging */ +static bool logging_enabled = false; +/* enable logging on start (-e option) */ +static bool log_on_start = false; +/* enable logging when armed (-a option) */ +static bool log_when_armed = false; +/* delay = 1 / rate (rate defined by -r option) */ +static useconds_t sleep_delay = 0; + +/* helper flag to track system state changes */ +static bool flag_system_armed = false; + +static pthread_t logwriter_pthread = 0; + +/** + * Log buffer writing thread. Open and close file here. + */ +static void *logwriter_thread(void *arg); + +/** + * SD log management function. + */ +__EXPORT int sdlog2_main(int argc, char *argv[]); + +/** + * Mainloop of sd log deamon. + */ +int sdlog2_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static void sdlog2_usage(const char *reason); + +/** + * Print the current status. + */ +static void sdlog2_status(void); + +/** + * Start logging: create new file and start log writer thread. + */ +static void sdlog2_start_log(void); + +/** + * Stop logging: stop log writer thread and close log file. + */ +static void sdlog2_stop_log(void); + +/** + * Write a header to log file: list of message formats. + */ +static void write_formats(int fd); + + +static bool 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); + +static void handle_status(struct vehicle_status_s *cmd); + +/** + * Create folder for current logging session. Store folder name in 'log_folder'. + */ +static int create_logfolder(void); + +/** + * Select first free log file name and open it. + */ +static int open_logfile(void); + +static void +sdlog2_usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + + errx(1, "usage: sdlog2 {start|stop|status} [-r <log rate>] [-b <buffer size>] -e -a\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"); +} + +/** + * The logger 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(). + */ +int sdlog2_main(int argc, char *argv[]) +{ + if (argc < 2) + sdlog2_usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + printf("sdlog2 already running\n"); + /* this is not an error */ + exit(0); + } + + main_thread_should_exit = false; + deamon_task = task_spawn_cmd("sdlog2", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT - 30, + 3000, + sdlog2_thread_main, + (const char **)argv); + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + if (!thread_running) { + printf("\tsdlog2 is not started\n"); + } + + main_thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + sdlog2_status(); + + } else { + printf("\tsdlog2 not started\n"); + } + + exit(0); + } + + sdlog2_usage("unrecognized command"); + exit(1); +} + +int create_logfolder() +{ + /* make folder on sdcard */ + uint16_t folder_number = 1; // start with folder sess001 + int mkdir_ret; + + /* look for the next folder that does not exist */ + while (folder_number <= MAX_NO_LOGFOLDER) { + /* set up folder path: e.g. /fs/microsd/sess001 */ + sprintf(folder_path, "%s/sess%03u", mountpoint, folder_number); + 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 */ + break; + + } else if (mkdir_ret == -1) { + /* folder exists already */ + folder_number++; + continue; + + } else { + warn("failed creating new folder"); + return -1; + } + } + + if (folder_number >= MAX_NO_LOGFOLDER) { + /* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */ + warnx("all %d possible folders exist already.", MAX_NO_LOGFOLDER); + return -1; + } + + return 0; +} + +int open_logfile() +{ + /* make folder on sdcard */ + uint16_t file_number = 1; // start with file log001 + + /* string to hold the path to the log */ + char path_buf[64] = ""; + + int fd = 0; + + /* look for the next file that does not exist */ + while (file_number <= MAX_NO_LOGFILE) { + /* set up file path: e.g. /fs/microsd/sess001/log001.bin */ + sprintf(path_buf, "%s/log%03u.bin", folder_path, file_number); + + if (file_exist(path_buf)) { + file_number++; + continue; + } + + fd = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC); + + if (fd == 0) { + warn("opening %s failed", path_buf); + } + + warnx("logging to: %s.", path_buf); + mavlink_log_info(mavlink_fd, "[sdlog2] log: %s", path_buf); + + return fd; + } + + if (file_number > MAX_NO_LOGFILE) { + /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ + warnx("all %d possible files exist already.", MAX_NO_LOGFILE); + return -1; + } + + return 0; +} + +static void *logwriter_thread(void *arg) +{ + /* set name */ + prctl(PR_SET_NAME, "sdlog2_writer", 0); + + struct logbuffer_s *logbuf = (struct logbuffer_s *)arg; + + int log_file = open_logfile(); + + /* write log messages formats */ + write_formats(log_file); + + int poll_count = 0; + + void *read_ptr; + int n = 0; + bool should_wait = false; + bool is_part = false; + + while (true) { + /* make sure threads are synchronized */ + pthread_mutex_lock(&logbuffer_mutex); + + /* update read pointer if needed */ + if (n > 0) { + logbuffer_mark_read(&lb, n); + } + + /* only wait if no data is available to process */ + if (should_wait && !logwriter_should_exit) { + /* blocking wait for new data at this line */ + pthread_cond_wait(&logbuffer_cond, &logbuffer_mutex); + } + + /* only get pointer to thread-safe data, do heavy I/O a few lines down */ + int available = logbuffer_get_ptr(logbuf, &read_ptr, &is_part); + +#ifdef SDLOG2_DEBUG + int rp = logbuf->read_ptr; + int wp = logbuf->write_ptr; +#endif + + /* continue */ + pthread_mutex_unlock(&logbuffer_mutex); + + if (available > 0) { + /* do heavy IO here */ + if (available > MAX_WRITE_CHUNK) { + n = MAX_WRITE_CHUNK; + + } else { + n = available; + } + + n = write(log_file, read_ptr, n); + + should_wait = (n == available) && !is_part; +#ifdef SDLOG2_DEBUG + printf("write %i %i of %i rp=%i wp=%i, is_part=%i, should_wait=%i\n", log_bytes_written, n, available, rp, wp, (int)is_part, (int)should_wait); +#endif + + if (n < 0) { + main_thread_should_exit = true; + err(1, "error writing log file"); + } + + if (n > 0) { + log_bytes_written += n; + } + + } else { + n = 0; +#ifdef SDLOG2_DEBUG + printf("no data available, main_thread_should_exit=%i, logwriter_should_exit=%i\n", (int)main_thread_should_exit, (int)logwriter_should_exit); +#endif + /* exit only with empty buffer */ + if (main_thread_should_exit || logwriter_should_exit) { +#ifdef SDLOG2_DEBUG + printf("break logwriter thread\n"); +#endif + break; + } + should_wait = true; + } + + if (++poll_count == 10) { + fsync(log_file); + poll_count = 0; + } + } + + fsync(log_file); + close(log_file); + +#ifdef SDLOG2_DEBUG + printf("logwriter thread exit\n"); +#endif + + return OK; +} + +void sdlog2_start_log() +{ + warnx("start logging."); + mavlink_log_info(mavlink_fd, "[sdlog2] start logging"); + + /* initialize statistics counter */ + log_bytes_written = 0; + start_time = hrt_absolute_time(); + log_msgs_written = 0; + log_msgs_skipped = 0; + + /* initialize log buffer emptying thread */ + 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); + + logwriter_should_exit = false; + + /* start log buffer emptying thread */ + if (0 != pthread_create(&logwriter_pthread, &receiveloop_attr, logwriter_thread, &lb)) { + errx(1, "error creating logwriter thread"); + } + + logging_enabled = true; + // XXX we have to destroy the attr at some point +} + +void sdlog2_stop_log() +{ + warnx("stop logging."); + mavlink_log_info(mavlink_fd, "[sdlog2] stop logging"); + + logging_enabled = false; + + /* wake up write thread one last time */ + pthread_mutex_lock(&logbuffer_mutex); + logwriter_should_exit = true; + pthread_cond_signal(&logbuffer_cond); + /* unlock, now the writer thread may return */ + pthread_mutex_unlock(&logbuffer_mutex); + + /* wait for write thread to return */ + int ret; + if ((ret = pthread_join(logwriter_pthread, NULL)) != 0) { + warnx("error joining logwriter thread: %i", ret); + } + logwriter_pthread = 0; + + sdlog2_status(); +} + + +void write_formats(int fd) +{ + /* construct message format packet */ + struct { + LOG_PACKET_HEADER; + struct log_format_s body; + } log_format_packet = { + LOG_PACKET_HEADER_INIT(LOG_FORMAT_MSG), + }; + + /* fill message format packet for each format and write to log */ + int i; + + for (i = 0; i < log_formats_num; i++) { + log_format_packet.body = log_formats[i]; + log_bytes_written += write(fd, &log_format_packet, sizeof(log_format_packet)); + } + + fsync(fd); +} + +int sdlog2_thread_main(int argc, char *argv[]) +{ + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + if (mavlink_fd < 0) { + warnx("failed to open MAVLink log stream, start mavlink app first."); + } + + /* log buffer size */ + int log_buffer_size = LOG_BUFFER_SIZE_DEFAULT; + + /* work around some stupidity in task_create's argv handling */ + argc -= 2; + argv += 2; + int ch; + + while ((ch = getopt(argc, argv, "r:b:ea")) != EOF) { + switch (ch) { + case 'r': { + unsigned long r = strtoul(optarg, NULL, 10); + + if (r == 0) { + sleep_delay = 0; + + } else { + sleep_delay = 1000000 / r; + } + } + break; + + case 'b': { + unsigned long s = strtoul(optarg, NULL, 10); + + if (s < 1) { + s = 1; + } + + log_buffer_size = 1024 * s; + } + break; + + case 'e': + log_on_start = true; + break; + + case 'a': + log_when_armed = true; + break; + + case '?': + if (optopt == 'c') { + warnx("Option -%c requires an argument.", optopt); + + } else if (isprint(optopt)) { + warnx("Unknown option `-%c'.", optopt); + + } else { + warnx("Unknown option character `\\x%x'.", optopt); + } + + default: + sdlog2_usage("unrecognized flag"); + errx(1, "exiting."); + } + } + + if (!file_exist(mountpoint)) { + errx(1, "logging mount point %s not present, exiting.", mountpoint); + } + + if (create_logfolder()) { + errx(1, "unable to create logging folder, exiting."); + } + + /* only print logging path, important to find log file later */ + warnx("logging to directory: %s", folder_path); + + /* initialize log buffer with specified size */ + warnx("log buffer size: %i bytes.", log_buffer_size); + + if (OK != logbuffer_init(&lb, log_buffer_size)) { + errx(1, "can't allocate log buffer, exiting."); + } + + /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ + /* number of messages */ + const ssize_t fdsc = 18; + /* Sanity check variable and index */ + ssize_t fdsc_count = 0; + /* file descriptors to wait for */ + struct pollfd fds[fdsc]; + + struct vehicle_status_s buf_status; + memset(&buf_status, 0, sizeof(buf_status)); + + /* warning! using union here to save memory, elements should be used separately! */ + union { + struct vehicle_command_s cmd; + struct sensor_combined_s sensor; + struct vehicle_attitude_s att; + struct vehicle_attitude_setpoint_s att_sp; + struct vehicle_rates_setpoint_s rates_sp; + struct actuator_outputs_s act_outputs; + struct actuator_controls_s act_controls; + struct actuator_controls_effective_s act_controls_effective; + struct vehicle_local_position_s local_pos; + struct vehicle_local_position_setpoint_s local_pos_sp; + struct vehicle_global_position_s global_pos; + struct vehicle_gps_position_s gps_pos; + struct vehicle_vicon_position_s vicon_pos; + struct optical_flow_s flow; + struct rc_channels_s rc; + struct differential_pressure_s diff_pres; + struct airspeed_s airspeed; + struct esc_status_s esc; + } buf; + memset(&buf, 0, sizeof(buf)); + + struct { + int cmd_sub; + int status_sub; + int sensor_sub; + int att_sub; + int att_sp_sub; + int rates_sp_sub; + int act_outputs_sub; + int act_controls_sub; + int act_controls_effective_sub; + int local_pos_sub; + int local_pos_sp_sub; + int global_pos_sub; + int gps_pos_sub; + int vicon_pos_sub; + int flow_sub; + int rc_sub; + int airspeed_sub; + int esc_sub; + } subs; + + /* log message buffer: header + body */ +#pragma pack(push, 1) + struct { + LOG_PACKET_HEADER; + union { + struct log_TIME_s log_TIME; + struct log_ATT_s log_ATT; + struct log_ATSP_s log_ATSP; + struct log_IMU_s log_IMU; + struct log_SENS_s log_SENS; + struct log_LPOS_s log_LPOS; + struct log_LPSP_s log_LPSP; + struct log_GPS_s log_GPS; + struct log_ATTC_s log_ATTC; + struct log_STAT_s log_STAT; + struct log_RC_s log_RC; + struct log_OUT0_s log_OUT0; + struct log_AIRS_s log_AIRS; + struct log_ARSP_s log_ARSP; + struct log_FLOW_s log_FLOW; + struct log_GPOS_s log_GPOS; + struct log_ESC_s log_ESC; + } body; + } log_msg = { + LOG_PACKET_HEADER_INIT(0) + }; +#pragma pack(pop) + memset(&log_msg.body, 0, sizeof(log_msg.body)); + + /* --- 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++; + + /* --- VEHICLE STATUS --- */ + subs.status_sub = orb_subscribe(ORB_ID(vehicle_status)); + fds[fdsc_count].fd = subs.status_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- GPS 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 COMBINED --- */ + subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + fds[fdsc_count].fd = subs.sensor_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- 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 --- */ + subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + fds[fdsc_count].fd = subs.att_sp_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- RATES SETPOINT --- */ + subs.rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + fds[fdsc_count].fd = subs.rates_sp_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- ACTUATOR OUTPUTS --- */ + subs.act_outputs_sub = orb_subscribe(ORB_ID_VEHICLE_CONTROLS); + fds[fdsc_count].fd = subs.act_outputs_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- ACTUATOR CONTROL --- */ + subs.act_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + fds[fdsc_count].fd = subs.act_controls_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- ACTUATOR CONTROL EFFECTIVE --- */ + subs.act_controls_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); + fds[fdsc_count].fd = subs.act_controls_effective_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- 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++; + + /* --- LOCAL POSITION SETPOINT --- */ + subs.local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); + fds[fdsc_count].fd = subs.local_pos_sp_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- 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 --- */ + 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++; + + /* --- OPTICAL FLOW --- */ + subs.flow_sub = orb_subscribe(ORB_ID(optical_flow)); + fds[fdsc_count].fd = subs.flow_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- RC CHANNELS --- */ + subs.rc_sub = orb_subscribe(ORB_ID(rc_channels)); + fds[fdsc_count].fd = subs.rc_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- AIRSPEED --- */ + subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + fds[fdsc_count].fd = subs.airspeed_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* --- ESCs --- */ + subs.esc_sub = orb_subscribe(ORB_ID(esc_status)); + fds[fdsc_count].fd = subs.esc_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.", __FILE__, __LINE__); + fdsc_count = fdsc; + } + + /* + * set up poll to block for new data, + * wait for a maximum of 1000 ms + */ + const int poll_timeout = 1000; + + thread_running = true; + + /* initialize thread synchronization */ + pthread_mutex_init(&logbuffer_mutex, NULL); + pthread_cond_init(&logbuffer_cond, NULL); + + /* track changes in sensor_combined topic */ + uint16_t gyro_counter = 0; + uint16_t accelerometer_counter = 0; + uint16_t magnetometer_counter = 0; + uint16_t baro_counter = 0; + uint16_t differential_pressure_counter = 0; + + /* enable logging on start if needed */ + if (log_on_start) + sdlog2_start_log(); + + while (!main_thread_should_exit) { + /* decide use usleep() or blocking poll() */ + bool use_sleep = sleep_delay > 0 && logging_enabled; + + /* poll all topics if logging enabled or only management (first 2) if not */ + int poll_ret = poll(fds, logging_enabled ? fdsc_count : 2, use_sleep ? 0 : poll_timeout); + + /* handle the poll result */ + if (poll_ret < 0) { + warnx("ERROR: poll error, stop logging."); + main_thread_should_exit = true; + + } else if (poll_ret > 0) { + + /* check all data subscriptions only if logging enabled, + * logging_enabled can be changed while checking vehicle_command and vehicle_status */ + bool check_data = logging_enabled; + int ifds = 0; + int handled_topics = 0; + + /* --- VEHICLE COMMAND - LOG MANAGEMENT --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd); + handle_command(&buf.cmd); + handled_topics++; + } + + /* --- VEHICLE STATUS - LOG MANAGEMENT --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status); + + if (log_when_armed) { + handle_status(&buf_status); + } + + handled_topics++; + } + + if (!logging_enabled || !check_data || handled_topics >= poll_ret) { + continue; + } + + ifds = 1; // Begin from fds[1] again + + pthread_mutex_lock(&logbuffer_mutex); + + /* write time stamp message */ + log_msg.msg_type = LOG_TIME_MSG; + log_msg.body.log_TIME.t = hrt_absolute_time(); + LOGBUFFER_WRITE_AND_COUNT(TIME); + + /* --- VEHICLE STATUS --- */ + if (fds[ifds++].revents & POLLIN) { + // Don't orb_copy, it's already done few lines above + log_msg.msg_type = LOG_STAT_MSG; + log_msg.body.log_STAT.state = (unsigned char) buf_status.state_machine; + log_msg.body.log_STAT.flight_mode = (unsigned char) buf_status.flight_mode; + log_msg.body.log_STAT.manual_control_mode = (unsigned char) buf_status.manual_control_mode; + log_msg.body.log_STAT.manual_sas_mode = (unsigned char) buf_status.manual_sas_mode; + log_msg.body.log_STAT.armed = (unsigned char) buf_status.flag_system_armed; + log_msg.body.log_STAT.battery_voltage = buf_status.voltage_battery; + log_msg.body.log_STAT.battery_current = buf_status.current_battery; + log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; + log_msg.body.log_STAT.battery_warning = (unsigned char) buf_status.battery_warning; + LOGBUFFER_WRITE_AND_COUNT(STAT); + } + + /* --- GPS POSITION --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + log_msg.msg_type = LOG_GPS_MSG; + log_msg.body.log_GPS.gps_time = buf.gps_pos.time_gps_usec; + log_msg.body.log_GPS.fix_type = buf.gps_pos.fix_type; + log_msg.body.log_GPS.eph = buf.gps_pos.eph_m; + log_msg.body.log_GPS.epv = buf.gps_pos.epv_m; + log_msg.body.log_GPS.lat = buf.gps_pos.lat; + log_msg.body.log_GPS.lon = buf.gps_pos.lon; + log_msg.body.log_GPS.alt = buf.gps_pos.alt * 0.001f; + log_msg.body.log_GPS.vel_n = buf.gps_pos.vel_n_m_s; + log_msg.body.log_GPS.vel_e = buf.gps_pos.vel_e_m_s; + 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); + } + + /* --- SENSOR COMBINED --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor); + bool write_IMU = false; + bool write_SENS = false; + + if (buf.sensor.gyro_counter != gyro_counter) { + gyro_counter = buf.sensor.gyro_counter; + write_IMU = true; + } + + if (buf.sensor.accelerometer_counter != accelerometer_counter) { + accelerometer_counter = buf.sensor.accelerometer_counter; + write_IMU = true; + } + + if (buf.sensor.magnetometer_counter != magnetometer_counter) { + magnetometer_counter = buf.sensor.magnetometer_counter; + write_IMU = true; + } + + if (buf.sensor.baro_counter != baro_counter) { + baro_counter = buf.sensor.baro_counter; + write_SENS = true; + } + + if (buf.sensor.differential_pressure_counter != differential_pressure_counter) { + differential_pressure_counter = buf.sensor.differential_pressure_counter; + write_SENS = true; + } + + if (write_IMU) { + log_msg.msg_type = LOG_IMU_MSG; + log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad_s[0]; + log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad_s[1]; + log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad_s[2]; + log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[0]; + log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[1]; + log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[2]; + log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[0]; + log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[1]; + log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[2]; + LOGBUFFER_WRITE_AND_COUNT(IMU); + } + + if (write_SENS) { + log_msg.msg_type = LOG_SENS_MSG; + log_msg.body.log_SENS.baro_pres = buf.sensor.baro_pres_mbar; + log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter; + log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius; + log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa; + LOGBUFFER_WRITE_AND_COUNT(SENS); + } + } + + /* --- ATTITUDE --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att); + log_msg.msg_type = LOG_ATT_MSG; + log_msg.body.log_ATT.roll = buf.att.roll; + log_msg.body.log_ATT.pitch = buf.att.pitch; + log_msg.body.log_ATT.yaw = buf.att.yaw; + log_msg.body.log_ATT.roll_rate = buf.att.rollspeed; + log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed; + log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed; + LOGBUFFER_WRITE_AND_COUNT(ATT); + } + + /* --- ATTITUDE SETPOINT --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.att_sp_sub, &buf.att_sp); + log_msg.msg_type = LOG_ATSP_MSG; + log_msg.body.log_ATSP.roll_sp = buf.att_sp.roll_body; + log_msg.body.log_ATSP.pitch_sp = buf.att_sp.pitch_body; + log_msg.body.log_ATSP.yaw_sp = buf.att_sp.yaw_body; + log_msg.body.log_ATSP.thrust_sp = buf.att_sp.thrust; + LOGBUFFER_WRITE_AND_COUNT(ATSP); + } + + /* --- RATES SETPOINT --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_rates_setpoint), subs.rates_sp_sub, &buf.rates_sp); + log_msg.msg_type = LOG_ARSP_MSG; + log_msg.body.log_ARSP.roll_rate_sp = buf.rates_sp.roll; + log_msg.body.log_ARSP.pitch_rate_sp = buf.rates_sp.pitch; + log_msg.body.log_ARSP.yaw_rate_sp = buf.rates_sp.yaw; + LOGBUFFER_WRITE_AND_COUNT(ARSP); + } + + /* --- ACTUATOR OUTPUTS --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(actuator_outputs_0), subs.act_outputs_sub, &buf.act_outputs); + log_msg.msg_type = LOG_OUT0_MSG; + memcpy(log_msg.body.log_OUT0.output, buf.act_outputs.output, sizeof(log_msg.body.log_OUT0.output)); + LOGBUFFER_WRITE_AND_COUNT(OUT0); + } + + /* --- ACTUATOR CONTROL --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.act_controls_sub, &buf.act_controls); + log_msg.msg_type = LOG_ATTC_MSG; + log_msg.body.log_ATTC.roll = buf.act_controls.control[0]; + log_msg.body.log_ATTC.pitch = buf.act_controls.control[1]; + log_msg.body.log_ATTC.yaw = buf.act_controls.control[2]; + log_msg.body.log_ATTC.thrust = buf.act_controls.control[3]; + LOGBUFFER_WRITE_AND_COUNT(ATTC); + } + + /* --- ACTUATOR CONTROL EFFECTIVE --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.act_controls_effective_sub, &buf.act_controls_effective); + // TODO not implemented yet + } + + /* --- LOCAL POSITION --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos); + log_msg.msg_type = LOG_LPOS_MSG; + 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.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.hdg = buf.local_pos.hdg; + log_msg.body.log_LPOS.home_lat = buf.local_pos.home_lat; + log_msg.body.log_LPOS.home_lon = buf.local_pos.home_lon; + log_msg.body.log_LPOS.home_alt = buf.local_pos.home_alt; + LOGBUFFER_WRITE_AND_COUNT(LPOS); + } + + /* --- LOCAL POSITION SETPOINT --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_local_position_setpoint), subs.local_pos_sp_sub, &buf.local_pos_sp); + log_msg.msg_type = LOG_LPSP_MSG; + log_msg.body.log_LPSP.x = buf.local_pos_sp.x; + log_msg.body.log_LPSP.y = buf.local_pos_sp.y; + log_msg.body.log_LPSP.z = buf.local_pos_sp.z; + log_msg.body.log_LPSP.yaw = buf.local_pos_sp.yaw; + LOGBUFFER_WRITE_AND_COUNT(LPSP); + } + + /* --- GLOBAL POSITION --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos); + log_msg.msg_type = LOG_GPOS_MSG; + log_msg.body.log_GPOS.lat = buf.global_pos.lat; + log_msg.body.log_GPOS.lon = buf.global_pos.lon; + log_msg.body.log_GPOS.alt = buf.global_pos.alt; + log_msg.body.log_GPOS.vel_n = buf.global_pos.vx; + log_msg.body.log_GPOS.vel_e = buf.global_pos.vy; + log_msg.body.log_GPOS.vel_d = buf.global_pos.vz; + LOGBUFFER_WRITE_AND_COUNT(GPOS); + } + + /* --- VICON POSITION --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos); + // TODO not implemented yet + } + + /* --- FLOW --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow); + log_msg.msg_type = LOG_FLOW_MSG; + log_msg.body.log_FLOW.flow_raw_x = buf.flow.flow_raw_x; + log_msg.body.log_FLOW.flow_raw_y = buf.flow.flow_raw_y; + log_msg.body.log_FLOW.flow_comp_x = buf.flow.flow_comp_x_m; + log_msg.body.log_FLOW.flow_comp_y = buf.flow.flow_comp_y_m; + log_msg.body.log_FLOW.distance = buf.flow.ground_distance_m; + log_msg.body.log_FLOW.quality = buf.flow.quality; + log_msg.body.log_FLOW.sensor_id = buf.flow.sensor_id; + LOGBUFFER_WRITE_AND_COUNT(FLOW); + } + + /* --- RC CHANNELS --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(rc_channels), subs.rc_sub, &buf.rc); + log_msg.msg_type = LOG_RC_MSG; + /* 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)); + LOGBUFFER_WRITE_AND_COUNT(RC); + } + + /* --- AIRSPEED --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(airspeed), subs.airspeed_sub, &buf.airspeed); + 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; + LOGBUFFER_WRITE_AND_COUNT(AIRS); + } + + /* --- ESCs --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(esc_status), subs.esc_sub, &buf.esc); + for (uint8_t i=0; i<buf.esc.esc_count; i++) + { + log_msg.msg_type = LOG_ESC_MSG; + log_msg.body.log_ESC.counter = buf.esc.counter; + log_msg.body.log_ESC.esc_count = buf.esc.esc_count; + log_msg.body.log_ESC.esc_connectiontype = buf.esc.esc_connectiontype; + log_msg.body.log_ESC.esc_num = i; + log_msg.body.log_ESC.esc_address = buf.esc.esc[i].esc_address; + log_msg.body.log_ESC.esc_version = buf.esc.esc[i].esc_version; + log_msg.body.log_ESC.esc_voltage = buf.esc.esc[i].esc_voltage; + log_msg.body.log_ESC.esc_current = buf.esc.esc[i].esc_current; + log_msg.body.log_ESC.esc_rpm = buf.esc.esc[i].esc_rpm; + log_msg.body.log_ESC.esc_temperature = buf.esc.esc[i].esc_temperature; + log_msg.body.log_ESC.esc_setpoint = buf.esc.esc[i].esc_setpoint; + log_msg.body.log_ESC.esc_setpoint_raw = buf.esc.esc[i].esc_setpoint_raw; + LOGBUFFER_WRITE_AND_COUNT(ESC); + } + } + +#ifdef SDLOG2_DEBUG + printf("fill rp=%i wp=%i count=%i\n", lb.read_ptr, lb.write_ptr, logbuffer_count(&lb)); +#endif + /* signal the other thread new data, but not yet unlock */ + if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { +#ifdef SDLOG2_DEBUG + printf("signal rp=%i wp=%i count=%i\n", lb.read_ptr, lb.write_ptr, logbuffer_count(&lb)); +#endif + /* only request write if several packets can be written at once */ + pthread_cond_signal(&logbuffer_cond); + } + + /* unlock, now the writer thread may run */ + pthread_mutex_unlock(&logbuffer_mutex); + } + + if (use_sleep) { + usleep(sleep_delay); + } + } + + if (logging_enabled) + sdlog2_stop_log(); + + pthread_mutex_destroy(&logbuffer_mutex); + pthread_cond_destroy(&logbuffer_cond); + + warnx("exiting."); + + thread_running = false; + + return 0; +} + +void sdlog2_status() +{ + float kibibytes = log_bytes_written / 1024.0f; + float mebibytes = kibibytes / 1024.0f; + float seconds = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f; + + warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs.", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped); + mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs.", log_msgs_written, log_msgs_skipped); +} + +/** + * @return 0 if file exists + */ +bool file_exist(const char *filename) +{ + struct stat buffer; + return stat(filename, &buffer) == 0; +} + +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) { + 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; + int param; + + /* request to set different system mode */ + switch (cmd->command) { + + case VEHICLE_CMD_PREFLIGHT_STORAGE: + param = (int)(cmd->param3); + + if (param == 1) { + sdlog2_start_log(); + + } else if (param == 0) { + sdlog2_stop_log(); + } + + break; + + default: + /* silently ignore */ + break; + } +} + +void handle_status(struct vehicle_status_s *status) +{ + if (status->flag_system_armed != flag_system_armed) { + flag_system_armed = status->flag_system_armed; + + if (flag_system_armed) { + sdlog2_start_log(); + + } else { + sdlog2_stop_log(); + } + } +} diff --git a/src/modules/sdlog2/sdlog2_format.h b/src/modules/sdlog2/sdlog2_format.h new file mode 100644 index 000000000..5c175ef7e --- /dev/null +++ b/src/modules/sdlog2/sdlog2_format.h @@ -0,0 +1,98 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * 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 + * 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 sdlog2_format.h + * + * General log format structures and macro. + * + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +/* +Format characters in the format string for binary log messages + b : int8_t + B : uint8_t + h : int16_t + H : uint16_t + i : int32_t + I : uint32_t + f : float + n : char[4] + N : char[16] + Z : char[64] + c : int16_t * 100 + C : uint16_t * 100 + e : int32_t * 100 + E : uint32_t * 100 + L : int32_t latitude/longitude + M : uint8_t flight mode + + q : int64_t + Q : uint64_t + */ + +#ifndef SDLOG2_FORMAT_H_ +#define SDLOG2_FORMAT_H_ + +#define LOG_PACKET_HEADER_LEN 3 +#define LOG_PACKET_HEADER uint8_t head1, head2, msg_type; +#define LOG_PACKET_HEADER_INIT(id) .head1 = HEAD_BYTE1, .head2 = HEAD_BYTE2, .msg_type = id + +// once the logging code is all converted we will remove these from +// this header +#define HEAD_BYTE1 0xA3 // Decimal 163 +#define HEAD_BYTE2 0x95 // Decimal 149 + +struct log_format_s { + uint8_t type; + uint8_t length; // full packet length including header + char name[4]; + char format[16]; + char labels[64]; +}; + +#define LOG_FORMAT(_name, _format, _labels) { \ + .type = LOG_##_name##_MSG, \ + .length = sizeof(struct log_##_name##_s) + LOG_PACKET_HEADER_LEN, \ + .name = #_name, \ + .format = _format, \ + .labels = _labels \ + } + +#define LOG_FORMAT_MSG 0x80 + +#define LOG_PACKET_SIZE(_name) LOG_PACKET_HEADER_LEN + sizeof(struct log_##_name##_s) + +#endif /* SDLOG2_FORMAT_H_ */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h new file mode 100644 index 000000000..abc882d23 --- /dev/null +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -0,0 +1,256 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * 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 + * 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 sdlog2_messages.h + * + * Log messages and structures definition. + * + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#ifndef SDLOG2_MESSAGES_H_ +#define SDLOG2_MESSAGES_H_ + +#include "sdlog2_format.h" + +/* define message formats */ + +#pragma pack(push, 1) +/* --- TIME - TIME STAMP --- */ +#define LOG_TIME_MSG 1 +struct log_TIME_s { + uint64_t t; +}; + +/* --- ATT - ATTITUDE --- */ +#define LOG_ATT_MSG 2 +struct log_ATT_s { + float roll; + float pitch; + float yaw; + float roll_rate; + float pitch_rate; + float yaw_rate; +}; + +/* --- ATSP - ATTITUDE SET POINT --- */ +#define LOG_ATSP_MSG 3 +struct log_ATSP_s { + float roll_sp; + float pitch_sp; + float yaw_sp; + float thrust_sp; +}; + +/* --- IMU - IMU SENSORS --- */ +#define LOG_IMU_MSG 4 +struct log_IMU_s { + float acc_x; + float acc_y; + float acc_z; + float gyro_x; + float gyro_y; + float gyro_z; + float mag_x; + float mag_y; + float mag_z; +}; + +/* --- SENS - OTHER SENSORS --- */ +#define LOG_SENS_MSG 5 +struct log_SENS_s { + float baro_pres; + float baro_alt; + float baro_temp; + float diff_pres; +}; + +/* --- LPOS - LOCAL POSITION --- */ +#define LOG_LPOS_MSG 6 +struct log_LPOS_s { + float x; + float y; + float z; + float vx; + float vy; + float vz; + float hdg; + int32_t home_lat; + int32_t home_lon; + float home_alt; +}; + +/* --- LPSP - LOCAL POSITION SETPOINT --- */ +#define LOG_LPSP_MSG 7 +struct log_LPSP_s { + float x; + float y; + float z; + float yaw; +}; + +/* --- GPS - GPS POSITION --- */ +#define LOG_GPS_MSG 8 +struct log_GPS_s { + uint64_t gps_time; + uint8_t fix_type; + float eph; + float epv; + int32_t lat; + int32_t lon; + float alt; + float vel_n; + float vel_e; + float vel_d; + float cog; +}; + +/* --- ATTC - ATTITUDE CONTROLS (ACTUATOR_0 CONTROLS)--- */ +#define LOG_ATTC_MSG 9 +struct log_ATTC_s { + float roll; + float pitch; + float yaw; + float thrust; +}; + +/* --- STAT - VEHICLE STATE --- */ +#define LOG_STAT_MSG 10 +struct log_STAT_s { + unsigned char state; + unsigned char flight_mode; + unsigned char manual_control_mode; + unsigned char manual_sas_mode; + unsigned char armed; + float battery_voltage; + float battery_current; + float battery_remaining; + unsigned char battery_warning; +}; + +/* --- RC - RC INPUT CHANNELS --- */ +#define LOG_RC_MSG 11 +struct log_RC_s { + float channel[8]; +}; + +/* --- OUT0 - ACTUATOR_0 OUTPUT --- */ +#define LOG_OUT0_MSG 12 +struct log_OUT0_s { + float output[8]; +}; + +/* --- AIRS - AIRSPEED --- */ +#define LOG_AIRS_MSG 13 +struct log_AIRS_s { + float indicated_airspeed; + float true_airspeed; +}; + +/* --- ARSP - ATTITUDE RATE SET POINT --- */ +#define LOG_ARSP_MSG 14 +struct log_ARSP_s { + float roll_rate_sp; + float pitch_rate_sp; + float yaw_rate_sp; +}; + +/* --- FLOW - OPTICAL FLOW --- */ +#define LOG_FLOW_MSG 15 +struct log_FLOW_s { + int16_t flow_raw_x; + int16_t flow_raw_y; + float flow_comp_x; + float flow_comp_y; + float distance; + uint8_t quality; + uint8_t sensor_id; +}; + +/* --- GPOS - GLOBAL POSITION ESTIMATE --- */ +#define LOG_GPOS_MSG 16 +struct log_GPOS_s { + int32_t lat; + int32_t lon; + float alt; + float vel_n; + float vel_e; + float vel_d; +}; + +/* --- ESC - ESC STATE --- */ +#define LOG_ESC_MSG 64 +struct log_ESC_s { + uint16_t counter; + uint8_t esc_count; + uint8_t esc_connectiontype; + + uint8_t esc_num; + uint16_t esc_address; + uint16_t esc_version; + uint16_t esc_voltage; + uint16_t esc_current; + uint16_t esc_rpm; + uint16_t esc_temperature; + float esc_setpoint; + uint16_t esc_setpoint_raw; +}; +#pragma pack(pop) + +/* construct list of all message formats */ + +static const struct log_format_s log_formats[] = { + LOG_FORMAT(TIME, "Q", "StartTime"), + LOG_FORMAT(ATT, "ffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate"), + LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), + LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), + LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"), + LOG_FORMAT(LPOS, "fffffffLLf", "X,Y,Z,VX,VY,VZ,Heading,HomeLat,HomeLon,HomeAlt"), + 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, "BBBBBfffB", "State,FlightMode,CtlMode,SASMode,Armed,BatV,BatC,BatRem,BatWarn"), + LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"), + 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, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"), + LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,No,Version,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), +}; + +static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); + +#endif /* SDLOG2_MESSAGES_H_ */ diff --git a/src/modules/sensors/module.mk b/src/modules/sensors/module.mk new file mode 100644 index 000000000..ebbc580e1 --- /dev/null +++ b/src/modules/sensors/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# 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 sensor data collector +# + +MODULE_COMMAND = sensors +MODULE_PRIORITY = "SCHED_PRIORITY_MAX-5" + +SRCS = sensors.cpp \ + sensor_params.c diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c new file mode 100644 index 000000000..f6f4d60c7 --- /dev/null +++ b/src/modules/sensors/sensor_params.c @@ -0,0 +1,185 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @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 + * 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 sensor_params.c + * + * Parameters defined by the sensors task. + */ + +#include <nuttx/config.h> + +#include <systemlib/param/param.h> + +PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f); +PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f); +PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f); + +PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f); +PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f); +PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f); + +PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f); +PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f); +PARAM_DEFINE_FLOAT(SENS_MAG_ZOFF, 0.0f); + +PARAM_DEFINE_FLOAT(SENS_MAG_XSCALE, 1.0f); +PARAM_DEFINE_FLOAT(SENS_MAG_YSCALE, 1.0f); +PARAM_DEFINE_FLOAT(SENS_MAG_ZSCALE, 1.0f); + +PARAM_DEFINE_FLOAT(SENS_ACC_XOFF, 0.0f); +PARAM_DEFINE_FLOAT(SENS_ACC_YOFF, 0.0f); +PARAM_DEFINE_FLOAT(SENS_ACC_ZOFF, 0.0f); + +PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f); +PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f); +PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f); + +PARAM_DEFINE_INT32(SENS_DPRES_OFF, 1667); + +PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); +PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); +PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f); +PARAM_DEFINE_FLOAT(RC1_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC1_DZ, 0.0f); + +PARAM_DEFINE_FLOAT(RC2_MIN, 1000); +PARAM_DEFINE_FLOAT(RC2_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC2_MAX, 2000); +PARAM_DEFINE_FLOAT(RC2_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC2_DZ, 0.0f); + +PARAM_DEFINE_FLOAT(RC3_MIN, 1000); +PARAM_DEFINE_FLOAT(RC3_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC3_MAX, 2000); +PARAM_DEFINE_FLOAT(RC3_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC3_DZ, 0.0f); + +PARAM_DEFINE_FLOAT(RC4_MIN, 1000); +PARAM_DEFINE_FLOAT(RC4_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC4_MAX, 2000); +PARAM_DEFINE_FLOAT(RC4_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC4_DZ, 30.0f); + +PARAM_DEFINE_FLOAT(RC5_MIN, 1000); +PARAM_DEFINE_FLOAT(RC5_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC5_MAX, 2000); +PARAM_DEFINE_FLOAT(RC5_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC5_DZ, 0.0f); + +PARAM_DEFINE_FLOAT(RC6_MIN, 1000); +PARAM_DEFINE_FLOAT(RC6_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC6_MAX, 2000); +PARAM_DEFINE_FLOAT(RC6_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC6_DZ, 0.0f); + +PARAM_DEFINE_FLOAT(RC7_MIN, 1000); +PARAM_DEFINE_FLOAT(RC7_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC7_MAX, 2000); +PARAM_DEFINE_FLOAT(RC7_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC7_DZ, 0.0f); + +PARAM_DEFINE_FLOAT(RC8_MIN, 1000); +PARAM_DEFINE_FLOAT(RC8_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC8_MAX, 2000); +PARAM_DEFINE_FLOAT(RC8_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC8_DZ, 0.0f); + +PARAM_DEFINE_FLOAT(RC9_MIN, 1000); +PARAM_DEFINE_FLOAT(RC9_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC9_MAX, 2000); +PARAM_DEFINE_FLOAT(RC9_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC9_DZ, 0.0f); + +PARAM_DEFINE_FLOAT(RC10_MIN, 1000); +PARAM_DEFINE_FLOAT(RC10_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC10_MAX, 2000); +PARAM_DEFINE_FLOAT(RC10_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC10_DZ, 0.0f); + +PARAM_DEFINE_FLOAT(RC11_MIN, 1000); +PARAM_DEFINE_FLOAT(RC11_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC11_MAX, 2000); +PARAM_DEFINE_FLOAT(RC11_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC11_DZ, 0.0f); + +PARAM_DEFINE_FLOAT(RC12_MIN, 1000); +PARAM_DEFINE_FLOAT(RC12_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC12_MAX, 2000); +PARAM_DEFINE_FLOAT(RC12_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC12_DZ, 0.0f); + +PARAM_DEFINE_FLOAT(RC13_MIN, 1000); +PARAM_DEFINE_FLOAT(RC13_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC13_MAX, 2000); +PARAM_DEFINE_FLOAT(RC13_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC13_DZ, 0.0f); + +PARAM_DEFINE_FLOAT(RC14_MIN, 1000); +PARAM_DEFINE_FLOAT(RC14_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC14_MAX, 2000); +PARAM_DEFINE_FLOAT(RC14_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f); + +PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */ + +/* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ +PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f)); + +PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); +PARAM_DEFINE_INT32(RC_MAP_PITCH, 2); +PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3); +PARAM_DEFINE_INT32(RC_MAP_YAW, 4); + +PARAM_DEFINE_INT32(RC_MAP_OVER_SW, 5); +PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 6); + +PARAM_DEFINE_INT32(RC_MAP_MAN_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_SAS_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_RTL_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); + +PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0); + +PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); /**< default function: camera yaw / azimuth */ +PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera pitch / tilt */ +PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); /**< default function: camera trigger */ +PARAM_DEFINE_INT32(RC_MAP_AUX4, 0); /**< default function: camera roll */ +PARAM_DEFINE_INT32(RC_MAP_AUX5, 0); /**< default function: payload drop */ + +PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.4f); +PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.4f); +PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 1.0f); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp new file mode 100644 index 000000000..1ded14a91 --- /dev/null +++ b/src/modules/sensors/sensors.cpp @@ -0,0 +1,1516 @@ +/**************************************************************************** + * + * 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 sensors.cpp + * Sensor readout process. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +#include <nuttx/config.h> + +#include <fcntl.h> +#include <poll.h> +#include <unistd.h> +#include <stdlib.h> +#include <string.h> +#include <stdbool.h> +#include <stdio.h> +#include <errno.h> +#include <math.h> + +#include <nuttx/analog/adc.h> + +#include <drivers/drv_hrt.h> +#include <drivers/drv_accel.h> +#include <drivers/drv_gyro.h> +#include <drivers/drv_mag.h> +#include <drivers/drv_baro.h> +#include <drivers/drv_rc_input.h> +#include <drivers/drv_adc.h> + +#include <systemlib/systemlib.h> +#include <systemlib/param/param.h> +#include <systemlib/err.h> +#include <systemlib/perf_counter.h> + +#include <systemlib/ppm_decode.h> +#include <systemlib/airspeed.h> + +#include <uORB/uORB.h> +#include <uORB/topics/sensor_combined.h> +#include <uORB/topics/rc_channels.h> +#include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/parameter_update.h> +#include <uORB/topics/battery_status.h> +#include <uORB/topics/differential_pressure.h> +#include <uORB/topics/airspeed.h> + +#define GYRO_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */ +#define ACC_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */ +#define MAGN_HEALTH_COUNTER_LIMIT_ERROR 100 /* 1000 ms downtime at 100 Hz update rate */ +#define BARO_HEALTH_COUNTER_LIMIT_ERROR 50 /* 500 ms downtime at 100 Hz update rate */ +#define ADC_HEALTH_COUNTER_LIMIT_ERROR 10 /* 100 ms downtime at 100 Hz update rate */ + +#define GYRO_HEALTH_COUNTER_LIMIT_OK 5 +#define ACC_HEALTH_COUNTER_LIMIT_OK 5 +#define MAGN_HEALTH_COUNTER_LIMIT_OK 5 +#define BARO_HEALTH_COUNTER_LIMIT_OK 5 +#define ADC_HEALTH_COUNTER_LIMIT_OK 5 + +#define ADC_BATTERY_VOLTAGE_CHANNEL 10 +#define ADC_AIRSPEED_VOLTAGE_CHANNEL 11 + +#define BAT_VOL_INITIAL 0.f +#define BAT_VOL_LOWPASS_1 0.99f +#define BAT_VOL_LOWPASS_2 0.01f +#define VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS 3.5f + +/** + * HACK - true temperature is much less than indicated temperature in baro, + * subtract 5 degrees in an attempt to account for the electrical upheating of the PCB + */ +#define PCB_TEMP_ESTIMATE_DEG 5.0f + +#define PPM_INPUT_TIMEOUT_INTERVAL 50000 /**< 50 ms timeout / 20 Hz */ + +#define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg) + +/** + * Sensor app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int sensors_main(int argc, char *argv[]); + +class Sensors +{ +public: + /** + * Constructor + */ + Sensors(); + + /** + * Destructor, also kills the sensors task. + */ + ~Sensors(); + + /** + * Start the sensors task. + * + * @return OK on success. + */ + int start(); + +private: + static const unsigned _rc_max_chan_count = RC_CHANNELS_MAX; /**< maximum number of r/c channels we handle */ + +#if CONFIG_HRT_PPM + hrt_abstime _ppm_last_valid; /**< last time we got a valid ppm signal */ + + /** + * Gather and publish PPM input data. + */ + void ppm_poll(); +#endif + + /* XXX should not be here - should be own driver */ + int _fd_adc; /**< ADC driver handle */ + hrt_abstime _last_adc; /**< last time we took input from the ADC */ + + bool _task_should_exit; /**< if true, sensor task should exit */ + int _sensors_task; /**< task handle for sensor task */ + + bool _hil_enabled; /**< if true, HIL is active */ + bool _publishing; /**< if true, we are publishing sensor data */ + + int _gyro_sub; /**< raw gyro data subscription */ + int _accel_sub; /**< raw accel data subscription */ + int _mag_sub; /**< raw mag data subscription */ + int _rc_sub; /**< raw rc channels data subscription */ + int _baro_sub; /**< raw baro data subscription */ + int _airspeed_sub; /**< airspeed subscription */ + int _diff_pres_sub; /**< raw differential pressure subscription */ + int _vstatus_sub; /**< vehicle status subscription */ + int _params_sub; /**< notification of parameter updates */ + int _manual_control_sub; /**< notification of manual control updates */ + + orb_advert_t _sensor_pub; /**< combined sensor data topic */ + orb_advert_t _manual_control_pub; /**< manual control signal topic */ + orb_advert_t _rc_pub; /**< raw r/c control topic */ + orb_advert_t _battery_pub; /**< battery status */ + orb_advert_t _airspeed_pub; /**< airspeed */ + orb_advert_t _diff_pres_pub; /**< differential_pressure */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + struct rc_channels_s _rc; /**< r/c channel data */ + struct battery_status_s _battery_status; /**< battery status */ + struct baro_report _barometer; /**< barometer data */ + struct differential_pressure_s _diff_pres; + struct airspeed_s _airspeed; + + struct { + float min[_rc_max_chan_count]; + float trim[_rc_max_chan_count]; + float max[_rc_max_chan_count]; + float rev[_rc_max_chan_count]; + float dz[_rc_max_chan_count]; + // float ex[_rc_max_chan_count]; + float scaling_factor[_rc_max_chan_count]; + + float gyro_offset[3]; + float gyro_scale[3]; + float mag_offset[3]; + float mag_scale[3]; + float accel_offset[3]; + float accel_scale[3]; + int diff_pres_offset_pa; + + int rc_type; + + int rc_map_roll; + int rc_map_pitch; + int rc_map_yaw; + int rc_map_throttle; + + int rc_map_manual_override_sw; + int rc_map_auto_mode_sw; + + int rc_map_manual_mode_sw; + int rc_map_sas_mode_sw; + int rc_map_rtl_sw; + int rc_map_offboard_ctrl_mode_sw; + + int rc_map_flaps; + + int rc_map_aux1; + int rc_map_aux2; + int rc_map_aux3; + int rc_map_aux4; + int rc_map_aux5; + + float rc_scale_roll; + float rc_scale_pitch; + float rc_scale_yaw; + float rc_scale_flaps; + + float battery_voltage_scaling; + } _parameters; /**< local copies of interesting parameters */ + + struct { + param_t min[_rc_max_chan_count]; + param_t trim[_rc_max_chan_count]; + param_t max[_rc_max_chan_count]; + param_t rev[_rc_max_chan_count]; + param_t dz[_rc_max_chan_count]; + // param_t ex[_rc_max_chan_count]; + param_t rc_type; + + param_t rc_demix; + + param_t gyro_offset[3]; + param_t gyro_scale[3]; + param_t accel_offset[3]; + param_t accel_scale[3]; + param_t mag_offset[3]; + param_t mag_scale[3]; + param_t diff_pres_offset_pa; + + param_t rc_map_roll; + param_t rc_map_pitch; + param_t rc_map_yaw; + param_t rc_map_throttle; + + param_t rc_map_manual_override_sw; + param_t rc_map_auto_mode_sw; + + param_t rc_map_manual_mode_sw; + param_t rc_map_sas_mode_sw; + param_t rc_map_rtl_sw; + param_t rc_map_offboard_ctrl_mode_sw; + + param_t rc_map_flaps; + + param_t rc_map_aux1; + param_t rc_map_aux2; + param_t rc_map_aux3; + 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 battery_voltage_scaling; + } _parameter_handles; /**< handles for interesting parameters */ + + + /** + * Update our local parameter cache. + */ + int parameters_update(); + + /** + * Do accel-related initialisation. + */ + void accel_init(); + + /** + * Do gyro-related initialisation. + */ + void gyro_init(); + + /** + * Do mag-related initialisation. + */ + void mag_init(); + + /** + * Do baro-related initialisation. + */ + void baro_init(); + + /** + * Do adc-related initialisation. + */ + void adc_init(); + + /** + * Poll the accelerometer for updated data. + * + * @param raw Combined sensor data structure into which + * data should be returned. + */ + void accel_poll(struct sensor_combined_s &raw); + + /** + * Poll the gyro for updated data. + * + * @param raw Combined sensor data structure into which + * data should be returned. + */ + void gyro_poll(struct sensor_combined_s &raw); + + /** + * Poll the magnetometer for updated data. + * + * @param raw Combined sensor data structure into which + * data should be returned. + */ + void mag_poll(struct sensor_combined_s &raw); + + /** + * Poll the barometer for updated data. + * + * @param raw Combined sensor data structure into which + * data should be returned. + */ + void baro_poll(struct sensor_combined_s &raw); + + /** + * Poll the differential pressure sensor for updated data. + * + * @param raw Combined sensor data structure into which + * data should be returned. + */ + void diff_pres_poll(struct sensor_combined_s &raw); + + /** + * Check for changes in vehicle status. + */ + void vehicle_status_poll(); + + /** + * Check for changes in parameters. + */ + void parameter_update_poll(bool forced = false); + + /** + * Poll the ADC and update readings to suit. + * + * @param raw Combined sensor data structure into which + * data should be returned. + */ + void adc_poll(struct sensor_combined_s &raw); + + /** + * Shim for calling task_main from task_create. + */ + static void task_main_trampoline(int argc, char *argv[]); + + /** + * Main sensor collection task. + */ + void task_main() __attribute__((noreturn)); +}; + +namespace sensors +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +Sensors *g_sensors; +} + +Sensors::Sensors() : +#ifdef CONFIG_HRT_PPM + _ppm_last_valid(0), +#endif + + _fd_adc(-1), + _last_adc(0), + + _task_should_exit(false), + _sensors_task(-1), + _hil_enabled(false), + _publishing(true), + +/* subscriptions */ + _gyro_sub(-1), + _accel_sub(-1), + _mag_sub(-1), + _rc_sub(-1), + _baro_sub(-1), + _vstatus_sub(-1), + _params_sub(-1), + _manual_control_sub(-1), + +/* publications */ + _sensor_pub(-1), + _manual_control_pub(-1), + _rc_pub(-1), + _battery_pub(-1), + _airspeed_pub(-1), + _diff_pres_pub(-1), + +/* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")) +{ + + /* basic r/c parameters */ + for (unsigned i = 0; i < _rc_max_chan_count; i++) { + char nbuf[16]; + + /* min values */ + sprintf(nbuf, "RC%d_MIN", i + 1); + _parameter_handles.min[i] = param_find(nbuf); + + /* trim values */ + sprintf(nbuf, "RC%d_TRIM", i + 1); + _parameter_handles.trim[i] = param_find(nbuf); + + /* max values */ + sprintf(nbuf, "RC%d_MAX", i + 1); + _parameter_handles.max[i] = param_find(nbuf); + + /* channel reverse */ + sprintf(nbuf, "RC%d_REV", i + 1); + _parameter_handles.rev[i] = param_find(nbuf); + + /* channel deadzone */ + sprintf(nbuf, "RC%d_DZ", i + 1); + _parameter_handles.dz[i] = param_find(nbuf); + + } + + _parameter_handles.rc_type = param_find("RC_TYPE"); + + /* mandatory input switched, mapped to channels 1-4 per default */ + _parameter_handles.rc_map_roll = param_find("RC_MAP_ROLL"); + _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"); + + /* mandatory mode switches, mapped to channel 5 and 6 per default */ + _parameter_handles.rc_map_manual_override_sw = param_find("RC_MAP_OVER_SW"); + _parameter_handles.rc_map_auto_mode_sw = param_find("RC_MAP_MODE_SW"); + + _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); + + /* optional mode switches, not mapped per default */ + _parameter_handles.rc_map_manual_mode_sw = param_find("RC_MAP_MAN_SW"); + _parameter_handles.rc_map_sas_mode_sw = param_find("RC_MAP_SAS_SW"); + _parameter_handles.rc_map_rtl_sw = param_find("RC_MAP_RTL_SW"); + _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); + + _parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1"); + _parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2"); + _parameter_handles.rc_map_aux3 = param_find("RC_MAP_AUX3"); + _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"); + + /* gyro offsets */ + _parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF"); + _parameter_handles.gyro_offset[1] = param_find("SENS_GYRO_YOFF"); + _parameter_handles.gyro_offset[2] = param_find("SENS_GYRO_ZOFF"); + _parameter_handles.gyro_scale[0] = param_find("SENS_GYRO_XSCALE"); + _parameter_handles.gyro_scale[1] = param_find("SENS_GYRO_YSCALE"); + _parameter_handles.gyro_scale[2] = param_find("SENS_GYRO_ZSCALE"); + + /* accel offsets */ + _parameter_handles.accel_offset[0] = param_find("SENS_ACC_XOFF"); + _parameter_handles.accel_offset[1] = param_find("SENS_ACC_YOFF"); + _parameter_handles.accel_offset[2] = param_find("SENS_ACC_ZOFF"); + _parameter_handles.accel_scale[0] = param_find("SENS_ACC_XSCALE"); + _parameter_handles.accel_scale[1] = param_find("SENS_ACC_YSCALE"); + _parameter_handles.accel_scale[2] = param_find("SENS_ACC_ZSCALE"); + + /* mag offsets */ + _parameter_handles.mag_offset[0] = param_find("SENS_MAG_XOFF"); + _parameter_handles.mag_offset[1] = param_find("SENS_MAG_YOFF"); + _parameter_handles.mag_offset[2] = param_find("SENS_MAG_ZOFF"); + + _parameter_handles.mag_scale[0] = param_find("SENS_MAG_XSCALE"); + _parameter_handles.mag_scale[1] = param_find("SENS_MAG_YSCALE"); + _parameter_handles.mag_scale[2] = param_find("SENS_MAG_ZSCALE"); + + /* Differential pressure offset */ + _parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF"); + + _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); + + /* fetch initial parameter values */ + parameters_update(); +} + +Sensors::~Sensors() +{ + if (_sensors_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(_sensors_task); + break; + } + } while (_sensors_task != -1); + } + + sensors::g_sensors = nullptr; +} + +int +Sensors::parameters_update() +{ + bool rc_valid = true; + + /* rc values */ + for (unsigned int i = 0; i < RC_CHANNELS_MAX; i++) { + + if (param_get(_parameter_handles.min[i], &(_parameters.min[i])) != OK) { + warnx("Failed getting min for chan %d", i); + } + + if (param_get(_parameter_handles.trim[i], &(_parameters.trim[i])) != OK) { + warnx("Failed getting trim for chan %d", i); + } + + if (param_get(_parameter_handles.max[i], &(_parameters.max[i])) != OK) { + warnx("Failed getting max for chan %d", i); + } + + if (param_get(_parameter_handles.rev[i], &(_parameters.rev[i])) != OK) { + warnx("Failed getting rev for chan %d", i); + } + + if (param_get(_parameter_handles.dz[i], &(_parameters.dz[i])) != OK) { + warnx("Failed getting dead zone for chan %d", i); + } + + _parameters.scaling_factor[i] = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]); + + /* handle blowup in the scaling factor calculation */ + if (!isfinite(_parameters.scaling_factor[i]) || + _parameters.scaling_factor[i] * _parameters.rev[i] < 0.000001f || + _parameters.scaling_factor[i] * _parameters.rev[i] > 0.2f) { + + /* scaling factors do not make sense, lock them down */ + _parameters.scaling_factor[i] = 0; + rc_valid = false; + } + + } + + /* handle wrong values */ + if (!rc_valid) + warnx("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n"); + + /* remote control type */ + if (param_get(_parameter_handles.rc_type, &(_parameters.rc_type)) != OK) { + warnx("Failed getting remote control type"); + } + + /* channel mapping */ + if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) { + warnx("Failed getting roll chan index"); + } + + if (param_get(_parameter_handles.rc_map_pitch, &(_parameters.rc_map_pitch)) != OK) { + warnx("Failed getting pitch chan index"); + } + + if (param_get(_parameter_handles.rc_map_yaw, &(_parameters.rc_map_yaw)) != OK) { + warnx("Failed getting yaw chan index"); + } + + if (param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle)) != OK) { + warnx("Failed getting throttle chan index"); + } + + if (param_get(_parameter_handles.rc_map_manual_override_sw, &(_parameters.rc_map_manual_override_sw)) != OK) { + warnx("Failed getting override sw chan index"); + } + + if (param_get(_parameter_handles.rc_map_auto_mode_sw, &(_parameters.rc_map_auto_mode_sw)) != OK) { + warnx("Failed getting auto mode sw chan index"); + } + + if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { + warnx("Failed getting flaps chan index"); + } + + if (param_get(_parameter_handles.rc_map_manual_mode_sw, &(_parameters.rc_map_manual_mode_sw)) != OK) { + warnx("Failed getting manual mode sw chan index"); + } + + if (param_get(_parameter_handles.rc_map_rtl_sw, &(_parameters.rc_map_rtl_sw)) != OK) { + warnx("Failed getting rtl sw chan index"); + } + + if (param_get(_parameter_handles.rc_map_sas_mode_sw, &(_parameters.rc_map_sas_mode_sw)) != OK) { + warnx("Failed getting sas mode sw chan index"); + } + + if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) { + warnx("Failed getting offboard control mode sw chan index"); + } + + if (param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)) != OK) { + warnx("Failed getting mode aux 1 index"); + } + + if (param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2)) != OK) { + warnx("Failed getting mode aux 2 index"); + } + + if (param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)) != OK) { + warnx("Failed getting mode aux 3 index"); + } + + if (param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)) != OK) { + warnx("Failed getting mode aux 4 index"); + } + + if (param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)) != OK) { + warnx("Failed getting mode aux 5 index"); + } + + if (param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll)) != OK) { + warnx("Failed getting rc scaling for roll"); + } + + if (param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch)) != OK) { + warnx("Failed getting rc scaling for pitch"); + } + + if (param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw)) != OK) { + warnx("Failed getting rc scaling for yaw"); + } + + if (param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps)) != OK) { + warnx("Failed getting rc scaling for flaps"); + } + + /* update RC function mappings */ + _rc.function[THROTTLE] = _parameters.rc_map_throttle - 1; + _rc.function[ROLL] = _parameters.rc_map_roll - 1; + _rc.function[PITCH] = _parameters.rc_map_pitch - 1; + _rc.function[YAW] = _parameters.rc_map_yaw - 1; + + _rc.function[OVERRIDE] = _parameters.rc_map_manual_override_sw - 1; + _rc.function[AUTO_MODE] = _parameters.rc_map_auto_mode_sw - 1; + + _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; + + _rc.function[MANUAL_MODE] = _parameters.rc_map_manual_mode_sw - 1; + _rc.function[RTL] = _parameters.rc_map_rtl_sw - 1; + _rc.function[SAS_MODE] = _parameters.rc_map_sas_mode_sw - 1; + _rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_ctrl_mode_sw - 1; + + _rc.function[AUX_1] = _parameters.rc_map_aux1 - 1; + _rc.function[AUX_2] = _parameters.rc_map_aux2 - 1; + _rc.function[AUX_3] = _parameters.rc_map_aux3 - 1; + _rc.function[AUX_4] = _parameters.rc_map_aux4 - 1; + _rc.function[AUX_5] = _parameters.rc_map_aux5 - 1; + + /* gyro offsets */ + param_get(_parameter_handles.gyro_offset[0], &(_parameters.gyro_offset[0])); + param_get(_parameter_handles.gyro_offset[1], &(_parameters.gyro_offset[1])); + param_get(_parameter_handles.gyro_offset[2], &(_parameters.gyro_offset[2])); + param_get(_parameter_handles.gyro_scale[0], &(_parameters.gyro_scale[0])); + param_get(_parameter_handles.gyro_scale[1], &(_parameters.gyro_scale[1])); + param_get(_parameter_handles.gyro_scale[2], &(_parameters.gyro_scale[2])); + + /* accel offsets */ + param_get(_parameter_handles.accel_offset[0], &(_parameters.accel_offset[0])); + param_get(_parameter_handles.accel_offset[1], &(_parameters.accel_offset[1])); + param_get(_parameter_handles.accel_offset[2], &(_parameters.accel_offset[2])); + param_get(_parameter_handles.accel_scale[0], &(_parameters.accel_scale[0])); + param_get(_parameter_handles.accel_scale[1], &(_parameters.accel_scale[1])); + param_get(_parameter_handles.accel_scale[2], &(_parameters.accel_scale[2])); + + /* mag offsets */ + param_get(_parameter_handles.mag_offset[0], &(_parameters.mag_offset[0])); + param_get(_parameter_handles.mag_offset[1], &(_parameters.mag_offset[1])); + param_get(_parameter_handles.mag_offset[2], &(_parameters.mag_offset[2])); + /* mag scaling */ + param_get(_parameter_handles.mag_scale[0], &(_parameters.mag_scale[0])); + param_get(_parameter_handles.mag_scale[1], &(_parameters.mag_scale[1])); + param_get(_parameter_handles.mag_scale[2], &(_parameters.mag_scale[2])); + + /* Airspeed offset */ + param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa)); + + /* scaling of ADC ticks to battery voltage */ + if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) { + warnx("Failed updating voltage scaling param"); + } + + return OK; +} + +void +Sensors::accel_init() +{ + int fd; + + fd = open(ACCEL_DEVICE_PATH, 0); + + if (fd < 0) { + warn("%s", ACCEL_DEVICE_PATH); + errx(1, "FATAL: no accelerometer found"); + + } else { + /* set the accel internal sampling rate up to at leat 500Hz */ + ioctl(fd, ACCELIOCSSAMPLERATE, 500); + + /* set the driver to poll at 500Hz */ + ioctl(fd, SENSORIOCSPOLLRATE, 500); + + warnx("using system accel"); + close(fd); + } +} + +void +Sensors::gyro_init() +{ + int fd; + + fd = open(GYRO_DEVICE_PATH, 0); + + if (fd < 0) { + warn("%s", GYRO_DEVICE_PATH); + errx(1, "FATAL: no gyro found"); + + } else { + /* set the gyro internal sampling rate up to at leat 500Hz */ + ioctl(fd, GYROIOCSSAMPLERATE, 500); + + /* set the driver to poll at 500Hz */ + ioctl(fd, SENSORIOCSPOLLRATE, 500); + + warnx("using system gyro"); + close(fd); + } +} + +void +Sensors::mag_init() +{ + int fd; + + fd = open(MAG_DEVICE_PATH, 0); + + if (fd < 0) { + warn("%s", MAG_DEVICE_PATH); + errx(1, "FATAL: no magnetometer found"); + } + + /* set the mag internal poll rate to at least 150Hz */ + ioctl(fd, MAGIOCSSAMPLERATE, 150); + + /* set the driver to poll at 150Hz */ + ioctl(fd, SENSORIOCSPOLLRATE, 150); + + close(fd); +} + +void +Sensors::baro_init() +{ + int fd; + + fd = open(BARO_DEVICE_PATH, 0); + + if (fd < 0) { + warn("%s", BARO_DEVICE_PATH); + warnx("No barometer found, ignoring"); + } + + /* set the driver to poll at 150Hz */ + ioctl(fd, SENSORIOCSPOLLRATE, 150); + + close(fd); +} + +void +Sensors::adc_init() +{ + + _fd_adc = open(ADC_DEVICE_PATH, O_RDONLY | O_NONBLOCK); + + if (_fd_adc < 0) { + warn(ADC_DEVICE_PATH); + warnx("FATAL: no ADC found"); + } +} + +void +Sensors::accel_poll(struct sensor_combined_s &raw) +{ + bool accel_updated; + orb_check(_accel_sub, &accel_updated); + + if (accel_updated) { + struct accel_report accel_report; + + orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); + + raw.accelerometer_m_s2[0] = accel_report.x; + raw.accelerometer_m_s2[1] = accel_report.y; + raw.accelerometer_m_s2[2] = accel_report.z; + + raw.accelerometer_raw[0] = accel_report.x_raw; + raw.accelerometer_raw[1] = accel_report.y_raw; + raw.accelerometer_raw[2] = accel_report.z_raw; + + raw.accelerometer_counter++; + } +} + +void +Sensors::gyro_poll(struct sensor_combined_s &raw) +{ + bool gyro_updated; + orb_check(_gyro_sub, &gyro_updated); + + if (gyro_updated) { + struct gyro_report gyro_report; + + orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); + + raw.gyro_rad_s[0] = gyro_report.x; + raw.gyro_rad_s[1] = gyro_report.y; + raw.gyro_rad_s[2] = gyro_report.z; + + raw.gyro_raw[0] = gyro_report.x_raw; + raw.gyro_raw[1] = gyro_report.y_raw; + raw.gyro_raw[2] = gyro_report.z_raw; + + raw.gyro_counter++; + } +} + +void +Sensors::mag_poll(struct sensor_combined_s &raw) +{ + bool mag_updated; + orb_check(_mag_sub, &mag_updated); + + if (mag_updated) { + struct mag_report mag_report; + + orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); + + raw.magnetometer_ga[0] = mag_report.x; + raw.magnetometer_ga[1] = mag_report.y; + raw.magnetometer_ga[2] = mag_report.z; + + raw.magnetometer_raw[0] = mag_report.x_raw; + raw.magnetometer_raw[1] = mag_report.y_raw; + raw.magnetometer_raw[2] = mag_report.z_raw; + + raw.magnetometer_counter++; + } +} + +void +Sensors::baro_poll(struct sensor_combined_s &raw) +{ + bool baro_updated; + orb_check(_baro_sub, &baro_updated); + + if (baro_updated) { + + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_barometer); + + raw.baro_pres_mbar = _barometer.pressure; // Pressure in mbar + raw.baro_alt_meter = _barometer.altitude; // Altitude in meters + raw.baro_temp_celcius = _barometer.temperature; // Temperature in degrees celcius + + raw.baro_counter++; + } +} + +void +Sensors::diff_pres_poll(struct sensor_combined_s &raw) +{ + bool updated; + orb_check(_diff_pres_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres); + + raw.differential_pressure_pa = _diff_pres.differential_pressure_pa; + raw.differential_pressure_counter++; + + _airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_pa); + _airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar*1e2f, + raw.baro_pres_mbar*1e2f, raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); + + /* announce the airspeed if needed, just publish else */ + if (_airspeed_pub > 0) { + orb_publish(ORB_ID(airspeed), _airspeed_pub, &_airspeed); + + } else { + _airspeed_pub = orb_advertise(ORB_ID(airspeed), &_airspeed); + } + } +} + +void +Sensors::vehicle_status_poll() +{ + struct vehicle_status_s vstatus; + 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); + + /* switching from non-HIL to HIL mode */ + //printf("[sensors] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled); + if (vstatus.flag_hil_enabled && !_hil_enabled) { + _hil_enabled = true; + _publishing = false; + + /* switching from HIL to non-HIL mode */ + + } else if (!_publishing && !_hil_enabled) { + _hil_enabled = false; + _publishing = true; + } + } +} + +void +Sensors::parameter_update_poll(bool forced) +{ + bool param_updated; + + /* Check if any parameter has changed */ + orb_check(_params_sub, ¶m_updated); + + if (param_updated || forced) { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), _params_sub, &update); + + /* update parameters */ + parameters_update(); + + /* update sensor offsets */ + int fd = open(GYRO_DEVICE_PATH, 0); + struct gyro_scale gscale = { + _parameters.gyro_offset[0], + _parameters.gyro_scale[0], + _parameters.gyro_offset[1], + _parameters.gyro_scale[1], + _parameters.gyro_offset[2], + _parameters.gyro_scale[2], + }; + + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) + warn("WARNING: failed to set scale / offsets for gyro"); + + close(fd); + + fd = open(ACCEL_DEVICE_PATH, 0); + struct accel_scale ascale = { + _parameters.accel_offset[0], + _parameters.accel_scale[0], + _parameters.accel_offset[1], + _parameters.accel_scale[1], + _parameters.accel_offset[2], + _parameters.accel_scale[2], + }; + + if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) + warn("WARNING: failed to set scale / offsets for accel"); + + close(fd); + + fd = open(MAG_DEVICE_PATH, 0); + struct mag_scale mscale = { + _parameters.mag_offset[0], + _parameters.mag_scale[0], + _parameters.mag_offset[1], + _parameters.mag_scale[1], + _parameters.mag_offset[2], + _parameters.mag_scale[2], + }; + + if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) + warn("WARNING: failed to set scale / offsets for mag"); + + close(fd); + +#if 0 + printf("CH0: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[0], (int)_parameters.min[0], (int)(_rc.chan[0].scaling_factor * 10000), (int)(_rc.chan[0].mid), (int)_rc.function[0]); + printf("CH1: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[1], (int)_parameters.min[1], (int)(_rc.chan[1].scaling_factor * 10000), (int)(_rc.chan[1].mid), (int)_rc.function[1]); + printf("MAN: %d %d\n", (int)(_rc.chan[0].scaled * 100), (int)(_rc.chan[1].scaled * 100)); + fflush(stdout); + usleep(5000); +#endif + } +} + +void +Sensors::adc_poll(struct sensor_combined_s &raw) +{ + + /* rate limit to 100 Hz */ + if (hrt_absolute_time() - _last_adc >= 10000) { + /* make space for a maximum of eight channels */ + struct adc_msg_s buf_adc[8]; + /* read all channels available */ + int ret = read(_fd_adc, &buf_adc, sizeof(buf_adc)); + + for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) { + + if (ret >= (int)sizeof(buf_adc[0])) { + + /* Save raw voltage values */ + 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); + } + + /* look for specific channels and process the raw voltage to measurement data */ + if (ADC_BATTERY_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { + /* Voltage in volts */ + float voltage = (buf_adc[i].am_data * _parameters.battery_voltage_scaling); + + if (voltage > VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) { + + /* one-time initialization of low-pass value to avoid long init delays */ + if (_battery_status.voltage_v < 3.0f) { + _battery_status.voltage_v = voltage; + } + + _battery_status.timestamp = hrt_absolute_time(); + _battery_status.voltage_v = (BAT_VOL_LOWPASS_1 * (_battery_status.voltage_v + BAT_VOL_LOWPASS_2 * voltage));; + /* current and discharge are unknown */ + _battery_status.current_a = -1.0f; + _battery_status.discharged_mah = -1.0f; + + /* announce the battery voltage if needed, just publish else */ + if (_battery_pub > 0) { + orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status); + + } else { + _battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status); + } + } + + } else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { + + /* calculate airspeed, raw is the difference from */ + float voltage = (float)(buf_adc[i].am_data ) * 3.3f / 4096.0f * 2.0f; //V_ref/4096 * (voltage divider factor) + + /** + * The voltage divider pulls the signal down, only act on + * a valid voltage from a connected sensor + */ + if (voltage > 0.4f) { + + float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor + + _diff_pres.timestamp = hrt_absolute_time(); + _diff_pres.differential_pressure_pa = diff_pres_pa; + _diff_pres.voltage = voltage; + + /* announce the airspeed if needed, just publish else */ + if (_diff_pres_pub > 0) { + orb_publish(ORB_ID(differential_pressure), _diff_pres_pub, &_diff_pres); + + } else { + _diff_pres_pub = orb_advertise(ORB_ID(differential_pressure), &_diff_pres); + } + } + } + + _last_adc = hrt_absolute_time(); + } + } + } +} + +#if CONFIG_HRT_PPM +void +Sensors::ppm_poll() +{ + + /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ + bool rc_updated; + orb_check(_rc_sub, &rc_updated); + + if (rc_updated) { + struct rc_input_values rc_input; + + orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); + + struct manual_control_setpoint_s manual_control; + + /* initialize to default values */ + manual_control.roll = NAN; + manual_control.pitch = NAN; + manual_control.yaw = NAN; + manual_control.throttle = NAN; + + manual_control.manual_mode_switch = NAN; + manual_control.manual_sas_switch = NAN; + manual_control.return_to_launch_switch = NAN; + manual_control.auto_offboard_input_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; + + unsigned channel_limit = rc_input.channel_count; + + if (channel_limit > _rc_max_chan_count) + channel_limit = _rc_max_chan_count; + + /* we are accepting this message */ + _ppm_last_valid = rc_input.timestamp; + + /* Read out values from raw message */ + 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]) + rc_input.values[i] = _parameters.min[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. + * + * 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), + * the total range is 2 (-1..1). + * If center (trim) == min, scale to 0..1, if center (trim) == max, + * scale to -1..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 (rc_input.values[i] > (_parameters.trim[i] + _parameters.dz[i])) { + _rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(_parameters.max[i] - _parameters.trim[i] - _parameters.dz[i]); + + } else if (rc_input.values[i] < (_parameters.trim[i] - _parameters.dz[i])) { + _rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i] + _parameters.dz[i]) / (float)(_parameters.trim[i] - _parameters.min[i] - _parameters.dz[i]); + + } else { + /* in the configured dead zone, output zero */ + _rc.chan[i].scaled = 0.0f; + } + + _rc.chan[i].scaled *= _parameters.rev[i]; + + /* handle any parameter-induced blowups */ + if (!isfinite(_rc.chan[i].scaled)) + _rc.chan[i].scaled = 0.0f; + } + + _rc.chan_count = rc_input.channel_count; + _rc.timestamp = rc_input.timestamp; + + manual_control.timestamp = rc_input.timestamp; + + /* 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; + } + + /* override switch input */ + manual_control.manual_override_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OVERRIDE]].scaled); + + /* mode switch input */ + manual_control.auto_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[AUTO_MODE]].scaled); + + /* 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; + } + } + + if (_rc.function[MANUAL_MODE] >= 0) { + manual_control.manual_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MANUAL_MODE]].scaled); + } + + if (_rc.function[SAS_MODE] >= 0) { + manual_control.manual_sas_switch = limit_minus_one_to_one(_rc.chan[_rc.function[SAS_MODE]].scaled); + } + + if (_rc.function[RTL] >= 0) { + manual_control.return_to_launch_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RTL]].scaled); + } + + if (_rc.function[OFFBOARD_MODE] >= 0) { + manual_control.auto_offboard_input_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); + } + + /* check if ready for publishing */ + 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); + + } else { + _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); + } + } + +} +#endif + +void +Sensors::task_main_trampoline(int argc, char *argv[]) +{ + sensors::g_sensors->task_main(); +} + +void +Sensors::task_main() +{ + + /* inform about start */ + printf("[sensors] Initializing..\n"); + fflush(stdout); + + /* start individual sensors */ + accel_init(); + gyro_init(); + mag_init(); + baro_init(); + adc_init(); + + /* + * do subscriptions + */ + _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); + _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); + _mag_sub = orb_subscribe(ORB_ID(sensor_mag)); + _rc_sub = orb_subscribe(ORB_ID(input_rc)); + _baro_sub = orb_subscribe(ORB_ID(sensor_baro)); + _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); + _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + + /* rate limit vehicle status updates to 5Hz */ + orb_set_interval(_vstatus_sub, 200); + + /* + * do advertisements + */ + struct sensor_combined_s raw; + memset(&raw, 0, sizeof(raw)); + raw.timestamp = hrt_absolute_time(); + raw.adc_voltage_v[0] = 0.0f; + raw.adc_voltage_v[1] = 0.0f; + raw.adc_voltage_v[2] = 0.0f; + raw.adc_voltage_v[3] = 0.0f; + + memset(&_battery_status, 0, sizeof(_battery_status)); + _battery_status.voltage_v = BAT_VOL_INITIAL; + + /* get a set of initial values */ + accel_poll(raw); + gyro_poll(raw); + mag_poll(raw); + baro_poll(raw); + diff_pres_poll(raw); + + parameter_update_poll(true /* forced */); + + /* advertise the sensor_combined topic and make the initial publication */ + _sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw); + + /* wakeup source(s) */ + struct pollfd fds[1]; + + /* use the gyro to pace output - XXX BROKEN if we are using the L3GD20 */ + fds[0].fd = _gyro_sub; + fds[0].events = POLLIN; + + 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); + + /* check vehicle status for changes to publication state */ + vehicle_status_poll(); + + /* check parameters for updates */ + parameter_update_poll(); + + /* store the time closest to all measurements (this is bogus, sensor timestamps should be propagated...) */ + raw.timestamp = hrt_absolute_time(); + + /* copy most recent sensor data */ + gyro_poll(raw); + accel_poll(raw); + mag_poll(raw); + baro_poll(raw); + + /* check battery voltage */ + adc_poll(raw); + + diff_pres_poll(raw); + + /* Inform other processes that new data is available to copy */ + if (_publishing) + orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw); + +#ifdef CONFIG_HRT_PPM + /* Look for new r/c input data */ + ppm_poll(); +#endif + + perf_end(_loop_perf); + } + + printf("[sensors] exiting.\n"); + + _sensors_task = -1; + _exit(0); +} + +int +Sensors::start() +{ + ASSERT(_sensors_task == -1); + + /* start the task */ + _sensors_task = task_spawn_cmd("sensors_task", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2048, + (main_t)&Sensors::task_main_trampoline, + nullptr); + + if (_sensors_task < 0) { + warn("task start failed"); + return -errno; + } + + return OK; +} + +int sensors_main(int argc, char *argv[]) +{ + if (argc < 1) + errx(1, "usage: sensors {start|stop|status}"); + + if (!strcmp(argv[1], "start")) { + + if (sensors::g_sensors != nullptr) + errx(1, "sensors task already running"); + + sensors::g_sensors = new Sensors; + + if (sensors::g_sensors == nullptr) + errx(1, "sensors task alloc failed"); + + if (OK != sensors::g_sensors->start()) { + delete sensors::g_sensors; + sensors::g_sensors = nullptr; + err(1, "sensors task start failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + if (sensors::g_sensors == nullptr) + errx(1, "sensors task not running"); + + delete sensors::g_sensors; + sensors::g_sensors = nullptr; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (sensors::g_sensors) { + errx(0, "task is running"); + + } else { + errx(1, "task is not running"); + } + } + + warnx("unrecognized command"); + return 1; +} + diff --git a/src/modules/systemlib/airspeed.c b/src/modules/systemlib/airspeed.c new file mode 100644 index 000000000..15bb833a9 --- /dev/null +++ b/src/modules/systemlib/airspeed.c @@ -0,0 +1,111 @@ +/**************************************************************************** + * + * Copyright (C) 2012-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 airspeed.c + * Airspeed estimation + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * + */ + +#include <stdio.h> +#include <math.h> +#include "conversions.h" +#include "airspeed.h" + + +/** + * Calculate indicated airspeed. + * + * Note that the indicated airspeed is not the true airspeed because it + * lacks the air density compensation. Use the calc_true_airspeed functions to get + * the true airspeed. + * + * @param differential_pressure total_ pressure - static pressure + * @return indicated airspeed in m/s + */ +float calc_indicated_airspeed(float differential_pressure) +{ + + if (differential_pressure > 0) { + return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + } else { + return -sqrtf((2.0f*fabs(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + } + +} + +/** + * Calculate true airspeed from indicated airspeed. + * + * Note that the true airspeed is NOT the groundspeed, because of the effects of wind + * + * @param speed_indicated current indicated airspeed + * @param pressure_ambient pressure at the side of the tube/airplane + * @param temperature_celsius air temperature in degrees celcius + * @return true airspeed in m/s + */ +float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_ambient, float temperature_celsius) +{ + return speed_indicated * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / get_air_density(pressure_ambient, temperature_celsius)); +} + +/** + * Directly calculate true airspeed + * + * Note that the true airspeed is NOT the groundspeed, because of the effects of wind + * + * @param total_pressure pressure inside the pitot/prandtl tube + * @param static_pressure pressure at the side of the tube/airplane + * @param temperature_celsius air temperature in degrees celcius + * @return true airspeed in m/s + */ +float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius) +{ + float density = get_air_density(static_pressure, temperature_celsius); + if (density < 0.0001f || !isfinite(density)) { + density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C; +// printf("[airspeed] Invalid air density, using density at sea level\n"); + } + + float pressure_difference = total_pressure - static_pressure; + + if(pressure_difference > 0) { + return sqrtf((2.0f*(pressure_difference)) / density); + } else + { + return -sqrtf((2.0f*fabs(pressure_difference)) / density); + } +} diff --git a/src/modules/systemlib/airspeed.h b/src/modules/systemlib/airspeed.h new file mode 100644 index 000000000..def53f0c1 --- /dev/null +++ b/src/modules/systemlib/airspeed.h @@ -0,0 +1,90 @@ +/**************************************************************************** + * + * Copyright (C) 2012-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 airspeed.h + * Airspeed estimation declarations + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * + */ + +#ifndef AIRSPEED_H_ +#define AIRSPEED_H_ + +#include "math.h" +#include "conversions.h" + + __BEGIN_DECLS + + /** + * Calculate indicated airspeed. + * + * Note that the indicated airspeed is not the true airspeed because it + * lacks the air density compensation. Use the calc_true_airspeed functions to get + * the true airspeed. + * + * @param total_pressure pressure inside the pitot/prandtl tube + * @param static_pressure pressure at the side of the tube/airplane + * @return indicated airspeed in m/s + */ + __EXPORT float calc_indicated_airspeed(float differential_pressure); + + /** + * Calculate true airspeed from indicated airspeed. + * + * Note that the true airspeed is NOT the groundspeed, because of the effects of wind + * + * @param speed_indicated current indicated airspeed + * @param pressure_ambient pressure at the side of the tube/airplane + * @param temperature_celsius air temperature in degrees celcius + * @return true airspeed in m/s + */ + __EXPORT float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_ambient, float temperature_celsius); + + /** + * Directly calculate true airspeed + * + * Note that the true airspeed is NOT the groundspeed, because of the effects of wind + * + * @param total_pressure pressure inside the pitot/prandtl tube + * @param static_pressure pressure at the side of the tube/airplane + * @param temperature_celsius air temperature in degrees celcius + * @return true airspeed in m/s + */ + __EXPORT float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius); + +__END_DECLS + +#endif diff --git a/src/modules/systemlib/bson/tinybson.c b/src/modules/systemlib/bson/tinybson.c new file mode 100644 index 000000000..8aca6a25d --- /dev/null +++ b/src/modules/systemlib/bson/tinybson.c @@ -0,0 +1,517 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file tinybson.c + * + * A simple subset SAX-style BSON parser and generator. + */ + +#include <unistd.h> +#include <string.h> +#include <stdlib.h> +#include <systemlib/err.h> + +#include "tinybson.h" + +#if 0 +# define debug(fmt, args...) do { warnx("BSON: " fmt, ##args); } while(0) +#else +# define debug(fmt, args...) do { } while(0) +#endif + +#define CODER_CHECK(_c) do { if (_c->dead) { debug("coder dead"); return -1; }} while(0) +#define CODER_KILL(_c, _reason) do { debug("killed: %s", _reason); _c->dead = true; return -1; } while(0) + +static int +read_x(bson_decoder_t decoder, void *p, size_t s) +{ + CODER_CHECK(decoder); + + if (decoder->fd > -1) + return (read(decoder->fd, p, s) == (int)s) ? 0 : -1; + + if (decoder->buf != NULL) { + /* staged operations to avoid integer overflow for corrupt data */ + if (s >= decoder->bufsize) + CODER_KILL(decoder, "buffer too small for read"); + if ((decoder->bufsize - s) < decoder->bufpos) + CODER_KILL(decoder, "not enough data for read"); + memcpy(p, (decoder->buf + decoder->bufpos), s); + decoder->bufpos += s; + return 0; + } + debug("no source"); + return -1; +} + +static int +read_int8(bson_decoder_t decoder, int8_t *b) +{ + return read_x(decoder, b, sizeof(*b)); +} + +static int +read_int32(bson_decoder_t decoder, int32_t *i) +{ + return read_x(decoder, i, sizeof(*i)); +} + +static int +read_int64(bson_decoder_t decoder, int64_t *i) +{ + return read_x(decoder, i, sizeof(*i)); +} + +static int +read_double(bson_decoder_t decoder, double *d) +{ + return read_x(decoder, d, sizeof(*d)); +} + +int +bson_decoder_init_file(bson_decoder_t decoder, int fd, bson_decoder_callback callback, void *private) +{ + int32_t junk; + + decoder->fd = fd; + decoder->buf = NULL; + decoder->dead = false; + decoder->callback = callback; + decoder->private = private; + decoder->nesting = 1; + decoder->pending = 0; + decoder->node.type = BSON_UNDEFINED; + + /* read and discard document size */ + if (read_int32(decoder, &junk)) + CODER_KILL(decoder, "failed discarding length"); + + /* ready for decoding */ + return 0; +} + +int +bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_decoder_callback callback, void *private) +{ + int32_t len; + + /* argument sanity */ + if ((buf == NULL) || (callback == NULL)) + return -1; + + decoder->fd = -1; + decoder->buf = (uint8_t *)buf; + decoder->dead = false; + if (bufsize == 0) { + decoder->bufsize = *(uint32_t *)buf; + debug("auto-detected %u byte object", decoder->bufsize); + } else { + decoder->bufsize = bufsize; + } + decoder->bufpos = 0; + decoder->callback = callback; + decoder->private = private; + decoder->nesting = 1; + decoder->pending = 0; + decoder->node.type = BSON_UNDEFINED; + + /* read and discard document size */ + if (read_int32(decoder, &len)) + CODER_KILL(decoder, "failed reading length"); + if ((len > 0) && (len > (int)decoder->bufsize)) + CODER_KILL(decoder, "document length larger than buffer"); + + /* ready for decoding */ + return 0; +} + +int +bson_decoder_next(bson_decoder_t decoder) +{ + int8_t tbyte; + int32_t tint; + unsigned nlen; + + CODER_CHECK(decoder); + + /* if the previous node was EOO, pop a nesting level */ + if (decoder->node.type == BSON_EOO) { + if (decoder->nesting > 0) + decoder->nesting--; + + /* if the nesting level is now zero, the top-level document is done */ + if (decoder->nesting == 0) { + /* like kill but not an error */ + debug("nesting is zero, document is done"); + decoder->fd = -1; + + /* return end-of-file to the caller */ + return 0; + } + } + + /* if there are unread bytes pending in the stream, discard them */ + while (decoder->pending > 0) { + if (read_int8(decoder, &tbyte)) + CODER_KILL(decoder, "read error discarding pending bytes"); + + decoder->pending--; + } + + /* get the type byte */ + if (read_int8(decoder, &tbyte)) + CODER_KILL(decoder, "read error on type byte"); + + decoder->node.type = tbyte; + decoder->pending = 0; + + debug("got type byte 0x%02x", decoder->node.type); + + /* EOO is special; it has no name/data following */ + if (decoder->node.type == BSON_EOO) { + decoder->node.name[0] = '\0'; + } else { + + /* get the node name */ + nlen = 0; + + for (;;) { + if (nlen >= BSON_MAXNAME) + CODER_KILL(decoder, "node name overflow"); + + if (read_int8(decoder, (int8_t *)&decoder->node.name[nlen])) + CODER_KILL(decoder, "read error on node name"); + + if (decoder->node.name[nlen] == '\0') + break; + + nlen++; + } + + debug("got name '%s'", decoder->node.name); + + switch (decoder->node.type) { + case BSON_BOOL: + if (read_int8(decoder, &tbyte)) + CODER_KILL(decoder, "read error on BSON_BOOL"); + decoder->node.b = (tbyte != 0); + break; + + case BSON_INT32: + if (read_int32(decoder, &tint)) + CODER_KILL(decoder, "read error on BSON_INT"); + decoder->node.i = tint; + break; + + case BSON_INT64: + if (read_int64(decoder, &decoder->node.i)) + CODER_KILL(decoder, "read error on BSON_INT"); + + break; + + case BSON_DOUBLE: + if (read_double(decoder, &decoder->node.d)) + CODER_KILL(decoder, "read error on BSON_DOUBLE"); + + break; + + case BSON_STRING: + if (read_int32(decoder, &decoder->pending)) + CODER_KILL(decoder, "read error on BSON_STRING length"); + break; + + case BSON_BINDATA: + if (read_int32(decoder, &decoder->pending)) + CODER_KILL(decoder, "read error on BSON_BINDATA size"); + + if (read_int8(decoder, &tbyte)) + CODER_KILL(decoder, "read error on BSON_BINDATA subtype"); + + decoder->node.subtype = tbyte; + break; + + /* XXX currently not supporting other types */ + default: + CODER_KILL(decoder, "unsupported node type"); + } + } + + /* call the callback and pass its results back */ + return decoder->callback(decoder, decoder->private, &decoder->node); +} + +int +bson_decoder_copy_data(bson_decoder_t decoder, void *buf) +{ + int result; + + CODER_CHECK(decoder); + + /* copy data */ + result = read_x(decoder, buf, decoder->pending); + + if (result != 0) + CODER_KILL(decoder, "read error on copy_data"); + + /* pending count is discharged */ + decoder->pending = 0; + return 0; +} + +size_t +bson_decoder_data_pending(bson_decoder_t decoder) +{ + return decoder->pending; +} + +static int +write_x(bson_encoder_t encoder, const void *p, size_t s) +{ + CODER_CHECK(encoder); + + if (encoder->fd > -1) + return (write(encoder->fd, p, s) == (int)s) ? 0 : -1; + + /* do we need to extend the buffer? */ + while ((encoder->bufpos + s) > encoder->bufsize) { + if (!encoder->realloc_ok) + CODER_KILL(encoder, "fixed-size buffer overflow"); + + uint8_t *newbuf = realloc(encoder->buf, encoder->bufsize + BSON_BUF_INCREMENT); + if (newbuf == NULL) + CODER_KILL(encoder, "could not grow buffer"); + + encoder->buf = newbuf; + encoder->bufsize += BSON_BUF_INCREMENT; + debug("allocated %d bytes", BSON_BUF_INCREMENT); + } + + memcpy(encoder->buf + encoder->bufpos, p, s); + encoder->bufpos += s; + debug("appended %d bytes", s); + + return 0; +} + +static int +write_int8(bson_encoder_t encoder, int8_t b) +{ + return write_x(encoder, &b, sizeof(b)); +} + +static int +write_int32(bson_encoder_t encoder, int32_t i) +{ + return write_x(encoder, &i, sizeof(i)); +} + +static int +write_int64(bson_encoder_t encoder, int64_t i) +{ + return write_x(encoder, &i, sizeof(i)); +} + +static int +write_double(bson_encoder_t encoder, double d) +{ + return write_x(encoder, &d, sizeof(d)); +} + +static int +write_name(bson_encoder_t encoder, const char *name) +{ + size_t len = strlen(name); + + if (len > BSON_MAXNAME) + CODER_KILL(encoder, "node name too long"); + + return write_x(encoder, name, len + 1); +} + +int +bson_encoder_init_file(bson_encoder_t encoder, int fd) +{ + encoder->fd = fd; + encoder->buf = NULL; + encoder->dead = false; + + if (write_int32(encoder, 0)) + CODER_KILL(encoder, "write error on document length"); + + return 0; +} + +int +bson_encoder_init_buf(bson_encoder_t encoder, void *buf, unsigned bufsize) +{ + encoder->fd = -1; + encoder->buf = (uint8_t *)buf; + encoder->bufpos = 0; + encoder->dead = false; + if (encoder->buf == NULL) { + encoder->bufsize = 0; + encoder->realloc_ok = true; + } else { + encoder->bufsize = bufsize; + encoder->realloc_ok = false; + } + + if (write_int32(encoder, 0)) + CODER_KILL(encoder, "write error on document length"); + + return 0; +} + +int +bson_encoder_fini(bson_encoder_t encoder) +{ + CODER_CHECK(encoder); + + if (write_int8(encoder, BSON_EOO)) + CODER_KILL(encoder, "write error on document terminator"); + + /* hack to fix up length for in-buffer documents */ + if (encoder->buf != NULL) { + int32_t len = bson_encoder_buf_size(encoder); + memcpy(encoder->buf, &len, sizeof(len)); + } + + return 0; +} + +int +bson_encoder_buf_size(bson_encoder_t encoder) +{ + CODER_CHECK(encoder); + + if (encoder->fd > -1) + return -1; + + return encoder->bufpos; +} + +void * +bson_encoder_buf_data(bson_encoder_t encoder) +{ + /* note, no CODER_CHECK here as the caller has to clean up dead buffers */ + + if (encoder->fd > -1) + return NULL; + + return encoder->buf; +} + +int bson_encoder_append_bool(bson_encoder_t encoder, const char *name, bool value) +{ + CODER_CHECK(encoder); + + if (write_int8(encoder, BSON_BOOL) || + write_name(encoder, name) || + write_int8(encoder, value ? 1 : 0)) + CODER_KILL(encoder, "write error on BSON_BOOL"); + + return 0; +} + +int +bson_encoder_append_int(bson_encoder_t encoder, const char *name, int64_t value) +{ + bool result; + + CODER_CHECK(encoder); + + /* use the smallest encoding that will hold the value */ + if (value == (int32_t)value) { + debug("encoding %lld as int32", value); + result = write_int8(encoder, BSON_INT32) || + write_name(encoder, name) || + write_int32(encoder, value); + } else { + debug("encoding %lld as int64", value); + result = write_int8(encoder, BSON_INT64) || + write_name(encoder, name) || + write_int64(encoder, value); + } + if (result) + CODER_KILL(encoder, "write error on BSON_INT"); + + return 0; +} + +int +bson_encoder_append_double(bson_encoder_t encoder, const char *name, double value) +{ + CODER_CHECK(encoder); + + if (write_int8(encoder, BSON_DOUBLE) || + write_name(encoder, name) || + write_double(encoder, value)) + CODER_KILL(encoder, "write error on BSON_DOUBLE"); + + + return 0; +} + +int +bson_encoder_append_string(bson_encoder_t encoder, const char *name, const char *string) +{ + size_t len; + + CODER_CHECK(encoder); + + len = strlen(string) + 1; /* include trailing nul */ + + if (write_int8(encoder, BSON_STRING) || + write_name(encoder, name) || + write_int32(encoder, len) || + write_x(encoder, string, len)) + CODER_KILL(encoder, "write error on BSON_STRING"); + + return 0; +} + +int +bson_encoder_append_binary(bson_encoder_t encoder, const char *name, bson_binary_subtype_t subtype, size_t size, const void *data) +{ + CODER_CHECK(encoder); + + if (write_int8(encoder, BSON_BINDATA) || + write_name(encoder, name) || + write_int32(encoder, size) || + write_int8(encoder, subtype) || + write_x(encoder, data, size)) + CODER_KILL(encoder, "write error on BSON_BINDATA"); + + return 0; +} diff --git a/src/modules/systemlib/bson/tinybson.h b/src/modules/systemlib/bson/tinybson.h new file mode 100644 index 000000000..666f8191a --- /dev/null +++ b/src/modules/systemlib/bson/tinybson.h @@ -0,0 +1,275 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** +* @file tinybson.h +* +* A simple subset SAX-style BSON parser and generator. See http://bsonspec.org +* +* Some types and defines taken from the standalone BSON parser/generator +* in the Mongo C connector. +*/ + +#ifndef _TINYBSON_H +#define _TINYBSON_H + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> + +/** subset of the BSON node types we might care about */ +typedef enum { + BSON_EOO = 0, + BSON_DOUBLE = 1, + BSON_STRING = 2, + BSON_OBJECT = 3, + BSON_ARRAY = 4, + BSON_BINDATA = 5, + BSON_UNDEFINED = 6, + BSON_BOOL = 8, + BSON_DATE = 9, + BSON_NULL = 10, + BSON_INT32 = 16, + BSON_INT64 = 18 +} bson_type_t; + +typedef enum bson_binary_subtype { + BSON_BIN_BINARY = 0, + BSON_BIN_USER = 128 +} bson_binary_subtype_t; + +/** + * Maximum node name length. + */ +#define BSON_MAXNAME 32 + +/** + * Buffer growth increment when writing to a buffer. + */ +#define BSON_BUF_INCREMENT 128 + +/** + * Node structure passed to the callback. + */ +typedef struct bson_node_s { + char name[BSON_MAXNAME]; + bson_type_t type; + bson_binary_subtype_t subtype; + union { + int64_t i; + double d; + bool b; + }; +} *bson_node_t; + +typedef struct bson_decoder_s *bson_decoder_t; + +/** + * Node callback. + * + * The node callback function's return value is returned by bson_decoder_next. + */ +typedef int (* bson_decoder_callback)(bson_decoder_t decoder, void *private, bson_node_t node); + +struct bson_decoder_s { + /* file reader state */ + int fd; + + /* buffer reader state */ + uint8_t *buf; + size_t bufsize; + unsigned bufpos; + + bool dead; + bson_decoder_callback callback; + void *private; + unsigned nesting; + struct bson_node_s node; + int32_t pending; +}; + +/** + * Initialise the decoder to read from a file. + * + * @param decoder Decoder state structure to be initialised. + * @param fd File to read BSON data from. + * @param callback Callback to be invoked by bson_decoder_next + * @param private Callback private data, stored in node. + * @return Zero on success. + */ +__EXPORT int bson_decoder_init_file(bson_decoder_t decoder, int fd, bson_decoder_callback callback, void *private); + +/** + * Initialise the decoder to read from a buffer in memory. + * + * @param decoder Decoder state structure to be initialised. + * @param buf Buffer to read from. + * @param bufsize Size of the buffer (BSON object may be smaller). May be + * passed as zero if the buffer size should be extracted from the + * BSON header only. + * @param callback Callback to be invoked by bson_decoder_next + * @param private Callback private data, stored in node. + * @return Zero on success. + */ +__EXPORT int bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_decoder_callback callback, void *private); + +/** + * Process the next node from the stream and invoke the callback. + * + * @param decoder Decoder state, must have been initialised with bson_decoder_init. + * @return -1 if parsing encountered an error, 0 if the BSON stream has ended, + * otherwise the return value from the callback. + */ +__EXPORT int bson_decoder_next(bson_decoder_t decoder); + +/** + * Copy node data. + * + * @param decoder Decoder state, must have been initialised with bson_decoder_init. + */ +__EXPORT int bson_decoder_copy_data(bson_decoder_t decoder, void *buf); + +/** + * Report copyable data size. + * + * @param decoder Decoder state, must have been initialised with bson_decoder_init. + */ +__EXPORT size_t bson_decoder_data_pending(bson_decoder_t decoder); + +/** + * Encoder state structure. + */ +typedef struct bson_encoder_s { + /* file writer state */ + int fd; + + /* buffer writer state */ + uint8_t *buf; + unsigned bufsize; + unsigned bufpos; + + bool realloc_ok; + bool dead; + +} *bson_encoder_t; + +/** + * Initialze the encoder for writing to a file. + * + * @param encoder Encoder state structure to be initialised. + * @param fd File to write to. + * @return Zero on success. + */ +__EXPORT int bson_encoder_init_file(bson_encoder_t encoder, int fd); + +/** + * Initialze the encoder for writing to a buffer. + * + * @param encoder Encoder state structure to be initialised. + * @param buf Buffer pointer to use, or NULL if the buffer + * should be allocated by the encoder. + * @param bufsize Maximum buffer size, or zero for no limit. If + * the buffer is supplied, the size of the supplied buffer. + * @return Zero on success. + */ +__EXPORT int bson_encoder_init_buf(bson_encoder_t encoder, void *buf, unsigned bufsize); + +/** + * Finalise the encoded stream. + * + * @param encoder The encoder to finalise. + */ +__EXPORT int bson_encoder_fini(bson_encoder_t encoder); + +/** + * Fetch the size of the encoded object; only valid for buffer operations. + */ +__EXPORT int bson_encoder_buf_size(bson_encoder_t encoder); + +/** + * Get a pointer to the encoded object buffer. + * + * Note that if the buffer was allocated by the encoder, it is the caller's responsibility + * to free this buffer. + */ +__EXPORT void *bson_encoder_buf_data(bson_encoder_t encoder); + +/** + * Append a boolean to the encoded stream. + * + * @param encoder Encoder state. + * @param name Node name. + * @param value Value to be encoded. + */ +__EXPORT int bson_encoder_append_bool(bson_encoder_t encoder, const char *name, bool value); + +/** + * Append an integer to the encoded stream. + * + * @param encoder Encoder state. + * @param name Node name. + * @param value Value to be encoded. + */ +__EXPORT int bson_encoder_append_int(bson_encoder_t encoder, const char *name, int64_t value); + +/** + * Append a double to the encoded stream + * + * @param encoder Encoder state. + * @param name Node name. + * @param value Value to be encoded. + */ +__EXPORT int bson_encoder_append_double(bson_encoder_t encoder, const char *name, double value); + +/** + * Append a string to the encoded stream. + * + * @param encoder Encoder state. + * @param name Node name. + * @param string Nul-terminated C string. + */ +__EXPORT int bson_encoder_append_string(bson_encoder_t encoder, const char *name, const char *string); + +/** + * Append a binary blob to the encoded stream. + * + * @param encoder Encoder state. + * @param name Node name. + * @param subtype Binary data subtype. + * @param size Data size. + * @param data Buffer containing data to be encoded. + */ +__EXPORT int bson_encoder_append_binary(bson_encoder_t encoder, const char *name, bson_binary_subtype_t subtype, size_t size, const void *data); + + +#endif diff --git a/src/modules/systemlib/conversions.c b/src/modules/systemlib/conversions.c new file mode 100644 index 000000000..ac94252c5 --- /dev/null +++ b/src/modules/systemlib/conversions.c @@ -0,0 +1,154 @@ +/**************************************************************************** + * + * 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 + * 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 conversions.c + * Implementation of commonly used conversions. + */ + +#include <nuttx/config.h> +#include <float.h> + +#include "conversions.h" + +int16_t +int16_t_from_bytes(uint8_t bytes[]) +{ + union { + uint8_t b[2]; + int16_t w; + } u; + + u.b[1] = bytes[0]; + u.b[0] = bytes[1]; + + return u.w; +} + +void rot2quat(const float R[9], float Q[4]) +{ + float q0_2; + float q1_2; + float q2_2; + float q3_2; + int32_t idx; + + /* conversion of rotation matrix to quaternion + * choose the largest component to begin with */ + q0_2 = (((1.0F + R[0]) + R[4]) + R[8]) / 4.0F; + q1_2 = (((1.0F + R[0]) - R[4]) - R[8]) / 4.0F; + q2_2 = (((1.0F - R[0]) + R[4]) - R[8]) / 4.0F; + q3_2 = (((1.0F - R[0]) - R[4]) + R[8]) / 4.0F; + + idx = 0; + + if (q0_2 < q1_2) { + q0_2 = q1_2; + + idx = 1; + } + + if (q0_2 < q2_2) { + q0_2 = q2_2; + idx = 2; + } + + if (q0_2 < q3_2) { + q0_2 = q3_2; + idx = 3; + } + + q0_2 = sqrtf(q0_2); + + /* solve for the remaining three components */ + if (idx == 0) { + q1_2 = q0_2; + q2_2 = (R[5] - R[7]) / 4.0F / q0_2; + q3_2 = (R[6] - R[2]) / 4.0F / q0_2; + q0_2 = (R[1] - R[3]) / 4.0F / q0_2; + + } else if (idx == 1) { + q2_2 = q0_2; + q1_2 = (R[5] - R[7]) / 4.0F / q0_2; + q3_2 = (R[3] + R[1]) / 4.0F / q0_2; + q0_2 = (R[6] + R[2]) / 4.0F / q0_2; + + } else if (idx == 2) { + q3_2 = q0_2; + q1_2 = (R[6] - R[2]) / 4.0F / q0_2; + q2_2 = (R[3] + R[1]) / 4.0F / q0_2; + q0_2 = (R[7] + R[5]) / 4.0F / q0_2; + + } else { + q1_2 = (R[1] - R[3]) / 4.0F / q0_2; + q2_2 = (R[6] + R[2]) / 4.0F / q0_2; + q3_2 = (R[7] + R[5]) / 4.0F / q0_2; + } + + /* return values */ + Q[0] = q1_2; + Q[1] = q2_2; + Q[2] = q3_2; + Q[3] = q0_2; +} + +void quat2rot(const float Q[4], float R[9]) +{ + float q0_2; + float q1_2; + float q2_2; + float q3_2; + + memset(&R[0], 0, 9U * sizeof(float)); + + q0_2 = Q[0] * Q[0]; + q1_2 = Q[1] * Q[1]; + q2_2 = Q[2] * Q[2]; + q3_2 = Q[3] * Q[3]; + + R[0] = ((q0_2 + q1_2) - q2_2) - q3_2; + R[3] = 2.0F * (Q[1] * Q[2] - Q[0] * Q[3]); + R[6] = 2.0F * (Q[1] * Q[3] + Q[0] * Q[2]); + R[1] = 2.0F * (Q[1] * Q[2] + Q[0] * Q[3]); + R[4] = ((q0_2 + q2_2) - q1_2) - q3_2; + R[7] = 2.0F * (Q[2] * Q[3] - Q[0] * Q[1]); + R[2] = 2.0F * (Q[1] * Q[3] - Q[0] * Q[2]); + R[5] = 2.0F * (Q[2] * Q[3] + Q[0] * Q[1]); + R[8] = ((q0_2 + q3_2) - q1_2) - q2_2; +} + +float get_air_density(float static_pressure, float temperature_celsius) +{ + return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS)); +} diff --git a/src/modules/systemlib/conversions.h b/src/modules/systemlib/conversions.h new file mode 100644 index 000000000..064426f21 --- /dev/null +++ b/src/modules/systemlib/conversions.h @@ -0,0 +1,90 @@ +/**************************************************************************** + * + * 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 conversions.h + * Definition of commonly used conversions. + * + * Includes bit / byte / geo representation and unit conversions. + */ + +#ifndef CONVERSIONS_H_ +#define CONVERSIONS_H_ +#include <float.h> +#include <stdint.h> +#include <systemlib/geo/geo.h> + +__BEGIN_DECLS + +/** + * Converts a signed 16 bit integer from big endian to little endian. + * + * This function is for commonly used 16 bit big endian sensor data, + * delivered by driver routines as two 8 bit numbers in big endian order. + * Common vendors with big endian representation are Invense, Bosch and + * Honeywell. ST micro devices tend to use a little endian representation. + */ +__EXPORT int16_t int16_t_from_bytes(uint8_t bytes[]); + +/** + * Converts a 3 x 3 rotation matrix to an unit quaternion. + * + * All orientations are expressed in NED frame. + * + * @param R rotation matrix to convert + * @param Q quaternion to write back to + */ +__EXPORT void rot2quat(const float R[9], float Q[4]); + +/** + * Converts an unit quaternion to a 3 x 3 rotation matrix. + * + * All orientations are expressed in NED frame. + * + * @param Q quaternion to convert + * @param R rotation matrix to write back to + */ +__EXPORT void quat2rot(const float Q[4], float R[9]); + +/** + * Calculates air density. + * + * @param static_pressure ambient pressure in millibar + * @param temperature_celcius air / ambient temperature in celcius + */ +__EXPORT float get_air_density(float static_pressure, float temperature_celsius); + +__END_DECLS + +#endif /* CONVERSIONS_H_ */ diff --git a/src/modules/systemlib/cpuload.c b/src/modules/systemlib/cpuload.c new file mode 100644 index 000000000..afc5b072c --- /dev/null +++ b/src/modules/systemlib/cpuload.c @@ -0,0 +1,170 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> + * Petri Tanskanen <petri.tanskanen@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 cpuload.c + * + * Measurement of CPU load of each individual task. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch> + */ +#include <nuttx/config.h> +#include <nuttx/sched.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> + +#include <arch/arch.h> + +#include <debug.h> + +#include <sys/time.h> + +#include <arch/board/board.h> +#include <drivers/drv_hrt.h> + +#include "cpuload.h" + +#ifdef CONFIG_SCHED_INSTRUMENTATION + +__EXPORT void sched_note_start(FAR struct tcb_s *tcb); +__EXPORT void sched_note_stop(FAR struct tcb_s *tcb); +__EXPORT void sched_note_switch(FAR struct tcb_s *pFromTcb, FAR struct tcb_s *pToTcb); + +__EXPORT struct system_load_s system_load; + +extern FAR struct _TCB *sched_gettcb(pid_t pid); + +void cpuload_initialize_once() +{ + system_load.start_time = hrt_absolute_time(); + int i; + + for (i = 0; i < CONFIG_MAX_TASKS; i++) { + system_load.tasks[i].valid = false; + } + + uint64_t now = hrt_absolute_time(); + + int static_tasks_count = 2; // there are at least 2 threads that should be initialized statically - "idle" and "init" + +#ifdef CONFIG_PAGING + static_tasks_count++; // include paging thread in initialization +#endif /* CONFIG_PAGING */ +#if CONFIG_SCHED_WORKQUEUE + static_tasks_count++; // include high priority work0 thread in initialization +#endif /* CONFIG_SCHED_WORKQUEUE */ +#if CONFIG_SCHED_LPWORK + static_tasks_count++; // include low priority work1 thread in initialization +#endif /* CONFIG_SCHED_WORKQUEUE */ + + // perform static initialization of "system" threads + for (system_load.total_count = 0; system_load.total_count < static_tasks_count; system_load.total_count++) + { + system_load.tasks[system_load.total_count].start_time = now; + system_load.tasks[system_load.total_count].total_runtime = 0; + system_load.tasks[system_load.total_count].curr_start_time = 0; + system_load.tasks[system_load.total_count].tcb = sched_gettcb(system_load.total_count); // it is assumed that these static threads have consecutive PIDs + system_load.tasks[system_load.total_count].valid = true; + } +} + +void sched_note_start(FAR struct tcb_s *tcb) +{ + /* search first free slot */ + int i; + + for (i = 1; i < CONFIG_MAX_TASKS; i++) { + if (!system_load.tasks[i].valid) { + /* slot is available */ + system_load.tasks[i].start_time = hrt_absolute_time(); + system_load.tasks[i].total_runtime = 0; + system_load.tasks[i].curr_start_time = 0; + system_load.tasks[i].tcb = tcb; + system_load.tasks[i].valid = true; + system_load.total_count++; + break; + } + } +} + +void sched_note_stop(FAR struct tcb_s *tcb) +{ + int i; + + for (i = 1; i < CONFIG_MAX_TASKS; i++) { + if (system_load.tasks[i].tcb->pid == tcb->pid) { + /* mark slot as fee */ + system_load.tasks[i].valid = false; + system_load.tasks[i].total_runtime = 0; + system_load.tasks[i].curr_start_time = 0; + system_load.tasks[i].tcb = NULL; + system_load.total_count--; + break; + } + } +} + +void sched_note_switch(FAR struct tcb_s *pFromTcb, FAR struct tcb_s *pToTcb) +{ + uint64_t new_time = hrt_absolute_time(); + + /* Kind of inefficient: find both tasks and update times */ + uint8_t both_found = 0; + + for (int i = 0; i < CONFIG_MAX_TASKS; i++) { + /* Task ending its current scheduling run */ + if (system_load.tasks[i].tcb->pid == pFromTcb->pid) { + //if (system_load.tasks[i].curr_start_time != 0) + { + system_load.tasks[i].total_runtime += new_time - system_load.tasks[i].curr_start_time; + } + both_found++; + + } else if (system_load.tasks[i].tcb->pid == pToTcb->pid) { + system_load.tasks[i].curr_start_time = new_time; + both_found++; + } + + /* Do only iterate as long as needed */ + if (both_found == 2) { + break; + } + } +} + +#endif /* CONFIG_SCHED_INSTRUMENTATION */ diff --git a/src/modules/systemlib/cpuload.h b/src/modules/systemlib/cpuload.h new file mode 100644 index 000000000..c7aa18d3c --- /dev/null +++ b/src/modules/systemlib/cpuload.h @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#ifdef CONFIG_SCHED_INSTRUMENTATION + +__BEGIN_DECLS + +#include <nuttx/sched.h> + +struct system_load_taskinfo_s { + uint64_t total_runtime; ///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load + uint64_t curr_start_time; ///< Start time of the current scheduling slot + uint64_t start_time; ///< FIRST start time of task + FAR struct tcb_s *tcb; ///< + bool valid; ///< Task is currently active / valid +}; + +struct system_load_s { + uint64_t start_time; ///< Global start time of measurements + struct system_load_taskinfo_s tasks[CONFIG_MAX_TASKS]; + uint8_t initialized; + int total_count; + int running_count; + int sleeping_count; +}; + +__EXPORT extern struct system_load_s system_load; + +__EXPORT void cpuload_initialize_once(void); + +__END_DECLS + +#endif diff --git a/src/modules/systemlib/err.c b/src/modules/systemlib/err.c new file mode 100644 index 000000000..6c0e876d1 --- /dev/null +++ b/src/modules/systemlib/err.c @@ -0,0 +1,193 @@ +/**************************************************************************** + * + * 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 err.c + * + * Simple error/warning functions, heavily inspired by the BSD functions of + * the same names. + */ + +#include <nuttx/config.h> + +#include <stdlib.h> +#include <errno.h> +#include <string.h> + +#include "err.h" + +#define NOCODE 1000 /* larger than maximum errno */ + +#if CONFIG_NFILE_STREAMS > 0 +# include <stdio.h> +#elif defined(CONFIG_ARCH_LOWPUTC) +# include <debug.h> +extern int lib_lowvprintf(const char *fmt, va_list ap); +#else +# warning Cannot output without one of CONFIG_NFILE_STREAMS or CONFIG_ARCH_LOWPUTC +#endif + +const char * +getprogname(void) +{ +#if CONFIG_TASK_NAME_SIZE > 0 + FAR struct tcb_s *thisproc = sched_self(); + + return thisproc->name; +#else + return "app"; +#endif +} + +static void +warnerr_core(int errcode, const char *fmt, va_list args) +{ +#if CONFIG_NFILE_STREAMS > 0 + fprintf(stderr, "%s: ", getprogname()); + vfprintf(stderr, fmt, args); + + /* convenience as many parts of NuttX use negative errno */ + if (errcode < 0) + errcode = -errcode; + + if (errcode < NOCODE) + fprintf(stderr, ": %s", strerror(errcode)); + + fprintf(stderr, "\n"); +#elif CONFIG_ARCH_LOWPUTC + lowsyslog("%s: ", getprogname()); + lowvyslog(fmt, args); + + /* convenience as many parts of NuttX use negative errno */ + if (errcode < 0) + errcode = -errcode; + + if (errcode < NOCODE) + lowsyslog(": %s", strerror(errcode)); + + lowsyslog("\n"); +#endif +} + +void +err(int exitcode, const char *fmt, ...) +{ + va_list args; + + va_start(args, fmt); + verr(exitcode, fmt, args); +} + +void +verr(int exitcode, const char *fmt, va_list args) +{ + warnerr_core(errno, fmt, args); + exit(exitcode); +} + +void +errc(int exitcode, int errcode, const char *fmt, ...) +{ + va_list args; + + va_start(args, fmt); + verrc(exitcode, errcode, fmt, args); +} + +void +verrc(int exitcode, int errcode, const char *fmt, va_list args) +{ + warnerr_core(errcode, fmt, args); + exit(exitcode); +} + +void +errx(int exitcode, const char *fmt, ...) +{ + va_list args; + + va_start(args, fmt); + verrx(exitcode, fmt, args); +} + +void +verrx(int exitcode, const char *fmt, va_list args) +{ + warnerr_core(NOCODE, fmt, args); + exit(exitcode); +} + +void +warn(const char *fmt, ...) +{ + va_list args; + + va_start(args, fmt); + vwarn(fmt, args); +} + +void +vwarn(const char *fmt, va_list args) +{ + warnerr_core(errno, fmt, args); +} + +void +warnc(int errcode, const char *fmt, ...) +{ + va_list args; + + va_start(args, fmt); + vwarnc(errcode, fmt, args); +} + +void +vwarnc(int errcode, const char *fmt, va_list args) +{ + warnerr_core(errcode, fmt, args); +} + +void +warnx(const char *fmt, ...) +{ + va_list args; + + va_start(args, fmt); + vwarnx(fmt, args); +} + +void +vwarnx(const char *fmt, va_list args) +{ + warnerr_core(NOCODE, fmt, args); +} diff --git a/src/modules/systemlib/err.h b/src/modules/systemlib/err.h new file mode 100644 index 000000000..ca13d6265 --- /dev/null +++ b/src/modules/systemlib/err.h @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file err.h + * + * Simple error/warning functions, heavily inspired by the BSD functions of + * the same names. + * + * The err() and warn() family of functions display a formatted error + * message on the standard error output. In all cases, the last + * component of the program name, a colon character, and a space are + * output. If the fmt argument is not NULL, the printf(3)-like formatted + * error message is output. The output is terminated by a newline + * character. + * + * The err(), errc(), verr(), verrc(), warn(), warnc(), vwarn(), and + * vwarnc() functions append an error message obtained from strerror(3) + * based on a supplied error code value or the global variable errno, + * preceded by another colon and space unless the fmt argument is NULL. + * + * In the case of the errc(), verrc(), warnc(), and vwarnc() functions, + * the code argument is used to look up the error message. + * + * The err(), verr(), warn(), and vwarn() functions use the global + * variable errno to look up the error message. + * + * The errx() and warnx() functions do not append an error message. + * + * The err(), verr(), errc(), verrc(), errx(), and verrx() functions do + * not return, but exit with the value of the argument eval. + * + */ + +#ifndef _SYSTEMLIB_ERR_H +#define _SYSTEMLIB_ERR_H + +#include <stdarg.h> + +__BEGIN_DECLS + +__EXPORT const char *getprogname(void); + +__EXPORT void err(int eval, const char *fmt, ...) __attribute__((noreturn, format(printf, 2, 3))); +__EXPORT void verr(int eval, const char *fmt, va_list) __attribute__((noreturn, format(printf, 2, 0))); +__EXPORT void errc(int eval, int code, const char *fmt, ...) __attribute__((noreturn, format(printf, 3, 4))); +__EXPORT void verrc(int eval, int code, const char *fmt, va_list) __attribute__((noreturn, format(printf, 3, 0))); +__EXPORT void errx(int eval, const char *fmt, ...) __attribute__((noreturn, format(printf, 2, 3))); +__EXPORT void verrx(int eval, const char *fmt, va_list) __attribute__((noreturn, format(printf, 2, 0))); +__EXPORT void warn(const char *fmt, ...) __attribute__((format(printf, 1, 2))); +__EXPORT void vwarn(const char *fmt, va_list) __attribute__((format(printf, 1, 0))); +__EXPORT void warnc(int code, const char *fmt, ...) __attribute__((format(printf, 2, 3))); +__EXPORT void vwarnc(int code, const char *fmt, va_list) __attribute__((format(printf, 2, 0))); +__EXPORT void warnx(const char *fmt, ...) __attribute__((format(printf, 1, 2))); +__EXPORT void vwarnx(const char *fmt, va_list) __attribute__((format(printf, 1, 0))); + +__END_DECLS + +#endif diff --git a/src/modules/systemlib/geo/geo.c b/src/modules/systemlib/geo/geo.c new file mode 100644 index 000000000..6463e6489 --- /dev/null +++ b/src/modules/systemlib/geo/geo.c @@ -0,0 +1,438 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: 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 geo.c + * + * Geo / math functions to perform geodesic calculations + * + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +#include <systemlib/geo/geo.h> +#include <nuttx/config.h> +#include <unistd.h> +#include <pthread.h> +#include <stdio.h> +#include <math.h> +#include <stdbool.h> + + +/* 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; + +__EXPORT 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; + +} + +__EXPORT 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); +} + +__EXPORT 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; + +} + + +__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) +{ + double lat_now_rad = lat_now / 180.0d * M_PI; + double lon_now_rad = lon_now / 180.0d * M_PI; + double lat_next_rad = lat_next / 180.0d * M_PI; + double lon_next_rad = lon_next / 180.0d * M_PI; + + + double d_lat = lat_next_rad - lat_now_rad; + double d_lon = lon_next_rad - lon_now_rad; + + double a = sin(d_lat / 2.0d) * sin(d_lat / 2.0d) + sin(d_lon / 2.0d) * sin(d_lon / 2.0d) * cos(lat_now_rad) * cos(lat_next_rad); + double c = 2.0d * atan2(sqrt(a), sqrt(1.0d - a)); + + const double radius_earth = 6371000.0d; + return radius_earth * c; +} + +__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) +{ + double lat_now_rad = lat_now * M_DEG_TO_RAD; + double lon_now_rad = lon_now * M_DEG_TO_RAD; + double lat_next_rad = lat_next * M_DEG_TO_RAD; + double lon_next_rad = lon_next * M_DEG_TO_RAD; + + double d_lat = lat_next_rad - lat_now_rad; + double d_lon = lon_next_rad - lon_now_rad; + + /* conscious mix of double and float trig function to maximize speed and efficiency */ + float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)); + + theta = _wrap_pi(theta); + + return theta; +} + +// Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu> + +__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end) +{ +// This function returns the distance to the nearest point on the track line. Distance is positive if current +// position is right of the track and negative if left of the track as seen from a point on the track line +// headed towards the end point. + + float dist_to_end; + float bearing_end; + float bearing_track; + float bearing_diff; + + int return_value = ERROR; // Set error flag, cleared when valid result calculated. + crosstrack_error->past_end = false; + crosstrack_error->distance = 0.0f; + crosstrack_error->bearing = 0.0f; + + // Return error if arguments are bad + if (lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) return return_value; + + bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); + bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end); + bearing_diff = bearing_track - bearing_end; + bearing_diff = _wrap_pi(bearing_diff); + + // Return past_end = true if past end point of line + if (bearing_diff > M_PI_2_F || bearing_diff < -M_PI_2_F) { + crosstrack_error->past_end = true; + return_value = OK; + return return_value; + } + + dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); + crosstrack_error->distance = (dist_to_end) * sin(bearing_diff); + + if (sin(bearing_diff) >= 0) { + crosstrack_error->bearing = _wrap_pi(bearing_track - M_PI_2_F); + + } else { + crosstrack_error->bearing = _wrap_pi(bearing_track + M_PI_2_F); + } + + return_value = OK; + + return return_value; + +} + + +__EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, + float radius, float arc_start_bearing, float arc_sweep) +{ + // This function returns the distance to the nearest point on the track arc. Distance is positive if current + // position is right of the arc and negative if left of the arc as seen from the closest point on the arc and + // headed towards the end point. + + // Determine if the current position is inside or outside the sector between the line from the center + // to the arc start and the line from the center to the arc end + float bearing_sector_start; + float bearing_sector_end; + float bearing_now = get_bearing_to_next_waypoint(lat_now, lon_now, lat_center, lon_center); + bool in_sector; + + int return_value = ERROR; // Set error flag, cleared when valid result calculated. + crosstrack_error->past_end = false; + crosstrack_error->distance = 0.0f; + crosstrack_error->bearing = 0.0f; + + // Return error if arguments are bad + if (lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) return return_value; + + + if (arc_sweep >= 0) { + bearing_sector_start = arc_start_bearing; + bearing_sector_end = arc_start_bearing + arc_sweep; + + if (bearing_sector_end > 2.0f * M_PI_F) bearing_sector_end -= M_TWOPI_F; + + } else { + bearing_sector_end = arc_start_bearing; + bearing_sector_start = arc_start_bearing - arc_sweep; + + if (bearing_sector_start < 0.0f) bearing_sector_start += M_TWOPI_F; + } + + in_sector = false; + + // Case where sector does not span zero + if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) in_sector = true; + + // Case where sector does span zero + if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start || bearing_now < bearing_sector_end)) in_sector = true; + + // If in the sector then calculate distance and bearing to closest point + if (in_sector) { + crosstrack_error->past_end = false; + float dist_to_center = get_distance_to_next_waypoint(lat_now, lon_now, lat_center, lon_center); + + if (dist_to_center <= radius) { + crosstrack_error->distance = radius - dist_to_center; + crosstrack_error->bearing = bearing_now + M_PI_F; + + } else { + crosstrack_error->distance = dist_to_center - radius; + crosstrack_error->bearing = bearing_now; + } + + // If out of the sector then calculate dist and bearing to start or end point + + } else { + + // Use the approximation that 111,111 meters in the y direction is 1 degree (of latitude) + // and 111,111 * cos(latitude) meters in the x direction is 1 degree (of longitude) to + // calculate the position of the start and end points. We should not be doing this often + // as this function generally will not be called repeatedly when we are out of the sector. + + // TO DO - this is messed up and won't compile + float start_disp_x = radius * sin(arc_start_bearing); + float start_disp_y = radius * cos(arc_start_bearing); + float end_disp_x = radius * sin(_wrapPI(arc_start_bearing + arc_sweep)); + float end_disp_y = radius * cos(_wrapPI(arc_start_bearing + arc_sweep)); + float lon_start = lon_now + start_disp_x / 111111.0d; + float lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0d; + float lon_end = lon_now + end_disp_x / 111111.0d; + float lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0d; + float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); + float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); + + + if (dist_to_start < dist_to_end) { + crosstrack_error->distance = dist_to_start; + crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); + + } else { + crosstrack_error->past_end = true; + crosstrack_error->distance = dist_to_end; + crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); + } + + } + + crosstrack_error->bearing = _wrapPI(crosstrack_error->bearing); + return_value = OK; + return return_value; +} + +__EXPORT float _wrap_pi(float bearing) +{ + /* value is inf or NaN */ + if (!isfinite(bearing) || bearing == 0) { + return bearing; + } + + int c = 0; + + while (bearing > M_PI_F && c < 30) { + bearing -= M_TWOPI_F; + c++; + } + + c = 0; + + while (bearing <= -M_PI_F && c < 30) { + bearing += M_TWOPI_F; + c++; + } + + return bearing; +} + +__EXPORT float _wrap_2pi(float bearing) +{ + /* value is inf or NaN */ + if (!isfinite(bearing)) { + return bearing; + } + + while (bearing >= M_TWOPI_F) { + bearing = bearing - M_TWOPI_F; + } + + while (bearing < 0.0f) { + bearing = bearing + M_TWOPI_F; + } + + return bearing; +} + +__EXPORT float _wrap_180(float bearing) +{ + /* value is inf or NaN */ + if (!isfinite(bearing)) { + return bearing; + } + + while (bearing > 180.0f) { + bearing = bearing - 360.0f; + } + + while (bearing <= -180.0f) { + bearing = bearing + 360.0f; + } + + return bearing; +} + +__EXPORT float _wrap_360(float bearing) +{ + /* value is inf or NaN */ + if (!isfinite(bearing)) { + return bearing; + } + + while (bearing >= 360.0f) { + bearing = bearing - 360.0f; + } + + while (bearing < 0.0f) { + bearing = bearing + 360.0f; + } + + return bearing; +} + + diff --git a/src/modules/systemlib/geo/geo.h b/src/modules/systemlib/geo/geo.h new file mode 100644 index 000000000..dadec51ec --- /dev/null +++ b/src/modules/systemlib/geo/geo.h @@ -0,0 +1,129 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: 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 geo.h + * + * Definition of geo / math functions to perform geodesic calculations + * + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu> + */ + +#pragma once + +__BEGIN_DECLS + +#include <stdbool.h> + +#define CONSTANTS_ONE_G 9.80665f /* m/s^2 */ +#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C 1.225f /* kg/m^3 */ +#define CONSTANTS_AIR_GAS_CONST 287.1f /* J/(kg * K) */ +#define CONSTANTS_ABSOLUTE_NULL_CELSIUS -273.15f /* °C */ +#define CONSTANTS_RADIUS_OF_EARTH 6371000 /* meters (m) */ + +/* compatibility aliases */ +#define RADIUS_OF_EARTH CONSTANTS_RADIUS_OF_EARTH +#define GRAVITY_MSS CONSTANTS_ONE_G + +// XXX remove +struct crosstrack_error_s { + bool past_end; // Flag indicating we are past the end of the line/arc segment + float distance; // Distance in meters to closest point on line/arc + float bearing; // Bearing in radians to closest point on line/arc +} ; + +/** + * 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°) + */ +__EXPORT void map_projection_init(double lat_0, double lon_0); + +/** + * 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°) + */ +__EXPORT void map_projection_project(double lat, double lon, float *x, float *y); + +/** + * 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°) + */ +__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon); + +/** + * Returns the distance to the next waypoint in meters. + * + * @param lat_now current position in degrees (47.1234567°, not 471234567°) + * @param lon_now current position in degrees (8.1234567°, not 81234567°) + * @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°) + * @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°) + */ +__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); + +/** + * Returns the bearing to the next waypoint in radians. + * + * @param lat_now current position in degrees (47.1234567°, not 471234567°) + * @param lon_now current position in degrees (8.1234567°, not 81234567°) + * @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°) + * @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°) + */ +__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); + +__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end); + +__EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, + float radius, float arc_start_bearing, float arc_sweep); + +__EXPORT float _wrap_180(float bearing); +__EXPORT float _wrap_360(float bearing); +__EXPORT float _wrap_pi(float bearing); +__EXPORT float _wrap_2pi(float bearing); + +__END_DECLS diff --git a/src/modules/systemlib/getopt_long.c b/src/modules/systemlib/getopt_long.c new file mode 100644 index 000000000..27c38635f --- /dev/null +++ b/src/modules/systemlib/getopt_long.c @@ -0,0 +1,404 @@ +/**************************************************************************** + +getopt.c - Read command line options + +AUTHOR: Gregory Pietsch +CREATED Fri Jan 10 21:13:05 1997 + +DESCRIPTION: + +The getopt() function parses the command line arguments. Its arguments argc +and argv are the argument count and array as passed to the main() function +on program invocation. The argument optstring is a list of available option +characters. If such a character is followed by a colon (`:'), the option +takes an argument, which is placed in optarg. If such a character is +followed by two colons, the option takes an optional argument, which is +placed in optarg. If the option does not take an argument, optarg is NULL. + +The external variable optind is the index of the next array element of argv +to be processed; it communicates from one call to the next which element to +process. + +The getopt_long() function works like getopt() except that it also accepts +long options started by two dashes `--'. If these take values, it is either +in the form + +--arg=value + + or + +--arg value + +It takes the additional arguments longopts which is a pointer to the first +element of an array of type GETOPT_LONG_OPTION_T. The last element of the +array has to be filled with NULL for the name field. + +The longind pointer points to the index of the current long option relative +to longopts if it is non-NULL. + +The getopt() function returns the option character if the option was found +successfully, `:' if there was a missing parameter for one of the options, +`?' for an unknown option character, and EOF for the end of the option list. + +The getopt_long() function's return value is described in the header file. + +The function getopt_long_only() is identical to getopt_long(), except that a +plus sign `+' can introduce long options as well as `--'. + +The following describes how to deal with options that follow non-option +argv-elements. + +If the caller did not specify anything, the default is REQUIRE_ORDER if the +environment variable POSIXLY_CORRECT is defined, PERMUTE otherwise. + +REQUIRE_ORDER means don't recognize them as options; stop option processing +when the first non-option is seen. This is what Unix does. This mode of +operation is selected by either setting the environment variable +POSIXLY_CORRECT, or using `+' as the first character of the optstring +parameter. + +PERMUTE is the default. We permute the contents of ARGV as we scan, so that +eventually all the non-options are at the end. This allows options to be +given in any order, even with programs that were not written to expect this. + +RETURN_IN_ORDER is an option available to programs that were written to +expect options and other argv-elements in any order and that care about the +ordering of the two. We describe each non-option argv-element as if it were +the argument of an option with character code 1. Using `-' as the first +character of the optstring parameter selects this mode of operation. + +The special argument `--' forces an end of option-scanning regardless of the +value of ordering. In the case of RETURN_IN_ORDER, only `--' can cause +getopt() and friends to return EOF with optind != argc. + +COPYRIGHT NOTICE AND DISCLAIMER: + +Copyright (C) 1997 Gregory Pietsch + +This file and the accompanying getopt.h header file are hereby placed in the +public domain without restrictions. Just give the author credit, don't +claim you wrote it or prevent anyone else from using it. + +Gregory Pietsch's current e-mail address: +gpietsch@comcast.net +****************************************************************************/ + +/* include files */ +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include "getopt_long.h" + +/* macros */ + +/* types */ +typedef enum GETOPT_ORDERING_T +{ + PERMUTE, + RETURN_IN_ORDER, + REQUIRE_ORDER +} GETOPT_ORDERING_T; + +/* globally-defined variables */ +char *optarg = NULL; +int optind = 0; +int opterr = 1; +int optopt = '?'; + +/* functions */ + +/* reverse_argv_elements: reverses num elements starting at argv */ +static void +reverse_argv_elements (char **argv, int num) +{ + int i; + char *tmp; + + for (i = 0; i < (num >> 1); i++) + { + tmp = argv[i]; + argv[i] = argv[num - i - 1]; + argv[num - i - 1] = tmp; + } +} + +/* permute: swap two blocks of argv-elements given their lengths */ +static void +permute (char **argv, int len1, int len2) +{ + reverse_argv_elements (argv, len1); + reverse_argv_elements (argv, len1 + len2); + reverse_argv_elements (argv, len2); +} + +/* is_option: is this argv-element an option or the end of the option list? */ +static int +is_option (char *argv_element, int only) +{ + return ((argv_element == NULL) + || (argv_element[0] == '-') || (only && argv_element[0] == '+')); +} + +/* getopt_internal: the function that does all the dirty work */ +static int +getopt_internal (int argc, char **argv, const char *shortopts, + const GETOPT_LONG_OPTION_T * longopts, int *longind, int only) +{ + GETOPT_ORDERING_T ordering = PERMUTE; + static size_t optwhere = 0; + size_t permute_from = 0; + int num_nonopts = 0; + int optindex = 0; + size_t match_chars = 0; + char *possible_arg = NULL; + int longopt_match = -1; + int has_arg = -1; + char *cp = NULL; + int arg_next = 0; + + /* first, deal with silly parameters and easy stuff */ + if (argc == 0 || argv == NULL || (shortopts == NULL && longopts == NULL) + || optind >= argc || argv[optind] == NULL) + return EOF; + if (strcmp (argv[optind], "--") == 0) + { + optind++; + return EOF; + } + /* if this is our first time through */ + if (optind == 0) + optind = optwhere = 1; + + /* define ordering */ + if (shortopts != NULL && (*shortopts == '-' || *shortopts == '+')) + { + ordering = (*shortopts == '-') ? RETURN_IN_ORDER : REQUIRE_ORDER; + shortopts++; + } + else + ordering = /*(getenv ("POSIXLY_CORRECT") != NULL) ? REQUIRE_ORDER :*/ PERMUTE; + + /* + * based on ordering, find our next option, if we're at the beginning of + * one + */ + if (optwhere == 1) + { + switch (ordering) + { + default: /* shouldn't happen */ + case PERMUTE: + permute_from = optind; + num_nonopts = 0; + while (!is_option (argv[optind], only)) + { + optind++; + num_nonopts++; + } + if (argv[optind] == NULL) + { + /* no more options */ + optind = permute_from; + return EOF; + } + else if (strcmp (argv[optind], "--") == 0) + { + /* no more options, but have to get `--' out of the way */ + permute (argv + permute_from, num_nonopts, 1); + optind = permute_from + 1; + return EOF; + } + break; + case RETURN_IN_ORDER: + if (!is_option (argv[optind], only)) + { + optarg = argv[optind++]; + return (optopt = 1); + } + break; + case REQUIRE_ORDER: + if (!is_option (argv[optind], only)) + return EOF; + break; + } + } + /* we've got an option, so parse it */ + + /* first, is it a long option? */ + if (longopts != NULL + && (memcmp (argv[optind], "--", 2) == 0 + || (only && argv[optind][0] == '+')) && optwhere == 1) + { + /* handle long options */ + if (memcmp (argv[optind], "--", 2) == 0) + optwhere = 2; + longopt_match = -1; + possible_arg = strchr (argv[optind] + optwhere, '='); + if (possible_arg == NULL) + { + /* no =, so next argv might be arg */ + match_chars = strlen (argv[optind]); + possible_arg = argv[optind] + match_chars; + match_chars = match_chars - optwhere; + } + else + match_chars = (possible_arg - argv[optind]) - optwhere; + for (optindex = 0; longopts[optindex].name != NULL; optindex++) + { + if (memcmp (argv[optind] + optwhere, + longopts[optindex].name, match_chars) == 0) + { + /* do we have an exact match? */ + if (match_chars == (unsigned) (strlen (longopts[optindex].name))) + { + longopt_match = optindex; + break; + } + /* do any characters match? */ + else + { + if (longopt_match < 0) + longopt_match = optindex; + else + { + /* we have ambiguous options */ + if (opterr) + fprintf (stderr, "%s: option `%s' is ambiguous " + "(could be `--%s' or `--%s')\n", + argv[0], + argv[optind], + longopts[longopt_match].name, + longopts[optindex].name); + return (optopt = '?'); + } + } + } + } + if (longopt_match >= 0) + has_arg = longopts[longopt_match].has_arg; + } + /* if we didn't find a long option, is it a short option? */ + if (longopt_match < 0 && shortopts != NULL) + { + cp = strchr (shortopts, argv[optind][optwhere]); + if (cp == NULL) + { + /* couldn't find option in shortopts */ + if (opterr) + fprintf (stderr, + "%s: invalid option -- `-%c'\n", + argv[0], argv[optind][optwhere]); + optwhere++; + if (argv[optind][optwhere] == '\0') + { + optind++; + optwhere = 1; + } + return (optopt = '?'); + } + has_arg = ((cp[1] == ':') + ? ((cp[2] == ':') ? OPTIONAL_ARG : REQUIRED_ARG) : NO_ARG); + possible_arg = argv[optind] + optwhere + 1; + optopt = *cp; + } + /* get argument and reset optwhere */ + arg_next = 0; + switch (has_arg) + { + case OPTIONAL_ARG: + if (*possible_arg == '=') + possible_arg++; + optarg = (*possible_arg != '\0') ? possible_arg : 0; + optwhere = 1; + break; + case REQUIRED_ARG: + if (*possible_arg == '=') + possible_arg++; + if (*possible_arg != '\0') + { + optarg = possible_arg; + optwhere = 1; + } + else if (optind + 1 >= argc) + { + if (opterr) + { + fprintf (stderr, "%s: argument required for option `", argv[0]); + if (longopt_match >= 0) + fprintf (stderr, "--%s'\n", longopts[longopt_match].name); + else + fprintf (stderr, "-%c'\n", *cp); + } + optind++; + return (optopt = ':'); + } + else + { + optarg = argv[optind + 1]; + arg_next = 1; + optwhere = 1; + } + break; + default: /* shouldn't happen */ + case NO_ARG: + if (longopt_match < 0) + { + optwhere++; + if (argv[optind][optwhere] == '\0') + optwhere = 1; + } + else + optwhere = 1; + optarg = NULL; + break; + } + + /* do we have to permute or otherwise modify optind? */ + if (ordering == PERMUTE && optwhere == 1 && num_nonopts != 0) + { + permute (argv + permute_from, num_nonopts, 1 + arg_next); + optind = permute_from + 1 + arg_next; + } + else if (optwhere == 1) + optind = optind + 1 + arg_next; + + /* finally return */ + if (longopt_match >= 0) + { + if (longind != NULL) + *longind = longopt_match; + if (longopts[longopt_match].flag != NULL) + { + *(longopts[longopt_match].flag) = longopts[longopt_match].val; + return 0; + } + else + return longopts[longopt_match].val; + } + else + return optopt; +} + +#if 0 +int +getopt (int argc, char **argv, char *optstring) +{ + return getopt_internal (argc, argv, optstring, NULL, NULL, 0); +} +#endif + +int +getopt_long (int argc, char **argv, const char *shortopts, + const GETOPT_LONG_OPTION_T * longopts, int *longind) +{ + return getopt_internal (argc, argv, shortopts, longopts, longind, 0); +} + +int +getopt_long_only (int argc, char **argv, const char *shortopts, + const GETOPT_LONG_OPTION_T * longopts, int *longind) +{ + return getopt_internal (argc, argv, shortopts, longopts, longind, 1); +} + +/* end of file GETOPT.C */ diff --git a/src/modules/systemlib/getopt_long.h b/src/modules/systemlib/getopt_long.h new file mode 100644 index 000000000..61e8e76f3 --- /dev/null +++ b/src/modules/systemlib/getopt_long.h @@ -0,0 +1,139 @@ +/**************************************************************************** + +getopt.h - Read command line options + +AUTHOR: Gregory Pietsch +CREATED Thu Jan 09 22:37:00 1997 + +DESCRIPTION: + +The getopt() function parses the command line arguments. Its arguments argc +and argv are the argument count and array as passed to the main() function +on program invocation. The argument optstring is a list of available option +characters. If such a character is followed by a colon (`:'), the option +takes an argument, which is placed in optarg. If such a character is +followed by two colons, the option takes an optional argument, which is +placed in optarg. If the option does not take an argument, optarg is NULL. + +The external variable optind is the index of the next array element of argv +to be processed; it communicates from one call to the next which element to +process. + +The getopt_long() function works like getopt() except that it also accepts +long options started by two dashes `--'. If these take values, it is either +in the form + +--arg=value + + or + +--arg value + +It takes the additional arguments longopts which is a pointer to the first +element of an array of type GETOPT_LONG_OPTION_T, defined below. The last +element of the array has to be filled with NULL for the name field. + +The longind pointer points to the index of the current long option relative +to longopts if it is non-NULL. + +The getopt() function returns the option character if the option was found +successfully, `:' if there was a missing parameter for one of the options, +`?' for an unknown option character, and EOF for the end of the option list. + +The getopt_long() function's return value is described below. + +The function getopt_long_only() is identical to getopt_long(), except that a +plus sign `+' can introduce long options as well as `--'. + +Describe how to deal with options that follow non-option ARGV-elements. + +If the caller did not specify anything, the default is REQUIRE_ORDER if the +environment variable POSIXLY_CORRECT is defined, PERMUTE otherwise. + +REQUIRE_ORDER means don't recognize them as options; stop option processing +when the first non-option is seen. This is what Unix does. This mode of +operation is selected by either setting the environment variable +POSIXLY_CORRECT, or using `+' as the first character of the optstring +parameter. + +PERMUTE is the default. We permute the contents of ARGV as we scan, so that +eventually all the non-options are at the end. This allows options to be +given in any order, even with programs that were not written to expect this. + +RETURN_IN_ORDER is an option available to programs that were written to +expect options and other ARGV-elements in any order and that care about the +ordering of the two. We describe each non-option ARGV-element as if it were +the argument of an option with character code 1. Using `-' as the first +character of the optstring parameter selects this mode of operation. + +The special argument `--' forces an end of option-scanning regardless of the +value of `ordering'. In the case of RETURN_IN_ORDER, only `--' can cause +getopt() and friends to return EOF with optind != argc. + +COPYRIGHT NOTICE AND DISCLAIMER: + +Copyright (C) 1997 Gregory Pietsch + +This file and the accompanying getopt.c implementation file are hereby +placed in the public domain without restrictions. Just give the author +credit, don't claim you wrote it or prevent anyone else from using it. + +Gregory Pietsch's current e-mail address: +gpietsch@comcast.net +****************************************************************************/ + +#ifndef GETOPT_H +#define GETOPT_H + +/* include files needed by this include file */ + +/* macros defined by this include file */ +#define NO_ARG 0 +#define REQUIRED_ARG 1 +#define OPTIONAL_ARG 2 + +/* types defined by this include file */ + +/* GETOPT_LONG_OPTION_T: The type of long option */ +typedef struct GETOPT_LONG_OPTION_T +{ + char *name; /* the name of the long option */ + int has_arg; /* one of the above macros */ + int *flag; /* determines if getopt_long() returns a + * value for a long option; if it is + * non-NULL, 0 is returned as a function + * value and the value of val is stored in + * the area pointed to by flag. Otherwise, + * val is returned. */ + int val; /* determines the value to return if flag is + * NULL. */ +} GETOPT_LONG_OPTION_T; + +#ifdef __cplusplus +extern "C" +{ +#endif + + /* externally-defined variables */ + extern char *optarg; + extern int optind; + extern int opterr; + extern int optopt; + + /* function prototypes */ +#if 0 + int getopt (int argc, char **argv, char *optstring); +#endif + __EXPORT int getopt_long (int argc, char **argv, const char *shortopts, + const GETOPT_LONG_OPTION_T * longopts, int *longind); + __EXPORT int getopt_long_only (int argc, char **argv, const char *shortopts, + const GETOPT_LONG_OPTION_T * longopts, int *longind); + +#ifdef __cplusplus +}; + +#endif + +#endif /* GETOPT_H */ + +/* END OF FILE getopt.h */ diff --git a/src/modules/systemlib/hx_stream.c b/src/modules/systemlib/hx_stream.c new file mode 100644 index 000000000..8d77f14a8 --- /dev/null +++ b/src/modules/systemlib/hx_stream.c @@ -0,0 +1,250 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file hx_stream.c + * + * A simple serial line framing protocol based on HDLC + * with 32-bit CRC protection. + */ + +#include <stdint.h> +#include <stdlib.h> +#include <stdbool.h> +#include <crc32.h> +#include <unistd.h> +#include <errno.h> +#include <string.h> +#include <stdio.h> + +#include "perf_counter.h" + +#include "hx_stream.h" + + +struct hx_stream { + uint8_t buf[HX_STREAM_MAX_FRAME + 4]; + unsigned frame_bytes; + bool escaped; + bool txerror; + + int fd; + hx_stream_rx_callback callback; + void *callback_arg; + + perf_counter_t pc_tx_frames; + perf_counter_t pc_rx_frames; + perf_counter_t pc_rx_errors; +}; + +/* + * Protocol magic numbers, straight out of HDLC. + */ +#define FBO 0x7e /**< Frame Boundary Octet */ +#define CEO 0x7c /**< Control Escape Octet */ + +static void hx_tx_raw(hx_stream_t stream, uint8_t c); +static void hx_tx_raw(hx_stream_t stream, uint8_t c); +static int hx_rx_frame(hx_stream_t stream); + +static void +hx_tx_raw(hx_stream_t stream, uint8_t c) +{ + if (write(stream->fd, &c, 1) != 1) + stream->txerror = true; +} + +static void +hx_tx_byte(hx_stream_t stream, uint8_t c) +{ + switch (c) { + case FBO: + case CEO: + hx_tx_raw(stream, CEO); + c ^= 0x20; + break; + } + + hx_tx_raw(stream, c); +} + +static int +hx_rx_frame(hx_stream_t stream) +{ + union { + uint8_t b[4]; + uint32_t w; + } u; + unsigned length = stream->frame_bytes; + + /* reset the stream */ + stream->frame_bytes = 0; + stream->escaped = false; + + /* not a real frame - too short */ + if (length < 4) { + if (length > 1) + perf_count(stream->pc_rx_errors); + + return 0; + } + + length -= 4; + + /* compute expected CRC */ + u.w = crc32(&stream->buf[0], length); + + /* compare computed and actual CRC */ + for (unsigned i = 0; i < 4; i++) { + if (u.b[i] != stream->buf[length + i]) { + perf_count(stream->pc_rx_errors); + return 0; + } + } + + /* frame is good */ + perf_count(stream->pc_rx_frames); + stream->callback(stream->callback_arg, &stream->buf[0], length); + return 1; +} + +hx_stream_t +hx_stream_init(int fd, + hx_stream_rx_callback callback, + void *arg) +{ + hx_stream_t stream; + + stream = (hx_stream_t)malloc(sizeof(struct hx_stream)); + + if (stream != NULL) { + memset(stream, 0, sizeof(struct hx_stream)); + stream->fd = fd; + stream->callback = callback; + stream->callback_arg = arg; + } + + return stream; +} + +void +hx_stream_free(hx_stream_t stream) +{ + /* free perf counters (OK if they are NULL) */ + perf_free(stream->pc_tx_frames); + perf_free(stream->pc_rx_frames); + perf_free(stream->pc_rx_errors); + + free(stream); +} + +void +hx_stream_set_counters(hx_stream_t stream, + perf_counter_t tx_frames, + perf_counter_t rx_frames, + perf_counter_t rx_errors) +{ + stream->pc_tx_frames = tx_frames; + stream->pc_rx_frames = rx_frames; + stream->pc_rx_errors = rx_errors; +} + +int +hx_stream_send(hx_stream_t stream, + const void *data, + size_t count) +{ + union { + uint8_t b[4]; + uint32_t w; + } u; + const uint8_t *p = (const uint8_t *)data; + unsigned resid = count; + + if (resid > HX_STREAM_MAX_FRAME) + return -EINVAL; + + /* start the frame */ + hx_tx_raw(stream, FBO); + + /* transmit the data */ + while (resid--) + hx_tx_byte(stream, *p++); + + /* compute the CRC */ + u.w = crc32(data, count); + + /* send the CRC */ + p = &u.b[0]; + resid = 4; + + while (resid--) + hx_tx_byte(stream, *p++); + + /* and the trailing frame separator */ + hx_tx_raw(stream, FBO); + + /* check for transmit error */ + if (stream->txerror) { + stream->txerror = false; + return -EIO; + } + + perf_count(stream->pc_tx_frames); + return 0; +} + +void +hx_stream_rx(hx_stream_t stream, uint8_t c) +{ + /* frame end? */ + if (c == FBO) { + hx_rx_frame(stream); + return; + } + + /* escaped? */ + if (stream->escaped) { + stream->escaped = false; + c ^= 0x20; + + } else if (c == CEO) { + /* now escaped, ignore the byte */ + stream->escaped = true; + return; + } + + /* save for later */ + if (stream->frame_bytes < sizeof(stream->buf)) + stream->buf[stream->frame_bytes++] = c; +} diff --git a/src/modules/systemlib/hx_stream.h b/src/modules/systemlib/hx_stream.h new file mode 100644 index 000000000..128689953 --- /dev/null +++ b/src/modules/systemlib/hx_stream.h @@ -0,0 +1,122 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file hx_stream.h + * + * A simple serial line framing protocol based on HDLC + * with 32-bit CRC protection. + */ + +#ifndef _SYSTEMLIB_HX_STREAM_H +#define _SYSTEMLIB_HX_STREAM_H + +#include <sys/types.h> + +#include "perf_counter.h" + +struct hx_stream; +typedef struct hx_stream *hx_stream_t; + +#define HX_STREAM_MAX_FRAME 64 + +typedef void (* hx_stream_rx_callback)(void *arg, const void *data, size_t length); + +__BEGIN_DECLS + +/** + * Allocate a new hx_stream object. + * + * @param fd The file handle over which the protocol will + * communicate. + * @param callback Called when a frame is received. + * @param callback_arg Passed to the callback. + * @return A handle to the stream, or NULL if memory could + * not be allocated. + */ +__EXPORT extern hx_stream_t hx_stream_init(int fd, + hx_stream_rx_callback callback, + void *arg); + +/** + * Free a hx_stream object. + * + * @param stream A handle returned from hx_stream_init. + */ +__EXPORT extern void hx_stream_free(hx_stream_t stream); + +/** + * Set performance counters for the stream. + * + * Any counter may be set to NULL to disable counting that datum. + * + * @param tx_frames Counter for transmitted frames. + * @param rx_frames Counter for received frames. + * @param rx_errors Counter for short and corrupt received frames. + */ +__EXPORT extern void hx_stream_set_counters(hx_stream_t stream, + perf_counter_t tx_frames, + perf_counter_t rx_frames, + perf_counter_t rx_errors); + +/** + * Send a frame. + * + * This function will block until all frame bytes are sent if + * the descriptor passed to hx_stream_init is marked blocking, + * otherwise it will return -1 (but may transmit a + * runt frame at the same time). + * + * @todo Handling of non-blocking streams needs to be better. + * + * @param stream A handle returned from hx_stream_init. + * @param data Pointer to the data to send. + * @param count The number of bytes to send. + * @return Zero on success, -errno on error. + */ +__EXPORT extern int hx_stream_send(hx_stream_t stream, + const void *data, + size_t count); + +/** + * Handle a byte from the stream. + * + * @param stream A handle returned from hx_stream_init. + * @param c The character to process. + */ +__EXPORT extern void hx_stream_rx(hx_stream_t stream, + uint8_t c); + +__END_DECLS + +#endif diff --git a/src/modules/systemlib/mixer/mixer.cpp b/src/modules/systemlib/mixer/mixer.cpp new file mode 100644 index 000000000..df0dfe838 --- /dev/null +++ b/src/modules/systemlib/mixer/mixer.cpp @@ -0,0 +1,152 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mixer.cpp + * + * Programmable multi-channel mixer library. + */ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <stdlib.h> +#include <string.h> +#include <fcntl.h> +#include <poll.h> +#include <errno.h> +#include <stdio.h> +#include <math.h> +#include <unistd.h> + +#include "mixer.h" + +Mixer::Mixer(ControlCallback control_cb, uintptr_t cb_handle) : + _next(nullptr), + _control_cb(control_cb), + _cb_handle(cb_handle) +{ +} + +float +Mixer::get_control(uint8_t group, uint8_t index) +{ + float value; + + _control_cb(_cb_handle, group, index, value); + + return value; +} + + +float +Mixer::scale(const mixer_scaler_s &scaler, float input) +{ + float output; + + if (input < 0.0f) { + output = (input * scaler.negative_scale) + scaler.offset; + + } else { + output = (input * scaler.positive_scale) + scaler.offset; + } + + if (output > scaler.max_output) { + output = scaler.max_output; + + } else if (output < scaler.min_output) { + output = scaler.min_output; + } + + return output; +} + +int +Mixer::scale_check(struct mixer_scaler_s &scaler) +{ + if (scaler.offset > 1.001f) + return 1; + + if (scaler.offset < -1.001f) + return 2; + + if (scaler.min_output > scaler.max_output) + return 3; + + if (scaler.min_output < -1.001f) + return 4; + + if (scaler.max_output > 1.001f) + return 5; + + return 0; +} + +/****************************************************************************/ + +NullMixer::NullMixer() : + Mixer(nullptr, 0) +{ +} + +unsigned +NullMixer::mix(float *outputs, unsigned space) +{ + if (space > 0) { + *outputs = 0.0f; + return 1; + } + + return 0; +} + +void +NullMixer::groups_required(uint32_t &groups) +{ + +} + +NullMixer * +NullMixer::from_text(const char *buf, unsigned &buflen) +{ + NullMixer *nm = nullptr; + + if ((buflen >= 2) && (buf[0] == 'Z') && (buf[1] == ':')) { + nm = new NullMixer; + buflen -= 2; + } + + return nm; +} diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h new file mode 100644 index 000000000..bbfa130a9 --- /dev/null +++ b/src/modules/systemlib/mixer/mixer.h @@ -0,0 +1,500 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mixer.h + * + * Generic, programmable, procedural control signal mixers. + * + * This library implements a generic mixer interface that can be used + * by any driver or subsytem that wants to combine several control signals + * into a single output. + * + * Terminology + * =========== + * + * control value + * A mixer input value, typically provided by some controlling + * component of the system. + * + * control group + * A collection of controls provided by a single controlling component. + * + * actuator + * The mixer output value. + * + * + * Mixing basics + * ============= + * + * An actuator derives its value from the combination of one or more + * control values. Each of the control values is scaled according to + * the actuator's configuration and then combined to produce the + * actuator value, which may then be further scaled to suit the specific + * output type. + * + * Internally, all scaling is performed using floating point values. + * Inputs and outputs are clamped to the range -1.0 to 1.0. + * + * control control control + * | | | + * v v v + * scale scale scale + * | | | + * | v | + * +-------> mix <------+ + * | + * scale + * | + * v + * out + * + * Scaling + * ------- + * + * Each scaler allows the input value to be scaled independently for + * inputs greater/less than zero. An offset can be applied to the output, + * as well as lower and upper boundary constraints. + * Negative scaling factors cause the output to be inverted (negative input + * produces positive output). + * + * Scaler pseudocode: + * + * if (input < 0) + * output = (input * NEGATIVE_SCALE) + OFFSET + * else + * output = (input * POSITIVE_SCALE) + OFFSET + * + * if (output < LOWER_LIMIT) + * output = LOWER_LIMIT + * if (output > UPPER_LIMIT) + * output = UPPER_LIMIT + * + * + * Mixing + * ------ + * + * Mixing behaviour varies based on the specific mixer class; each + * mixer class describes its behaviour in more detail. + * + * + * Controls + * -------- + * + * The precise assignment of controls may vary depending on the + * application, but the following assignments should be used + * when appropriate. Some mixer classes have specific assumptions + * about the assignment of controls. + * + * control | standard meaning + * --------+----------------------- + * 0 | roll + * 1 | pitch + * 2 | yaw + * 3 | primary thrust + */ + + +#ifndef _SYSTEMLIB_MIXER_MIXER_H +#define _SYSTEMLIB_MIXER_MIXER_H value + +#include "drivers/drv_mixer.h" + +/** + * Abstract class defining a mixer mixing zero or more inputs to + * one or more outputs. + */ +class __EXPORT Mixer +{ +public: + /** next mixer in a list */ + Mixer *_next; + + /** + * Fetch a control value. + * + * @param handle Token passed when the callback is registered. + * @param control_group The group to fetch the control from. + * @param control_index The group-relative index to fetch the control from. + * @param control The returned control + * @return Zero if the value was fetched, nonzero otherwise. + */ + typedef int (* ControlCallback)(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &control); + + /** + * Constructor. + * + * @param control_cb Callback invoked when reading controls. + */ + Mixer(ControlCallback control_cb, uintptr_t cb_handle); + virtual ~Mixer() {}; + + /** + * Perform the mixing function. + * + * @param outputs Array into which mixed output(s) should be placed. + * @param space The number of available entries in the output array; + * @return The number of entries in the output array that were populated. + */ + virtual unsigned mix(float *outputs, unsigned space) = 0; + + /** + * Analyses the mix configuration and updates a bitmask of groups + * that are required. + * + * @param groups A bitmask of groups (0-31) that the mixer requires. + */ + virtual void groups_required(uint32_t &groups) = 0; + +protected: + /** client-supplied callback used when fetching control values */ + ControlCallback _control_cb; + uintptr_t _cb_handle; + + /** + * Invoke the client callback to fetch a control value. + * + * @param group Control group to fetch from. + * @param index Control index to fetch. + * @return The control value. + */ + float get_control(uint8_t group, uint8_t index); + + /** + * Perform simpler linear scaling. + * + * @param scaler The scaler configuration. + * @param input The value to be scaled. + * @return The scaled value. + */ + static float scale(const mixer_scaler_s &scaler, float input); + + /** + * Validate a scaler + * + * @param scaler The scaler to be validated. + * @return Zero if good, nonzero otherwise. + */ + static int scale_check(struct mixer_scaler_s &scaler); + +private: +}; + +/** + * Group of mixers, built up from single mixers and processed + * in order when mixing. + */ +class __EXPORT MixerGroup : public Mixer +{ +public: + MixerGroup(ControlCallback control_cb, uintptr_t cb_handle); + ~MixerGroup(); + + virtual unsigned mix(float *outputs, unsigned space); + virtual void groups_required(uint32_t &groups); + + /** + * Add a mixer to the group. + * + * @param mixer The mixer to be added. + */ + void add_mixer(Mixer *mixer); + + /** + * Remove all the mixers from the group. + */ + void reset(); + + /** + * Adds mixers to the group based on a text description in a buffer. + * + * Mixer definitions begin with a single capital letter and a colon. + * The actual format of the mixer definition varies with the individual + * mixers; they are summarised here, but see ROMFS/mixers/README for + * more details. + * + * Null Mixer + * .......... + * + * The null mixer definition has the form: + * + * Z: + * + * Simple Mixer + * ............ + * + * A simple mixer definition begins with: + * + * M: <control count> + * O: <-ve scale> <+ve scale> <offset> <lower limit> <upper limit> + * + * The definition continues with <control count> entries describing the control + * inputs and their scaling, in the form: + * + * S: <group> <index> <-ve scale> <+ve scale> <offset> <lower limit> <upper limit> + * + * Multirotor Mixer + * ................ + * + * The multirotor mixer definition is a single line of the form: + * + * R: <geometry> <roll scale> <pitch scale> <yaw scale> <deadband> + * + * @param buf The mixer configuration buffer. + * @param buflen The length of the buffer, updated to reflect + * bytes as they are consumed. + * @return Zero on successful load, nonzero otherwise. + */ + int load_from_buf(const char *buf, unsigned &buflen); + +private: + Mixer *_first; /**< linked list of mixers */ +}; + +/** + * Null mixer; returns zero. + * + * Used as a placeholder for output channels that are unassigned in groups. + */ +class __EXPORT NullMixer : public Mixer +{ +public: + NullMixer(); + ~NullMixer() {}; + + /** + * Factory method. + * + * Given a pointer to a buffer containing a text description of the mixer, + * returns a pointer to a new instance of the mixer. + * + * @param buf Buffer containing a text description of + * the mixer. + * @param buflen Length of the buffer in bytes, adjusted + * to reflect the bytes consumed. + * @return A new NullMixer instance, or nullptr + * if the text format is bad. + */ + static NullMixer *from_text(const char *buf, unsigned &buflen); + + virtual unsigned mix(float *outputs, unsigned space); + virtual void groups_required(uint32_t &groups); +}; + +/** + * Simple summing mixer. + * + * Collects zero or more inputs and mixes them to a single output. + */ +class __EXPORT SimpleMixer : public Mixer +{ +public: + /** + * Constructor + * + * @param mixinfo Mixer configuration. The pointer passed + * becomes the property of the mixer and + * will be freed when the mixer is deleted. + */ + SimpleMixer(ControlCallback control_cb, + uintptr_t cb_handle, + mixer_simple_s *mixinfo); + ~SimpleMixer(); + + /** + * Factory method with full external configuration. + * + * Given a pointer to a buffer containing a text description of the mixer, + * returns a pointer to a new instance of the mixer. + * + * @param control_cb The callback to invoke when fetching a + * control value. + * @param cb_handle Handle passed to the control callback. + * @param buf Buffer containing a text description of + * the mixer. + * @param buflen Length of the buffer in bytes, adjusted + * to reflect the bytes consumed. + * @return A new SimpleMixer instance, or nullptr + * if the text format is bad. + */ + static SimpleMixer *from_text(Mixer::ControlCallback control_cb, + uintptr_t cb_handle, + const char *buf, + unsigned &buflen); + + /** + * Factory method for PWM/PPM input to internal float representation. + * + * @param control_cb The callback to invoke when fetching a + * control value. + * @param cb_handle Handle passed to the control callback. + * @param input The control index used when fetching the input. + * @param min The PWM/PPM value considered to be "minimum" (gives -1.0 out) + * @param mid The PWM/PPM value considered to be the midpoint (gives 0.0 out) + * @param max The PWM/PPM value considered to be "maximum" (gives 1.0 out) + * @return A new SimpleMixer instance, or nullptr if one could not be + * allocated. + */ + static SimpleMixer *pwm_input(Mixer::ControlCallback control_cb, + uintptr_t cb_handle, + unsigned input, + uint16_t min, + uint16_t mid, + uint16_t max); + + virtual unsigned mix(float *outputs, unsigned space); + virtual void groups_required(uint32_t &groups); + + /** + * Check that the mixer configuration as loaded is sensible. + * + * Note that this function will call control_cb, but only cares about + * error returns, not the input value. + * + * @return Zero if the mixer makes sense, nonzero otherwise. + */ + int check(); + +protected: + +private: + mixer_simple_s *_info; + + static int parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler); + static int parse_control_scaler(const char *buf, + unsigned &buflen, + mixer_scaler_s &scaler, + uint8_t &control_group, + uint8_t &control_index); +}; + +/** + * Multi-rotor mixer for pre-defined vehicle geometries. + * + * Collects four inputs (roll, pitch, yaw, thrust) and mixes them to + * a set of outputs based on the configured geometry. + */ +class __EXPORT MultirotorMixer : public Mixer +{ +public: + /** + * Supported multirotor geometries. + * + * XXX add more + */ + enum Geometry { + QUAD_X = 0, /**< quad in X configuration */ + QUAD_PLUS, /**< quad in + configuration */ + QUAD_V, /**< quad in V configuration */ + QUAD_WIDE, /**< quad in wide configuration */ + HEX_X, /**< hex in X configuration */ + HEX_PLUS, /**< hex in + configuration */ + OCTA_X, + OCTA_PLUS, + + MAX_GEOMETRY + }; + + /** + * Precalculated rotor mix. + */ + struct Rotor { + float roll_scale; /**< scales roll for this rotor */ + float pitch_scale; /**< scales pitch for this rotor */ + float yaw_scale; /**< scales yaw for this rotor */ + }; + + /** + * Constructor. + * + * @param control_cb Callback invoked to read inputs. + * @param cb_handle Passed to control_cb. + * @param geometry The selected geometry. + * @param roll_scale Scaling factor applied to roll inputs + * compared to thrust. + * @param pitch_scale Scaling factor applied to pitch inputs + * compared to thrust. + * @param yaw_wcale Scaling factor applied to yaw inputs compared + * to thrust. + * @param deadband Minumum rotor control output value; usually + * tuned to ensure that rotors never stall at the + * low end of their control range. + */ + MultirotorMixer(ControlCallback control_cb, + uintptr_t cb_handle, + Geometry geometry, + float roll_scale, + float pitch_scale, + float yaw_scale, + float deadband); + ~MultirotorMixer(); + + /** + * Factory method. + * + * Given a pointer to a buffer containing a text description of the mixer, + * returns a pointer to a new instance of the mixer. + * + * @param control_cb The callback to invoke when fetching a + * control value. + * @param cb_handle Handle passed to the control callback. + * @param buf Buffer containing a text description of + * the mixer. + * @param buflen Length of the buffer in bytes, adjusted + * to reflect the bytes consumed. + * @return A new MultirotorMixer instance, or nullptr + * if the text format is bad. + */ + static MultirotorMixer *from_text(Mixer::ControlCallback control_cb, + uintptr_t cb_handle, + const char *buf, + unsigned &buflen); + + virtual unsigned mix(float *outputs, unsigned space); + virtual void groups_required(uint32_t &groups); + +private: + float _roll_scale; + float _pitch_scale; + float _yaw_scale; + float _deadband; + + unsigned _rotor_count; + const Rotor *_rotors; + +}; + +#endif diff --git a/src/modules/systemlib/mixer/mixer_group.cpp b/src/modules/systemlib/mixer/mixer_group.cpp new file mode 100644 index 000000000..1dbc512cd --- /dev/null +++ b/src/modules/systemlib/mixer/mixer_group.cpp @@ -0,0 +1,185 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mixer_group.cpp + * + * Mixer collection. + */ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <stdlib.h> +#include <string.h> +#include <fcntl.h> +#include <poll.h> +#include <errno.h> +#include <stdio.h> +#include <math.h> +#include <unistd.h> + +#include "mixer.h" + +#define debug(fmt, args...) do { } while(0) +//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) +//#include <debug.h> +//#define debug(fmt, args...) lowsyslog(fmt "\n", ##args) + +MixerGroup::MixerGroup(ControlCallback control_cb, uintptr_t cb_handle) : + Mixer(control_cb, cb_handle), + _first(nullptr) +{ +} + +MixerGroup::~MixerGroup() +{ + reset(); +} + +void +MixerGroup::add_mixer(Mixer *mixer) +{ + Mixer **mpp; + + mpp = &_first; + + while (*mpp != nullptr) + mpp = &((*mpp)->_next); + + *mpp = mixer; + mixer->_next = nullptr; +} + +void +MixerGroup::reset() +{ + Mixer *mixer; + + /* discard sub-mixers */ + while (_first != nullptr) { + mixer = _first; + _first = mixer->_next; + delete mixer; + mixer = nullptr; + } +} + +unsigned +MixerGroup::mix(float *outputs, unsigned space) +{ + Mixer *mixer = _first; + unsigned index = 0; + + while ((mixer != nullptr) && (index < space)) { + index += mixer->mix(outputs + index, space - index); + mixer = mixer->_next; + } + + return index; +} + +void +MixerGroup::groups_required(uint32_t &groups) +{ + Mixer *mixer = _first; + + while (mixer != nullptr) { + mixer->groups_required(groups); + mixer = mixer->_next; + } +} + +int +MixerGroup::load_from_buf(const char *buf, unsigned &buflen) +{ + int ret = -1; + const char *end = buf + buflen; + + /* + * Loop until either we have emptied the buffer, or we have failed to + * allocate something when we expected to. + */ + while (buflen > 0) { + Mixer *m = nullptr; + const char *p = end - buflen; + unsigned resid = buflen; + + /* + * Use the next character as a hint to decide which mixer class to construct. + */ + switch (*p) { + case 'Z': + m = NullMixer::from_text(p, resid); + break; + + case 'M': + m = SimpleMixer::from_text(_control_cb, _cb_handle, p, resid); + break; + + case 'R': + m = MultirotorMixer::from_text(_control_cb, _cb_handle, p, resid); + break; + + default: + /* it's probably junk or whitespace, skip a byte and retry */ + buflen--; + continue; + } + + /* + * If we constructed something, add it to the group. + */ + if (m != nullptr) { + add_mixer(m); + + /* we constructed something */ + ret = 0; + + /* only adjust buflen if parsing was successful */ + buflen = resid; + } else { + + /* + * There is data in the buffer that we expected to parse, but it didn't, + * so give up for now. + */ + break; + } + } + + /* nothing more in the buffer for us now */ + return ret; +} diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp new file mode 100644 index 000000000..8ded0b05c --- /dev/null +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -0,0 +1,312 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mixer_multirotor.cpp + * + * Multi-rotor mixers. + */ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <stdlib.h> +#include <string.h> +#include <fcntl.h> +#include <poll.h> +#include <errno.h> +#include <stdio.h> +#include <math.h> +#include <unistd.h> +#include <math.h> + +#include "mixer.h" + +#define debug(fmt, args...) do { } while(0) +//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) +//#include <debug.h> +//#define debug(fmt, args...) lowsyslog(fmt "\n", ##args) + +/* + * Clockwise: 1 + * Counter-clockwise: -1 + */ + +namespace +{ + +/* + * These tables automatically generated by multi_tables - do not edit. + */ +const MultirotorMixer::Rotor _config_quad_x[] = { + { -0.707107, 0.707107, 1.00 }, + { 0.707107, -0.707107, 1.00 }, + { 0.707107, 0.707107, -1.00 }, + { -0.707107, -0.707107, -1.00 }, +}; +const MultirotorMixer::Rotor _config_quad_plus[] = { + { -1.000000, 0.000000, 1.00 }, + { 1.000000, 0.000000, 1.00 }, + { 0.000000, 1.000000, -1.00 }, + { -0.000000, -1.000000, -1.00 }, +}; +const MultirotorMixer::Rotor _config_quad_v[] = { + { -0.927184, 0.374607, 1.00 }, + { 0.694658, -0.719340, 1.00 }, + { 0.927184, 0.374607, -1.00 }, + { -0.694658, -0.719340, -1.00 }, +}; +const MultirotorMixer::Rotor _config_quad_wide[] = { + { -0.927184, 0.374607, 1.00 }, + { 0.777146, -0.629320, 1.00 }, + { 0.927184, 0.374607, -1.00 }, + { -0.777146, -0.629320, -1.00 }, +}; +const MultirotorMixer::Rotor _config_hex_x[] = { + { -1.000000, 0.000000, -1.00 }, + { 1.000000, 0.000000, 1.00 }, + { 0.500000, 0.866025, -1.00 }, + { -0.500000, -0.866025, 1.00 }, + { -0.500000, 0.866025, 1.00 }, + { 0.500000, -0.866025, -1.00 }, +}; +const MultirotorMixer::Rotor _config_hex_plus[] = { + { 0.000000, 1.000000, -1.00 }, + { -0.000000, -1.000000, 1.00 }, + { 0.866025, -0.500000, -1.00 }, + { -0.866025, 0.500000, 1.00 }, + { 0.866025, 0.500000, 1.00 }, + { -0.866025, -0.500000, -1.00 }, +}; +const MultirotorMixer::Rotor _config_octa_x[] = { + { -0.382683, 0.923880, -1.00 }, + { 0.382683, -0.923880, -1.00 }, + { -0.923880, 0.382683, 1.00 }, + { -0.382683, -0.923880, 1.00 }, + { 0.382683, 0.923880, 1.00 }, + { 0.923880, -0.382683, 1.00 }, + { 0.923880, 0.382683, -1.00 }, + { -0.923880, -0.382683, -1.00 }, +}; +const MultirotorMixer::Rotor _config_octa_plus[] = { + { 0.000000, 1.000000, -1.00 }, + { -0.000000, -1.000000, -1.00 }, + { -0.707107, 0.707107, 1.00 }, + { -0.707107, -0.707107, 1.00 }, + { 0.707107, 0.707107, 1.00 }, + { 0.707107, -0.707107, 1.00 }, + { 1.000000, 0.000000, -1.00 }, + { -1.000000, 0.000000, -1.00 }, +}; +const MultirotorMixer::Rotor *_config_index[MultirotorMixer::Geometry::MAX_GEOMETRY] = { + &_config_quad_x[0], + &_config_quad_plus[0], + &_config_quad_v[0], + &_config_quad_wide[0], + &_config_hex_x[0], + &_config_hex_plus[0], + &_config_octa_x[0], + &_config_octa_plus[0], +}; +const unsigned _config_rotor_count[MultirotorMixer::Geometry::MAX_GEOMETRY] = { + 4, /* quad_x */ + 4, /* quad_plus */ + 4, /* quad_v */ + 4, /* quad_wide */ + 6, /* hex_x */ + 6, /* hex_plus */ + 8, /* octa_x */ + 8, /* octa_plus */ +}; + +} + +MultirotorMixer::MultirotorMixer(ControlCallback control_cb, + uintptr_t cb_handle, + Geometry geometry, + float roll_scale, + float pitch_scale, + float yaw_scale, + float deadband) : + Mixer(control_cb, cb_handle), + _roll_scale(roll_scale), + _pitch_scale(pitch_scale), + _yaw_scale(yaw_scale), + _deadband(-1.0f + deadband), /* shift to output range here to avoid runtime calculation */ + _rotor_count(_config_rotor_count[geometry]), + _rotors(_config_index[geometry]) +{ +} + +MultirotorMixer::~MultirotorMixer() +{ +} + +MultirotorMixer * +MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen) +{ + MultirotorMixer::Geometry geometry; + char geomname[8]; + int s[4]; + int used; + + if (sscanf(buf, "R: %s %d %d %d %d%n", geomname, &s[0], &s[1], &s[2], &s[3], &used) != 5) { + debug("multirotor parse failed on '%s'", buf); + return nullptr; + } + + if (used > (int)buflen) { + debug("multirotor spec used %d of %u", used, buflen); + return nullptr; + } + + buflen -= used; + + if (!strcmp(geomname, "4+")) { + geometry = MultirotorMixer::QUAD_PLUS; + + } else if (!strcmp(geomname, "4x")) { + geometry = MultirotorMixer::QUAD_X; + + } else if (!strcmp(geomname, "4v")) { + geometry = MultirotorMixer::QUAD_V; + + } else if (!strcmp(geomname, "4w")) { + geometry = MultirotorMixer::QUAD_WIDE; + + } else if (!strcmp(geomname, "6+")) { + geometry = MultirotorMixer::HEX_PLUS; + + } else if (!strcmp(geomname, "6x")) { + geometry = MultirotorMixer::HEX_X; + + } else if (!strcmp(geomname, "8+")) { + geometry = MultirotorMixer::OCTA_PLUS; + + } else if (!strcmp(geomname, "8x")) { + geometry = MultirotorMixer::OCTA_X; + + } else { + debug("unrecognised geometry '%s'", geomname); + return nullptr; + } + + debug("adding multirotor mixer '%s'", geomname); + + return new MultirotorMixer( + control_cb, + cb_handle, + geometry, + s[0] / 10000.0f, + s[1] / 10000.0f, + s[2] / 10000.0f, + s[3] / 10000.0f); +} + +unsigned +MultirotorMixer::mix(float *outputs, unsigned space) +{ + float roll = get_control(0, 0) * _roll_scale; + //lowsyslog("roll: %d, get_control0: %d, %d\n", (int)(roll), (int)(get_control(0, 0)), (int)(_roll_scale)); + float pitch = get_control(0, 1) * _pitch_scale; + float yaw = get_control(0, 2) * _yaw_scale; + float thrust = get_control(0, 3); + //lowsyslog("thrust: %d, get_control3: %d\n", (int)(thrust), (int)(get_control(0, 3))); + float max = 0.0f; + float fixup_scale; + + /* use an output factor to prevent too strong control signals at low throttle */ + float min_thrust = 0.05f; + float max_thrust = 1.0f; + float startpoint_full_control = 0.40f; + float output_factor; + + /* keep roll, pitch and yaw control to 0 below min thrust */ + if (thrust <= min_thrust) { + output_factor = 0.0f; + /* linearly increase the output factor from 0 to 1 between min_thrust and startpoint_full_control */ + + } else if (thrust < startpoint_full_control && thrust > min_thrust) { + output_factor = (thrust / max_thrust) / (startpoint_full_control - min_thrust); + /* and then stay at full control */ + + } else { + output_factor = max_thrust; + } + + roll *= output_factor; + pitch *= output_factor; + yaw *= output_factor; + + + /* perform initial mix pass yielding un-bounded outputs */ + for (unsigned i = 0; i < _rotor_count; i++) { + float tmp = roll * _rotors[i].roll_scale + + pitch * _rotors[i].pitch_scale + + yaw * _rotors[i].yaw_scale + + thrust; + + if (tmp > max) + max = tmp; + + outputs[i] = tmp; + } + + /* scale values into the -1.0 - 1.0 range */ + if (max > 1.0f) { + fixup_scale = 2.0f / max; + + } else { + fixup_scale = 2.0f; + } + + for (unsigned i = 0; i < _rotor_count; i++) + outputs[i] = -1.0f + (outputs[i] * fixup_scale); + + /* ensure outputs are out of the deadband */ + for (unsigned i = 0; i < _rotor_count; i++) + if (outputs[i] < _deadband) + outputs[i] = _deadband; + + return _rotor_count; +} + +void +MultirotorMixer::groups_required(uint32_t &groups) +{ + /* XXX for now, hardcoded to indexes 0-3 in control group zero */ + groups |= (1 << 0); +} + diff --git a/src/modules/systemlib/mixer/mixer_simple.cpp b/src/modules/systemlib/mixer/mixer_simple.cpp new file mode 100644 index 000000000..07dc5f37f --- /dev/null +++ b/src/modules/systemlib/mixer/mixer_simple.cpp @@ -0,0 +1,334 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mixer_simple.cpp + * + * Simple summing mixer. + */ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <stdlib.h> +#include <string.h> +#include <fcntl.h> +#include <poll.h> +#include <errno.h> +#include <stdio.h> +#include <math.h> +#include <unistd.h> +#include <ctype.h> + +#include "mixer.h" + +#define debug(fmt, args...) do { } while(0) +//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) + +SimpleMixer::SimpleMixer(ControlCallback control_cb, + uintptr_t cb_handle, + mixer_simple_s *mixinfo) : + Mixer(control_cb, cb_handle), + _info(mixinfo) +{ +} + +SimpleMixer::~SimpleMixer() +{ + if (_info != nullptr) + free(_info); +} + +static const char * +findtag(const char *buf, unsigned &buflen, char tag) +{ + while (buflen >= 2) { + if ((buf[0] == tag) && (buf[1] == ':')) + return buf; + buf++; + buflen--; + } + return nullptr; +} + +static void +skipline(const char *buf, unsigned &buflen) +{ + const char *p; + + /* if we can find a CR or NL in the buffer, skip up to it */ + if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen))) + buflen -= (p - buf); +} + +int +SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler) +{ + int ret; + int s[5]; + + buf = findtag(buf, buflen, 'O'); + if ((buf == nullptr) || (buflen < 12)) + return -1; + + if ((ret = sscanf(buf, "O: %d %d %d %d %d", + &s[0], &s[1], &s[2], &s[3], &s[4])) != 5) { + debug("scaler parse failed on '%s' (got %d)", buf, ret); + return -1; + } + skipline(buf, buflen); + + scaler.negative_scale = s[0] / 10000.0f; + scaler.positive_scale = s[1] / 10000.0f; + scaler.offset = s[2] / 10000.0f; + scaler.min_output = s[3] / 10000.0f; + scaler.max_output = s[4] / 10000.0f; + + return 0; +} + +int +SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, uint8_t &control_group, uint8_t &control_index) +{ + unsigned u[2]; + int s[5]; + + buf = findtag(buf, buflen, 'S'); + if ((buf == nullptr) || (buflen < 16)) + return -1; + + if (sscanf(buf, "S: %u %u %d %d %d %d %d", + &u[0], &u[1], &s[0], &s[1], &s[2], &s[3], &s[4]) != 7) { + debug("control parse failed on '%s'", buf); + return -1; + } + skipline(buf, buflen); + + control_group = u[0]; + control_index = u[1]; + scaler.negative_scale = s[0] / 10000.0f; + scaler.positive_scale = s[1] / 10000.0f; + scaler.offset = s[2] / 10000.0f; + scaler.min_output = s[3] / 10000.0f; + scaler.max_output = s[4] / 10000.0f; + + return 0; +} + +SimpleMixer * +SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen) +{ + SimpleMixer *sm = nullptr; + mixer_simple_s *mixinfo = nullptr; + unsigned inputs; + int used; + const char *end = buf + buflen; + + /* get the base info for the mixer */ + if (sscanf(buf, "M: %u%n", &inputs, &used) != 1) { + debug("simple parse failed on '%s'", buf); + goto out; + } + + buflen -= used; + + mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs)); + + if (mixinfo == nullptr) { + debug("could not allocate memory for mixer info"); + goto out; + } + + mixinfo->control_count = inputs; + + if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler)) + goto out; + + for (unsigned i = 0; i < inputs; i++) { + if (parse_control_scaler(end - buflen, buflen, + mixinfo->controls[i].scaler, + mixinfo->controls[i].control_group, + mixinfo->controls[i].control_index)) + goto out; + } + + sm = new SimpleMixer(control_cb, cb_handle, mixinfo); + + if (sm != nullptr) { + mixinfo = nullptr; + debug("loaded mixer with %d inputs", inputs); + + } else { + debug("could not allocate memory for mixer"); + } + +out: + + if (mixinfo != nullptr) + free(mixinfo); + + return sm; +} + +SimpleMixer * +SimpleMixer::pwm_input(Mixer::ControlCallback control_cb, uintptr_t cb_handle, unsigned input, uint16_t min, uint16_t mid, uint16_t max) +{ + SimpleMixer *sm = nullptr; + mixer_simple_s *mixinfo = nullptr; + + mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(1)); + + if (mixinfo == nullptr) { + debug("could not allocate memory for mixer info"); + goto out; + } + + mixinfo->control_count = 1; + + /* + * Always pull from group 0, with the input value giving the channel. + */ + mixinfo->controls[0].control_group = 0; + mixinfo->controls[0].control_index = input; + + /* + * Conversion uses both the input and output side of the mixer. + * + * The input side is used to slide the control value such that the min argument + * results in a value of zero. + * + * The output side is used to apply the scaling for the min/max values so that + * the resulting output is a -1.0 ... 1.0 value for the min...max range. + */ + mixinfo->controls[0].scaler.negative_scale = 1.0f; + mixinfo->controls[0].scaler.positive_scale = 1.0f; + mixinfo->controls[0].scaler.offset = -mid; + mixinfo->controls[0].scaler.min_output = -(mid - min); + mixinfo->controls[0].scaler.max_output = (max - mid); + + mixinfo->output_scaler.negative_scale = 500.0f / (mid - min); + mixinfo->output_scaler.positive_scale = 500.0f / (max - mid); + mixinfo->output_scaler.offset = 0.0f; + mixinfo->output_scaler.min_output = -1.0f; + mixinfo->output_scaler.max_output = 1.0f; + + sm = new SimpleMixer(control_cb, cb_handle, mixinfo); + + if (sm != nullptr) { + mixinfo = nullptr; + debug("PWM input mixer for %d", input); + + } else { + debug("could not allocate memory for PWM input mixer"); + } + +out: + + if (mixinfo != nullptr) + free(mixinfo); + + return sm; +} + +unsigned +SimpleMixer::mix(float *outputs, unsigned space) +{ + float sum = 0.0f; + + if (_info == nullptr) + return 0; + + if (space < 1) + return 0; + + for (unsigned i = 0; i < _info->control_count; i++) { + float input; + + _control_cb(_cb_handle, + _info->controls[i].control_group, + _info->controls[i].control_index, + input); + + sum += scale(_info->controls[i].scaler, input); + } + + *outputs = scale(_info->output_scaler, sum); + return 1; +} + +void +SimpleMixer::groups_required(uint32_t &groups) +{ + for (unsigned i = 0; i < _info->control_count; i++) + groups |= 1 << _info->controls[i].control_group; +} + +int +SimpleMixer::check() +{ + int ret; + float junk; + + /* sanity that presumes that a mixer includes a control no more than once */ + /* max of 32 groups due to groups_required API */ + if (_info->control_count > 32) + return -2; + + /* validate the output scaler */ + ret = scale_check(_info->output_scaler); + + if (ret != 0) + return ret; + + /* validate input scalers */ + for (unsigned i = 0; i < _info->control_count; i++) { + + /* verify that we can fetch the control */ + if (_control_cb(_cb_handle, + _info->controls[i].control_group, + _info->controls[i].control_index, + junk) != 0) { + return -3; + } + + /* validate the scaler */ + ret = scale_check(_info->controls[i].scaler); + + if (ret != 0) + return (10 * i + ret); + } + + return 0; +} diff --git a/src/modules/systemlib/mixer/module.mk b/src/modules/systemlib/mixer/module.mk new file mode 100644 index 000000000..4d45e1c50 --- /dev/null +++ b/src/modules/systemlib/mixer/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# mixer library +# +LIBNAME = mixerlib + +SRCS = mixer.cpp \ + mixer_group.cpp \ + mixer_multirotor.cpp \ + mixer_simple.cpp diff --git a/src/modules/systemlib/mixer/multi_tables b/src/modules/systemlib/mixer/multi_tables new file mode 100755 index 000000000..683c63040 --- /dev/null +++ b/src/modules/systemlib/mixer/multi_tables @@ -0,0 +1,107 @@ +#!/usr/bin/tclsh +# +# Generate multirotor mixer scale tables compatible with the ArduCopter layout +# + +proc rad {a} { expr ($a / 360.0) * 2 * acos(-1) } +proc rcos {a} { expr cos([rad $a])} + +set quad_x { + 45 CCW + -135 CCW + -45 CW + 135 CW +} + +set quad_plus { + 90 CCW + -90 CCW + 0 CW + 180 CW +} + +set quad_v { + 68 CCW + -136 CCW + -68 CW + 136 CW +} + +set quad_wide { + 68 CCW + -129 CCW + -68 CW + 129 CW +} + +set hex_x { + 90 CW + -90 CCW + -30 CW + 150 CCW + 30 CCW + -150 CW +} + +set hex_plus { + 0 CW + 180 CCW + -120 CW + 60 CCW + -60 CCW + 120 CW +} + +set octa_x { + 22.5 CW + -157.5 CW + 67.5 CCW + 157.5 CCW + -22.5 CCW + -112.5 CCW + -67.5 CW + 112.5 CW +} + +set octa_plus { + 0 CW + 180 CW + 45 CCW + 135 CCW + -45 CCW + -135 CCW + -90 CW + 90 CW +} + +set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus octa_x octa_plus} + +proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %5.2f }," [rcos [expr $a + 90]] [rcos $a] [expr -$d]]} + +foreach table $tables { + puts [format "const MultirotorMixer::Rotor _config_%s\[\] = {" $table] + + upvar #0 $table angles + foreach {angle dir} $angles { + if {$dir == "CW"} { + set dd 1.0 + } else { + set dd -1.0 + } + factors $angle $dd + } + puts "};" +} + +puts "const MultirotorMixer::Rotor *_config_index\[MultirotorMixer::Geometry::MAX_GEOMETRY\] = {" +foreach table $tables { + puts [format "\t&_config_%s\[0\]," $table] +} +puts "};" + +puts "const unsigned _config_rotor_count\[MultirotorMixer::Geometry::MAX_GEOMETRY\] = {" +foreach table $tables { + upvar #0 $table angles + puts [format "\t%u, /* %s */" [expr [llength $angles] / 2] $table] +} +puts "};" diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk new file mode 100644 index 000000000..fd0289c9a --- /dev/null +++ b/src/modules/systemlib/module.mk @@ -0,0 +1,50 @@ +############################################################################ +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# System utility library +# + +SRCS = err.c \ + hx_stream.c \ + perf_counter.c \ + param/param.c \ + bson/tinybson.c \ + conversions.c \ + cpuload.c \ + getopt_long.c \ + up_cxxinitialize.c \ + pid/pid.c \ + geo/geo.c \ + systemlib.c \ + airspeed.c diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c new file mode 100644 index 000000000..69a9bdf9b --- /dev/null +++ b/src/modules/systemlib/param/param.c @@ -0,0 +1,805 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file param.c + * + * Global parameter store. + * + * Note that it might make sense to convert this into a driver. That would + * offer some interesting options regarding state for e.g. ORB advertisements + * and background parameter saving. + */ + +#include <debug.h> +#include <string.h> +#include <stdbool.h> +#include <fcntl.h> +#include <unistd.h> +#include <systemlib/err.h> +#include <errno.h> + +#include <sys/stat.h> + +#include <drivers/drv_hrt.h> + +#include "systemlib/param/param.h" +#include "systemlib/uthash/utarray.h" +#include "systemlib/bson/tinybson.h" + +#include "uORB/uORB.h" +#include "uORB/topics/parameter_update.h" + +#if 1 +# define debug(fmt, args...) do { warnx(fmt, ##args); } while(0) +#else +# define debug(fmt, args...) do { } while(0) +#endif + +/** + * Array of static parameter info. + */ +extern char __param_start, __param_end; +static const struct param_info_s *param_info_base = (struct param_info_s *) &__param_start; +static const struct param_info_s *param_info_limit = (struct param_info_s *) &__param_end; +#define param_info_count ((unsigned)(param_info_limit - param_info_base)) + +/** + * Storage for modified parameters. + */ +struct param_wbuf_s { + param_t param; + union param_value_u val; + bool unsaved; +}; + +/** flexible array holding modified parameter values */ +UT_array *param_values; + +/** array info for the modified parameters array */ +const UT_icd param_icd = {sizeof(struct param_wbuf_s), NULL, NULL, NULL}; + +/** parameter update topic */ +ORB_DEFINE(parameter_update, struct parameter_update_s); + +/** parameter update topic handle */ +static orb_advert_t param_topic = -1; + +/** lock the parameter store */ +static void +param_lock(void) +{ + /* XXX */ +} + +/** unlock the parameter store */ +static void +param_unlock(void) +{ + /* XXX */ +} + +/** assert that the parameter store is locked */ +static void +param_assert_locked(void) +{ + /* XXX */ +} + +/** + * Test whether a param_t is value. + * + * @param param The parameter handle to test. + * @return True if the handle is valid. + */ +static bool +handle_in_range(param_t param) +{ + return (param < param_info_count); +} + +/** + * Compare two modifid parameter structures to determine ordering. + * + * This function is suitable for passing to qsort or bsearch. + */ +static int +param_compare_values(const void *a, const void *b) +{ + struct param_wbuf_s *pa = (struct param_wbuf_s *)a; + struct param_wbuf_s *pb = (struct param_wbuf_s *)b; + + if (pa->param < pb->param) + return -1; + + if (pa->param > pb->param) + return 1; + + return 0; +} + +/** + * Locate the modified parameter structure for a parameter, if it exists. + * + * @param param The parameter being searched. + * @return The structure holding the modified value, or + * NULL if the parameter has not been modified. + */ +static struct param_wbuf_s * +param_find_changed(param_t param) { + struct param_wbuf_s *s = NULL; + + param_assert_locked(); + + if (param_values != NULL) { +#if 0 /* utarray_find requires bsearch, not available */ + struct param_wbuf_s key; + key.param = param; + s = utarray_find(param_values, &key, param_compare_values); +#else + + while ((s = (struct param_wbuf_s *)utarray_next(param_values, s)) != NULL) { + if (s->param == param) + break; + } + +#endif + } + + return s; +} + +static void +param_notify_changes(void) +{ + struct parameter_update_s pup = { .timestamp = hrt_absolute_time() }; + + /* + * If we don't have a handle to our topic, create one now; otherwise + * just publish. + */ + if (param_topic == -1) { + param_topic = orb_advertise(ORB_ID(parameter_update), &pup); + + } else { + orb_publish(ORB_ID(parameter_update), param_topic, &pup); + } +} + +param_t +param_find(const char *name) +{ + param_t param; + + /* perform a linear search of the known parameters */ + for (param = 0; handle_in_range(param); param++) { + if (!strcmp(param_info_base[param].name, name)) + return param; + } + + /* not found */ + return PARAM_INVALID; +} + +unsigned +param_count(void) +{ + return param_info_count; +} + +param_t +param_for_index(unsigned index) +{ + if (index < param_info_count) + return (param_t)index; + + return PARAM_INVALID; +} + +int +param_get_index(param_t param) +{ + if (handle_in_range(param)) + return (unsigned)param; + + return -1; +} + +const char * +param_name(param_t param) +{ + if (handle_in_range(param)) + return param_info_base[param].name; + + return NULL; +} + +bool +param_value_is_default(param_t param) +{ + return param_find_changed(param) ? false : true; +} + +bool +param_value_unsaved(param_t param) +{ + static struct param_wbuf_s *s; + + s = param_find_changed(param); + + if (s && s->unsaved) + return true; + + return false; +} + +enum param_type_e +param_type(param_t param) +{ + if (handle_in_range(param)) + return param_info_base[param].type; + + return PARAM_TYPE_UNKNOWN; +} + +size_t +param_size(param_t param) +{ + if (handle_in_range(param)) { + switch (param_type(param)) { + case PARAM_TYPE_INT32: + case PARAM_TYPE_FLOAT: + return 4; + + case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: + /* decode structure size from type value */ + return param_type(param) - PARAM_TYPE_STRUCT; + + default: + return 0; + } + } + + return 0; +} + +/** + * Obtain a pointer to the storage allocated for a parameter. + * + * @param param The parameter whose storage is sought. + * @return A pointer to the parameter value, or NULL + * if the parameter does not exist. + */ +static const void * +param_get_value_ptr(param_t param) +{ + const void *result = NULL; + + param_assert_locked(); + + if (handle_in_range(param)) { + + const union param_value_u *v; + + /* work out whether we're fetching the default or a written value */ + struct param_wbuf_s *s = param_find_changed(param); + + if (s != NULL) { + v = &s->val; + + } else { + v = ¶m_info_base[param].val; + } + + if (param_type(param) == PARAM_TYPE_STRUCT) { + result = v->p; + + } else { + result = v; + } + } + + return result; +} + +int +param_get(param_t param, void *val) +{ + int result = -1; + + param_lock(); + + const void *v = param_get_value_ptr(param); + + if (val != NULL) { + memcpy(val, v, param_size(param)); + result = 0; + } + + param_unlock(); + + return result; +} + +static int +param_set_internal(param_t param, const void *val, bool mark_saved) +{ + int result = -1; + bool params_changed = false; + + param_lock(); + + if (param_values == NULL) + utarray_new(param_values, ¶m_icd); + + if (param_values == NULL) { + debug("failed to allocate modified values array"); + goto out; + } + + if (handle_in_range(param)) { + + struct param_wbuf_s *s = param_find_changed(param); + + if (s == NULL) { + + /* construct a new parameter */ + struct param_wbuf_s buf = { + .param = param, + .val.p = NULL, + .unsaved = false + }; + + /* add it to the array and sort */ + utarray_push_back(param_values, &buf); + utarray_sort(param_values, param_compare_values); + + /* find it after sorting */ + s = param_find_changed(param); + } + + /* update the changed value */ + switch (param_type(param)) { + case PARAM_TYPE_INT32: + s->val.i = *(int32_t *)val; + break; + + case PARAM_TYPE_FLOAT: + s->val.f = *(float *)val; + break; + + case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: + if (s->val.p == NULL) { + s->val.p = malloc(param_size(param)); + + if (s->val.p == NULL) { + debug("failed to allocate parameter storage"); + goto out; + } + } + + memcpy(s->val.p, val, param_size(param)); + break; + + default: + goto out; + } + + s->unsaved = !mark_saved; + params_changed = true; + result = 0; + } + +out: + param_unlock(); + + /* + * If we set something, now that we have unlocked, go ahead and advertise that + * a thing has been set. + */ + if (params_changed) + param_notify_changes(); + + return result; +} + +int +param_set(param_t param, const void *val) +{ + return param_set_internal(param, val, false); +} + +void +param_reset(param_t param) +{ + struct param_wbuf_s *s = NULL; + + param_lock(); + + if (handle_in_range(param)) { + + /* look for a saved value */ + s = param_find_changed(param); + + /* if we found one, erase it */ + if (s != NULL) { + int pos = utarray_eltidx(param_values, s); + utarray_erase(param_values, pos, 1); + } + } + + param_unlock(); + + if (s != NULL) + param_notify_changes(); +} + +void +param_reset_all(void) +{ + param_lock(); + + if (param_values != NULL) { + utarray_free(param_values); + } + + /* mark as reset / deleted */ + param_values = NULL; + + param_unlock(); + + param_notify_changes(); +} + +static const char *param_default_file = "/eeprom/parameters"; +static char *param_user_file = NULL; + +int +param_set_default_file(const char* filename) +{ + if (param_user_file != NULL) { + free(param_user_file); + param_user_file = NULL; + } + if (filename) + param_user_file = strdup(filename); + return 0; +} + +const char * +param_get_default_file(void) +{ + return (param_user_file != NULL) ? param_user_file : param_default_file; +} + +int +param_save_default(void) +{ + /* delete the file in case it exists */ + unlink(param_get_default_file()); + + /* create the file */ + int fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL); + + if (fd < 0) { + warn("opening '%s' for writing failed", param_get_default_file()); + return -1; + } + + int result = param_export(fd, false); + close(fd); + + if (result != 0) { + warn("error exporting parameters to '%s'", param_get_default_file()); + unlink(param_get_default_file()); + return -2; + } + + return 0; +} + +/** + * @return 0 on success, 1 if all params have not yet been stored, -1 if device open failed, -2 if writing parameters failed + */ +int +param_load_default(void) +{ + int fd = open(param_get_default_file(), O_RDONLY); + + if (fd < 0) { + /* no parameter file is OK, otherwise this is an error */ + if (errno != ENOENT) { + warn("open '%s' for reading failed", param_get_default_file()); + return -1; + } + return 1; + } + + int result = param_load(fd); + close(fd); + + if (result != 0) { + warn("error reading parameters from '%s'", param_get_default_file()); + return -2; + } + + return 0; +} + +int +param_export(int fd, bool only_unsaved) +{ + struct param_wbuf_s *s = NULL; + struct bson_encoder_s encoder; + int result = -1; + + param_lock(); + + bson_encoder_init_file(&encoder, fd); + + /* no modified parameters -> we are done */ + if (param_values == NULL) { + result = 0; + goto out; + } + + while ((s = (struct param_wbuf_s *)utarray_next(param_values, s)) != NULL) { + + int32_t i; + float f; + + /* + * If we are only saving values changed since last save, and this + * one hasn't, then skip it + */ + if (only_unsaved && !s->unsaved) + continue; + + s->unsaved = false; + + /* append the appropriate BSON type object */ + switch (param_type(s->param)) { + case PARAM_TYPE_INT32: + param_get(s->param, &i); + + if (bson_encoder_append_int(&encoder, param_name(s->param), i)) { + debug("BSON append failed for '%s'", param_name(s->param)); + goto out; + } + + break; + + case PARAM_TYPE_FLOAT: + param_get(s->param, &f); + + if (bson_encoder_append_double(&encoder, param_name(s->param), f)) { + debug("BSON append failed for '%s'", param_name(s->param)); + goto out; + } + + break; + + case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: + if (bson_encoder_append_binary(&encoder, + param_name(s->param), + BSON_BIN_BINARY, + param_size(s->param), + param_get_value_ptr(s->param))) { + debug("BSON append failed for '%s'", param_name(s->param)); + goto out; + } + + break; + + default: + debug("unrecognized parameter type"); + goto out; + } + } + + result = 0; + +out: + param_unlock(); + + if (result == 0) + result = bson_encoder_fini(&encoder); + + return result; +} + +struct param_import_state { + bool mark_saved; +}; + +static int +param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node) +{ + float f; + int32_t i; + void *v, *tmp = NULL; + int result = -1; + struct param_import_state *state = (struct param_import_state *)private; + + /* + * EOO means the end of the parameter object. (Currently not supporting + * nested BSON objects). + */ + if (node->type == BSON_EOO) { + debug("end of parameters"); + return 0; + } + + /* + * Find the parameter this node represents. If we don't know it, + * ignore the node. + */ + param_t param = param_find(node->name); + + if (param == PARAM_INVALID) { + debug("ignoring unrecognised parameter '%s'", node->name); + return 1; + } + + /* + * Handle setting the parameter from the node + */ + + switch (node->type) { + case BSON_INT32: + if (param_type(param) != PARAM_TYPE_INT32) { + debug("unexpected type for '%s", node->name); + goto out; + } + + i = node->i; + v = &i; + break; + + case BSON_DOUBLE: + if (param_type(param) != PARAM_TYPE_FLOAT) { + debug("unexpected type for '%s", node->name); + goto out; + } + + f = node->d; + v = &f; + break; + + case BSON_BINDATA: + if (node->subtype != BSON_BIN_BINARY) { + debug("unexpected subtype for '%s", node->name); + goto out; + } + + if (bson_decoder_data_pending(decoder) != param_size(param)) { + debug("bad size for '%s'", node->name); + goto out; + } + + /* XXX check actual file data size? */ + tmp = malloc(param_size(param)); + + if (tmp == NULL) { + debug("failed allocating for '%s'", node->name); + goto out; + } + + if (bson_decoder_copy_data(decoder, tmp)) { + debug("failed copying data for '%s'", node->name); + goto out; + } + + v = tmp; + break; + + default: + debug("unrecognised node type"); + goto out; + } + + if (param_set_internal(param, v, state->mark_saved)) { + debug("error setting value for '%s'", node->name); + goto out; + } + + if (tmp != NULL) { + free(tmp); + tmp = NULL; + } + + /* don't return zero, that means EOF */ + result = 1; + +out: + + if (tmp != NULL) + free(tmp); + + return result; +} + +static int +param_import_internal(int fd, bool mark_saved) +{ + struct bson_decoder_s decoder; + int result = -1; + struct param_import_state state; + + if (bson_decoder_init_file(&decoder, fd, param_import_callback, &state)) { + debug("decoder init failed"); + goto out; + } + + state.mark_saved = mark_saved; + + do { + result = bson_decoder_next(&decoder); + + } while (result > 0); + +out: + + if (result < 0) + debug("BSON error decoding parameters"); + + return result; +} + +int +param_import(int fd) +{ + return param_import_internal(fd, false); +} + +int +param_load(int fd) +{ + param_reset_all(); + return param_import_internal(fd, true); +} + +void +param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_changed) +{ + param_t param; + + for (param = 0; handle_in_range(param); param++) { + + /* if requested, skip unchanged values */ + if (only_changed && (param_find_changed(param) == NULL)) + continue; + + func(arg, param); + } +} diff --git a/src/modules/systemlib/param/param.h b/src/modules/systemlib/param/param.h new file mode 100644 index 000000000..084cd931a --- /dev/null +++ b/src/modules/systemlib/param/param.h @@ -0,0 +1,336 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file param.h + * + * Global parameter store. + * + * Note that a number of API members are marked const or pure; these + * assume that the set of parameters cannot change, or that a parameter + * cannot change type or size over its lifetime. If any of these assumptions + * are invalidated, the attributes should be re-evaluated. + */ + +#ifndef _SYSTEMLIB_PARAM_PARAM_H +#define _SYSTEMLIB_PARAM_PARAM_H + +#include <stdint.h> +#include <stdbool.h> +#include <sys/types.h> + +/** Maximum size of the parameter backing file */ +#define PARAM_FILE_MAXSIZE 4096 + +__BEGIN_DECLS + +/** + * Parameter types. + */ +typedef enum param_type_e { + /* globally-known parameter types */ + PARAM_TYPE_INT32 = 0, + PARAM_TYPE_FLOAT, + + /* structure parameters; size is encoded in the type value */ + PARAM_TYPE_STRUCT = 100, + PARAM_TYPE_STRUCT_MAX = 16384 + PARAM_TYPE_STRUCT, + + PARAM_TYPE_UNKNOWN = 0xffff +} param_type_t; + +/** + * Parameter handle. + * + * Parameters are represented by parameter handles, which can + * be obtained by looking up (or creating?) parameters. + */ +typedef uintptr_t param_t; + +/** + * Handle returned when a parameter cannot be found. + */ +#define PARAM_INVALID ((uintptr_t)0xffffffff) + +/** + * Look up a parameter by name. + * + * @param name The canonical name of the parameter being looked up. + * @return A handle to the parameter, or PARAM_INVALID if the parameter does not exist. + */ +__EXPORT param_t param_find(const char *name); + +/** + * Return the total number of parameters. + * + * @return The number of parameters. + */ +__EXPORT unsigned param_count(void); + +/** + * Look up a parameter by index. + * + * @param index An index from 0 to n, where n is param_count()-1. + * @return A handle to the parameter, or PARAM_INVALID if the index is out of range. + */ +__EXPORT param_t param_for_index(unsigned index); + +/** + * Look up the index of a parameter. + * + * @param param The parameter to obtain the index for. + * @return The index, or -1 if the parameter does not exist. + */ +__EXPORT int param_get_index(param_t param); + +/** + * Obtain the name of a parameter. + * + * @param param A handle returned by param_find or passed by param_foreach. + * @return The name assigned to the parameter, or NULL if the handle is invalid. + */ +__EXPORT const char *param_name(param_t param); + +/** + * Test whether a parameter's value has changed from the default. + * + * @return If true, the parameter's value has not been changed from the default. + */ +__EXPORT bool param_value_is_default(param_t param); + +/** + * Test whether a parameter's value has been changed but not saved. + * + * @return If true, the parameter's value has not been saved. + */ +__EXPORT bool param_value_unsaved(param_t param); + +/** + * Obtain the type of a parameter. + * + * @param param A handle returned by param_find or passed by param_foreach. + * @return The type assigned to the parameter. + */ +__EXPORT param_type_t param_type(param_t param); + +/** + * Determine the size of a parameter. + * + * @param param A handle returned by param_find or passed by param_foreach. + * @return The size of the parameter's value. + */ +__EXPORT size_t param_size(param_t param); + +/** + * Copy the value of a parameter. + * + * @param param A handle returned by param_find or passed by param_foreach. + * @param val Where to return the value, assumed to point to suitable storage for the parameter type. + * For structures, a bitwise copy of the structure is performed to this address. + * @return Zero if the parameter's value could be returned, nonzero otherwise. + */ +__EXPORT int param_get(param_t param, void *val); + +/** + * Set the value of a parameter. + * + * @param param A handle returned by param_find or passed by param_foreach. + * @param val The value to set; assumed to point to a variable of the parameter type. + * For structures, the pointer is assumed to point to a structure to be copied. + * @return Zero if the parameter's value could be set from a scalar, nonzero otherwise. + */ +__EXPORT int param_set(param_t param, const void *val); + +/** + * Reset a parameter to its default value. + * + * This function frees any storage used by struct parameters, and returns the parameter + * to its default value. + * + * @param param A handle returned by param_find or passed by param_foreach. + */ +__EXPORT void param_reset(param_t param); + +/** + * Reset all parameters to their default values. + * + * This function also releases the storage used by struct parameters. + */ +__EXPORT void param_reset_all(void); + +/** + * Export changed parameters to a file. + * + * @param fd File descriptor to export to. + * @param only_unsaved Only export changed parameters that have not yet been exported. + * @return Zero on success, nonzero on failure. + */ +__EXPORT int param_export(int fd, bool only_unsaved); + +/** + * Import parameters from a file, discarding any unrecognized parameters. + * + * This function merges the imported parameters with the current parameter set. + * + * @param fd File descriptor to import from. (Currently expected to be a file.) + * @return Zero on success, nonzero if an error occurred during import. + * Note that in the failure case, parameters may be inconsistent. + */ +__EXPORT int param_import(int fd); + +/** + * Load parameters from a file. + * + * This function resets all parameters to their default values, then loads new + * values from a file. + * + * @param fd File descriptor to import from. (Currently expected to be a file.) + * @return Zero on success, nonzero if an error occurred during import. + * Note that in the failure case, parameters may be inconsistent. + */ +__EXPORT int param_load(int fd); + +/** + * Apply a function to each parameter. + * + * Note that the parameter set is not locked during the traversal. It also does + * not hold an internal state, so the callback function can block or sleep between + * parameter callbacks. + * + * @param func The function to invoke for each parameter. + * @param arg Argument passed to the function. + * @param only_changed If true, the function is only called for parameters whose values have + * been changed from the default. + */ +__EXPORT void param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_changed); + +/** + * Set the default parameter file name. + * + * @param filename Path to the default parameter file. The file is not require to + * exist. + * @return Zero on success. + */ +__EXPORT int param_set_default_file(const char* filename); + +/** + * Get the default parameter file name. + * + * @return The path to the current default parameter file; either as + * a result of a call to param_set_default_file, or the + * built-in default. + */ +__EXPORT const char* param_get_default_file(void); + +/** + * Save parameters to the default file. + * + * This function saves all parameters with non-default values. + * + * @return Zero on success. + */ +__EXPORT int param_save_default(void); + +/** + * Load parameters from the default parameter file. + * + * @return Zero on success. + */ +__EXPORT int param_load_default(void); + +/* + * Macros creating static parameter definitions. + * + * Note that these structures are not known by name; they are + * collected into a section that is iterated by the parameter + * code. + * + * Note that these macros cannot be used in C++ code due to + * their use of designated initializers. They should probably + * be refactored to avoid the use of a union for param_value_u. + */ + +/** define an int32 parameter */ +#define PARAM_DEFINE_INT32(_name, _default) \ + static const \ + __attribute__((used, section("__param"))) \ + struct param_info_s __param__##_name = { \ + #_name, \ + PARAM_TYPE_INT32, \ + .val.i = _default \ + } + +/** define a float parameter */ +#define PARAM_DEFINE_FLOAT(_name, _default) \ + static const \ + __attribute__((used, section("__param"))) \ + struct param_info_s __param__##_name = { \ + #_name, \ + PARAM_TYPE_FLOAT, \ + .val.f = _default \ + } + +/** define a parameter that points to a structure */ +#define PARAM_DEFINE_STRUCT(_name, _default) \ + static const \ + __attribute__((used, section("__param"))) \ + struct param_info_s __param__##_name = { \ + #_name, \ + PARAM_TYPE_STRUCT + sizeof(_default), \ + .val.p = &_default; \ + } + +/** + * Parameter value union. + */ +union param_value_u { + void *p; + int32_t i; + float f; +}; + +/** + * Static parameter definition structure. + * + * This is normally not used by user code; see the PARAM_DEFINE macros + * instead. + */ +struct param_info_s { + const char *name; + param_type_t type; + union param_value_u val; +}; + +__END_DECLS + +#endif diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c new file mode 100644 index 000000000..879f4715a --- /dev/null +++ b/src/modules/systemlib/perf_counter.c @@ -0,0 +1,317 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file perf_counter.c + * + * @brief Performance measuring tools. + */ + +#include <stdlib.h> +#include <stdio.h> +#include <sys/queue.h> +#include <drivers/drv_hrt.h> + +#include "perf_counter.h" + +/** + * Header common to all counters. + */ +struct perf_ctr_header { + sq_entry_t link; /**< list linkage */ + enum perf_counter_type type; /**< counter type */ + const char *name; /**< counter name */ +}; + +/** + * PC_EVENT counter. + */ +struct perf_ctr_count { + struct perf_ctr_header hdr; + uint64_t event_count; +}; + +/** + * PC_ELAPSED counter. + */ +struct perf_ctr_elapsed { + struct perf_ctr_header hdr; + uint64_t event_count; + uint64_t time_start; + uint64_t time_total; + uint64_t time_least; + uint64_t time_most; +}; + +/** + * PC_INTERVAL counter. + */ +struct perf_ctr_interval { + struct perf_ctr_header hdr; + uint64_t event_count; + uint64_t time_event; + uint64_t time_first; + uint64_t time_last; + uint64_t time_least; + uint64_t time_most; + +}; + +/** + * List of all known counters. + */ +static sq_queue_t perf_counters; + + +perf_counter_t +perf_alloc(enum perf_counter_type type, const char *name) +{ + perf_counter_t ctr = NULL; + + switch (type) { + case PC_COUNT: + ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_count), 1); + break; + + case PC_ELAPSED: + ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_elapsed), 1); + break; + + case PC_INTERVAL: + ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_interval), 1); + break; + + default: + break; + } + + if (ctr != NULL) { + ctr->type = type; + ctr->name = name; + sq_addfirst(&ctr->link, &perf_counters); + } + + return ctr; +} + +void +perf_free(perf_counter_t handle) +{ + if (handle == NULL) + return; + + sq_rem(&handle->link, &perf_counters); + free(handle); +} + +void +perf_count(perf_counter_t handle) +{ + if (handle == NULL) + return; + + switch (handle->type) { + case PC_COUNT: + ((struct perf_ctr_count *)handle)->event_count++; + break; + + case PC_INTERVAL: { + struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; + hrt_abstime now = hrt_absolute_time(); + + switch (pci->event_count) { + case 0: + pci->time_first = now; + break; + case 1: + pci->time_least = now - pci->time_last; + pci->time_most = now - pci->time_last; + break; + default: { + hrt_abstime interval = now - pci->time_last; + if (interval < pci->time_least) + pci->time_least = interval; + if (interval > pci->time_most) + pci->time_most = interval; + break; + } + } + pci->time_last = now; + pci->event_count++; + break; + } + + default: + break; + } +} + +void +perf_begin(perf_counter_t handle) +{ + if (handle == NULL) + return; + + switch (handle->type) { + case PC_ELAPSED: + ((struct perf_ctr_elapsed *)handle)->time_start = hrt_absolute_time(); + break; + + default: + break; + } +} + +void +perf_end(perf_counter_t handle) +{ + if (handle == NULL) + return; + + switch (handle->type) { + case PC_ELAPSED: { + struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; + hrt_abstime elapsed = hrt_absolute_time() - pce->time_start; + + pce->event_count++; + pce->time_total += elapsed; + + if ((pce->time_least > elapsed) || (pce->time_least == 0)) + pce->time_least = elapsed; + + if (pce->time_most < elapsed) + pce->time_most = elapsed; + } + + default: + break; + } +} + +void +perf_reset(perf_counter_t handle) +{ + if (handle == NULL) + return; + + switch (handle->type) { + case PC_COUNT: + ((struct perf_ctr_count *)handle)->event_count = 0; + break; + + case PC_ELAPSED: { + struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; + pce->event_count = 0; + pce->time_start = 0; + pce->time_total = 0; + pce->time_least = 0; + pce->time_most = 0; + break; + } + + case PC_INTERVAL: { + struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; + pci->event_count = 0; + pci->time_event = 0; + pci->time_first = 0; + pci->time_last = 0; + pci->time_least = 0; + pci->time_most = 0; + break; + } + } +} + +void +perf_print_counter(perf_counter_t handle) +{ + if (handle == NULL) + return; + + switch (handle->type) { + case PC_COUNT: + printf("%s: %llu events\n", + handle->name, + ((struct perf_ctr_count *)handle)->event_count); + break; + + case PC_ELAPSED: { + struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; + + printf("%s: %llu events, %lluus elapsed, min %lluus max %lluus\n", + handle->name, + pce->event_count, + pce->time_total, + pce->time_least, + pce->time_most); + break; + } + + case PC_INTERVAL: { + struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; + + printf("%s: %llu events, %llu avg, min %lluus max %lluus\n", + handle->name, + pci->event_count, + (pci->time_last - pci->time_first) / pci->event_count, + pci->time_least, + pci->time_most); + break; + } + + default: + break; + } +} + +void +perf_print_all(void) +{ + perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters); + + while (handle != NULL) { + perf_print_counter(handle); + handle = (perf_counter_t)sq_next(&handle->link); + } +} + +void +perf_reset_all(void) +{ + perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters); + + while (handle != NULL) { + perf_reset(handle); + handle = (perf_counter_t)sq_next(&handle->link); + } +} diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h new file mode 100644 index 000000000..5c2cb15b2 --- /dev/null +++ b/src/modules/systemlib/perf_counter.h @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file perf_counter.h + * Performance measuring tools. + */ + +#ifndef _SYSTEMLIB_PERF_COUNTER_H +#define _SYSTEMLIB_PERF_COUNTER_H value + +/** + * Counter types. + */ +enum perf_counter_type { + PC_COUNT, /**< count the number of times an event occurs */ + PC_ELAPSED, /**< measure the time elapsed performing an event */ + PC_INTERVAL /**< measure the interval between instances of an event */ +}; + +struct perf_ctr_header; +typedef struct perf_ctr_header *perf_counter_t; + +__BEGIN_DECLS + +/** + * Create a new counter. + * + * @param type The type of the new counter. + * @param name The counter name. + * @return Handle for the new counter, or NULL if a counter + * could not be allocated. + */ +__EXPORT extern perf_counter_t perf_alloc(enum perf_counter_type type, const char *name); + +/** + * Free a counter. + * + * @param handle The performance counter's handle. + */ +__EXPORT extern void perf_free(perf_counter_t handle); + +/** + * Count a performance event. + * + * This call only affects counters that take single events; PC_COUNT etc. + * + * @param handle The handle returned from perf_alloc. + */ +__EXPORT extern void perf_count(perf_counter_t handle); + +/** + * Begin a performance event. + * + * This call applies to counters that operate over ranges of time; PC_ELAPSED etc. + * + * @param handle The handle returned from perf_alloc. + */ +__EXPORT extern void perf_begin(perf_counter_t handle); + +/** + * End a performance event. + * + * This call applies to counters that operate over ranges of time; PC_ELAPSED etc. + * + * @param handle The handle returned from perf_alloc. + */ +__EXPORT extern void perf_end(perf_counter_t handle); + +/** + * Reset a performance event. + * + * This call resets performance counter to initial state + * + * @param handle The handle returned from perf_alloc. + */ +__EXPORT extern void perf_reset(perf_counter_t handle); + +/** + * Print one performance counter. + * + * @param handle The counter to print. + */ +__EXPORT extern void perf_print_counter(perf_counter_t handle); + +/** + * Print all of the performance counters. + */ +__EXPORT extern void perf_print_all(void); + +/** + * Reset all of the performance counters. + */ +__EXPORT extern void perf_reset_all(void); + +__END_DECLS + +#endif diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c new file mode 100644 index 000000000..4996a8f66 --- /dev/null +++ b/src/modules/systemlib/pid/pid.c @@ -0,0 +1,206 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Laurens Mackay <mackayl@student.ethz.ch> + * Tobias Naegeli <naegelit@student.ethz.ch> + * Martin Rutschmann <rutmarti@student.ethz.ch> + * Anton Babushkin <anton.babushkin@me.com> + * 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 + * 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 pid.c + * + * Implementation of generic PID control interface. + * + * @author Laurens Mackay <mackayl@student.ethz.ch> + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Martin Rutschmann <rutmarti@student.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + * @author Julian Oes <joes@student.ethz.ch> + */ + +#include "pid.h" +#include <math.h> + +__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, + float limit, uint8_t mode, float dt_min) +{ + pid->kp = kp; + pid->ki = ki; + pid->kd = kd; + pid->intmax = intmax; + pid->limit = limit; + pid->mode = mode; + pid->dt_min = dt_min; + pid->count = 0.0f; + pid->saturated = 0.0f; + pid->last_output = 0.0f; + pid->sp = 0.0f; + pid->error_previous = 0.0f; + pid->integral = 0.0f; +} +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit) +{ + int ret = 0; + + if (isfinite(kp)) { + pid->kp = kp; + + } else { + ret = 1; + } + + if (isfinite(ki)) { + pid->ki = ki; + + } else { + ret = 1; + } + + if (isfinite(kd)) { + pid->kd = kd; + + } else { + ret = 1; + } + + if (isfinite(intmax)) { + pid->intmax = intmax; + + } else { + ret = 1; + } + + if (isfinite(limit)) { + pid->limit = limit; + + } else { + ret = 1; + } + + return ret; +} + +//void pid_set(PID_t *pid, float sp) +//{ +// pid->sp = sp; +// pid->error_previous = 0; +// pid->integral = 0; +//} + +/** + * + * @param pid + * @param val + * @param dt + * @return + */ +__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt) +{ + /* error = setpoint - actual_position + integral = integral + (error*dt) + derivative = (error - previous_error)/dt + output = (Kp*error) + (Ki*integral) + (Kd*derivative) + previous_error = error + wait(dt) + goto start + */ + + if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt)) { + return pid->last_output; + } + + float i, d; + pid->sp = sp; + + // Calculated current error value + float error = pid->sp - val; + + // Calculate or measured current error derivative + if (pid->mode == PID_MODE_DERIVATIV_CALC) { + d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min); + pid->error_previous = error; + + } else if (pid->mode == PID_MODE_DERIVATIV_CALC_NO_SP) { + d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min); + pid->error_previous = -val; + + } else if (pid->mode == PID_MODE_DERIVATIV_SET) { + d = -val_dot; + + } else { + d = 0.0f; + } + + if (!isfinite(d)) { + d = 0.0f; + } + + // Calculate the error integral and check for saturation + i = pid->integral + (error * dt); + + if (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit || + fabsf(i) > pid->intmax) { + i = pid->integral; // If saturated then do not update integral value + pid->saturated = 1; + + } else { + if (!isfinite(i)) { + i = 0.0f; + } + + pid->integral = i; + pid->saturated = 0; + } + + // Calculate the output. Limit output magnitude to pid->limit + float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd); + + if (isfinite(output)) { + if (output > pid->limit) { + output = pid->limit; + + } else if (output < -pid->limit) { + output = -pid->limit; + } + + pid->last_output = output; + } + + return pid->last_output; +} + + +__EXPORT void pid_reset_integral(PID_t *pid) +{ + pid->integral = 0; +} diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h new file mode 100644 index 000000000..eca228464 --- /dev/null +++ b/src/modules/systemlib/pid/pid.h @@ -0,0 +1,94 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Laurens Mackay <mackayl@student.ethz.ch> + * Tobias Naegeli <naegelit@student.ethz.ch> + * Martin Rutschmann <rutmarti@student.ethz.ch> + * Anton Babushkin <anton.babushkin@me.com> + * 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 + * 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 pid.h + * + * Definition of generic PID control interface. + * + * @author Laurens Mackay <mackayl@student.ethz.ch> + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Martin Rutschmann <rutmarti@student.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + * @author Julian Oes <joes@student.ethz.ch> + */ + +#ifndef PID_H_ +#define PID_H_ + +#include <stdint.h> + +__BEGIN_DECLS + +/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error + * val_dot in pid_calculate() will be ignored */ +#define PID_MODE_DERIVATIV_CALC 0 +/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored + * val_dot in pid_calculate() will be ignored */ +#define PID_MODE_DERIVATIV_CALC_NO_SP 1 +/* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */ +#define PID_MODE_DERIVATIV_SET 2 +// Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID) +#define PID_MODE_DERIVATIV_NONE 9 + +typedef struct { + float kp; + float ki; + float kd; + float intmax; + float sp; + float integral; + float error_previous; + float last_output; + float limit; + float dt_min; + uint8_t mode; + uint8_t count; + uint8_t saturated; +} PID_t; + +__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode, float dt_min); +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit); +//void pid_set(PID_t *pid, float sp); +__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); + +__EXPORT void pid_reset_integral(PID_t *pid); + +__END_DECLS + +#endif /* PID_H_ */ diff --git a/src/modules/systemlib/ppm_decode.c b/src/modules/systemlib/ppm_decode.c new file mode 100644 index 000000000..a5d2f738d --- /dev/null +++ b/src/modules/systemlib/ppm_decode.c @@ -0,0 +1,248 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ppm_decode.c + * + * PPM input decoder. + */ + +#include <nuttx/config.h> + +#include <stdint.h> +#include <string.h> + +#include <drivers/drv_hrt.h> + +#include "ppm_decode.h" + +/* + * PPM decoder tuning parameters. + * + * The PPM decoder works as follows. + * + * Initially, the decoder waits in the UNSYNCH state for two edges + * separated by PPM_MIN_START. Once the second edge is detected, + * the decoder moves to the ARM state. + * + * The ARM state expects an edge within PPM_MAX_PULSE_WIDTH, being the + * timing mark for the first channel. If this is detected, it moves to + * the INACTIVE state. + * + * The INACTIVE phase waits for and discards the next edge, as it is not + * significant. Once the edge is detected, it moves to the ACTIVE stae. + * + * The ACTIVE state expects an edge within PPM_MAX_PULSE_WIDTH, and when + * received calculates the time from the previous mark and records + * this time as the value for the next channel. + * + * If at any time waiting for an edge, the delay from the previous edge + * exceeds PPM_MIN_START the frame is deemed to have ended and the recorded + * values are advertised to clients. + */ +#define PPM_MAX_PULSE_WIDTH 500 /* maximum width of a pulse */ +#define PPM_MIN_CHANNEL_VALUE 800 /* shortest valid channel signal */ +#define PPM_MAX_CHANNEL_VALUE 2200 /* longest valid channel signal */ +#define PPM_MIN_START 2500 /* shortest valid start gap */ + +/* Input timeout - after this interval we assume signal is lost */ +#define PPM_INPUT_TIMEOUT 100 * 1000 /* 100ms */ + +/* Number of same-sized frames required to 'lock' */ +#define PPM_CHANNEL_LOCK 3 /* should be less than the input timeout */ + +/* decoded PPM buffer */ +#define PPM_MIN_CHANNELS 4 +#define PPM_MAX_CHANNELS 12 + +/* + * Public decoder state + */ +uint16_t ppm_buffer[PPM_MAX_CHANNELS]; +unsigned ppm_decoded_channels; +hrt_abstime ppm_last_valid_decode; + +static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS]; + +/* PPM decoder state machine */ +static struct { + uint16_t last_edge; /* last capture time */ + uint16_t last_mark; /* last significant edge */ + unsigned next_channel; + unsigned count_max; + enum { + UNSYNCH = 0, + ARM, + ACTIVE, + INACTIVE + } phase; +} ppm; + + +void +ppm_input_init(unsigned count_max) +{ + ppm_decoded_channels = 0; + ppm_last_valid_decode = 0; + + memset(&ppm, 0, sizeof(ppm)); + ppm.count_max = count_max; +} + +void +ppm_input_decode(bool reset, unsigned count) +{ + uint16_t width; + uint16_t interval; + unsigned i; + + /* if we missed an edge, we have to give up */ + if (reset) + goto error; + + /* how long since the last edge? */ + width = count - ppm.last_edge; + + if (count < ppm.last_edge) + width += ppm.count_max; /* handle wrapped count */ + + ppm.last_edge = count; + + /* + * If this looks like a start pulse, then push the last set of values + * and reset the state machine. + * + * Note that this is not a "high performance" design; it implies a whole + * frame of latency between the pulses being received and their being + * considered valid. + */ + if (width >= PPM_MIN_START) { + + /* + * If the number of channels changes unexpectedly, we don't want + * to just immediately jump on the new count as it may be a result + * of noise or dropped edges. Instead, take a few frames to settle. + */ + if (ppm.next_channel != ppm_decoded_channels) { + static unsigned new_channel_count; + static unsigned new_channel_holdoff; + + if (new_channel_count != ppm.next_channel) { + /* start the lock counter for the new channel count */ + new_channel_count = ppm.next_channel; + new_channel_holdoff = PPM_CHANNEL_LOCK; + + } else if (new_channel_holdoff > 0) { + /* this frame matched the last one, decrement the lock counter */ + new_channel_holdoff--; + + } else { + /* we have seen PPM_CHANNEL_LOCK frames with the new count, accept it */ + ppm_decoded_channels = new_channel_count; + new_channel_count = 0; + } + + } else { + /* frame channel count matches expected, let's use it */ + if (ppm.next_channel > PPM_MIN_CHANNELS) { + for (i = 0; i < ppm.next_channel; i++) + ppm_buffer[i] = ppm_temp_buffer[i]; + + ppm_last_valid_decode = hrt_absolute_time(); + } + } + + /* reset for the next frame */ + ppm.next_channel = 0; + + /* next edge is the reference for the first channel */ + ppm.phase = ARM; + + return; + } + + switch (ppm.phase) { + case UNSYNCH: + /* we are waiting for a start pulse - nothing useful to do here */ + return; + + case ARM: + + /* we expect a pulse giving us the first mark */ + if (width > PPM_MAX_PULSE_WIDTH) + goto error; /* pulse was too long */ + + /* record the mark timing, expect an inactive edge */ + ppm.last_mark = count; + ppm.phase = INACTIVE; + return; + + case INACTIVE: + /* this edge is not interesting, but now we are ready for the next mark */ + ppm.phase = ACTIVE; + + /* note that we don't bother looking at the timing of this edge */ + + return; + + case ACTIVE: + + /* we expect a well-formed pulse */ + if (width > PPM_MAX_PULSE_WIDTH) + goto error; /* pulse was too long */ + + /* determine the interval from the last mark */ + interval = count - ppm.last_mark; + ppm.last_mark = count; + + /* if the mark-mark timing is out of bounds, abandon the frame */ + if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE)) + goto error; + + /* if we have room to store the value, do so */ + if (ppm.next_channel < PPM_MAX_CHANNELS) + ppm_temp_buffer[ppm.next_channel++] = interval; + + ppm.phase = INACTIVE; + return; + + } + + /* the state machine is corrupted; reset it */ + +error: + /* we don't like the state of the decoder, reset it and try again */ + ppm.phase = UNSYNCH; + ppm_decoded_channels = 0; +} + diff --git a/src/modules/systemlib/ppm_decode.h b/src/modules/systemlib/ppm_decode.h new file mode 100644 index 000000000..6c5e15345 --- /dev/null +++ b/src/modules/systemlib/ppm_decode.h @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ppm_decode.h + * + * PPM input decoder. + */ + +#pragma once + +#include <drivers/drv_hrt.h> + +/** + * Maximum number of channels that we will decode. + */ +#define PPM_MAX_CHANNELS 12 + +/* PPM input nominal min/max values */ +#define PPM_MIN 1000 +#define PPM_MAX 2000 +#define PPM_MID ((PPM_MIN + PPM_MAX) / 2) + +__BEGIN_DECLS + +/* + * PPM decoder state + */ +__EXPORT extern uint16_t ppm_buffer[PPM_MAX_CHANNELS]; /**< decoded PPM channel values */ +__EXPORT extern unsigned ppm_decoded_channels; /**< count of decoded channels */ +__EXPORT extern hrt_abstime ppm_last_valid_decode; /**< timestamp of the last valid decode */ + +/** + * Initialise the PPM input decoder. + * + * @param count_max The maximum value of the counter passed to + * ppm_input_decode, used to determine how to + * handle counter wrap. + */ +__EXPORT void ppm_input_init(unsigned count_max); + +/** + * Inform the decoder of an edge on the PPM input. + * + * This function can be registered with the HRT as the PPM edge handler. + * + * @param reset If set, the edge detector has missed one or + * more edges and the decoder needs to be reset. + * @param count A microsecond timestamp corresponding to the + * edge, in the range 0-count_max. This value + * is expected to wrap. + */ +__EXPORT void ppm_input_decode(bool reset, unsigned count); + +__END_DECLS
\ No newline at end of file diff --git a/src/modules/systemlib/scheduling_priorities.h b/src/modules/systemlib/scheduling_priorities.h new file mode 100644 index 000000000..be1dbfcd8 --- /dev/null +++ b/src/modules/systemlib/scheduling_priorities.h @@ -0,0 +1,48 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include <nuttx/sched.h> + +/* SCHED_PRIORITY_MAX */ +#define SCHED_PRIORITY_FAST_DRIVER SCHED_PRIORITY_MAX +#define SCHED_PRIORITY_WATCHDOG (SCHED_PRIORITY_MAX - 5) +#define SCHED_PRIORITY_ACTUATOR_OUTPUTS (SCHED_PRIORITY_MAX - 15) +#define SCHED_PRIORITY_ATTITUDE_CONTROL (SCHED_PRIORITY_MAX - 25) +#define SCHED_PRIORITY_SLOW_DRIVER (SCHED_PRIORITY_MAX - 35) +#define SCHED_PRIORITY_POSITION_CONTROL (SCHED_PRIORITY_MAX - 40) +/* SCHED_PRIORITY_DEFAULT */ +#define SCHED_PRIORITY_LOGGING (SCHED_PRIORITY_DEFAULT - 10) +#define SCHED_PRIORITY_PARAMS (SCHED_PRIORITY_DEFAULT - 15) +/* SCHED_PRIORITY_IDLE */ diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c new file mode 100644 index 000000000..3283aad8a --- /dev/null +++ b/src/modules/systemlib/systemlib.c @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * 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 + * 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 systemlib.c + * Implementation of commonly used low-level system-call like functions. + */ + +#include <nuttx/config.h> +#include <unistd.h> +#include <stdio.h> +#include <fcntl.h> +#include <sched.h> +#include <signal.h> +#include <sys/stat.h> +#include <unistd.h> +#include <float.h> +#include <string.h> + +#include "systemlib.h" + +static void kill_task(FAR struct tcb_s *tcb, FAR void *arg); + +void killall() +{ +// printf("Sending SIGUSR1 to all processes now\n"); + + /* iterate through all tasks and send kill signal */ + sched_foreach(kill_task, NULL); +} + +static void kill_task(FAR struct tcb_s *tcb, FAR void *arg) +{ + kill(tcb->pid, SIGUSR1); +} + +int task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, const char *argv[]) +{ + int pid; + + sched_lock(); + + /* create the task */ + pid = task_create(name, priority, stack_size, entry, argv); + + if (pid > 0) { + + /* configure the scheduler */ + struct sched_param param; + + param.sched_priority = priority; + sched_setscheduler(pid, scheduler, ¶m); + + /* XXX do any other private task accounting here before the task starts */ + } + + sched_unlock(); + + return pid; +} diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h new file mode 100644 index 000000000..0194b5e52 --- /dev/null +++ b/src/modules/systemlib/systemlib.h @@ -0,0 +1,122 @@ +/**************************************************************************** + * + * 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 systemlib.h + * Definition of commonly used low-level system-call like functions. + */ + +#ifndef SYSTEMLIB_H_ +#define SYSTEMLIB_H_ +#include <float.h> +#include <stdint.h> + +/** Reboots the board */ +extern void up_systemreset(void) noreturn_function; + +__BEGIN_DECLS + +/** Sends SIGUSR1 to all processes */ +__EXPORT void killall(void); + +/** Default scheduler type */ +#if CONFIG_RR_INTERVAL > 0 +# define SCHED_DEFAULT SCHED_RR +#else +# define SCHED_DEFAULT SCHED_FIFO +#endif + +/** Starts a task and performs any specific accounting, scheduler setup, etc. */ +__EXPORT int task_spawn_cmd(const char *name, + int priority, + int scheduler, + int stack_size, + main_t entry, + const char *argv[]); + +enum MULT_PORTS { + MULT_0_US2_RXTX = 0, + MULT_1_US2_FLOW, + MULT_2_GPIO_12, + MULT_COUNT +}; + +/* Check max multi port count */ +#if (MULT_COUNT > 33) +#error "MULT_COUNT HAS TO BE LESS THAN OR EQUAL 33" +#endif + +/* FMU board info, to be stored in the first 64 bytes of the FMU EEPROM */ +#pragma pack(push,1) +struct fmu_board_info_s { + char header[3]; /**< {'P', 'X', '4'} */ + char board_name[20]; /**< Human readable board name, \0 terminated */ + uint8_t board_id; /**< board ID, constantly increasing number per board */ + uint8_t board_version; /**< board version, major * 10 + minor: v1.7 = 17 */ + uint8_t multi_port_config[MULT_COUNT]; /**< Configuration of multi ports 1-3 */ + uint8_t reserved[33 - MULT_COUNT]; /**< Reserved space for more multi ports */ + uint16_t production_year; + uint8_t production_month; + uint8_t production_day; + uint8_t production_fab; + uint8_t production_tester; +}; /**< stores autopilot board information meta data from EEPROM */ +#pragma pack(pop) + +/* Carrier board info, to be stored in the 128 byte board info EEPROM */ +#pragma pack(push,1) +struct carrier_board_info_s { + char header[3]; /**< {'P', 'X', '4'} */ + char board_name[20]; /**< Human readable board name, \0 terminated */ + uint8_t board_id; /**< board ID, constantly increasing number per board */ + uint8_t board_version; /**< board version, major * 10 + minor: v1.7 = 17 */ + uint8_t multi_port_config[MULT_COUNT]; /**< Configuration of multi ports 1-3 */ + uint8_t reserved[33 - MULT_COUNT]; /**< Reserved space for more multi ports */ + uint16_t production_year; + uint8_t production_month; + uint8_t production_day; + uint8_t production_fab; + uint8_t production_tester; + char board_custom_data[64]; +}; /**< stores carrier board information meta data from EEPROM */ +#pragma pack(pop) + +struct __multiport_info { + const char *port_names[MULT_COUNT]; +}; +__EXPORT extern const struct __multiport_info multiport_info; + +__END_DECLS + +#endif /* SYSTEMLIB_H_ */ diff --git a/src/modules/systemlib/up_cxxinitialize.c b/src/modules/systemlib/up_cxxinitialize.c new file mode 100644 index 000000000..c78f29570 --- /dev/null +++ b/src/modules/systemlib/up_cxxinitialize.c @@ -0,0 +1,150 @@ +/************************************************************************************ + * configs/stm32f4discovery/src/up_cxxinitialize.c + * arch/arm/src/board/up_cxxinitialize.c + * + * Copyright (C) 2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ************************************************************************************/ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include <nuttx/config.h> + +#include <debug.h> + +#include <nuttx/arch.h> + +//#include <arch/stm32/chip.h> +//#include "chip.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ +/* Debug ****************************************************************************/ +/* Non-standard debug that may be enabled just for testing the static constructors */ + +#ifndef CONFIG_DEBUG +# undef CONFIG_DEBUG_CXX +#endif + +#ifdef CONFIG_DEBUG_CXX +# define cxxdbg dbg +# define cxxlldbg lldbg +# ifdef CONFIG_DEBUG_VERBOSE +# define cxxvdbg vdbg +# define cxxllvdbg llvdbg +# else +# define cxxvdbg(x...) +# define cxxllvdbg(x...) +# endif +#else +# define cxxdbg(x...) +# define cxxlldbg(x...) +# define cxxvdbg(x...) +# define cxxllvdbg(x...) +#endif + +/************************************************************************************ + * Private Types + ************************************************************************************/ +/* This type defines one entry in initialization array */ + +typedef void (*initializer_t)(void); + +/************************************************************************************ + * External references + ************************************************************************************/ +/* _sinit and _einit are symbols exported by the linker script that mark the + * beginning and the end of the C++ initialization section. + */ + +extern initializer_t _sinit; +extern initializer_t _einit; + +/* _stext and _etext are symbols exported by the linker script that mark the + * beginning and the end of text. + */ + +extern uint32_t _stext; +extern uint32_t _etext; + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/**************************************************************************** + * Name: up_cxxinitialize + * + * Description: + * If C++ and C++ static constructors are supported, then this function + * must be provided by board-specific logic in order to perform + * initialization of the static C++ class instances. + * + * This function should then be called in the application-specific + * user_start logic in order to perform the C++ initialization. NOTE + * that no component of the core NuttX RTOS logic is involved; This + * function defintion only provides the 'contract' between application + * specific C++ code and platform-specific toolchain support + * + ***************************************************************************/ + +__EXPORT void up_cxxinitialize(void) +{ + initializer_t *initp; + + cxxdbg("_sinit: %p _einit: %p _stext: %p _etext: %p\n", + &_sinit, &_einit, &_stext, &_etext); + + /* Visit each entry in the initialzation table */ + + for (initp = &_sinit; initp != &_einit; initp++) + { + initializer_t initializer = *initp; + cxxdbg("initp: %p initializer: %p\n", initp, initializer); + + /* Make sure that the address is non-NULL and lies in the text region + * defined by the linker script. Some toolchains may put NULL values + * or counts in the initialization table + */ + + if ((void*)initializer > (void*)&_stext && (void*)initializer < (void*)&_etext) + { + cxxdbg("Calling %p\n", initializer); + initializer(); + } + } +} diff --git a/src/modules/systemlib/uthash/doc/userguide.txt b/src/modules/systemlib/uthash/doc/userguide.txt new file mode 100644 index 000000000..3e65a52fc --- /dev/null +++ b/src/modules/systemlib/uthash/doc/userguide.txt @@ -0,0 +1,1682 @@ +uthash User Guide +================= +Troy D. Hanson <thanson@users.sourceforge.net> +v1.9.6, April 2012 + +include::sflogo.txt[] +include::topnav.txt[] + +A hash in C +----------- +include::toc.txt[] + +This document is written for C programmers. Since you're reading this, chances +are that you know a hash is used for looking up items using a key. In scripting +languages like Perl, hashes are used all the time. In C, hashes don't exist in +the language itself. This software provides a hash table for C structures. + +What can it do? +~~~~~~~~~~~~~~~~~ +This software supports these operations on items in a hash table: + +1. add +2. find +3. delete +4. count +5. iterate +6. sort +7. select + +Is it fast? +~~~~~~~~~~~ +Add, find and delete are normally constant-time operations. This is influenced +by your key domain and the hash function. + +This hash aims to be minimalistic and efficient. It's around 900 lines of C. +It inlines automatically because it's implemented as macros. It's fast as long +as the hash function is suited to your keys. You can use the default hash +function, or easily compare performance and choose from among several other +<<hash_functions,built-in hash functions>>. + +Is it a library? +~~~~~~~~~~~~~~~~ +No, it's just a single header file: `uthash.h`. All you need to do is copy +the header file into your project, and: + + #include "uthash.h" + +Since uthash is a header file only, there is no library code to link against. + +C/C++ and platforms +~~~~~~~~~~~~~~~~~~~ +This software can be used in C and C++ programs. It has been tested on: + + * Linux + * Mac OS X + * Windows using Visual Studio 2008 and 2010 + * Solaris + * OpenBSD + * FreeBSD + +Test suite +^^^^^^^^^^ +To run the test suite, enter the `tests` directory. Then, + + * on Unix platforms, run `make` + * on Windows, run the "do_tests_win32.cmd" batch file. (You may edit the + batch file if your Visual Studio is installed in a non-standard location). + +BSD licensed +~~~~~~~~~~~~ +This software is made available under the +link:license.html[revised BSD license]. +It is free and open source. + +Obtaining uthash +~~~~~~~~~~~~~~~~ +Please follow the link to download on the +http://uthash.sourceforge.net[uthash website] at http://uthash.sourceforge.net. + +A number of platforms include uthash in their package repositories. For example, +Debian/Ubuntu users may simply `aptitude install uthash-dev`. + +Getting help +~~~~~~~~~~~~ +Feel free to mailto:tdh@tkhanson.net[email the author] at +tdh@tkhanson.net. + +Resources +~~~~~~~~~ +Users of uthash may wish to follow the news feed for information about new +releases. Also, there are some extra bonus headers included with uthash. + +News:: + The author has a news feed for http://troydhanson.wordpress.com/[software updates] image:img/rss.png[(RSS)]. +Extras included with uthash:: + uthash ships with these "extras"-- independent headers similar to uthash. + First link:utlist.html[utlist.h] provides linked list macros for C structures. + Second, link:utarray.html[utarray.h] implements dynamic arrays using macros. + Third, link:utstring.html[utstring.h] implements a basic dynamic string. +Other software:: + Other open-source software by the author is listed at http://tkhanson.net. + +Who's using it? +~~~~~~~~~~~~~~~ +Since releasing uthash in 2006, it has been downloaded thousands of times, +incorporated into commercial software, academic research, and into other +open-source software. + +Your structure +-------------- + +In uthash, a hash table is comprised of structures. Each structure represents a +key-value association. One or more of the structure fields constitute the key. +The structure pointer itself is the value. + +.Defining a structure that can be hashed +---------------------------------------------------------------------- +#include "uthash.h" + +struct my_struct { + int id; /* key */ + char name[10]; + UT_hash_handle hh; /* makes this structure hashable */ +}; +---------------------------------------------------------------------- + +Note that, in uthash, your structure will never be moved or copied into another +location when you add it into a hash table. This means that you can keep other +data structures that safely point to your structure-- regardless of whether you +add or delete it from a hash table during your program's lifetime. + +The key +~~~~~~~ +There are no restrictions on the data type or name of the key field. The key +can also comprise multiple contiguous fields, having any names and data types. + +.Any data type... really? +***************************************************************************** +Yes, your key and structure can have any data type. Unlike function calls with +fixed prototypes, uthash consists of macros-- whose arguments are untyped-- and +thus able to work with any type of structure or key. +***************************************************************************** + +Unique keys +^^^^^^^^^^^ +As with any hash, every item must have a unique key. Your application must +enforce key uniqueness. Before you add an item to the hash table, you must +first know (if in doubt, check!) that the key is not already in use. You +can check whether a key already exists in the hash table using `HASH_FIND`. + +The hash handle +~~~~~~~~~~~~~~~ +The `UT_hash_handle` field must be present in your structure. It is used for +the internal bookkeeping that makes the hash work. It does not require +initialization. It can be named anything, but you can simplify matters by +naming it `hh`. This allows you to use the easier "convenience" macros to add, +find and delete items. + +A word about memory +~~~~~~~~~~~~~~~~~~~ +Some have asked how uthash cleans up its internal memory. The answer is simple: +'when you delete the final item' from a hash table, uthash releases all the +internal memory associated with that hash table, and sets its pointer to NULL. + +Hash operations +--------------- + +This section introduces the uthash macros by example. For a more succinct +listing, see <<Macro_reference,Macro Reference>>. + +.Convenience vs. general macros: +***************************************************************************** +The uthash macros fall into two categories. The 'convenience' macros can be used +with integer, pointer or string keys (and require that you chose the conventional +name `hh` for the `UT_hash_handle` field). The convenience macros take fewer +arguments than the general macros, making their usage a bit simpler for these +common types of keys. + +The 'general' macros can be used for any types of keys, or for multi-field keys, +or when the `UT_hash_handle` has been named something other than `hh`. These +macros take more arguments and offer greater flexibility in return. But if the +convenience macros suit your needs, use them-- your code will be more readable. +***************************************************************************** + +Declare the hash +~~~~~~~~~~~~~~~~ +Your hash must be declared as a `NULL`-initialized pointer to your structure. + + struct my_struct *users = NULL; /* important! initialize to NULL */ + +Add item +~~~~~~~~ +Allocate and initialize your structure as you see fit. The only aspect +of this that matters to uthash is that your key must be initialized to +a unique value. Then call `HASH_ADD`. (Here we use the convenience macro +`HASH_ADD_INT`, which offers simplified usage for keys of type `int`). + +.Add an item to a hash +---------------------------------------------------------------------- +void add_user(int user_id, char *name) { + struct my_struct *s; + + s = malloc(sizeof(struct my_struct)); + s->id = user_id; + strcpy(s->name, name); + HASH_ADD_INT( users, id, s ); /* id: name of key field */ +} +---------------------------------------------------------------------- + +The first parameter to `HASH_ADD_INT` is the hash table, and the +second parameter is the 'name' of the key field. Here, this is `id`. The +last parameter is a pointer to the structure being added. + +[[validc]] +.Wait.. the field name is a parameter? +******************************************************************************* +If you find it strange that `id`, which is the 'name of a field' in the +structure, can be passed as a parameter, welcome to the world of macros. Don't +worry- the C preprocessor expands this to valid C code. +******************************************************************************* + +Key must not be modified while in-use +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Once a structure has been added to the hash, do not change the value of its key. +Instead, delete the item from the hash, change the key, and then re-add it. + +Passing the hash pointer into functions +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +In the example above `users` is a global variable, but what if the caller wanted +to pass the hash pointer 'into' the `add_user` function? At first glance it would +appear that you could simply pass `users` as an argument, but that won't work +right. + + /* bad */ + void add_user(struct my_struct *users, int user_id, char *name) { + ... + HASH_ADD_INT( users, id, s ); + } + +You really need to pass 'a pointer' to the hash pointer: + + /* good */ + void add_user(struct my_struct **users, int user_id, char *name) { ... + ... + HASH_ADD_INT( *users, id, s ); + } + +Note that we dereferenced the pointer in the `HASH_ADD` also. + +The reason it's necessary to deal with a pointer to the hash pointer is simple: +the hash macros modify it (in other words, they modify the 'pointer itself' not +just what it points to). + +Find item +~~~~~~~~~ +To look up a structure in a hash, you need its key. Then call `HASH_FIND`. +(Here we use the convenience macro `HASH_FIND_INT` for keys of type `int`). + +.Find a structure using its key +---------------------------------------------------------------------- +struct my_struct *find_user(int user_id) { + struct my_struct *s; + + HASH_FIND_INT( users, &user_id, s ); /* s: output pointer */ + return s; +} +---------------------------------------------------------------------- + +Here, the hash table is `users`, and `&user_id` points to the key (an integer +in this case). Last, `s` is the 'output' variable of `HASH_FIND_INT`. The +final result is that `s` points to the structure with the given key, or +is `NULL` if the key wasn't found in the hash. + +[NOTE] +The middle argument is a 'pointer' to the key. You can't pass a literal key +value to `HASH_FIND`. Instead assign the literal value to a variable, and pass +a pointer to the variable. + + +Delete item +~~~~~~~~~~~ +To delete a structure from a hash, you must have a pointer to it. (If you only +have the key, first do a `HASH_FIND` to get the structure pointer). + +.Delete an item from a hash +---------------------------------------------------------------------- +void delete_user(struct my_struct *user) { + HASH_DEL( users, user); /* user: pointer to deletee */ + free(user); /* optional; it's up to you! */ +} +---------------------------------------------------------------------- + +Here again, `users` is the hash table, and `user` is a pointer to the +structure we want to remove from the hash. + +uthash never frees your structure +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Deleting a structure just removes it from the hash table-- it doesn't `free` +it. The choice of when to free your structure is entirely up to you; uthash +will never free your structure. + +Delete can change the pointer +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +The hash table pointer (which initially points to the first item added to the +hash) can change in response to `HASH_DEL` (i.e. if you delete the first item +in the hash table). + +Iterative deletion +^^^^^^^^^^^^^^^^^^ +The `HASH_ITER` macro is a deletion-safe iteration construct which expands +to a simple 'for' loop. + +.Delete all items from a hash +---------------------------------------------------------------------- +void delete_all() { + struct my_struct *current_user, *tmp; + + HASH_ITER(hh, users, current_user, tmp) { + HASH_DEL(users,current_user); /* delete; users advances to next */ + free(current_user); /* optional- if you want to free */ + } +} +---------------------------------------------------------------------- + +All-at-once deletion +^^^^^^^^^^^^^^^^^^^^ +If you only want to delete all the items, but not free them or do any +per-element clean up, you can do this more efficiently in a single operation: + + HASH_CLEAR(hh,users); + +Afterward, the list head (here, `users`) will be set to `NULL`. + +Count items +~~~~~~~~~~~ + +The number of items in the hash table can be obtained using `HASH_COUNT`: + +.Count of items in the hash table +---------------------------------------------------------------------- +unsigned int num_users; +num_users = HASH_COUNT(users); +printf("there are %u users\n", num_users); +---------------------------------------------------------------------- + +Incidentally, this works even the list (`users`, here) is `NULL`, in +which case the count is 0. + +Iterating and sorting +~~~~~~~~~~~~~~~~~~~~~ + +You can loop over the items in the hash by starting from the beginning and +following the `hh.next` pointer. + +.Iterating over all the items in a hash +---------------------------------------------------------------------- +void print_users() { + struct my_struct *s; + + for(s=users; s != NULL; s=s->hh.next) { + printf("user id %d: name %s\n", s->id, s->name); + } +} +---------------------------------------------------------------------- + +There is also an `hh.prev` pointer you could use to iterate backwards through +the hash, starting from any known item. + +[[deletesafe]] +Deletion-safe iteration +^^^^^^^^^^^^^^^^^^^^^^^ +In the example above, it would not be safe to delete and free `s` in the body +of the 'for' loop, (because `s` is derefenced each time the loop iterates). +This is easy to rewrite correctly (by copying the `s->hh.next` pointer to a +temporary variable 'before' freeing `s`), but it comes up often enough that a +deletion-safe iteration macro, `HASH_ITER`, is included. It expands to a +`for`-loop header. Here is how it could be used to rewrite the last example: + + struct my_struct *s, *tmp; + + HASH_ITER(hh, users, s, tmp) { + printf("user id %d: name %s\n", s->id, s->name); + /* ... it is safe to delete and free s here */ + } + +.A hash is also a doubly-linked list. +******************************************************************************* +Iterating backward and forward through the items in the hash is possible +because of the `hh.prev` and `hh.next` fields. All the items in the hash can +be reached by repeatedly following these pointers, thus the hash is also a +doubly-linked list. +******************************************************************************* + +If you're using uthash in a C++ program, you need an extra cast on the `for` +iterator, e.g., `s=(struct my_struct*)s->hh.next`. + +Sorted iteration +^^^^^^^^^^^^^^^^ +The items in the hash are, by default, traversed in the order they were added +("insertion order") when you follow the `hh.next` pointer. But you can sort +the items into a new order using `HASH_SORT`. E.g., + + HASH_SORT( users, name_sort ); + +The second argument is a pointer to a comparison function. It must accept two +arguments which are pointers to two items to compare. Its return value should +be less than zero, zero, or greater than zero, if the first item sorts before, +equal to, or after the second item, respectively. (Just like `strcmp`). + +.Sorting the items in the hash +---------------------------------------------------------------------- +int name_sort(struct my_struct *a, struct my_struct *b) { + return strcmp(a->name,b->name); +} + +int id_sort(struct my_struct *a, struct my_struct *b) { + return (a->id - b->id); +} + +void sort_by_name() { + HASH_SORT(users, name_sort); +} + +void sort_by_id() { + HASH_SORT(users, id_sort); +} +---------------------------------------------------------------------- + +When the items in the hash are sorted, the first item may change position. In +the example above, `users` may point to a different structure after calling +`HASH_SORT`. + +A complete example +~~~~~~~~~~~~~~~~~~ + +We'll repeat all the code and embellish it with a `main()` function to form a +working example. + +If this code was placed in a file called `example.c` in the same directory as +`uthash.h`, it could be compiled and run like this: + + cc -o example example.c + ./example + +Follow the prompts to try the program. + +.A complete program +---------------------------------------------------------------------- +#include <stdio.h> /* gets */ +#include <stdlib.h> /* atoi, malloc */ +#include <string.h> /* strcpy */ +#include "uthash.h" + +struct my_struct { + int id; /* key */ + char name[10]; + UT_hash_handle hh; /* makes this structure hashable */ +}; + +struct my_struct *users = NULL; + +void add_user(int user_id, char *name) { + struct my_struct *s; + + s = (struct my_struct*)malloc(sizeof(struct my_struct)); + s->id = user_id; + strcpy(s->name, name); + HASH_ADD_INT( users, id, s ); /* id: name of key field */ +} + +struct my_struct *find_user(int user_id) { + struct my_struct *s; + + HASH_FIND_INT( users, &user_id, s ); /* s: output pointer */ + return s; +} + +void delete_user(struct my_struct *user) { + HASH_DEL( users, user); /* user: pointer to deletee */ + free(user); +} + +void delete_all() { + struct my_struct *current_user, *tmp; + + HASH_ITER(hh, users, current_user, tmp) { + HASH_DEL(users,current_user); /* delete it (users advances to next) */ + free(current_user); /* free it */ + } +} + +void print_users() { + struct my_struct *s; + + for(s=users; s != NULL; s=(struct my_struct*)(s->hh.next)) { + printf("user id %d: name %s\n", s->id, s->name); + } +} + +int name_sort(struct my_struct *a, struct my_struct *b) { + return strcmp(a->name,b->name); +} + +int id_sort(struct my_struct *a, struct my_struct *b) { + return (a->id - b->id); +} + +void sort_by_name() { + HASH_SORT(users, name_sort); +} + +void sort_by_id() { + HASH_SORT(users, id_sort); +} + +int main(int argc, char *argv[]) { + char in[10]; + int id=1, running=1; + struct my_struct *s; + unsigned num_users; + + while (running) { + printf("1. add user\n"); + printf("2. find user\n"); + printf("3. delete user\n"); + printf("4. delete all users\n"); + printf("5. sort items by name\n"); + printf("6. sort items by id\n"); + printf("7. print users\n"); + printf("8. count users\n"); + printf("9. quit\n"); + gets(in); + switch(atoi(in)) { + case 1: + printf("name?\n"); + add_user(id++, gets(in)); + break; + case 2: + printf("id?\n"); + s = find_user(atoi(gets(in))); + printf("user: %s\n", s ? s->name : "unknown"); + break; + case 3: + printf("id?\n"); + s = find_user(atoi(gets(in))); + if (s) delete_user(s); + else printf("id unknown\n"); + break; + case 4: + delete_all(); + break; + case 5: + sort_by_name(); + break; + case 6: + sort_by_id(); + break; + case 7: + print_users(); + break; + case 8: + num_users=HASH_COUNT(users); + printf("there are %u users\n", num_users); + break; + case 9: + running=0; + break; + } + } + + delete_all(); /* free any structures */ + return 0; +} +---------------------------------------------------------------------- + +This program is included in the distribution in `tests/example.c`. You can run +`make example` in that directory to compile it easily. + +Standard key types +------------------ +This section goes into specifics of how to work with different kinds of keys. +You can use nearly any type of key-- integers, strings, pointers, structures, etc. + +[NOTE] +.A note about float +================================================================================ +You can use floating point keys. This comes with the same caveats as with any +program that tests floating point equality. In other words, even the tiniest +difference in two floating point numbers makes them distinct keys. +================================================================================ + +Integer keys +~~~~~~~~~~~~ +The preceding examples demonstrated use of integer keys. To recap, use the +convenience macros `HASH_ADD_INT` and `HASH_FIND_INT` for structures with +integer keys. (The other operations such as `HASH_DELETE` and `HASH_SORT` are +the same for all types of keys). + +String keys +~~~~~~~~~~~ +If your structure has a string key, the operations to use depend on whether your +structure 'points to' the key (`char *`) or the string resides `within` the +structure (`char a[10]`). *This distinction is important*. As we'll see below, +you need to use `HASH_ADD_KEYPTR` when your structure 'points' to a key (that is, +the key itself is 'outside' of the structure); in contrast, use `HASH_ADD_STR` +for a string key that is contained *within* your structure. + +[NOTE] +.char[ ] vs. char* +================================================================================ +The string is 'within' the structure in the first example below-- `name` is a +`char[10]` field. In the second example, the key is 'outside' of the +structure-- `name` is a `char *`. So the first example uses `HASH_ADD_STR` but +the second example uses `HASH_ADD_KEYPTR`. For information on this macro, see +the <<Macro_reference,Macro reference>>. +================================================================================ + +String 'within' structure +^^^^^^^^^^^^^^^^^^^^^^^^^ + +.A string-keyed hash (string within structure) +---------------------------------------------------------------------- +#include <string.h> /* strcpy */ +#include <stdlib.h> /* malloc */ +#include <stdio.h> /* printf */ +#include "uthash.h" + +struct my_struct { + char name[10]; /* key (string is WITHIN the structure) */ + int id; + UT_hash_handle hh; /* makes this structure hashable */ +}; + + +int main(int argc, char *argv[]) { + const char **n, *names[] = { "joe", "bob", "betty", NULL }; + struct my_struct *s, *tmp, *users = NULL; + int i=0; + + for (n = names; *n != NULL; n++) { + s = (struct my_struct*)malloc(sizeof(struct my_struct)); + strncpy(s->name, *n,10); + s->id = i++; + HASH_ADD_STR( users, name, s ); + } + + HASH_FIND_STR( users, "betty", s); + if (s) printf("betty's id is %d\n", s->id); + + /* free the hash table contents */ + HASH_ITER(hh, users, s, tmp) { + HASH_DEL(users, s); + free(s); + } + return 0; +} +---------------------------------------------------------------------- + +This example is included in the distribution in `tests/test15.c`. It prints: + + betty's id is 2 + +String 'pointer' in structure +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Now, here is the same example but using a `char *` key instead of `char [ ]`: + +.A string-keyed hash (structure points to string) +---------------------------------------------------------------------- +#include <string.h> /* strcpy */ +#include <stdlib.h> /* malloc */ +#include <stdio.h> /* printf */ +#include "uthash.h" + +struct my_struct { + const char *name; /* key */ + int id; + UT_hash_handle hh; /* makes this structure hashable */ +}; + + +int main(int argc, char *argv[]) { + const char **n, *names[] = { "joe", "bob", "betty", NULL }; + struct my_struct *s, *tmp, *users = NULL; + int i=0; + + for (n = names; *n != NULL; n++) { + s = (struct my_struct*)malloc(sizeof(struct my_struct)); + s->name = *n; + s->id = i++; + HASH_ADD_KEYPTR( hh, users, s->name, strlen(s->name), s ); + } + + HASH_FIND_STR( users, "betty", s); + if (s) printf("betty's id is %d\n", s->id); + + /* free the hash table contents */ + HASH_ITER(hh, users, s, tmp) { + HASH_DEL(users, s); + free(s); + } + return 0; +} +---------------------------------------------------------------------- + +This example is included in `tests/test40.c`. + +Pointer keys +~~~~~~~~~~~~ +Your key can be a pointer. To be very clear, this means the 'pointer itself' +can be the key (in contrast, if the thing 'pointed to' is the key, this is a +different use case handled by `HASH_ADD_KEYPTR`). + +Here is a simple example where a structure has a pointer member, called `key`. + +.A pointer key +---------------------------------------------------------------------- +#include <stdio.h> +#include <stdlib.h> +#include "uthash.h" + +typedef struct { + void *key; + int i; + UT_hash_handle hh; +} el_t; + +el_t *hash = NULL; +char *someaddr = NULL; + +int main() { + el_t *d; + el_t *e = (el_t*)malloc(sizeof(el_t)); + if (!e) return -1; + e->key = (void*)someaddr; + e->i = 1; + HASH_ADD_PTR(hash,key,e); + HASH_FIND_PTR(hash, &someaddr, d); + if (d) printf("found\n"); + + /* release memory */ + HASH_DEL(hash,e); + free(e); + return 0; +} +---------------------------------------------------------------------- + +This example is included in `tests/test57.c`. Note that the end of the program +deletes the element out of the hash, (and since no more elements remain in the +hash), uthash releases its internal memory. + +Structure keys +~~~~~~~~~~~~~~ +Your key field can have any data type. To uthash, it is just a sequence of +bytes. Therefore, even a nested structure can be used as a key. We'll use the +general macros `HASH_ADD` and `HASH_FIND` to demonstrate. + +NOTE: Structures contain padding (wasted internal space used to fulfill +alignment requirements for the members of the structure). These padding bytes +'must be zeroed' before adding an item to the hash or looking up an item. +Therefore always zero the whole structure before setting the members of +interest. The example below does this-- see the two calls to `memset`. + +.A key which is a structure +---------------------------------------------------------------------- +#include <stdlib.h> +#include <stdio.h> +#include "uthash.h" + +typedef struct { + char a; + int b; +} record_key_t; + +typedef struct { + record_key_t key; + /* ... other data ... */ + UT_hash_handle hh; +} record_t; + +int main(int argc, char *argv[]) { + record_t l, *p, *r, *tmp, *records = NULL; + + r = (record_t*)malloc( sizeof(record_t) ); + memset(r, 0, sizeof(record_t)); + r->key.a = 'a'; + r->key.b = 1; + HASH_ADD(hh, records, key, sizeof(record_key_t), r); + + memset(&l, 0, sizeof(record_t)); + l.key.a = 'a'; + l.key.b = 1; + HASH_FIND(hh, records, &l.key, sizeof(record_key_t), p); + + if (p) printf("found %c %d\n", p->key.a, p->key.b); + + HASH_ITER(hh, records, p, tmp) { + HASH_DEL(records, p); + free(p); + } + return 0; +} + +---------------------------------------------------------------------- + +This usage is nearly the same as use of a compound key explained below. + +Note that the general macros require the name of the `UT_hash_handle` to be +passed as the first argument (here, this is `hh`). The general macros are +documented in <<Macro_reference,Macro Reference>>. + +Advanced Topics +--------------- + +Compound keys +~~~~~~~~~~~~~ +Your key can even comprise multiple contiguous fields. + +.A multi-field key +---------------------------------------------------------------------- +#include <stdlib.h> /* malloc */ +#include <stddef.h> /* offsetof */ +#include <stdio.h> /* printf */ +#include <string.h> /* memset */ +#include "uthash.h" + +#define UTF32 1 + +typedef struct { + UT_hash_handle hh; + int len; + char encoding; /* these two fields */ + int text[]; /* comprise the key */ +} msg_t; + +typedef struct { + char encoding; + int text[]; +} lookup_key_t; + +int main(int argc, char *argv[]) { + unsigned keylen; + msg_t *msg, *tmp, *msgs = NULL; + lookup_key_t *lookup_key; + + int beijing[] = {0x5317, 0x4eac}; /* UTF-32LE for 北京 */ + + /* allocate and initialize our structure */ + msg = (msg_t*)malloc( sizeof(msg_t) + sizeof(beijing) ); + memset(msg, 0, sizeof(msg_t)+sizeof(beijing)); /* zero fill */ + msg->len = sizeof(beijing); + msg->encoding = UTF32; + memcpy(msg->text, beijing, sizeof(beijing)); + + /* calculate the key length including padding, using formula */ + keylen = offsetof(msg_t, text) /* offset of last key field */ + + sizeof(beijing) /* size of last key field */ + - offsetof(msg_t, encoding); /* offset of first key field */ + + /* add our structure to the hash table */ + HASH_ADD( hh, msgs, encoding, keylen, msg); + + /* look it up to prove that it worked :-) */ + msg=NULL; + + lookup_key = (lookup_key_t*)malloc(sizeof(*lookup_key) + sizeof(beijing)); + memset(lookup_key, 0, sizeof(*lookup_key) + sizeof(beijing)); + lookup_key->encoding = UTF32; + memcpy(lookup_key->text, beijing, sizeof(beijing)); + HASH_FIND( hh, msgs, &lookup_key->encoding, keylen, msg ); + if (msg) printf("found \n"); + free(lookup_key); + + HASH_ITER(hh, msgs, msg, tmp) { + HASH_DEL(msgs, msg); + free(msg); + } + return 0; +} +---------------------------------------------------------------------- + +This example is included in the distribution in `tests/test22.c`. + +If you use multi-field keys, recognize that the compiler pads adjacent fields +(by inserting unused space between them) in order to fulfill the alignment +requirement of each field. For example a structure containing a `char` followed +by an `int` will normally have 3 "wasted" bytes of padding after the char, in +order to make the `int` field start on a multiple-of-4 address (4 is the length +of the int). + +.Calculating the length of a multi-field key: +******************************************************************************* +To determine the key length when using a multi-field key, you must include any +intervening structure padding the compiler adds for alignment purposes. + +An easy way to calculate the key length is to use the `offsetof` macro from +`<stddef.h>`. The formula is: + + key length = offsetof(last_key_field) + + sizeof(last_key_field) + - offsetof(first_key_field) + +In the example above, the `keylen` variable is set using this formula. +******************************************************************************* + +When dealing with a multi-field key, you must zero-fill your structure before +`HASH_ADD`'ing it to a hash table, or using its fields in a `HASH_FIND` key. + +In the previous example, `memset` is used to initialize the structure by +zero-filling it. This zeroes out any padding between the key fields. If we +didn't zero-fill the structure, this padding would contain random values. The +random values would lead to `HASH_FIND` failures; as two "identical" keys will +appear to mismatch if there are any differences within their padding. + +[[multilevel]] +Multi-level hash tables +~~~~~~~~~~~~~~~~~~~~~~~ +A multi-level hash table arises when each element of a hash table contains its +own secondary hash table. There can be any number of levels. In a scripting +language you might see: + + $items{bob}{age}=37 + +The C program below builds this example in uthash: the hash table is called +`items`. It contains one element (`bob`) whose own hash table contains one +element (`age`) with value 37. No special functions are necessary to build +a multi-level hash table. + +While this example represents both levels (`bob` and `age`) using the same +structure, it would also be fine to use two different structure definitions. +It would also be fine if there were three or more levels instead of two. + +.Multi-level hash table +---------------------------------------------------------------------- +#include <stdio.h> +#include <string.h> +#include <stdlib.h> +#include "uthash.h" + +/* hash of hashes */ +typedef struct item { + char name[10]; + struct item *sub; + int val; + UT_hash_handle hh; +} item_t; + +item_t *items=NULL; + +int main(int argc, char *argvp[]) { + item_t *item1, *item2, *tmp1, *tmp2; + + /* make initial element */ + item_t *i = malloc(sizeof(*i)); + strcpy(i->name, "bob"); + i->sub = NULL; + i->val = 0; + HASH_ADD_STR(items, name, i); + + /* add a sub hash table off this element */ + item_t *s = malloc(sizeof(*s)); + strcpy(s->name, "age"); + s->sub = NULL; + s->val = 37; + HASH_ADD_STR(i->sub, name, s); + + /* iterate over hash elements */ + HASH_ITER(hh, items, item1, tmp1) { + HASH_ITER(hh, item1->sub, item2, tmp2) { + printf("$items{%s}{%s} = %d\n", item1->name, item2->name, item2->val); + } + } + + /* clean up both hash tables */ + HASH_ITER(hh, items, item1, tmp1) { + HASH_ITER(hh, item1->sub, item2, tmp2) { + HASH_DEL(item1->sub, item2); + free(item2); + } + HASH_DEL(items, item1); + free(item1); + } + + return 0; +} +---------------------------------------------------------------------- +The example above is included in `tests/test59.c`. + +[[multihash]] +Items in several hash tables +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +A structure can be added to more than one hash table. A few reasons you might do +this include: + +- each hash table may use an alternative key; +- each hash table may have its own sort order; +- or you might simply use multiple hash tables for grouping purposes. E.g., + you could have users in an `admin_users` and a `users` hash table. + +Your structure needs to have a `UT_hash_handle` field for each hash table to +which it might be added. You can name them anything. E.g., + + UT_hash_handle hh1, hh2; + +Items with alternative keys +~~~~~~~~~~~~~~~~~~~~~~~~~~~ +You might create a hash table keyed on an ID field, and another hash table keyed +on username (if usernames are unique). You can add the same user structure to +both hash tables (without duplication of the structure), allowing lookup of a +user structure by their name or ID. The way to achieve this is to have a +separate `UT_hash_handle` for each hash to which the structure may be added. + +.A structure with two alternative keys +---------------------------------------------------------------------- +struct my_struct { + int id; /* usual key */ + char username[10]; /* alternative key */ + UT_hash_handle hh1; /* handle for first hash table */ + UT_hash_handle hh2; /* handle for second hash table */ +}; +---------------------------------------------------------------------- + +In the example above, the structure can now be added to two separate hash +tables. In one hash, `id` is its key, while in the other hash, `username` is +its key. (There is no requirement that the two hashes have different key +fields. They could both use the same key, such as `id`). + +Notice the structure has two hash handles (`hh1` and `hh2`). In the code +below, notice that each hash handle is used exclusively with a particular hash +table. (`hh1` is always used with the `users_by_id` hash, while `hh2` is +always used with the `users_by_name` hash table). + +.Two keys on a structure +---------------------------------------------------------------------- + struct my_struct *users_by_id = NULL, *users_by_name = NULL, *s; + int i; + char *name; + + s = malloc(sizeof(struct my_struct)); + s->id = 1; + strcpy(s->username, "thanson"); + + /* add the structure to both hash tables */ + HASH_ADD(hh1, users_by_id, id, sizeof(int), s); + HASH_ADD(hh2, users_by_name, username, strlen(s->username), s); + + /* lookup user by ID in the "users_by_id" hash table */ + i=1; + HASH_FIND(hh1, users_by_id, &i, sizeof(int), s); + if (s) printf("found id %d: %s\n", i, s->username); + + /* lookup user by username in the "users_by_name" hash table */ + name = "thanson"; + HASH_FIND(hh2, users_by_name, name, strlen(name), s); + if (s) printf("found user %s: %d\n", name, s->id); +---------------------------------------------------------------------- + + +Several sort orders +~~~~~~~~~~~~~~~~~~~ +It comes as no suprise that two hash tables can have different sort orders, but +this fact can also be used advantageously to sort the 'same items' in several +ways. This is based on the ability to store a structure in several hash tables. + +Extending the previous example, suppose we have many users. We have added each +user structure to the `users_by_id` hash table and the `users_by_name` hash table. +(To reiterate, this is done without the need to have two copies of each structure). +Now we can define two sort functions, then use `HASH_SRT`. + + int sort_by_id(struct my_struct *a, struct my_struct *b) { + if (a->id == b->id) return 0; + return (a->id < b->id) ? -1 : 1; + } + + int sort_by_name(struct my_struct *a, struct my_struct *b) { + return strcmp(a->username,b->username); + } + + HASH_SRT(hh1, users_by_id, sort_by_id); + HASH_SRT(hh2, users_by_name, sort_by_name); + +Now iterating over the items in `users_by_id` will traverse them in id-order +while, naturally, iterating over `users_by_name` will traverse them in +name-order. The items are fully forward-and-backward linked in each order. +So even for one set of users, we might store them in two hash tables to provide +easy iteration in two different sort orders. + +Bloom filter (faster misses) +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +Programs that generate a fair miss rate (`HASH_FIND` that result in `NULL`) may +benefit from the built-in Bloom filter support. This is disabled by default, +because programs that generate only hits would incur a slight penalty from it. +Also, programs that do deletes should not use the Bloom filter. While the +program would operate correctly, deletes diminish the benefit of the filter. +To enable the Bloom filter, simply compile with `-DHASH_BLOOM=n` like: + + -DHASH_BLOOM=27 + +where the number can be any value up to 32 which determines the amount of memory +used by the filter, as shown below. Using more memory makes the filter more +accurate and has the potential to speed up your program by making misses bail +out faster. + +.Bloom filter sizes for selected values of n +[width="50%",cols="10m,30",grid="none",options="header"] +|===================================================================== +| n | Bloom filter size (per hash table) +| 16 | 8 kilobytes +| 20 | 128 kilobytes +| 24 | 2 megabytes +| 28 | 32 megabytes +| 32 | 512 megabytes +|===================================================================== + +Bloom filters are only a performance feature; they do not change the results of +hash operations in any way. The only way to gauge whether or not a Bloom filter +is right for your program is to test it. Reasonable values for the size of the +Bloom filter are 16-32 bits. + +Select +~~~~~~ +An experimental 'select' operation is provided that inserts those items from a +source hash that satisfy a given condition into a destination hash. This +insertion is done with somewhat more efficiency than if this were using +`HASH_ADD`, namely because the hash function is not recalculated for keys of the +selected items. This operation does not remove any items from the source hash. +Rather the selected items obtain dual presence in both hashes. The destination +hash may already have items in it; the selected items are added to it. In order +for a structure to be usable with `HASH_SELECT`, it must have two or more hash +handles. (As described <<multihash,here>>, a structure can exist in many +hash tables at the same time; it must have a separate hash handle for each one). + + user_t *users=NULL, *admins=NULL; /* two hash tables */ + + typedef struct { + int id; + UT_hash_handle hh; /* handle for users hash */ + UT_hash_handle ah; /* handle for admins hash */ + } user_t; + +Now suppose we have added some users, and want to select just the administrator +users who have id's less than 1024. + + #define is_admin(x) (((user_t*)x)->id < 1024) + HASH_SELECT(ah,admins,hh,users,is_admin); + +The first two parameters are the 'destination' hash handle and hash table, the +second two parameters are the 'source' hash handle and hash table, and the last +parameter is the 'select condition'. Here we used a macro `is_admin()` but we +could just as well have used a function. + + int is_admin(void *userv) { + user_t *user = (user_t*)userv; + return (user->id < 1024) ? 1 : 0; + } + +If the select condition always evaluates to true, this operation is +essentially a 'merge' of the source hash into the destination hash. Of course, +the source hash remains unchanged under any use of `HASH_SELECT`. It only adds +items to the destination hash selectively. + +The two hash handles must differ. An example of using `HASH_SELECT` is included +in `tests/test36.c`. + + +[[hash_functions]] +Built-in hash functions +~~~~~~~~~~~~~~~~~~~~~~~ +Internally, a hash function transforms a key into a bucket number. You don't +have to take any action to use the default hash function, currently Jenkin's. + +Some programs may benefit from using another of the built-in hash functions. +There is a simple analysis utility included with uthash to help you determine +if another hash function will give you better performance. + +You can use a different hash function by compiling your program with +`-DHASH_FUNCTION=HASH_xyz` where `xyz` is one of the symbolic names listed +below. E.g., + + cc -DHASH_FUNCTION=HASH_BER -o program program.c + +.Built-in hash functions +[width="50%",cols="^5m,20",grid="none",options="header"] +|=============================================================================== +|Symbol | Name +|JEN | Jenkins (default) +|BER | Bernstein +|SAX | Shift-Add-Xor +|OAT | One-at-a-time +|FNV | Fowler/Noll/Vo +|SFH | Paul Hsieh +|MUR | MurmurHash v3 (see note) +|=============================================================================== + +[NOTE] +.MurmurHash +================================================================================ +A special symbol must be defined if you intend to use MurmurHash. To use it, add +`-DHASH_USING_NO_STRICT_ALIASING` to your `CFLAGS`. And, if you are using +the gcc compiler with optimization, add `-fno-strict-aliasing` to your `CFLAGS`. +================================================================================ + +Which hash function is best? +^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +You can easily determine the best hash function for your key domain. To do so, +you'll need to run your program once in a data-collection pass, and then run +the collected data through an included analysis utility. + +First you must build the analysis utility. From the top-level directory, + + cd tests/ + make + +We'll use `test14.c` to demonstrate the data-collection and analysis steps +(here using `sh` syntax to redirect file descriptor 3 to a file): + +.Using keystats +-------------------------------------------------------------------------------- +% cc -DHASH_EMIT_KEYS=3 -I../src -o test14 test14.c +% ./test14 3>test14.keys +% ./keystats test14.keys +fcn ideal% #items #buckets dup% fl add_usec find_usec del-all usec +--- ------ ---------- ---------- ----- -- ---------- ---------- ------------ +SFH 91.6% 1219 256 0% ok 92 131 25 +FNV 90.3% 1219 512 0% ok 107 97 31 +SAX 88.7% 1219 512 0% ok 111 109 32 +OAT 87.2% 1219 256 0% ok 99 138 26 +JEN 86.7% 1219 256 0% ok 87 130 27 +BER 86.2% 1219 256 0% ok 121 129 27 +-------------------------------------------------------------------------------- + +[NOTE] +The number 3 in `-DHASH_EMIT_KEYS=3` is a file descriptor. Any file descriptor +that your program doesn't use for its own purposes can be used instead of 3. +The data-collection mode enabled by `-DHASH_EMIT_KEYS=x` should not be used in +production code. + +Usually, you should just pick the first hash function that is listed. Here, this +is `SFH`. This is the function that provides the most even distribution for +your keys. If several have the same `ideal%`, then choose the fastest one +according to the `find_usec` column. + +keystats column reference +^^^^^^^^^^^^^^^^^^^^^^^^^ +fcn:: + symbolic name of hash function +ideal%:: + The percentage of items in the hash table which can be looked up within an + ideal number of steps. (Further explained below). +#items:: + the number of keys that were read in from the emitted key file +#buckets:: + the number of buckets in the hash after all the keys were added +dup%:: + the percent of duplicate keys encountered in the emitted key file. + Duplicates keys are filtered out to maintain key uniqueness. (Duplicates + are normal. For example, if the application adds an item to a hash, + deletes it, then re-adds it, the key is written twice to the emitted file.) +flags:: + this is either `ok`, or `nx` (noexpand) if the expansion inhibited flag is + set, described in <<expansion,Expansion internals>>. It is not recommended + to use a hash function that has the `noexpand` flag set. +add_usec:: + the clock time in microseconds required to add all the keys to a hash +find_usec:: + the clock time in microseconds required to look up every key in the hash +del-all usec:: + the clock time in microseconds required to delete every item in the hash + +[[ideal]] +ideal% +^^^^^^ + +.What is ideal%? +***************************************************************************** +The 'n' items in a hash are distributed into 'k' buckets. Ideally each bucket +would contain an equal share '(n/k)' of the items. In other words, the maximum +linear position of any item in a bucket chain would be 'n/k' if every bucket is +equally used. If some buckets are overused and others are underused, the +overused buckets will contain items whose linear position surpasses 'n/k'. +Such items are considered non-ideal. + +As you might guess, `ideal%` is the percentage of ideal items in the hash. These +items have favorable linear positions in their bucket chains. As `ideal%` +approaches 100%, the hash table approaches constant-time lookup performance. +***************************************************************************** + +[[hashscan]] +hashscan +~~~~~~~~ +NOTE: This utility is only available on Linux, and on FreeBSD (8.1 and up). + +A utility called `hashscan` is included in the `tests/` directory. It +is built automatically when you run `make` in that directory. This tool +examines a running process and reports on the uthash tables that it finds in +that program's memory. It can also save the keys from each table in a format +that can be fed into `keystats`. + +Here is an example of using `hashscan`. First ensure that it is built: + + cd tests/ + make + +Since `hashscan` needs a running program to inspect, we'll start up a simple +program that makes a hash table and then sleeps as our test subject: + + ./test_sleep & + pid: 9711 + +Now that we have a test program, let's run `hashscan` on it: + + ./hashscan 9711 + Address ideal items buckets mc fl bloom/sat fcn keys saved to + ------------------ ----- -------- -------- -- -- --------- --- ------------- + 0x862e038 81% 10000 4096 11 ok 16 14% JEN + +If we wanted to copy out all its keys for external analysis using `keystats`, +add the `-k` flag: + + ./hashscan -k 9711 + Address ideal items buckets mc fl bloom/sat fcn keys saved to + ------------------ ----- -------- -------- -- -- --------- --- ------------- + 0x862e038 81% 10000 4096 11 ok 16 14% JEN /tmp/9711-0.key + +Now we could run `./keystats /tmp/9711-0.key` to analyze which hash function +has the best characteristics on this set of keys. + +hashscan column reference +^^^^^^^^^^^^^^^^^^^^^^^^^ +Address:: + virtual address of the hash table +ideal:: + The percentage of items in the table which can be looked up within an ideal + number of steps. See <<ideal>> in the `keystats` section. +items:: + number of items in the hash table +buckets:: + number of buckets in the hash table +mc:: + the maximum chain length found in the hash table (uthash usually tries to + keep fewer than 10 items in each bucket, or in some cases a multiple of 10) +fl:: + flags (either `ok`, or `NX` if the expansion-inhibited flag is set) +bloom/sat:: + if the hash table uses a Bloom filter, this is the size (as a power of two) + of the filter (e.g. 16 means the filter is 2^16 bits in size). The second + number is the "saturation" of the bits expressed as a percentage. The lower + the percentage, the more potential benefit to identify cache misses quickly. +fcn:: + symbolic name of hash function +keys saved to:: + file to which keys were saved, if any + +.How hashscan works +***************************************************************************** +When hashscan runs, it attaches itself to the target process, which suspends +the target process momentarily. During this brief suspension, it scans the +target's virtual memory for the signature of a uthash hash table. It then +checks if a valid hash table structure accompanies the signature and reports +what it finds. When it detaches, the target process resumes running normally. +The hashscan is performed "read-only"-- the target process is not modified. +Since hashscan is analyzing a momentary snapshot of a running process, it may +return different results from one run to another. +***************************************************************************** + +[[expansion]] +Expansion internals +~~~~~~~~~~~~~~~~~~~ +Internally this hash manages the number of buckets, with the goal of having +enough buckets so that each one contains only a small number of items. + +.Why does the number of buckets matter? +******************************************************************************** +When looking up an item by its key, this hash scans linearly through the items +in the appropriate bucket. In order for the linear scan to run in constant +time, the number of items in each bucket must be bounded. This is accomplished +by increasing the number of buckets as needed. +******************************************************************************** + +Normal expansion +^^^^^^^^^^^^^^^^ +This hash attempts to keep fewer than 10 items in each bucket. When an item is +added that would cause a bucket to exceed this number, the number of buckets in +the hash is doubled and the items are redistributed into the new buckets. In an +ideal world, each bucket will then contain half as many items as it did before. + +Bucket expansion occurs automatically and invisibly as needed. There is +no need for the application to know when it occurs. + +Per-bucket expansion threshold +++++++++++++++++++++++++++++++ +Normally all buckets share the same threshold (10 items) at which point bucket +expansion is triggered. During the process of bucket expansion, uthash can +adjust this expansion-trigger threshold on a per-bucket basis if it sees that +certain buckets are over-utilized. + +When this threshold is adjusted, it goes from 10 to a multiple of 10 (for that +particular bucket). The multiple is based on how many times greater the actual +chain length is than the ideal length. It is a practical measure to reduce +excess bucket expansion in the case where a hash function over-utilizes a few +buckets but has good overall distribution. However, if the overall distribution +gets too bad, uthash changes tactics. + +Inhibited expansion +^^^^^^^^^^^^^^^^^^^ +You usually don't need to know or worry about this, particularly if you used +the `keystats` utility during development to select a good hash for your keys. + +A hash function may yield an uneven distribution of items across the buckets. +In moderation this is not a problem. Normal bucket expansion takes place as +the chain lengths grow. But when significant imbalance occurs (because the hash +function is not well suited to the key domain), bucket expansion may be +ineffective at reducing the chain lengths. + +Imagine a very bad hash function which always puts every item in bucket 0. No +matter how many times the number of buckets is doubled, the chain length of +bucket 0 stays the same. In a situation like this, the best behavior is to +stop expanding, and accept O(n) lookup performance. This is what uthash +does. It degrades gracefully if the hash function is ill-suited to the keys. + +If two consecutive bucket expansions yield `ideal%` values below 50%, uthash +inhibits expansion for that hash table. Once set, the 'bucket expansion +inhibited' flag remains in effect as long as the hash has items in it. +Inhibited expansion may cause `HASH_FIND` to exhibit worse than constant-time +performance. + +Hooks +~~~~~ +You don't need to use these hooks- they are only here if you want to modify +the behavior of uthash. Hooks can be used to change how uthash allocates +memory, and to run code in response to certain internal events. + +malloc/free +^^^^^^^^^^^ +By default this hash implementation uses `malloc` and `free` to manage memory. +If your application uses its own custom allocator, this hash can use them too. + +.Specifying alternate memory management functions +---------------------------------------------------------------------------- +#include "uthash.h" + +/* undefine the defaults */ +#undef uthash_malloc +#undef uthash_free + +/* re-define, specifying alternate functions */ +#define uthash_malloc(sz) my_malloc(sz) +#define uthash_free(ptr,sz) my_free(ptr) + +... +---------------------------------------------------------------------------- + +Notice that `uthash_free` receives two parameters. The `sz` parameter is for +convenience on embedded platforms that manage their own memory. + +Out of memory +^^^^^^^^^^^^^ +If memory allocation fails (i.e., the malloc function returned `NULL`), the +default behavior is to terminate the process by calling `exit(-1)`. This can +be modified by re-defining the `uthash_fatal` macro. + + #undef uthash_fatal + #define uthash_fatal(msg) my_fatal_function(msg); + +The fatal function should terminate the process or `longjmp` back to a safe +place. Uthash does not support "returning a failure" if memory cannot be +allocated. + +Internal events +^^^^^^^^^^^^^^^ +There is no need for the application to set these hooks or take action in +response to these events. They are mainly for diagnostic purposes. + +These two hooks are "notification" hooks which get executed if uthash is +expanding buckets, or setting the 'bucket expansion inhibited' flag. Normally +both of these hooks are undefined and thus compile away to nothing. + +Expansion ++++++++++ +There is a hook for the bucket expansion event. + +.Bucket expansion hook +---------------------------------------------------------------------------- +#include "uthash.h" + +#undef uthash_expand_fyi +#define uthash_expand_fyi(tbl) printf("expanded to %d buckets\n", tbl->num_buckets) + +... +---------------------------------------------------------------------------- + +Expansion-inhibition +++++++++++++++++++++ +This hook can be defined to code to execute in the event that uthash decides to +set the 'bucket expansion inhibited' flag. + +.Bucket expansion inhibited hook +---------------------------------------------------------------------------- +#include "uthash.h" + +#undef uthash_noexpand_fyi +#define uthash_noexpand_fyi printf("warning: bucket expansion inhibited\n"); + +... +---------------------------------------------------------------------------- + + +Debug mode +~~~~~~~~~~ +If a program that uses this hash is compiled with `-DHASH_DEBUG=1`, a special +internal consistency-checking mode is activated. In this mode, the integrity +of the whole hash is checked following every add or delete operation. This is +for debugging the uthash software only, not for use in production code. + +In the `tests/` directory, running `make debug` will run all the tests in +this mode. + +In this mode, any internal errors in the hash data structure will cause a +message to be printed to `stderr` and the program to exit. + +The `UT_hash_handle` data structure includes `next`, `prev`, `hh_next` and +`hh_prev` fields. The former two fields determine the "application" ordering +(that is, insertion order-- the order the items were added). The latter two +fields determine the "bucket chain" order. These link the `UT_hash_handles` +together in a doubly-linked list that is a bucket chain. + +Checks performed in `-DHASH_DEBUG=1` mode: + +- the hash is walked in its entirety twice: once in 'bucket' order and a + second time in 'application' order +- the total number of items encountered in both walks is checked against the + stored number +- during the walk in 'bucket' order, each item's `hh_prev` pointer is compared + for equality with the last visited item +- during the walk in 'application' order, each item's `prev` pointer is compared + for equality with the last visited item + +.Macro debugging: +******************************************************************************** +Sometimes it's difficult to interpret a compiler warning on a line which +contains a macro call. In the case of uthash, one macro can expand to dozens of +lines. In this case, it is helpful to expand the macros and then recompile. +By doing so, the warning message will refer to the exact line within the macro. + +Here is an example of how to expand the macros and then recompile. This uses the +`test1.c` program in the `tests/` subdirectory. + + gcc -E -I../src test1.c > /tmp/a.c + egrep -v '^#' /tmp/a.c > /tmp/b.c + indent /tmp/b.c + gcc -o /tmp/b /tmp/b.c + +The last line compiles the original program (test1.c) with all macros expanded. +If there was a warning, the referenced line number can be checked in `/tmp/b.c`. +******************************************************************************** + +Thread safety +~~~~~~~~~~~~~ +You can use uthash in a threaded program. But you must do the locking. Use a +read-write lock to protect against concurrent writes. It is ok to have +concurrent readers (since uthash 1.5). + +For example using pthreads you can create an rwlock like this: + + pthread_rwlock_t lock; + if (pthread_rwlock_init(&lock,NULL) != 0) fatal("can't create rwlock"); + +Then, readers must acquire the read lock before doing any `HASH_FIND` calls or +before iterating over the hash elements: + + if (pthread_rwlock_rdlock(&lock) != 0) fatal("can't get rdlock"); + HASH_FIND_INT(elts, &i, e); + pthread_rwlock_unlock(&lock); + +Writers must acquire the exclusive write lock before doing any update. Add, +delete, and sort are all updates that must be locked. + + if (pthread_rwlock_wrlock(&lock) != 0) fatal("can't get wrlock"); + HASH_DEL(elts, e); + pthread_rwlock_unlock(&lock); + +If you prefer, you can use a mutex instead of a read-write lock, but this will +reduce reader concurrency to a single thread at a time. + +An example program using uthash with a read-write lock is included in +`tests/threads/test1.c`. + +[[Macro_reference]] +Macro reference +--------------- + +Convenience macros +~~~~~~~~~~~~~~~~~~ +The convenience macros do the same thing as the generalized macros, but +require fewer arguments. + +In order to use the convenience macros, + +1. the structure's `UT_hash_handle` field must be named `hh`, and +2. for add or find, the key field must be of type `int` or `char[]` or pointer + +.Convenience macros +[width="90%",cols="10m,30m",grid="none",options="header"] +|=============================================================================== +|macro | arguments +|HASH_ADD_INT | (head, keyfield_name, item_ptr) +|HASH_FIND_INT | (head, key_ptr, item_ptr) +|HASH_ADD_STR | (head, keyfield_name, item_ptr) +|HASH_FIND_STR | (head, key_ptr, item_ptr) +|HASH_ADD_PTR | (head, keyfield_name, item_ptr) +|HASH_FIND_PTR | (head, key_ptr, item_ptr) +|HASH_DEL | (head, item_ptr) +|HASH_SORT | (head, cmp) +|HASH_COUNT | (head) +|=============================================================================== + +General macros +~~~~~~~~~~~~~~ + +These macros add, find, delete and sort the items in a hash. You need to +use the general macros if your `UT_hash_handle` is named something other +than `hh`, or if your key's data type isn't `int` or `char[]`. + +.General macros +[width="90%",cols="10m,30m",grid="none",options="header"] +|=============================================================================== +|macro | arguments +|HASH_ADD | (hh_name, head, keyfield_name, key_len, item_ptr) +|HASH_ADD_KEYPTR| (hh_name, head, key_ptr, key_len, item_ptr) +|HASH_FIND | (hh_name, head, key_ptr, key_len, item_ptr) +|HASH_DELETE | (hh_name, head, item_ptr) +|HASH_SRT | (hh_name, head, cmp) +|HASH_CNT | (hh_name, head) +|HASH_CLEAR | (hh_name, head) +|HASH_SELECT | (dst_hh_name, dst_head, src_hh_name, src_head, condition) +|HASH_ITER | (hh_name, head, item_ptr, tmp_item_ptr) +|=============================================================================== + +[NOTE] +`HASH_ADD_KEYPTR` is used when the structure contains a pointer to the +key, rather than the key itself. + + +Argument descriptions +^^^^^^^^^^^^^^^^^^^^^ +hh_name:: + name of the `UT_hash_handle` field in the structure. Conventionally called + `hh`. +head:: + the structure pointer variable which acts as the "head" of the hash. So + named because it initially points to the first item that is added to the hash. +keyfield_name:: + the name of the key field in the structure. (In the case of a multi-field + key, this is the first field of the key). If you're new to macros, it + might seem strange to pass the name of a field as a parameter. See + <<validc,note>>. +key_len:: + the length of the key field in bytes. E.g. for an integer key, this is + `sizeof(int)`, while for a string key it's `strlen(key)`. (For a + multi-field key, see the notes in this guide on calculating key length). +key_ptr:: + for `HASH_FIND`, this is a pointer to the key to look up in the hash + (since it's a pointer, you can't directly pass a literal value here). For + `HASH_ADD_KEYPTR`, this is the address of the key of the item being added. +item_ptr:: + pointer to the structure being added, deleted, or looked up, or the current + pointer during iteration. This is an input parameter for `HASH_ADD` and + `HASH_DELETE` macros, and an output parameter for `HASH_FIND` and + `HASH_ITER`. (When using `HASH_ITER` to iterate, `tmp_item_ptr` + is another variable of the same type as `item_ptr`, used internally). +cmp:: + pointer to comparison function which accepts two arguments (pointers to + items to compare) and returns an int specifying whether the first item + should sort before, equal to, or after the second item (like `strcmp`). +condition:: + a function or macro which accepts a single argument-- a void pointer to a + structure, which needs to be cast to the appropriate structure type. The + function or macro should return (or evaluate to) a non-zero value if the + structure should be "selected" for addition to the destination hash. + +// vim: set tw=80 wm=2 syntax=asciidoc: + diff --git a/src/modules/systemlib/uthash/doc/utarray.txt b/src/modules/systemlib/uthash/doc/utarray.txt new file mode 100644 index 000000000..37830f124 --- /dev/null +++ b/src/modules/systemlib/uthash/doc/utarray.txt @@ -0,0 +1,376 @@ +utarray: dynamic array macros for C +=================================== +Troy D. Hanson <thanson@users.sourceforge.net> +v1.9.5, November 2011 + +include::sflogo.txt[] +include::topnav_utarray.txt[] + +Introduction +------------ +include::toc.txt[] + +A set of general-purpose dynamic array macros for C structures are included with +uthash in `utarray.h`. To use these macros in your own C program, just +copy `utarray.h` into your source directory and use it in your programs. + + #include "utarray.h" + +The dynamic array supports basic operations such as push, pop, and erase on the +array elements. These array elements can be any simple datatype or structure. +The array <<operations,operations>> are based loosely on the C++ STL vector methods. + +Internally the dynamic array contains a contiguous memory region into which +the elements are copied. This buffer is grown as needed using `realloc` to +accomodate all the data that is pushed into it. + +Download +~~~~~~~~ +To download the `utarray.h` header file, follow the link on the +http://uthash.sourceforge.net[uthash home page]. + +BSD licensed +~~~~~~~~~~~~ +This software is made available under the +link:license.html[revised BSD license]. +It is free and open source. + +Platforms +~~~~~~~~~ +The 'utarray' macros have been tested on: + + * Linux, + * Mac OS X, + * Windows, using Visual Studio 2008 and Visual Studio 2010 + +Usage +----- + +Declaration +~~~~~~~~~~~ + +The array itself has the data type `UT_array`, regardless of the type of +elements to be stored in it. It is declared like, + + UT_array *nums; + +New and free +~~~~~~~~~~~~ +The next step is to create the array using `utarray_new`. Later when you're +done with the array, `utarray_free` will free it and all its elements. + +Push, pop, etc +~~~~~~~~~~~~~~ +The central features of the utarray involve putting elements into it, taking +them out, and iterating over them. There are several <<operations,operations>> +to pick from that deal with either single elements or ranges of elements at a +time. In the examples below we will use only the push operation to insert +elements. + +Elements +-------- + +Support for dynamic arrays of integers or strings is especially easy. These are +best shown by example: + +Integers +~~~~~~~~ +This example makes a utarray of integers, pushes 0-9 into it, then prints it. +Lastly it frees it. + +.Integer elements +------------------------------------------------------------------------------- +#include <stdio.h> +#include "utarray.h" + +int main() { + UT_array *nums; + int i, *p; + + utarray_new(nums,&ut_int_icd); + for(i=0; i < 10; i++) utarray_push_back(nums,&i); + + for(p=(int*)utarray_front(nums); + p!=NULL; + p=(int*)utarray_next(nums,p)) { + printf("%d\n",*p); + } + + utarray_free(nums); + + return 0; +} +------------------------------------------------------------------------------- + +The second argument to `utarray_push_back` is always a 'pointer' to the type +(so a literal cannot be used). So for integers, it is an `int*`. + +Strings +~~~~~~~ +In this example we make a utarray of strings, push two strings into it, print +it and free it. + +.String elements +------------------------------------------------------------------------------- +#include <stdio.h> +#include "utarray.h" + +int main() { + UT_array *strs; + char *s, **p; + + utarray_new(strs,&ut_str_icd); + + s = "hello"; utarray_push_back(strs, &s); + s = "world"; utarray_push_back(strs, &s); + p = NULL; + while ( (p=(char**)utarray_next(strs,p))) { + printf("%s\n",*p); + } + + utarray_free(strs); + + return 0; +} +------------------------------------------------------------------------------- + +In this example, since the element is a `char*`, we pass a pointer to it +(`char**`) as the second argument to `utarray_push_back`. Note that "push" makes +a copy of the source string and pushes that copy into the array. + +About UT_icd +~~~~~~~~~~~~ + +Arrays be made of any type of element, not just integers and strings. The +elements can be basic types or structures. Unless you're dealing with integers +and strings (which use pre-defined `ut_int_icd` and `ut_str_icd`), you'll need +to define a `UT_icd` helper structure. This structure contains everything that +utarray needs to initialize, copy or destruct elements. + + typedef struct { + size_t sz; + init_f *init; + ctor_f *copy; + dtor_f *dtor; + } UT_icd; + +The three function pointers `init`, `copy`, and `dtor` have these prototypes: + + typedef void (ctor_f)(void *dst, const void *src); + typedef void (dtor_f)(void *elt); + typedef void (init_f)(void *elt); + +The `sz` is just the size of the element being stored in the array. + +The `init` function will be invoked whenever utarray needs to initialize an +empty element. This only happens as a byproduct of `utarray_resize` or +`utarray_extend_back`. If `init` is `NULL`, it defaults to zero filling the +new element using memset. + +The `copy` function is used whenever an element is copied into the array. +It is invoked during `utarray_push_back`, `utarray_insert`, `utarray_inserta`, +or `utarray_concat`. If `copy` is `NULL`, it defaults to a bitwise copy using +memcpy. + +The `dtor` function is used to clean up an element that is being removed from +the array. It may be invoked due to `utarray_resize`, `utarray_pop_back`, +`utarray_erase`, `utarray_clear`, `utarray_done` or `utarray_free`. If the +elements need no cleanup upon destruction, `dtor` may be `NULL`. + +Scalar types +~~~~~~~~~~~~ + +The next example uses `UT_icd` with all its defaults to make a utarray of +`long` elements. This example pushes two longs, prints them, and frees the +array. + +.long elements +------------------------------------------------------------------------------- +#include <stdio.h> +#include "utarray.h" + +UT_icd long_icd = {sizeof(long), NULL, NULL, NULL }; + +int main() { + UT_array *nums; + long l, *p; + utarray_new(nums, &long_icd); + + l=1; utarray_push_back(nums, &l); + l=2; utarray_push_back(nums, &l); + + p=NULL; + while( (p=(long*)utarray_next(nums,p))) printf("%ld\n", *p); + + utarray_free(nums); + return 0; +} +------------------------------------------------------------------------------- + +Structures +~~~~~~~~~~ + +Structures can be used as utarray elements. If the structure requires no +special effort to initialize, copy or destruct, we can use `UT_icd` with all +its defaults. This example shows a structure that consists of two integers. Here +we push two values, print them and free the array. + +.Structure (simple) +------------------------------------------------------------------------------- +#include <stdio.h> +#include "utarray.h" + +typedef struct { + int a; + int b; +} intpair_t; + +UT_icd intpair_icd = {sizeof(intpair_t), NULL, NULL, NULL}; + +int main() { + + UT_array *pairs; + intpair_t ip, *p; + utarray_new(pairs,&intpair_icd); + + ip.a=1; ip.b=2; utarray_push_back(pairs, &ip); + ip.a=10; ip.b=20; utarray_push_back(pairs, &ip); + + for(p=(intpair_t*)utarray_front(pairs); + p!=NULL; + p=(intpair_t*)utarray_next(pairs,p)) { + printf("%d %d\n", p->a, p->b); + } + + utarray_free(pairs); + return 0; +} +------------------------------------------------------------------------------- + +The real utility of `UT_icd` is apparent when the elements of the utarray are +structures that require special work to initialize, copy or destruct. + +For example, when a structure contains pointers to related memory areas that +need to be copied when the structure is copied (and freed when the structure is +freed), we can use custom `init`, `copy`, and `dtor` members in the `UT_icd`. + +Here we take an example of a structure that contains an integer and a string. +When this element is copied (such as when an element is pushed into the array), +we want to "deep copy" the `s` pointer (so the original element and the new +element point to their own copies of `s`). When an element is destructed, we +want to "deep free" its copy of `s`. Lastly, this example is written to work +even if `s` has the value `NULL`. + +.Structure (complex) +------------------------------------------------------------------------------- +#include <stdio.h> +#include <stdlib.h> +#include "utarray.h" + +typedef struct { + int a; + char *s; +} intchar_t; + +void intchar_copy(void *_dst, const void *_src) { + intchar_t *dst = (intchar_t*)_dst, *src = (intchar_t*)_src; + dst->a = src->a; + dst->s = src->s ? strdup(src->s) : NULL; +} + +void intchar_dtor(void *_elt) { + intchar_t *elt = (intchar_t*)_elt; + if (elt->s) free(elt->s); +} + +UT_icd intchar_icd = {sizeof(intchar_t), NULL, intchar_copy, intchar_dtor}; + +int main() { + UT_array *intchars; + intchar_t ic, *p; + utarray_new(intchars, &intchar_icd); + + ic.a=1; ic.s="hello"; utarray_push_back(intchars, &ic); + ic.a=2; ic.s="world"; utarray_push_back(intchars, &ic); + + p=NULL; + while( (p=(intchar_t*)utarray_next(intchars,p))) { + printf("%d %s\n", p->a, (p->s ? p->s : "null")); + } + + utarray_free(intchars); + return 0; +} + +------------------------------------------------------------------------------- + +[[operations]] +Reference +--------- +This table lists all the utarray operations. These are loosely based on the C++ +vector class. + +Operations +~~~~~~~~~~ + +[width="100%",cols="50<m,40<",grid="none",options="none"] +|=============================================================================== +| utarray_new(UT_array *a, UT_icd *icd)| allocate a new array +| utarray_free(UT_array *a) | free an allocated array +| utarray_init(UT_array *a,UT_icd *icd)| init an array (non-alloc) +| utarray_done(UT_array *a) | dispose of an array (non-allocd) +| utarray_reserve(UT_array *a,int n) | ensure space available for 'n' more elements +| utarray_push_back(UT_array *a,void *p) | push element p onto a +| utarray_pop_back(UT_array *a) | pop last element from a +| utarray_extend_back(UT_array *a) | push empty element onto a +| utarray_len(UT_array *a) | get length of a +| utarray_eltptr(UT_array *a,int j) | get pointer of element from index +| utarray_eltidx(UT_array *a,void *e) | get index of element from pointer +| utarray_insert(UT_array *a,void *p, int j) | insert element p to index j +| utarray_inserta(UT_array *a,UT_array *w, int j) | insert array w into array a at index j +| utarray_resize(UT_array *dst,int num) | extend or shrink array to num elements +| utarray_concat(UT_array *dst,UT_array *src) | copy src to end of dst array +| utarray_erase(UT_array *a,int pos,int len) | remove len elements from a[pos]..a[pos+len-1] +| utarray_clear(UT_array *a) | clear all elements from a, setting its length to zero +| utarray_sort(UT_array *a,cmpfcn *cmp) | sort elements of a using comparison function +| utarray_find(UT_array *a,void *v, cmpfcn *cmp) | find element v in utarray (must be sorted) +| utarray_front(UT_array *a) | get first element of a +| utarray_next(UT_array *a,void *e) | get element of a following e (front if e is NULL) +| utarray_prev(UT_array *a,void *e) | get element of a before e (back if e is NULL) +| utarray_back(UT_array *a) | get last element of a +|=============================================================================== + +Notes +~~~~~ + +1. `utarray_new` and `utarray_free` are used to allocate a new array and free it, + while `utarray_init` and `utarray_done` can be used if the UT_array is already + allocated and just needs to be initialized or have its internal resources + freed. +2. `utarray_reserve` takes the "delta" of elements to reserve (not the total + desired capacity of the array-- this differs from the C++ STL "reserve" notion) +3. `utarray_sort` expects a comparison function having the usual `strcmp` -like + convention where it accepts two elements (a and b) and returns a negative + value if a precedes b, 0 if a and b sort equally, and positive if b precedes a. + This is an example of a comparison function: + + int intsort(const void *a,const void*b) { + int _a = *(int*)a; + int _b = *(int*)b; + return _a - _b; + } + +4. `utarray_find` uses a binary search to locate an element having a certain value + according to the given comparison function. The utarray must be first sorted + using the same comparison function. An example of using `utarray_find` with + a utarray of strings is included in `tests/test61.c`. + +5. A 'pointer' to a particular element (obtained using `utarray_eltptr` or + `utarray_front`, `utarray_next`, `utarray_prev`, `utarray_back`) becomes invalid whenever + another element is inserted into the utarray. This is because the internal + memory management may need to `realloc` the element storage to a new address. + For this reason, it's usually better to refer to an element by its integer + 'index' in code whose duration may include element insertion. + +// vim: set nowrap syntax=asciidoc: + diff --git a/src/modules/systemlib/uthash/doc/utlist.txt b/src/modules/systemlib/uthash/doc/utlist.txt new file mode 100644 index 000000000..fbf961c27 --- /dev/null +++ b/src/modules/systemlib/uthash/doc/utlist.txt @@ -0,0 +1,219 @@ +utlist: linked list macros for C structures +=========================================== +Troy D. Hanson <thanson@users.sourceforge.net> +v1.9.5, November 2011 + +include::sflogo.txt[] +include::topnav_utlist.txt[] + +Introduction +------------ +include::toc.txt[] + +A set of general-purpose 'linked list' macros for C structures are included with +uthash in `utlist.h`. To use these macros in your own C program, just +copy `utlist.h` into your source directory and use it in your programs. + + #include "utlist.h" + +These macros support the basic linked list operations: adding and deleting +elements, sorting them and iterating over them. + +Download +~~~~~~~~ +To download the `utlist.h` header file, follow the link on the +http://uthash.sourceforge.net[uthash home page]. + +BSD licensed +~~~~~~~~~~~~ +This software is made available under the +link:license.html[revised BSD license]. +It is free and open source. + +Platforms +~~~~~~~~~ +The 'utlist' macros have been tested on: + + * Linux, + * Mac OS X, and + * Windows, using Visual Studio 2008, Visual Studio 2010, or Cygwin/MinGW. + +Using utlist +------------ + +Types of lists +~~~~~~~~~~~~~~ +Three types of linked lists are supported: + +- *singly-linked* lists, +- *doubly-linked* lists, and +- *circular, doubly-linked* lists + +Efficiency +^^^^^^^^^^ +For all types of lists, prepending elements and deleting elements are +constant-time operations. Appending to a singly-linked list is an 'O(n)' +operation but appending to a doubly-linked list is constant time using these +macros. (This is because, in the utlist implementation of the doubly-linked +list, the head element's `prev` member points back to the list tail, even when +the list is non-circular). Sorting is an 'O(n log(n))' operation. Iteration +and searching are `O(n)` for all list types. + +List elements +~~~~~~~~~~~~~ +You can use any structure with these macros, as long as the structure +contains a `next` pointer. If you want to make a doubly-linked list, +the element also needs to have a `prev` pointer. + + typedef struct element { + char *name; + struct element *prev; /* needed for a doubly-linked list only */ + struct element *next; /* needed for singly- or doubly-linked lists */ + } element; + +You can name your structure anything. In the example above it is called `element`. +Within a particular list, all elements must be of the same type. + +List head +~~~~~~~~~ +The list head is simply a pointer to your element structure. You can name it +anything. *It must be initialized to `NULL`*. + + element *head = NULL; + +List operations +~~~~~~~~~~~~~~~ +The lists support inserting or deleting elements, sorting the elements and +iterating over them. + +[width="100%",cols="10<m,10<m,10<m",grid="cols",options="header"] +|=============================================================================== +|Singly-linked | Doubly-linked | Circular, doubly-linked +|LL_PREPEND(head,add); | DL_PREPEND(head,add); | CDL_PREPEND(head,add; +|LL_APPEND(head,add); | DL_APPEND(head,add); | +|LL_CONCAT(head1,head2); | DL_CONCAT(head1,head2); | +|LL_DELETE(head,del); | DL_DELETE(head,del); | CDL_DELETE(head,del); +|LL_SORT(head,cmp); | DL_SORT(head,cmp); | CDL_SORT(head,cmp); +|LL_FOREACH(head,elt) {...}| DL_FOREACH(head,elt) {...} | CDL_FOREACH(head,elt) {...} +|LL_FOREACH_SAFE(head,elt,tmp) {...}| DL_FOREACH_SAFE(head,elt,tmp) {...} | CDL_FOREACH_SAFE(head,elt,tmp1,tmp2) {...} +|LL_SEARCH_SCALAR(head,elt,mbr,val);| DL_SEARCH_SCALAR(head,elt,mbr,val); | CDL_SEARCH_SCALAR(head,elt,mbr,val); +|LL_SEARCH(head,elt,like,cmp);| DL_SEARCH(head,elt,like,cmp); | CDL_SEARCH(head,elt,like,cmp); +|=============================================================================== + +'Prepend' means to insert an element in front of the existing list head (if any), +changing the list head to the new element. 'Append' means to add an element at the +end of the list, so it becomes the new tail element. 'Concatenate' takes two +properly constructed lists and appends the second list to the first. (Visual +Studio 2008 does not support `LL_CONCAT` and `DL_CONCAT`, but VS2010 is ok.) + +The 'sort' operation never moves the elements in memory; rather it only adjusts +the list order by altering the `prev` and `next` pointers in each element. Also +the sort operation can change the list head to point to a new element. + +The 'foreach' operation is for easy iteration over the list from the head to the +tail. A usage example is shown below. You can of course just use the `prev` and +`next` pointers directly instead of using the 'foreach' macros. +The 'foreach_safe' operation should be used if you plan to delete any of the list +elements while iterating. + +The 'search' operation is a shortcut for iteration in search of a particular +element. It is not any faster than manually iterating and testing each element. +There are two forms: the "scalar" version searches for an element using a +simple equality test on a given structure member, while the general version takes an +element to which all others in the list will be compared using a `cmp` function. + + +The parameters shown in the table above are explained here: + +head:: + The list head (a pointer to your list element structure). +add:: + A pointer to the list element structure you are adding to the list. +del:: + A pointer to the list element structure you are deleting from the list. +elt:: + A pointer that will be assigned to each list element in succession (see + example) in the case of iteration macros; also, the output pointer from + the search macros. +like:: + An element pointer, having the same type as `elt`, for which the search macro + seeks a match (if found, the match is stored in `elt`). A match is determined + by the given `cmp` function. +cmp:: + pointer to comparison function which accepts two arguments-- these are + pointers to two element structures to be compared. The comparison function + must return an `int` that is negative, zero, or positive, which specifies + whether the first item should sort before, equal to, or after the second item, + respectively. (In other words, the same convention that is used by `strcmp`). + Note that under Visual Studio 2008 you may need to declare the two arguments + as `void *` and then cast them back to their actual types. +tmp:: + A pointer of the same type as `elt`. Used internally. Need not be initialized. +mbr:: + In the scalar search macro, the name of a member within the `elt` structure which + will be tested (using `==`) for equality with the value `val`. +val:: + In the scalar search macro, specifies the value of (of structure member + `field`) of the element being sought. + +Example +~~~~~~~ +This example program reads names from a text file (one name per line), and +appends each name to a doubly-linked list. Then it sorts and prints them. + +.A doubly-linked list +-------------------------------------------------------------------------------- +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include "utlist.h" + +#define BUFLEN 20 + +typedef struct el { + char bname[BUFLEN]; + struct el *next, *prev; +} el; + +int namecmp(el *a, el *b) { + return strcmp(a->bname,b->bname); +} + +el *head = NULL; /* important- initialize to NULL! */ + +int main(int argc, char *argv[]) { + el *name, *elt, *tmp, etmp; + + char linebuf[BUFLEN]; + FILE *file; + + if ( (file = fopen( "test11.dat", "r" )) == NULL ) { + perror("can't open: "); + exit(-1); + } + + while (fgets(linebuf,BUFLEN,file) != NULL) { + if ( (name = (el*)malloc(sizeof(el))) == NULL) exit(-1); + strncpy(name->bname,linebuf,BUFLEN); + DL_APPEND(head, name); + } + DL_SORT(head, namecmp); + DL_FOREACH(head,elt) printf("%s", elt->bname); + + memcpy(&etmp.bname, "WES\n", 5); + DL_SEARCH(head,elt,&etmp,namecmp); + if (elt) printf("found %s\n", elt->bname); + + /* now delete each element, use the safe iterator */ + DL_FOREACH_SAFE(head,elt,tmp) { + DL_DELETE(head,elt); + } + + fclose(file); + + return 0; +} +-------------------------------------------------------------------------------- + +// vim: set nowrap syntax=asciidoc: + diff --git a/src/modules/systemlib/uthash/doc/utstring.txt b/src/modules/systemlib/uthash/doc/utstring.txt new file mode 100644 index 000000000..abfdcd107 --- /dev/null +++ b/src/modules/systemlib/uthash/doc/utstring.txt @@ -0,0 +1,178 @@ +utstring: dynamic string macros for C +===================================== +Troy D. Hanson <thanson@users.sourceforge.net> +v1.9.5, November 2011 + +include::sflogo.txt[] +include::topnav_utstring.txt[] + +Introduction +------------ +include::toc.txt[] + +A set of very basic dynamic string macros for C programs are included with +uthash in `utstring.h`. To use these macros in your own C program, just +copy `utstring.h` into your source directory and use it in your programs. + + #include "utstring.h" + +The dynamic string supports basic operations such as inserting data (including +binary data-- despite its name, utstring is not limited to string content), +concatenation, getting the length and content, and clearing it. The string +<<operations,operations>> are listed below. + +Download +~~~~~~~~ +To download the `utstring.h` header file, follow the link on the +http://uthash.sourceforge.net[uthash home page]. + +BSD licensed +~~~~~~~~~~~~ +This software is made available under the +link:license.html[revised BSD license]. +It is free and open source. + +Platforms +~~~~~~~~~ +The 'utstring' macros have been tested on: + + * Linux, + * Windows, using Visual Studio 2008 and Visual Studio 2010 + +Usage +----- + +Declaration +~~~~~~~~~~~ + +The dynamic string itself has the data type `UT_string`. It is declared like, + + UT_string *str; + +New and free +~~~~~~~~~~~~ +The next step is to create the string using `utstring_new`. Later when you're +done with it, `utstring_free` will free it and all its content. + +Manipulation +~~~~~~~~~~~~ +The `utstring_printf` or `utstring_bincpy` operations insert (copy) data into +the string. To concatenate one utstring to another, use `utstring_concat`. To +clear the content of the string, use `utstring_clear`. The length of the string +is available from `utstring_len`, and its content from `utstring_body`. This +evaluates to a `char*`. The buffer it points to is always null-terminated. +So, it can be used directly with external functions that expect a string. +This automatic null terminator is not counted in the length of the string. + +Samples +~~~~~~~ + +These examples show how to use utstring. + +.Sample 1 +------------------------------------------------------------------------------- +#include <stdio.h> +#include "utstring.h" + +int main() { + UT_string *s; + + utstring_new(s); + utstring_printf(s, "hello world!" ); + printf("%s\n", utstring_body(s)); + + utstring_free(s); + return 0; +} +------------------------------------------------------------------------------- + +The next example is meant to demonstrate that printf 'appends' to the string. +It also shows concatenation. + +.Sample 2 +------------------------------------------------------------------------------- +#include <stdio.h> +#include "utstring.h" + +int main() { + UT_string *s, *t; + + utstring_new(s); + utstring_new(t); + + utstring_printf(s, "hello " ); + utstring_printf(s, "world " ); + + utstring_printf(t, "hi " ); + utstring_printf(t, "there " ); + + utstring_concat(s, t); + printf("length: %u\n", utstring_len(s)); + printf("%s\n", utstring_body(s)); + + utstring_free(s); + utstring_free(t); + return 0; +} +------------------------------------------------------------------------------- + +The last example shows how binary data can be inserted into the string. It also +clears the string and prints new data into it. + +.Sample 3 +------------------------------------------------------------------------------- +#include <stdio.h> +#include "utstring.h" + +int main() { + UT_string *s; + char binary[] = "\xff\xff"; + + utstring_new(s); + utstring_bincpy(s, binary, sizeof(binary)); + printf("length is %u\n", utstring_len(s)); + + utstring_clear(s); + utstring_printf(s,"number %d", 10); + printf("%s\n", utstring_body(s)); + + utstring_free(s); + return 0; +} +------------------------------------------------------------------------------- + +[[operations]] +Reference +--------- +These are the utstring operations. + +Operations +~~~~~~~~~~ + +[width="100%",cols="50<m,40<",grid="none",options="none"] +|=============================================================================== +| utstring_new(s) | allocate a new utstring +| utstring_renew(s) | allocate a new utstring (if s is `NULL`) otherwise clears it +| utstring_free(s) | free an allocated utstring +| utstring_init(s) | init a utstring (non-alloc) +| utstring_done(s) | dispose of a utstring (non-allocd) +| utstring_printf(s,fmt,...) | printf into a utstring (appends) +| utstring_bincpy(s,bin,len) | insert binary data of length len (appends) +| utstring_concat(dst,src) | concatenate src utstring to end of dst utstring +| utstring_clear(s) | clear the content of s (setting its length to 0) +| utstring_len(s) | obtain the length of s as an unsigned integer +| utstring_body(s) | get `char*` to body of s (buffer is always null-terminated) +|=============================================================================== + +Notes +~~~~~ + +1. `utstring_new` and `utstring_free` are used to allocate a new string and free it, + while `utstring_init` and `utstring_done` can be used if the UT_string is already + allocated and just needs to be initialized or have its internal resources + freed. +2. `utstring_printf` is actually a function defined statically in `utstring.h` + rather than a macro. + +// vim: set nowrap syntax=asciidoc: + diff --git a/src/modules/systemlib/uthash/utarray.h b/src/modules/systemlib/uthash/utarray.h new file mode 100644 index 000000000..4ffb630bf --- /dev/null +++ b/src/modules/systemlib/uthash/utarray.h @@ -0,0 +1,233 @@ +/* +Copyright (c) 2008-2012, Troy D. Hanson http://uthash.sourceforge.net +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + +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. +*/ + +/* a dynamic array implementation using macros + * see http://uthash.sourceforge.net/utarray + */ +#ifndef UTARRAY_H +#define UTARRAY_H + +#define UTARRAY_VERSION 1.9.6 + +#ifdef __GNUC__ +#define _UNUSED_ __attribute__ ((__unused__)) +#else +#define _UNUSED_ +#endif + +#include <stddef.h> /* size_t */ +#include <string.h> /* memset, etc */ +#include <stdlib.h> /* exit */ + +#define oom() exit(-1) + +typedef void (ctor_f)(void *dst, const void *src); +typedef void (dtor_f)(void *elt); +typedef void (init_f)(void *elt); +typedef struct { + size_t sz; + init_f *init; + ctor_f *copy; + dtor_f *dtor; +} UT_icd; + +typedef struct { + unsigned i,n;/* i: index of next available slot, n: num slots */ + UT_icd icd; /* initializer, copy and destructor functions */ + char *d; /* n slots of size icd->sz*/ +} UT_array; + +#define utarray_init(a,_icd) do { \ + memset(a,0,sizeof(UT_array)); \ + (a)->icd=*_icd; \ +} while(0) + +#define utarray_done(a) do { \ + if ((a)->n) { \ + if ((a)->icd.dtor) { \ + size_t _ut_i; \ + for(_ut_i=0; _ut_i < (a)->i; _ut_i++) { \ + (a)->icd.dtor(utarray_eltptr(a,_ut_i)); \ + } \ + } \ + free((a)->d); \ + } \ + (a)->n=0; \ +} while(0) + +#define utarray_new(a,_icd) do { \ + a=(UT_array*)malloc(sizeof(UT_array)); \ + utarray_init(a,_icd); \ +} while(0) + +#define utarray_free(a) do { \ + utarray_done(a); \ + free(a); \ +} while(0) + +#define utarray_reserve(a,by) do { \ + if (((a)->i+by) > ((a)->n)) { \ + while(((a)->i+by) > ((a)->n)) { (a)->n = ((a)->n ? (2*(a)->n) : 8); } \ + if ( ((a)->d=(char*)realloc((a)->d, (a)->n*(a)->icd.sz)) == NULL) oom(); \ + } \ +} while(0) + +#define utarray_push_back(a,p) do { \ + utarray_reserve(a,1); \ + if ((a)->icd.copy) { (a)->icd.copy( _utarray_eltptr(a,(a)->i++), p); } \ + else { memcpy(_utarray_eltptr(a,(a)->i++), p, (a)->icd.sz); }; \ +} while(0) + +#define utarray_pop_back(a) do { \ + if ((a)->icd.dtor) { (a)->icd.dtor( _utarray_eltptr(a,--((a)->i))); } \ + else { (a)->i--; } \ +} while(0) + +#define utarray_extend_back(a) do { \ + utarray_reserve(a,1); \ + if ((a)->icd.init) { (a)->icd.init(_utarray_eltptr(a,(a)->i)); } \ + else { memset(_utarray_eltptr(a,(a)->i),0,(a)->icd.sz); } \ + (a)->i++; \ +} while(0) + +#define utarray_len(a) ((a)->i) + +#define utarray_eltptr(a,j) (((j) < (a)->i) ? _utarray_eltptr(a,j) : NULL) +#define _utarray_eltptr(a,j) ((char*)((a)->d + ((a)->icd.sz*(j) ))) + +#define utarray_insert(a,p,j) do { \ + utarray_reserve(a,1); \ + if (j > (a)->i) break; \ + if ((j) < (a)->i) { \ + memmove( _utarray_eltptr(a,(j)+1), _utarray_eltptr(a,j), \ + ((a)->i - (j))*((a)->icd.sz)); \ + } \ + if ((a)->icd.copy) { (a)->icd.copy( _utarray_eltptr(a,j), p); } \ + else { memcpy(_utarray_eltptr(a,j), p, (a)->icd.sz); }; \ + (a)->i++; \ +} while(0) + +#define utarray_inserta(a,w,j) do { \ + if (utarray_len(w) == 0) break; \ + if (j > (a)->i) break; \ + utarray_reserve(a,utarray_len(w)); \ + if ((j) < (a)->i) { \ + memmove(_utarray_eltptr(a,(j)+utarray_len(w)), \ + _utarray_eltptr(a,j), \ + ((a)->i - (j))*((a)->icd.sz)); \ + } \ + if ((a)->icd.copy) { \ + size_t _ut_i; \ + for(_ut_i=0;_ut_i<(w)->i;_ut_i++) { \ + (a)->icd.copy(_utarray_eltptr(a,j+_ut_i), _utarray_eltptr(w,_ut_i)); \ + } \ + } else { \ + memcpy(_utarray_eltptr(a,j), _utarray_eltptr(w,0), \ + utarray_len(w)*((a)->icd.sz)); \ + } \ + (a)->i += utarray_len(w); \ +} while(0) + +#define utarray_resize(dst,num) do { \ + size_t _ut_i; \ + if (dst->i > (size_t)(num)) { \ + if ((dst)->icd.dtor) { \ + for(_ut_i=num; _ut_i < dst->i; _ut_i++) { \ + (dst)->icd.dtor(utarray_eltptr(dst,_ut_i)); \ + } \ + } \ + } else if (dst->i < (size_t)(num)) { \ + utarray_reserve(dst,num-dst->i); \ + if ((dst)->icd.init) { \ + for(_ut_i=dst->i; _ut_i < num; _ut_i++) { \ + (dst)->icd.init(utarray_eltptr(dst,_ut_i)); \ + } \ + } else { \ + memset(_utarray_eltptr(dst,dst->i),0,(dst)->icd.sz*(num-dst->i)); \ + } \ + } \ + dst->i = num; \ +} while(0) + +#define utarray_concat(dst,src) do { \ + utarray_inserta((dst),(src),utarray_len(dst)); \ +} while(0) + +#define utarray_erase(a,pos,len) do { \ + if ((a)->icd.dtor) { \ + size_t _ut_i; \ + for(_ut_i=0; _ut_i < len; _ut_i++) { \ + (a)->icd.dtor(utarray_eltptr((a),pos+_ut_i)); \ + } \ + } \ + if ((a)->i > (pos+len)) { \ + memmove( _utarray_eltptr((a),pos), _utarray_eltptr((a),pos+len), \ + (((a)->i)-(pos+len))*((a)->icd.sz)); \ + } \ + (a)->i -= (len); \ +} while(0) + +#define utarray_renew(a,u) do { \ + if (a) utarray_clear(a); \ + else utarray_new((a),(u)); \ +} while(0) + +#define utarray_clear(a) do { \ + if ((a)->i > 0) { \ + if ((a)->icd.dtor) { \ + size_t _ut_i; \ + for(_ut_i=0; _ut_i < (a)->i; _ut_i++) { \ + (a)->icd.dtor(utarray_eltptr(a,_ut_i)); \ + } \ + } \ + (a)->i = 0; \ + } \ +} while(0) + +#define utarray_sort(a,cmp) do { \ + qsort((a)->d, (a)->i, (a)->icd.sz, cmp); \ +} while(0) + +#define utarray_find(a,v,cmp) bsearch((v),(a)->d,(a)->i,(a)->icd.sz,cmp) + +#define utarray_front(a) (((a)->i) ? (_utarray_eltptr(a,0)) : NULL) +#define utarray_next(a,e) (((e)==NULL) ? utarray_front(a) : ((((a)->i) > (utarray_eltidx(a,e)+1)) ? _utarray_eltptr(a,utarray_eltidx(a,e)+1) : NULL)) +#define utarray_prev(a,e) (((e)==NULL) ? utarray_back(a) : ((utarray_eltidx(a,e) > 0) ? _utarray_eltptr(a,utarray_eltidx(a,e)-1) : NULL)) +#define utarray_back(a) (((a)->i) ? (_utarray_eltptr(a,(a)->i-1)) : NULL) +#define utarray_eltidx(a,e) (((char*)(e) >= (char*)((a)->d)) ? (((char*)(e) - (char*)((a)->d))/(a)->icd.sz) : -1) + +/* last we pre-define a few icd for common utarrays of ints and strings */ +static void utarray_str_cpy(void *dst, const void *src) { + char **_src = (char**)src, **_dst = (char**)dst; + *_dst = (*_src == NULL) ? NULL : strdup(*_src); +} +static void utarray_str_dtor(void *elt) { + char **eltc = (char**)elt; + if (*eltc) free(*eltc); +} +static const UT_icd ut_str_icd _UNUSED_ = {sizeof(char*),NULL,utarray_str_cpy,utarray_str_dtor}; +static const UT_icd ut_int_icd _UNUSED_ = {sizeof(int),NULL,NULL,NULL}; +static const UT_icd ut_ptr_icd _UNUSED_ = {sizeof(void*),NULL,NULL,NULL}; + + +#endif /* UTARRAY_H */ diff --git a/src/modules/systemlib/uthash/uthash.h b/src/modules/systemlib/uthash/uthash.h new file mode 100644 index 000000000..9f83fc34f --- /dev/null +++ b/src/modules/systemlib/uthash/uthash.h @@ -0,0 +1,915 @@ +/* +Copyright (c) 2003-2012, Troy D. Hanson http://uthash.sourceforge.net +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + +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. +*/ + +#ifndef UTHASH_H +#define UTHASH_H + +#include <string.h> /* memcmp,strlen */ +#include <stddef.h> /* ptrdiff_t */ +#include <stdlib.h> /* exit() */ + +/* These macros use decltype or the earlier __typeof GNU extension. + As decltype is only available in newer compilers (VS2010 or gcc 4.3+ + when compiling c++ source) this code uses whatever method is needed + or, for VS2008 where neither is available, uses casting workarounds. */ +#ifdef _MSC_VER /* MS compiler */ +#if _MSC_VER >= 1600 && defined(__cplusplus) /* VS2010 or newer in C++ mode */ +#define DECLTYPE(x) (decltype(x)) +#else /* VS2008 or older (or VS2010 in C mode) */ +#define NO_DECLTYPE +#define DECLTYPE(x) +#endif +#else /* GNU, Sun and other compilers */ +#define DECLTYPE(x) (__typeof(x)) +#endif + +#ifdef NO_DECLTYPE +#define DECLTYPE_ASSIGN(dst,src) \ +do { \ + char **_da_dst = (char**)(&(dst)); \ + *_da_dst = (char*)(src); \ +} while(0) +#else +#define DECLTYPE_ASSIGN(dst,src) \ +do { \ + (dst) = DECLTYPE(dst)(src); \ +} while(0) +#endif + +/* a number of the hash function use uint32_t which isn't defined on win32 */ +#ifdef _MSC_VER +typedef unsigned int uint32_t; +typedef unsigned char uint8_t; +#else +#include <inttypes.h> /* uint32_t */ +#endif + +#define UTHASH_VERSION 1.9.6 + +#ifndef uthash_fatal +#define uthash_fatal(msg) exit(-1) /* fatal error (out of memory,etc) */ +#endif +#ifndef uthash_malloc +#define uthash_malloc(sz) malloc(sz) /* malloc fcn */ +#endif +#ifndef uthash_free +#define uthash_free(ptr,sz) free(ptr) /* free fcn */ +#endif + +#ifndef uthash_noexpand_fyi +#define uthash_noexpand_fyi(tbl) /* can be defined to log noexpand */ +#endif +#ifndef uthash_expand_fyi +#define uthash_expand_fyi(tbl) /* can be defined to log expands */ +#endif + +/* initial number of buckets */ +#define HASH_INITIAL_NUM_BUCKETS 32 /* initial number of buckets */ +#define HASH_INITIAL_NUM_BUCKETS_LOG2 5 /* lg2 of initial number of buckets */ +#define HASH_BKT_CAPACITY_THRESH 10 /* expand when bucket count reaches */ + +/* calculate the element whose hash handle address is hhe */ +#define ELMT_FROM_HH(tbl,hhp) ((void*)(((char*)(hhp)) - ((tbl)->hho))) + +#define HASH_FIND(hh,head,keyptr,keylen,out) \ +do { \ + unsigned _hf_bkt,_hf_hashv; \ + out=NULL; \ + if (head) { \ + HASH_FCN(keyptr,keylen, (head)->hh.tbl->num_buckets, _hf_hashv, _hf_bkt); \ + if (HASH_BLOOM_TEST((head)->hh.tbl, _hf_hashv)) { \ + HASH_FIND_IN_BKT((head)->hh.tbl, hh, (head)->hh.tbl->buckets[ _hf_bkt ], \ + keyptr,keylen,out); \ + } \ + } \ +} while (0) + +#ifdef HASH_BLOOM +#define HASH_BLOOM_BITLEN (1ULL << HASH_BLOOM) +#define HASH_BLOOM_BYTELEN (HASH_BLOOM_BITLEN/8) + ((HASH_BLOOM_BITLEN%8) ? 1:0) +#define HASH_BLOOM_MAKE(tbl) \ +do { \ + (tbl)->bloom_nbits = HASH_BLOOM; \ + (tbl)->bloom_bv = (uint8_t*)uthash_malloc(HASH_BLOOM_BYTELEN); \ + if (!((tbl)->bloom_bv)) { uthash_fatal( "out of memory"); } \ + memset((tbl)->bloom_bv, 0, HASH_BLOOM_BYTELEN); \ + (tbl)->bloom_sig = HASH_BLOOM_SIGNATURE; \ +} while (0) + +#define HASH_BLOOM_FREE(tbl) \ +do { \ + uthash_free((tbl)->bloom_bv, HASH_BLOOM_BYTELEN); \ +} while (0) + +#define HASH_BLOOM_BITSET(bv,idx) (bv[(idx)/8] |= (1U << ((idx)%8))) +#define HASH_BLOOM_BITTEST(bv,idx) (bv[(idx)/8] & (1U << ((idx)%8))) + +#define HASH_BLOOM_ADD(tbl,hashv) \ + HASH_BLOOM_BITSET((tbl)->bloom_bv, (hashv & (uint32_t)((1ULL << (tbl)->bloom_nbits) - 1))) + +#define HASH_BLOOM_TEST(tbl,hashv) \ + HASH_BLOOM_BITTEST((tbl)->bloom_bv, (hashv & (uint32_t)((1ULL << (tbl)->bloom_nbits) - 1))) + +#else +#define HASH_BLOOM_MAKE(tbl) +#define HASH_BLOOM_FREE(tbl) +#define HASH_BLOOM_ADD(tbl,hashv) +#define HASH_BLOOM_TEST(tbl,hashv) (1) +#endif + +#define HASH_MAKE_TABLE(hh,head) \ +do { \ + (head)->hh.tbl = (UT_hash_table*)uthash_malloc( \ + sizeof(UT_hash_table)); \ + if (!((head)->hh.tbl)) { uthash_fatal( "out of memory"); } \ + memset((head)->hh.tbl, 0, sizeof(UT_hash_table)); \ + (head)->hh.tbl->tail = &((head)->hh); \ + (head)->hh.tbl->num_buckets = HASH_INITIAL_NUM_BUCKETS; \ + (head)->hh.tbl->log2_num_buckets = HASH_INITIAL_NUM_BUCKETS_LOG2; \ + (head)->hh.tbl->hho = (char*)(&(head)->hh) - (char*)(head); \ + (head)->hh.tbl->buckets = (UT_hash_bucket*)uthash_malloc( \ + HASH_INITIAL_NUM_BUCKETS*sizeof(struct UT_hash_bucket)); \ + if (! (head)->hh.tbl->buckets) { uthash_fatal( "out of memory"); } \ + memset((head)->hh.tbl->buckets, 0, \ + HASH_INITIAL_NUM_BUCKETS*sizeof(struct UT_hash_bucket)); \ + HASH_BLOOM_MAKE((head)->hh.tbl); \ + (head)->hh.tbl->signature = HASH_SIGNATURE; \ +} while(0) + +#define HASH_ADD(hh,head,fieldname,keylen_in,add) \ + HASH_ADD_KEYPTR(hh,head,&((add)->fieldname),keylen_in,add) + +#define HASH_ADD_KEYPTR(hh,head,keyptr,keylen_in,add) \ +do { \ + unsigned _ha_bkt; \ + (add)->hh.next = NULL; \ + (add)->hh.key = (char*)keyptr; \ + (add)->hh.keylen = (unsigned)keylen_in; \ + if (!(head)) { \ + head = (add); \ + (head)->hh.prev = NULL; \ + HASH_MAKE_TABLE(hh,head); \ + } else { \ + (head)->hh.tbl->tail->next = (add); \ + (add)->hh.prev = ELMT_FROM_HH((head)->hh.tbl, (head)->hh.tbl->tail); \ + (head)->hh.tbl->tail = &((add)->hh); \ + } \ + (head)->hh.tbl->num_items++; \ + (add)->hh.tbl = (head)->hh.tbl; \ + HASH_FCN(keyptr,keylen_in, (head)->hh.tbl->num_buckets, \ + (add)->hh.hashv, _ha_bkt); \ + HASH_ADD_TO_BKT((head)->hh.tbl->buckets[_ha_bkt],&(add)->hh); \ + HASH_BLOOM_ADD((head)->hh.tbl,(add)->hh.hashv); \ + HASH_EMIT_KEY(hh,head,keyptr,keylen_in); \ + HASH_FSCK(hh,head); \ +} while(0) + +#define HASH_TO_BKT( hashv, num_bkts, bkt ) \ +do { \ + bkt = ((hashv) & ((num_bkts) - 1)); \ +} while(0) + +/* delete "delptr" from the hash table. + * "the usual" patch-up process for the app-order doubly-linked-list. + * The use of _hd_hh_del below deserves special explanation. + * These used to be expressed using (delptr) but that led to a bug + * if someone used the same symbol for the head and deletee, like + * HASH_DELETE(hh,users,users); + * We want that to work, but by changing the head (users) below + * we were forfeiting our ability to further refer to the deletee (users) + * in the patch-up process. Solution: use scratch space to + * copy the deletee pointer, then the latter references are via that + * scratch pointer rather than through the repointed (users) symbol. + */ +#define HASH_DELETE(hh,head,delptr) \ +do { \ + unsigned _hd_bkt; \ + struct UT_hash_handle *_hd_hh_del; \ + if ( ((delptr)->hh.prev == NULL) && ((delptr)->hh.next == NULL) ) { \ + uthash_free((head)->hh.tbl->buckets, \ + (head)->hh.tbl->num_buckets*sizeof(struct UT_hash_bucket) ); \ + HASH_BLOOM_FREE((head)->hh.tbl); \ + uthash_free((head)->hh.tbl, sizeof(UT_hash_table)); \ + head = NULL; \ + } else { \ + _hd_hh_del = &((delptr)->hh); \ + if ((delptr) == ELMT_FROM_HH((head)->hh.tbl,(head)->hh.tbl->tail)) { \ + (head)->hh.tbl->tail = \ + (UT_hash_handle*)((char*)((delptr)->hh.prev) + \ + (head)->hh.tbl->hho); \ + } \ + if ((delptr)->hh.prev) { \ + ((UT_hash_handle*)((char*)((delptr)->hh.prev) + \ + (head)->hh.tbl->hho))->next = (delptr)->hh.next; \ + } else { \ + DECLTYPE_ASSIGN(head,(delptr)->hh.next); \ + } \ + if (_hd_hh_del->next) { \ + ((UT_hash_handle*)((char*)_hd_hh_del->next + \ + (head)->hh.tbl->hho))->prev = \ + _hd_hh_del->prev; \ + } \ + HASH_TO_BKT( _hd_hh_del->hashv, (head)->hh.tbl->num_buckets, _hd_bkt); \ + HASH_DEL_IN_BKT(hh,(head)->hh.tbl->buckets[_hd_bkt], _hd_hh_del); \ + (head)->hh.tbl->num_items--; \ + } \ + HASH_FSCK(hh,head); \ +} while (0) + + +/* convenience forms of HASH_FIND/HASH_ADD/HASH_DEL */ +#define HASH_FIND_STR(head,findstr,out) \ + HASH_FIND(hh,head,findstr,strlen(findstr),out) +#define HASH_ADD_STR(head,strfield,add) \ + HASH_ADD(hh,head,strfield,strlen(add->strfield),add) +#define HASH_FIND_INT(head,findint,out) \ + HASH_FIND(hh,head,findint,sizeof(int),out) +#define HASH_ADD_INT(head,intfield,add) \ + HASH_ADD(hh,head,intfield,sizeof(int),add) +#define HASH_FIND_PTR(head,findptr,out) \ + HASH_FIND(hh,head,findptr,sizeof(void *),out) +#define HASH_ADD_PTR(head,ptrfield,add) \ + HASH_ADD(hh,head,ptrfield,sizeof(void *),add) +#define HASH_DEL(head,delptr) \ + HASH_DELETE(hh,head,delptr) + +/* HASH_FSCK checks hash integrity on every add/delete when HASH_DEBUG is defined. + * This is for uthash developer only; it compiles away if HASH_DEBUG isn't defined. + */ +#ifdef HASH_DEBUG +#define HASH_OOPS(...) do { fprintf(stderr,__VA_ARGS__); exit(-1); } while (0) +#define HASH_FSCK(hh,head) \ +do { \ + unsigned _bkt_i; \ + unsigned _count, _bkt_count; \ + char *_prev; \ + struct UT_hash_handle *_thh; \ + if (head) { \ + _count = 0; \ + for( _bkt_i = 0; _bkt_i < (head)->hh.tbl->num_buckets; _bkt_i++) { \ + _bkt_count = 0; \ + _thh = (head)->hh.tbl->buckets[_bkt_i].hh_head; \ + _prev = NULL; \ + while (_thh) { \ + if (_prev != (char*)(_thh->hh_prev)) { \ + HASH_OOPS("invalid hh_prev %p, actual %p\n", \ + _thh->hh_prev, _prev ); \ + } \ + _bkt_count++; \ + _prev = (char*)(_thh); \ + _thh = _thh->hh_next; \ + } \ + _count += _bkt_count; \ + if ((head)->hh.tbl->buckets[_bkt_i].count != _bkt_count) { \ + HASH_OOPS("invalid bucket count %d, actual %d\n", \ + (head)->hh.tbl->buckets[_bkt_i].count, _bkt_count); \ + } \ + } \ + if (_count != (head)->hh.tbl->num_items) { \ + HASH_OOPS("invalid hh item count %d, actual %d\n", \ + (head)->hh.tbl->num_items, _count ); \ + } \ + /* traverse hh in app order; check next/prev integrity, count */ \ + _count = 0; \ + _prev = NULL; \ + _thh = &(head)->hh; \ + while (_thh) { \ + _count++; \ + if (_prev !=(char*)(_thh->prev)) { \ + HASH_OOPS("invalid prev %p, actual %p\n", \ + _thh->prev, _prev ); \ + } \ + _prev = (char*)ELMT_FROM_HH((head)->hh.tbl, _thh); \ + _thh = ( _thh->next ? (UT_hash_handle*)((char*)(_thh->next) + \ + (head)->hh.tbl->hho) : NULL ); \ + } \ + if (_count != (head)->hh.tbl->num_items) { \ + HASH_OOPS("invalid app item count %d, actual %d\n", \ + (head)->hh.tbl->num_items, _count ); \ + } \ + } \ +} while (0) +#else +#define HASH_FSCK(hh,head) +#endif + +/* When compiled with -DHASH_EMIT_KEYS, length-prefixed keys are emitted to + * the descriptor to which this macro is defined for tuning the hash function. + * The app can #include <unistd.h> to get the prototype for write(2). */ +#ifdef HASH_EMIT_KEYS +#define HASH_EMIT_KEY(hh,head,keyptr,fieldlen) \ +do { \ + unsigned _klen = fieldlen; \ + write(HASH_EMIT_KEYS, &_klen, sizeof(_klen)); \ + write(HASH_EMIT_KEYS, keyptr, fieldlen); \ +} while (0) +#else +#define HASH_EMIT_KEY(hh,head,keyptr,fieldlen) +#endif + +/* default to Jenkin's hash unless overridden e.g. DHASH_FUNCTION=HASH_SAX */ +#ifdef HASH_FUNCTION +#define HASH_FCN HASH_FUNCTION +#else +#define HASH_FCN HASH_JEN +#endif + +/* The Bernstein hash function, used in Perl prior to v5.6 */ +#define HASH_BER(key,keylen,num_bkts,hashv,bkt) \ +do { \ + unsigned _hb_keylen=keylen; \ + char *_hb_key=(char*)(key); \ + (hashv) = 0; \ + while (_hb_keylen--) { (hashv) = ((hashv) * 33) + *_hb_key++; } \ + bkt = (hashv) & (num_bkts-1); \ +} while (0) + + +/* SAX/FNV/OAT/JEN hash functions are macro variants of those listed at + * http://eternallyconfuzzled.com/tuts/algorithms/jsw_tut_hashing.aspx */ +#define HASH_SAX(key,keylen,num_bkts,hashv,bkt) \ +do { \ + unsigned _sx_i; \ + char *_hs_key=(char*)(key); \ + hashv = 0; \ + for(_sx_i=0; _sx_i < keylen; _sx_i++) \ + hashv ^= (hashv << 5) + (hashv >> 2) + _hs_key[_sx_i]; \ + bkt = hashv & (num_bkts-1); \ +} while (0) + +#define HASH_FNV(key,keylen,num_bkts,hashv,bkt) \ +do { \ + unsigned _fn_i; \ + char *_hf_key=(char*)(key); \ + hashv = 2166136261UL; \ + for(_fn_i=0; _fn_i < keylen; _fn_i++) \ + hashv = (hashv * 16777619) ^ _hf_key[_fn_i]; \ + bkt = hashv & (num_bkts-1); \ +} while(0) + +#define HASH_OAT(key,keylen,num_bkts,hashv,bkt) \ +do { \ + unsigned _ho_i; \ + char *_ho_key=(char*)(key); \ + hashv = 0; \ + for(_ho_i=0; _ho_i < keylen; _ho_i++) { \ + hashv += _ho_key[_ho_i]; \ + hashv += (hashv << 10); \ + hashv ^= (hashv >> 6); \ + } \ + hashv += (hashv << 3); \ + hashv ^= (hashv >> 11); \ + hashv += (hashv << 15); \ + bkt = hashv & (num_bkts-1); \ +} while(0) + +#define HASH_JEN_MIX(a,b,c) \ +do { \ + a -= b; a -= c; a ^= ( c >> 13 ); \ + b -= c; b -= a; b ^= ( a << 8 ); \ + c -= a; c -= b; c ^= ( b >> 13 ); \ + a -= b; a -= c; a ^= ( c >> 12 ); \ + b -= c; b -= a; b ^= ( a << 16 ); \ + c -= a; c -= b; c ^= ( b >> 5 ); \ + a -= b; a -= c; a ^= ( c >> 3 ); \ + b -= c; b -= a; b ^= ( a << 10 ); \ + c -= a; c -= b; c ^= ( b >> 15 ); \ +} while (0) + +#define HASH_JEN(key,keylen,num_bkts,hashv,bkt) \ +do { \ + unsigned _hj_i,_hj_j,_hj_k; \ + char *_hj_key=(char*)(key); \ + hashv = 0xfeedbeef; \ + _hj_i = _hj_j = 0x9e3779b9; \ + _hj_k = (unsigned)keylen; \ + while (_hj_k >= 12) { \ + _hj_i += (_hj_key[0] + ( (unsigned)_hj_key[1] << 8 ) \ + + ( (unsigned)_hj_key[2] << 16 ) \ + + ( (unsigned)_hj_key[3] << 24 ) ); \ + _hj_j += (_hj_key[4] + ( (unsigned)_hj_key[5] << 8 ) \ + + ( (unsigned)_hj_key[6] << 16 ) \ + + ( (unsigned)_hj_key[7] << 24 ) ); \ + hashv += (_hj_key[8] + ( (unsigned)_hj_key[9] << 8 ) \ + + ( (unsigned)_hj_key[10] << 16 ) \ + + ( (unsigned)_hj_key[11] << 24 ) ); \ + \ + HASH_JEN_MIX(_hj_i, _hj_j, hashv); \ + \ + _hj_key += 12; \ + _hj_k -= 12; \ + } \ + hashv += keylen; \ + switch ( _hj_k ) { \ + case 11: hashv += ( (unsigned)_hj_key[10] << 24 ); \ + case 10: hashv += ( (unsigned)_hj_key[9] << 16 ); \ + case 9: hashv += ( (unsigned)_hj_key[8] << 8 ); \ + case 8: _hj_j += ( (unsigned)_hj_key[7] << 24 ); \ + case 7: _hj_j += ( (unsigned)_hj_key[6] << 16 ); \ + case 6: _hj_j += ( (unsigned)_hj_key[5] << 8 ); \ + case 5: _hj_j += _hj_key[4]; \ + case 4: _hj_i += ( (unsigned)_hj_key[3] << 24 ); \ + case 3: _hj_i += ( (unsigned)_hj_key[2] << 16 ); \ + case 2: _hj_i += ( (unsigned)_hj_key[1] << 8 ); \ + case 1: _hj_i += _hj_key[0]; \ + } \ + HASH_JEN_MIX(_hj_i, _hj_j, hashv); \ + bkt = hashv & (num_bkts-1); \ +} while(0) + +/* The Paul Hsieh hash function */ +#undef get16bits +#if (defined(__GNUC__) && defined(__i386__)) || defined(__WATCOMC__) \ + || defined(_MSC_VER) || defined (__BORLANDC__) || defined (__TURBOC__) +#define get16bits(d) (*((const uint16_t *) (d))) +#endif + +#if !defined (get16bits) +#define get16bits(d) ((((uint32_t)(((const uint8_t *)(d))[1])) << 8) \ + +(uint32_t)(((const uint8_t *)(d))[0]) ) +#endif +#define HASH_SFH(key,keylen,num_bkts,hashv,bkt) \ +do { \ + char *_sfh_key=(char*)(key); \ + uint32_t _sfh_tmp, _sfh_len = keylen; \ + \ + int _sfh_rem = _sfh_len & 3; \ + _sfh_len >>= 2; \ + hashv = 0xcafebabe; \ + \ + /* Main loop */ \ + for (;_sfh_len > 0; _sfh_len--) { \ + hashv += get16bits (_sfh_key); \ + _sfh_tmp = (get16bits (_sfh_key+2) << 11) ^ hashv; \ + hashv = (hashv << 16) ^ _sfh_tmp; \ + _sfh_key += 2*sizeof (uint16_t); \ + hashv += hashv >> 11; \ + } \ + \ + /* Handle end cases */ \ + switch (_sfh_rem) { \ + case 3: hashv += get16bits (_sfh_key); \ + hashv ^= hashv << 16; \ + hashv ^= _sfh_key[sizeof (uint16_t)] << 18; \ + hashv += hashv >> 11; \ + break; \ + case 2: hashv += get16bits (_sfh_key); \ + hashv ^= hashv << 11; \ + hashv += hashv >> 17; \ + break; \ + case 1: hashv += *_sfh_key; \ + hashv ^= hashv << 10; \ + hashv += hashv >> 1; \ + } \ + \ + /* Force "avalanching" of final 127 bits */ \ + hashv ^= hashv << 3; \ + hashv += hashv >> 5; \ + hashv ^= hashv << 4; \ + hashv += hashv >> 17; \ + hashv ^= hashv << 25; \ + hashv += hashv >> 6; \ + bkt = hashv & (num_bkts-1); \ +} while(0) + +#ifdef HASH_USING_NO_STRICT_ALIASING +/* The MurmurHash exploits some CPU's (x86,x86_64) tolerance for unaligned reads. + * For other types of CPU's (e.g. Sparc) an unaligned read causes a bus error. + * MurmurHash uses the faster approach only on CPU's where we know it's safe. + * + * Note the preprocessor built-in defines can be emitted using: + * + * gcc -m64 -dM -E - < /dev/null (on gcc) + * cc -## a.c (where a.c is a simple test file) (Sun Studio) + */ +#if (defined(__i386__) || defined(__x86_64__)) +#define MUR_GETBLOCK(p,i) p[i] +#else /* non intel */ +#define MUR_PLUS0_ALIGNED(p) (((unsigned long)p & 0x3) == 0) +#define MUR_PLUS1_ALIGNED(p) (((unsigned long)p & 0x3) == 1) +#define MUR_PLUS2_ALIGNED(p) (((unsigned long)p & 0x3) == 2) +#define MUR_PLUS3_ALIGNED(p) (((unsigned long)p & 0x3) == 3) +#define WP(p) ((uint32_t*)((unsigned long)(p) & ~3UL)) +#if (defined(__BIG_ENDIAN__) || defined(SPARC) || defined(__ppc__) || defined(__ppc64__)) +#define MUR_THREE_ONE(p) ((((*WP(p))&0x00ffffff) << 8) | (((*(WP(p)+1))&0xff000000) >> 24)) +#define MUR_TWO_TWO(p) ((((*WP(p))&0x0000ffff) <<16) | (((*(WP(p)+1))&0xffff0000) >> 16)) +#define MUR_ONE_THREE(p) ((((*WP(p))&0x000000ff) <<24) | (((*(WP(p)+1))&0xffffff00) >> 8)) +#else /* assume little endian non-intel */ +#define MUR_THREE_ONE(p) ((((*WP(p))&0xffffff00) >> 8) | (((*(WP(p)+1))&0x000000ff) << 24)) +#define MUR_TWO_TWO(p) ((((*WP(p))&0xffff0000) >>16) | (((*(WP(p)+1))&0x0000ffff) << 16)) +#define MUR_ONE_THREE(p) ((((*WP(p))&0xff000000) >>24) | (((*(WP(p)+1))&0x00ffffff) << 8)) +#endif +#define MUR_GETBLOCK(p,i) (MUR_PLUS0_ALIGNED(p) ? ((p)[i]) : \ + (MUR_PLUS1_ALIGNED(p) ? MUR_THREE_ONE(p) : \ + (MUR_PLUS2_ALIGNED(p) ? MUR_TWO_TWO(p) : \ + MUR_ONE_THREE(p)))) +#endif +#define MUR_ROTL32(x,r) (((x) << (r)) | ((x) >> (32 - (r)))) +#define MUR_FMIX(_h) \ +do { \ + _h ^= _h >> 16; \ + _h *= 0x85ebca6b; \ + _h ^= _h >> 13; \ + _h *= 0xc2b2ae35l; \ + _h ^= _h >> 16; \ +} while(0) + +#define HASH_MUR(key,keylen,num_bkts,hashv,bkt) \ +do { \ + const uint8_t *_mur_data = (const uint8_t*)(key); \ + const int _mur_nblocks = (keylen) / 4; \ + uint32_t _mur_h1 = 0xf88D5353; \ + uint32_t _mur_c1 = 0xcc9e2d51; \ + uint32_t _mur_c2 = 0x1b873593; \ + const uint32_t *_mur_blocks = (const uint32_t*)(_mur_data+_mur_nblocks*4); \ + int _mur_i; \ + for(_mur_i = -_mur_nblocks; _mur_i; _mur_i++) { \ + uint32_t _mur_k1 = MUR_GETBLOCK(_mur_blocks,_mur_i); \ + _mur_k1 *= _mur_c1; \ + _mur_k1 = MUR_ROTL32(_mur_k1,15); \ + _mur_k1 *= _mur_c2; \ + \ + _mur_h1 ^= _mur_k1; \ + _mur_h1 = MUR_ROTL32(_mur_h1,13); \ + _mur_h1 = _mur_h1*5+0xe6546b64; \ + } \ + const uint8_t *_mur_tail = (const uint8_t*)(_mur_data + _mur_nblocks*4); \ + uint32_t _mur_k1=0; \ + switch((keylen) & 3) { \ + case 3: _mur_k1 ^= _mur_tail[2] << 16; \ + case 2: _mur_k1 ^= _mur_tail[1] << 8; \ + case 1: _mur_k1 ^= _mur_tail[0]; \ + _mur_k1 *= _mur_c1; \ + _mur_k1 = MUR_ROTL32(_mur_k1,15); \ + _mur_k1 *= _mur_c2; \ + _mur_h1 ^= _mur_k1; \ + } \ + _mur_h1 ^= (keylen); \ + MUR_FMIX(_mur_h1); \ + hashv = _mur_h1; \ + bkt = hashv & (num_bkts-1); \ +} while(0) +#endif /* HASH_USING_NO_STRICT_ALIASING */ + +/* key comparison function; return 0 if keys equal */ +#define HASH_KEYCMP(a,b,len) memcmp(a,b,len) + +/* iterate over items in a known bucket to find desired item */ +#define HASH_FIND_IN_BKT(tbl,hh,head,keyptr,keylen_in,out) \ +do { \ + if (head.hh_head) DECLTYPE_ASSIGN(out,ELMT_FROM_HH(tbl,head.hh_head)); \ + else out=NULL; \ + while (out) { \ + if ((out)->hh.keylen == keylen_in) { \ + if ((HASH_KEYCMP((out)->hh.key,keyptr,keylen_in)) == 0) break; \ + } \ + if ((out)->hh.hh_next) DECLTYPE_ASSIGN(out,ELMT_FROM_HH(tbl,(out)->hh.hh_next)); \ + else out = NULL; \ + } \ +} while(0) + +/* add an item to a bucket */ +#define HASH_ADD_TO_BKT(head,addhh) \ +do { \ + head.count++; \ + (addhh)->hh_next = head.hh_head; \ + (addhh)->hh_prev = NULL; \ + if (head.hh_head) { (head).hh_head->hh_prev = (addhh); } \ + (head).hh_head=addhh; \ + if (head.count >= ((head.expand_mult+1) * HASH_BKT_CAPACITY_THRESH) \ + && (addhh)->tbl->noexpand != 1) { \ + HASH_EXPAND_BUCKETS((addhh)->tbl); \ + } \ +} while(0) + +/* remove an item from a given bucket */ +#define HASH_DEL_IN_BKT(hh,head,hh_del) \ + (head).count--; \ + if ((head).hh_head == hh_del) { \ + (head).hh_head = hh_del->hh_next; \ + } \ + if (hh_del->hh_prev) { \ + hh_del->hh_prev->hh_next = hh_del->hh_next; \ + } \ + if (hh_del->hh_next) { \ + hh_del->hh_next->hh_prev = hh_del->hh_prev; \ + } + +/* Bucket expansion has the effect of doubling the number of buckets + * and redistributing the items into the new buckets. Ideally the + * items will distribute more or less evenly into the new buckets + * (the extent to which this is true is a measure of the quality of + * the hash function as it applies to the key domain). + * + * With the items distributed into more buckets, the chain length + * (item count) in each bucket is reduced. Thus by expanding buckets + * the hash keeps a bound on the chain length. This bounded chain + * length is the essence of how a hash provides constant time lookup. + * + * The calculation of tbl->ideal_chain_maxlen below deserves some + * explanation. First, keep in mind that we're calculating the ideal + * maximum chain length based on the *new* (doubled) bucket count. + * In fractions this is just n/b (n=number of items,b=new num buckets). + * Since the ideal chain length is an integer, we want to calculate + * ceil(n/b). We don't depend on floating point arithmetic in this + * hash, so to calculate ceil(n/b) with integers we could write + * + * ceil(n/b) = (n/b) + ((n%b)?1:0) + * + * and in fact a previous version of this hash did just that. + * But now we have improved things a bit by recognizing that b is + * always a power of two. We keep its base 2 log handy (call it lb), + * so now we can write this with a bit shift and logical AND: + * + * ceil(n/b) = (n>>lb) + ( (n & (b-1)) ? 1:0) + * + */ +#define HASH_EXPAND_BUCKETS(tbl) \ +do { \ + unsigned _he_bkt; \ + unsigned _he_bkt_i; \ + struct UT_hash_handle *_he_thh, *_he_hh_nxt; \ + UT_hash_bucket *_he_new_buckets, *_he_newbkt; \ + _he_new_buckets = (UT_hash_bucket*)uthash_malloc( \ + 2 * tbl->num_buckets * sizeof(struct UT_hash_bucket)); \ + if (!_he_new_buckets) { uthash_fatal( "out of memory"); } \ + memset(_he_new_buckets, 0, \ + 2 * tbl->num_buckets * sizeof(struct UT_hash_bucket)); \ + tbl->ideal_chain_maxlen = \ + (tbl->num_items >> (tbl->log2_num_buckets+1)) + \ + ((tbl->num_items & ((tbl->num_buckets*2)-1)) ? 1 : 0); \ + tbl->nonideal_items = 0; \ + for(_he_bkt_i = 0; _he_bkt_i < tbl->num_buckets; _he_bkt_i++) \ + { \ + _he_thh = tbl->buckets[ _he_bkt_i ].hh_head; \ + while (_he_thh) { \ + _he_hh_nxt = _he_thh->hh_next; \ + HASH_TO_BKT( _he_thh->hashv, tbl->num_buckets*2, _he_bkt); \ + _he_newbkt = &(_he_new_buckets[ _he_bkt ]); \ + if (++(_he_newbkt->count) > tbl->ideal_chain_maxlen) { \ + tbl->nonideal_items++; \ + _he_newbkt->expand_mult = _he_newbkt->count / \ + tbl->ideal_chain_maxlen; \ + } \ + _he_thh->hh_prev = NULL; \ + _he_thh->hh_next = _he_newbkt->hh_head; \ + if (_he_newbkt->hh_head) _he_newbkt->hh_head->hh_prev = \ + _he_thh; \ + _he_newbkt->hh_head = _he_thh; \ + _he_thh = _he_hh_nxt; \ + } \ + } \ + uthash_free( tbl->buckets, tbl->num_buckets*sizeof(struct UT_hash_bucket) ); \ + tbl->num_buckets *= 2; \ + tbl->log2_num_buckets++; \ + tbl->buckets = _he_new_buckets; \ + tbl->ineff_expands = (tbl->nonideal_items > (tbl->num_items >> 1)) ? \ + (tbl->ineff_expands+1) : 0; \ + if (tbl->ineff_expands > 1) { \ + tbl->noexpand=1; \ + uthash_noexpand_fyi(tbl); \ + } \ + uthash_expand_fyi(tbl); \ +} while(0) + + +/* This is an adaptation of Simon Tatham's O(n log(n)) mergesort */ +/* Note that HASH_SORT assumes the hash handle name to be hh. + * HASH_SRT was added to allow the hash handle name to be passed in. */ +#define HASH_SORT(head,cmpfcn) HASH_SRT(hh,head,cmpfcn) +#define HASH_SRT(hh,head,cmpfcn) \ +do { \ + unsigned _hs_i; \ + unsigned _hs_looping,_hs_nmerges,_hs_insize,_hs_psize,_hs_qsize; \ + struct UT_hash_handle *_hs_p, *_hs_q, *_hs_e, *_hs_list, *_hs_tail; \ + if (head) { \ + _hs_insize = 1; \ + _hs_looping = 1; \ + _hs_list = &((head)->hh); \ + while (_hs_looping) { \ + _hs_p = _hs_list; \ + _hs_list = NULL; \ + _hs_tail = NULL; \ + _hs_nmerges = 0; \ + while (_hs_p) { \ + _hs_nmerges++; \ + _hs_q = _hs_p; \ + _hs_psize = 0; \ + for ( _hs_i = 0; _hs_i < _hs_insize; _hs_i++ ) { \ + _hs_psize++; \ + _hs_q = (UT_hash_handle*)((_hs_q->next) ? \ + ((void*)((char*)(_hs_q->next) + \ + (head)->hh.tbl->hho)) : NULL); \ + if (! (_hs_q) ) break; \ + } \ + _hs_qsize = _hs_insize; \ + while ((_hs_psize > 0) || ((_hs_qsize > 0) && _hs_q )) { \ + if (_hs_psize == 0) { \ + _hs_e = _hs_q; \ + _hs_q = (UT_hash_handle*)((_hs_q->next) ? \ + ((void*)((char*)(_hs_q->next) + \ + (head)->hh.tbl->hho)) : NULL); \ + _hs_qsize--; \ + } else if ( (_hs_qsize == 0) || !(_hs_q) ) { \ + _hs_e = _hs_p; \ + _hs_p = (UT_hash_handle*)((_hs_p->next) ? \ + ((void*)((char*)(_hs_p->next) + \ + (head)->hh.tbl->hho)) : NULL); \ + _hs_psize--; \ + } else if (( \ + cmpfcn(DECLTYPE(head)(ELMT_FROM_HH((head)->hh.tbl,_hs_p)), \ + DECLTYPE(head)(ELMT_FROM_HH((head)->hh.tbl,_hs_q))) \ + ) <= 0) { \ + _hs_e = _hs_p; \ + _hs_p = (UT_hash_handle*)((_hs_p->next) ? \ + ((void*)((char*)(_hs_p->next) + \ + (head)->hh.tbl->hho)) : NULL); \ + _hs_psize--; \ + } else { \ + _hs_e = _hs_q; \ + _hs_q = (UT_hash_handle*)((_hs_q->next) ? \ + ((void*)((char*)(_hs_q->next) + \ + (head)->hh.tbl->hho)) : NULL); \ + _hs_qsize--; \ + } \ + if ( _hs_tail ) { \ + _hs_tail->next = ((_hs_e) ? \ + ELMT_FROM_HH((head)->hh.tbl,_hs_e) : NULL); \ + } else { \ + _hs_list = _hs_e; \ + } \ + _hs_e->prev = ((_hs_tail) ? \ + ELMT_FROM_HH((head)->hh.tbl,_hs_tail) : NULL); \ + _hs_tail = _hs_e; \ + } \ + _hs_p = _hs_q; \ + } \ + _hs_tail->next = NULL; \ + if ( _hs_nmerges <= 1 ) { \ + _hs_looping=0; \ + (head)->hh.tbl->tail = _hs_tail; \ + DECLTYPE_ASSIGN(head,ELMT_FROM_HH((head)->hh.tbl, _hs_list)); \ + } \ + _hs_insize *= 2; \ + } \ + HASH_FSCK(hh,head); \ + } \ +} while (0) + +/* This function selects items from one hash into another hash. + * The end result is that the selected items have dual presence + * in both hashes. There is no copy of the items made; rather + * they are added into the new hash through a secondary hash + * hash handle that must be present in the structure. */ +#define HASH_SELECT(hh_dst, dst, hh_src, src, cond) \ +do { \ + unsigned _src_bkt, _dst_bkt; \ + void *_last_elt=NULL, *_elt; \ + UT_hash_handle *_src_hh, *_dst_hh, *_last_elt_hh=NULL; \ + ptrdiff_t _dst_hho = ((char*)(&(dst)->hh_dst) - (char*)(dst)); \ + if (src) { \ + for(_src_bkt=0; _src_bkt < (src)->hh_src.tbl->num_buckets; _src_bkt++) { \ + for(_src_hh = (src)->hh_src.tbl->buckets[_src_bkt].hh_head; \ + _src_hh; \ + _src_hh = _src_hh->hh_next) { \ + _elt = ELMT_FROM_HH((src)->hh_src.tbl, _src_hh); \ + if (cond(_elt)) { \ + _dst_hh = (UT_hash_handle*)(((char*)_elt) + _dst_hho); \ + _dst_hh->key = _src_hh->key; \ + _dst_hh->keylen = _src_hh->keylen; \ + _dst_hh->hashv = _src_hh->hashv; \ + _dst_hh->prev = _last_elt; \ + _dst_hh->next = NULL; \ + if (_last_elt_hh) { _last_elt_hh->next = _elt; } \ + if (!dst) { \ + DECLTYPE_ASSIGN(dst,_elt); \ + HASH_MAKE_TABLE(hh_dst,dst); \ + } else { \ + _dst_hh->tbl = (dst)->hh_dst.tbl; \ + } \ + HASH_TO_BKT(_dst_hh->hashv, _dst_hh->tbl->num_buckets, _dst_bkt); \ + HASH_ADD_TO_BKT(_dst_hh->tbl->buckets[_dst_bkt],_dst_hh); \ + (dst)->hh_dst.tbl->num_items++; \ + _last_elt = _elt; \ + _last_elt_hh = _dst_hh; \ + } \ + } \ + } \ + } \ + HASH_FSCK(hh_dst,dst); \ +} while (0) + +#define HASH_CLEAR(hh,head) \ +do { \ + if (head) { \ + uthash_free((head)->hh.tbl->buckets, \ + (head)->hh.tbl->num_buckets*sizeof(struct UT_hash_bucket)); \ + HASH_BLOOM_FREE((head)->hh.tbl); \ + uthash_free((head)->hh.tbl, sizeof(UT_hash_table)); \ + (head)=NULL; \ + } \ +} while(0) + +#ifdef NO_DECLTYPE +#define HASH_ITER(hh,head,el,tmp) \ +for((el)=(head), (*(char**)(&(tmp)))=(char*)((head)?(head)->hh.next:NULL); \ + el; (el)=(tmp),(*(char**)(&(tmp)))=(char*)((tmp)?(tmp)->hh.next:NULL)) +#else +#define HASH_ITER(hh,head,el,tmp) \ +for((el)=(head),(tmp)=DECLTYPE(el)((head)?(head)->hh.next:NULL); \ + el; (el)=(tmp),(tmp)=DECLTYPE(el)((tmp)?(tmp)->hh.next:NULL)) +#endif + +/* obtain a count of items in the hash */ +#define HASH_COUNT(head) HASH_CNT(hh,head) +#define HASH_CNT(hh,head) ((head)?((head)->hh.tbl->num_items):0) + +typedef struct UT_hash_bucket { + struct UT_hash_handle *hh_head; + unsigned count; + + /* expand_mult is normally set to 0. In this situation, the max chain length + * threshold is enforced at its default value, HASH_BKT_CAPACITY_THRESH. (If + * the bucket's chain exceeds this length, bucket expansion is triggered). + * However, setting expand_mult to a non-zero value delays bucket expansion + * (that would be triggered by additions to this particular bucket) + * until its chain length reaches a *multiple* of HASH_BKT_CAPACITY_THRESH. + * (The multiplier is simply expand_mult+1). The whole idea of this + * multiplier is to reduce bucket expansions, since they are expensive, in + * situations where we know that a particular bucket tends to be overused. + * It is better to let its chain length grow to a longer yet-still-bounded + * value, than to do an O(n) bucket expansion too often. + */ + unsigned expand_mult; + +} UT_hash_bucket; + +/* random signature used only to find hash tables in external analysis */ +#define HASH_SIGNATURE 0xa0111fe1 +#define HASH_BLOOM_SIGNATURE 0xb12220f2 + +typedef struct UT_hash_table { + UT_hash_bucket *buckets; + unsigned num_buckets, log2_num_buckets; + unsigned num_items; + struct UT_hash_handle *tail; /* tail hh in app order, for fast append */ + ptrdiff_t hho; /* hash handle offset (byte pos of hash handle in element */ + + /* in an ideal situation (all buckets used equally), no bucket would have + * more than ceil(#items/#buckets) items. that's the ideal chain length. */ + unsigned ideal_chain_maxlen; + + /* nonideal_items is the number of items in the hash whose chain position + * exceeds the ideal chain maxlen. these items pay the penalty for an uneven + * hash distribution; reaching them in a chain traversal takes >ideal steps */ + unsigned nonideal_items; + + /* ineffective expands occur when a bucket doubling was performed, but + * afterward, more than half the items in the hash had nonideal chain + * positions. If this happens on two consecutive expansions we inhibit any + * further expansion, as it's not helping; this happens when the hash + * function isn't a good fit for the key domain. When expansion is inhibited + * the hash will still work, albeit no longer in constant time. */ + unsigned ineff_expands, noexpand; + + uint32_t signature; /* used only to find hash tables in external analysis */ +#ifdef HASH_BLOOM + uint32_t bloom_sig; /* used only to test bloom exists in external analysis */ + uint8_t *bloom_bv; + char bloom_nbits; +#endif + +} UT_hash_table; + +typedef struct UT_hash_handle { + struct UT_hash_table *tbl; + void *prev; /* prev element in app order */ + void *next; /* next element in app order */ + struct UT_hash_handle *hh_prev; /* previous hh in bucket order */ + struct UT_hash_handle *hh_next; /* next hh in bucket order */ + void *key; /* ptr to enclosing struct's key */ + unsigned keylen; /* enclosing struct's key len */ + unsigned hashv; /* result of hash-fcn(key) */ +} UT_hash_handle; + +#endif /* UTHASH_H */ diff --git a/src/modules/systemlib/uthash/utlist.h b/src/modules/systemlib/uthash/utlist.h new file mode 100644 index 000000000..1578acad2 --- /dev/null +++ b/src/modules/systemlib/uthash/utlist.h @@ -0,0 +1,522 @@ +/* +Copyright (c) 2007-2012, Troy D. Hanson http://uthash.sourceforge.net +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + +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. +*/ + +#ifndef UTLIST_H +#define UTLIST_H + +#define UTLIST_VERSION 1.9.6 + +#include <assert.h> + +/* + * This file contains macros to manipulate singly and doubly-linked lists. + * + * 1. LL_ macros: singly-linked lists. + * 2. DL_ macros: doubly-linked lists. + * 3. CDL_ macros: circular doubly-linked lists. + * + * To use singly-linked lists, your structure must have a "next" pointer. + * To use doubly-linked lists, your structure must "prev" and "next" pointers. + * Either way, the pointer to the head of the list must be initialized to NULL. + * + * ----------------.EXAMPLE ------------------------- + * struct item { + * int id; + * struct item *prev, *next; + * } + * + * struct item *list = NULL: + * + * int main() { + * struct item *item; + * ... allocate and populate item ... + * DL_APPEND(list, item); + * } + * -------------------------------------------------- + * + * For doubly-linked lists, the append and delete macros are O(1) + * For singly-linked lists, append and delete are O(n) but prepend is O(1) + * The sort macro is O(n log(n)) for all types of single/double/circular lists. + */ + +/* These macros use decltype or the earlier __typeof GNU extension. + As decltype is only available in newer compilers (VS2010 or gcc 4.3+ + when compiling c++ code), this code uses whatever method is needed + or, for VS2008 where neither is available, uses casting workarounds. */ +#ifdef _MSC_VER /* MS compiler */ +#if _MSC_VER >= 1600 && defined(__cplusplus) /* VS2010 or newer in C++ mode */ +#define LDECLTYPE(x) decltype(x) +#else /* VS2008 or older (or VS2010 in C mode) */ +#define NO_DECLTYPE +#define LDECLTYPE(x) char* +#endif +#else /* GNU, Sun and other compilers */ +#define LDECLTYPE(x) __typeof(x) +#endif + +/* for VS2008 we use some workarounds to get around the lack of decltype, + * namely, we always reassign our tmp variable to the list head if we need + * to dereference its prev/next pointers, and save/restore the real head.*/ +#ifdef NO_DECLTYPE +#define _SV(elt,list) _tmp = (char*)(list); {char **_alias = (char**)&(list); *_alias = (elt); } +#define _NEXT(elt,list) ((char*)((list)->next)) +#define _NEXTASGN(elt,list,to) { char **_alias = (char**)&((list)->next); *_alias=(char*)(to); } +#define _PREV(elt,list) ((char*)((list)->prev)) +#define _PREVASGN(elt,list,to) { char **_alias = (char**)&((list)->prev); *_alias=(char*)(to); } +#define _RS(list) { char **_alias = (char**)&(list); *_alias=_tmp; } +#define _CASTASGN(a,b) { char **_alias = (char**)&(a); *_alias=(char*)(b); } +#else +#define _SV(elt,list) +#define _NEXT(elt,list) ((elt)->next) +#define _NEXTASGN(elt,list,to) ((elt)->next)=(to) +#define _PREV(elt,list) ((elt)->prev) +#define _PREVASGN(elt,list,to) ((elt)->prev)=(to) +#define _RS(list) +#define _CASTASGN(a,b) (a)=(b) +#endif + +/****************************************************************************** + * The sort macro is an adaptation of Simon Tatham's O(n log(n)) mergesort * + * Unwieldy variable names used here to avoid shadowing passed-in variables. * + *****************************************************************************/ +#define LL_SORT(list, cmp) \ +do { \ + LDECLTYPE(list) _ls_p; \ + LDECLTYPE(list) _ls_q; \ + LDECLTYPE(list) _ls_e; \ + LDECLTYPE(list) _ls_tail; \ + LDECLTYPE(list) _ls_oldhead; \ + LDECLTYPE(list) _tmp; \ + int _ls_insize, _ls_nmerges, _ls_psize, _ls_qsize, _ls_i, _ls_looping; \ + if (list) { \ + _ls_insize = 1; \ + _ls_looping = 1; \ + while (_ls_looping) { \ + _CASTASGN(_ls_p,list); \ + _CASTASGN(_ls_oldhead,list); \ + list = NULL; \ + _ls_tail = NULL; \ + _ls_nmerges = 0; \ + while (_ls_p) { \ + _ls_nmerges++; \ + _ls_q = _ls_p; \ + _ls_psize = 0; \ + for (_ls_i = 0; _ls_i < _ls_insize; _ls_i++) { \ + _ls_psize++; \ + _SV(_ls_q,list); _ls_q = _NEXT(_ls_q,list); _RS(list); \ + if (!_ls_q) break; \ + } \ + _ls_qsize = _ls_insize; \ + while (_ls_psize > 0 || (_ls_qsize > 0 && _ls_q)) { \ + if (_ls_psize == 0) { \ + _ls_e = _ls_q; _SV(_ls_q,list); _ls_q = _NEXT(_ls_q,list); _RS(list); _ls_qsize--; \ + } else if (_ls_qsize == 0 || !_ls_q) { \ + _ls_e = _ls_p; _SV(_ls_p,list); _ls_p = _NEXT(_ls_p,list); _RS(list); _ls_psize--; \ + } else if (cmp(_ls_p,_ls_q) <= 0) { \ + _ls_e = _ls_p; _SV(_ls_p,list); _ls_p = _NEXT(_ls_p,list); _RS(list); _ls_psize--; \ + } else { \ + _ls_e = _ls_q; _SV(_ls_q,list); _ls_q = _NEXT(_ls_q,list); _RS(list); _ls_qsize--; \ + } \ + if (_ls_tail) { \ + _SV(_ls_tail,list); _NEXTASGN(_ls_tail,list,_ls_e); _RS(list); \ + } else { \ + _CASTASGN(list,_ls_e); \ + } \ + _ls_tail = _ls_e; \ + } \ + _ls_p = _ls_q; \ + } \ + _SV(_ls_tail,list); _NEXTASGN(_ls_tail,list,NULL); _RS(list); \ + if (_ls_nmerges <= 1) { \ + _ls_looping=0; \ + } \ + _ls_insize *= 2; \ + } \ + } else _tmp=NULL; /* quiet gcc unused variable warning */ \ +} while (0) + +#define DL_SORT(list, cmp) \ +do { \ + LDECLTYPE(list) _ls_p; \ + LDECLTYPE(list) _ls_q; \ + LDECLTYPE(list) _ls_e; \ + LDECLTYPE(list) _ls_tail; \ + LDECLTYPE(list) _ls_oldhead; \ + LDECLTYPE(list) _tmp; \ + int _ls_insize, _ls_nmerges, _ls_psize, _ls_qsize, _ls_i, _ls_looping; \ + if (list) { \ + _ls_insize = 1; \ + _ls_looping = 1; \ + while (_ls_looping) { \ + _CASTASGN(_ls_p,list); \ + _CASTASGN(_ls_oldhead,list); \ + list = NULL; \ + _ls_tail = NULL; \ + _ls_nmerges = 0; \ + while (_ls_p) { \ + _ls_nmerges++; \ + _ls_q = _ls_p; \ + _ls_psize = 0; \ + for (_ls_i = 0; _ls_i < _ls_insize; _ls_i++) { \ + _ls_psize++; \ + _SV(_ls_q,list); _ls_q = _NEXT(_ls_q,list); _RS(list); \ + if (!_ls_q) break; \ + } \ + _ls_qsize = _ls_insize; \ + while (_ls_psize > 0 || (_ls_qsize > 0 && _ls_q)) { \ + if (_ls_psize == 0) { \ + _ls_e = _ls_q; _SV(_ls_q,list); _ls_q = _NEXT(_ls_q,list); _RS(list); _ls_qsize--; \ + } else if (_ls_qsize == 0 || !_ls_q) { \ + _ls_e = _ls_p; _SV(_ls_p,list); _ls_p = _NEXT(_ls_p,list); _RS(list); _ls_psize--; \ + } else if (cmp(_ls_p,_ls_q) <= 0) { \ + _ls_e = _ls_p; _SV(_ls_p,list); _ls_p = _NEXT(_ls_p,list); _RS(list); _ls_psize--; \ + } else { \ + _ls_e = _ls_q; _SV(_ls_q,list); _ls_q = _NEXT(_ls_q,list); _RS(list); _ls_qsize--; \ + } \ + if (_ls_tail) { \ + _SV(_ls_tail,list); _NEXTASGN(_ls_tail,list,_ls_e); _RS(list); \ + } else { \ + _CASTASGN(list,_ls_e); \ + } \ + _SV(_ls_e,list); _PREVASGN(_ls_e,list,_ls_tail); _RS(list); \ + _ls_tail = _ls_e; \ + } \ + _ls_p = _ls_q; \ + } \ + _CASTASGN(list->prev, _ls_tail); \ + _SV(_ls_tail,list); _NEXTASGN(_ls_tail,list,NULL); _RS(list); \ + if (_ls_nmerges <= 1) { \ + _ls_looping=0; \ + } \ + _ls_insize *= 2; \ + } \ + } else _tmp=NULL; /* quiet gcc unused variable warning */ \ +} while (0) + +#define CDL_SORT(list, cmp) \ +do { \ + LDECLTYPE(list) _ls_p; \ + LDECLTYPE(list) _ls_q; \ + LDECLTYPE(list) _ls_e; \ + LDECLTYPE(list) _ls_tail; \ + LDECLTYPE(list) _ls_oldhead; \ + LDECLTYPE(list) _tmp; \ + LDECLTYPE(list) _tmp2; \ + int _ls_insize, _ls_nmerges, _ls_psize, _ls_qsize, _ls_i, _ls_looping; \ + if (list) { \ + _ls_insize = 1; \ + _ls_looping = 1; \ + while (_ls_looping) { \ + _CASTASGN(_ls_p,list); \ + _CASTASGN(_ls_oldhead,list); \ + list = NULL; \ + _ls_tail = NULL; \ + _ls_nmerges = 0; \ + while (_ls_p) { \ + _ls_nmerges++; \ + _ls_q = _ls_p; \ + _ls_psize = 0; \ + for (_ls_i = 0; _ls_i < _ls_insize; _ls_i++) { \ + _ls_psize++; \ + _SV(_ls_q,list); \ + if (_NEXT(_ls_q,list) == _ls_oldhead) { \ + _ls_q = NULL; \ + } else { \ + _ls_q = _NEXT(_ls_q,list); \ + } \ + _RS(list); \ + if (!_ls_q) break; \ + } \ + _ls_qsize = _ls_insize; \ + while (_ls_psize > 0 || (_ls_qsize > 0 && _ls_q)) { \ + if (_ls_psize == 0) { \ + _ls_e = _ls_q; _SV(_ls_q,list); _ls_q = _NEXT(_ls_q,list); _RS(list); _ls_qsize--; \ + if (_ls_q == _ls_oldhead) { _ls_q = NULL; } \ + } else if (_ls_qsize == 0 || !_ls_q) { \ + _ls_e = _ls_p; _SV(_ls_p,list); _ls_p = _NEXT(_ls_p,list); _RS(list); _ls_psize--; \ + if (_ls_p == _ls_oldhead) { _ls_p = NULL; } \ + } else if (cmp(_ls_p,_ls_q) <= 0) { \ + _ls_e = _ls_p; _SV(_ls_p,list); _ls_p = _NEXT(_ls_p,list); _RS(list); _ls_psize--; \ + if (_ls_p == _ls_oldhead) { _ls_p = NULL; } \ + } else { \ + _ls_e = _ls_q; _SV(_ls_q,list); _ls_q = _NEXT(_ls_q,list); _RS(list); _ls_qsize--; \ + if (_ls_q == _ls_oldhead) { _ls_q = NULL; } \ + } \ + if (_ls_tail) { \ + _SV(_ls_tail,list); _NEXTASGN(_ls_tail,list,_ls_e); _RS(list); \ + } else { \ + _CASTASGN(list,_ls_e); \ + } \ + _SV(_ls_e,list); _PREVASGN(_ls_e,list,_ls_tail); _RS(list); \ + _ls_tail = _ls_e; \ + } \ + _ls_p = _ls_q; \ + } \ + _CASTASGN(list->prev,_ls_tail); \ + _CASTASGN(_tmp2,list); \ + _SV(_ls_tail,list); _NEXTASGN(_ls_tail,list,_tmp2); _RS(list); \ + if (_ls_nmerges <= 1) { \ + _ls_looping=0; \ + } \ + _ls_insize *= 2; \ + } \ + } else _tmp=NULL; /* quiet gcc unused variable warning */ \ +} while (0) + +/****************************************************************************** + * singly linked list macros (non-circular) * + *****************************************************************************/ +#define LL_PREPEND(head,add) \ +do { \ + (add)->next = head; \ + head = add; \ +} while (0) + +#define LL_CONCAT(head1,head2) \ +do { \ + LDECLTYPE(head1) _tmp; \ + if (head1) { \ + _tmp = head1; \ + while (_tmp->next) { _tmp = _tmp->next; } \ + _tmp->next=(head2); \ + } else { \ + (head1)=(head2); \ + } \ +} while (0) + +#define LL_APPEND(head,add) \ +do { \ + LDECLTYPE(head) _tmp; \ + (add)->next=NULL; \ + if (head) { \ + _tmp = head; \ + while (_tmp->next) { _tmp = _tmp->next; } \ + _tmp->next=(add); \ + } else { \ + (head)=(add); \ + } \ +} while (0) + +#define LL_DELETE(head,del) \ +do { \ + LDECLTYPE(head) _tmp; \ + if ((head) == (del)) { \ + (head)=(head)->next; \ + } else { \ + _tmp = head; \ + while (_tmp->next && (_tmp->next != (del))) { \ + _tmp = _tmp->next; \ + } \ + if (_tmp->next) { \ + _tmp->next = ((del)->next); \ + } \ + } \ +} while (0) + +/* Here are VS2008 replacements for LL_APPEND and LL_DELETE */ +#define LL_APPEND_VS2008(head,add) \ +do { \ + if (head) { \ + (add)->next = head; /* use add->next as a temp variable */ \ + while ((add)->next->next) { (add)->next = (add)->next->next; } \ + (add)->next->next=(add); \ + } else { \ + (head)=(add); \ + } \ + (add)->next=NULL; \ +} while (0) + +#define LL_DELETE_VS2008(head,del) \ +do { \ + if ((head) == (del)) { \ + (head)=(head)->next; \ + } else { \ + char *_tmp = (char*)(head); \ + while ((head)->next && ((head)->next != (del))) { \ + head = (head)->next; \ + } \ + if ((head)->next) { \ + (head)->next = ((del)->next); \ + } \ + { \ + char **_head_alias = (char**)&(head); \ + *_head_alias = _tmp; \ + } \ + } \ +} while (0) +#ifdef NO_DECLTYPE +#undef LL_APPEND +#define LL_APPEND LL_APPEND_VS2008 +#undef LL_DELETE +#define LL_DELETE LL_DELETE_VS2008 +#undef LL_CONCAT /* no LL_CONCAT_VS2008 */ +#undef DL_CONCAT /* no DL_CONCAT_VS2008 */ +#endif +/* end VS2008 replacements */ + +#define LL_FOREACH(head,el) \ + for(el=head;el;el=(el)->next) + +#define LL_FOREACH_SAFE(head,el,tmp) \ + for((el)=(head);(el) && (tmp = (el)->next, 1); (el) = tmp) + +#define LL_SEARCH_SCALAR(head,out,field,val) \ +do { \ + LL_FOREACH(head,out) { \ + if ((out)->field == (val)) break; \ + } \ +} while(0) + +#define LL_SEARCH(head,out,elt,cmp) \ +do { \ + LL_FOREACH(head,out) { \ + if ((cmp(out,elt))==0) break; \ + } \ +} while(0) + +/****************************************************************************** + * doubly linked list macros (non-circular) * + *****************************************************************************/ +#define DL_PREPEND(head,add) \ +do { \ + (add)->next = head; \ + if (head) { \ + (add)->prev = (head)->prev; \ + (head)->prev = (add); \ + } else { \ + (add)->prev = (add); \ + } \ + (head) = (add); \ +} while (0) + +#define DL_APPEND(head,add) \ +do { \ + if (head) { \ + (add)->prev = (head)->prev; \ + (head)->prev->next = (add); \ + (head)->prev = (add); \ + (add)->next = NULL; \ + } else { \ + (head)=(add); \ + (head)->prev = (head); \ + (head)->next = NULL; \ + } \ +} while (0) + +#define DL_CONCAT(head1,head2) \ +do { \ + LDECLTYPE(head1) _tmp; \ + if (head2) { \ + if (head1) { \ + _tmp = (head2)->prev; \ + (head2)->prev = (head1)->prev; \ + (head1)->prev->next = (head2); \ + (head1)->prev = _tmp; \ + } else { \ + (head1)=(head2); \ + } \ + } \ +} while (0) + +#define DL_DELETE(head,del) \ +do { \ + assert((del)->prev != NULL); \ + if ((del)->prev == (del)) { \ + (head)=NULL; \ + } else if ((del)==(head)) { \ + (del)->next->prev = (del)->prev; \ + (head) = (del)->next; \ + } else { \ + (del)->prev->next = (del)->next; \ + if ((del)->next) { \ + (del)->next->prev = (del)->prev; \ + } else { \ + (head)->prev = (del)->prev; \ + } \ + } \ +} while (0) + + +#define DL_FOREACH(head,el) \ + for(el=head;el;el=(el)->next) + +/* this version is safe for deleting the elements during iteration */ +#define DL_FOREACH_SAFE(head,el,tmp) \ + for((el)=(head);(el) && (tmp = (el)->next, 1); (el) = tmp) + +/* these are identical to their singly-linked list counterparts */ +#define DL_SEARCH_SCALAR LL_SEARCH_SCALAR +#define DL_SEARCH LL_SEARCH + +/****************************************************************************** + * circular doubly linked list macros * + *****************************************************************************/ +#define CDL_PREPEND(head,add) \ +do { \ + if (head) { \ + (add)->prev = (head)->prev; \ + (add)->next = (head); \ + (head)->prev = (add); \ + (add)->prev->next = (add); \ + } else { \ + (add)->prev = (add); \ + (add)->next = (add); \ + } \ +(head)=(add); \ +} while (0) + +#define CDL_DELETE(head,del) \ +do { \ + if ( ((head)==(del)) && ((head)->next == (head))) { \ + (head) = 0L; \ + } else { \ + (del)->next->prev = (del)->prev; \ + (del)->prev->next = (del)->next; \ + if ((del) == (head)) (head)=(del)->next; \ + } \ +} while (0) + +#define CDL_FOREACH(head,el) \ + for(el=head;el;el=((el)->next==head ? 0L : (el)->next)) + +#define CDL_FOREACH_SAFE(head,el,tmp1,tmp2) \ + for((el)=(head), ((tmp1)=(head)?((head)->prev):NULL); \ + (el) && ((tmp2)=(el)->next, 1); \ + ((el) = (((el)==(tmp1)) ? 0L : (tmp2)))) + +#define CDL_SEARCH_SCALAR(head,out,field,val) \ +do { \ + CDL_FOREACH(head,out) { \ + if ((out)->field == (val)) break; \ + } \ +} while(0) + +#define CDL_SEARCH(head,out,elt,cmp) \ +do { \ + CDL_FOREACH(head,out) { \ + if ((cmp(out,elt))==0) break; \ + } \ +} while(0) + +#endif /* UTLIST_H */ + diff --git a/src/modules/systemlib/uthash/utstring.h b/src/modules/systemlib/uthash/utstring.h new file mode 100644 index 000000000..a181ad778 --- /dev/null +++ b/src/modules/systemlib/uthash/utstring.h @@ -0,0 +1,148 @@ +/* +Copyright (c) 2008-2012, Troy D. Hanson http://uthash.sourceforge.net +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + +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. +*/ + +/* a dynamic string implementation using macros + * see http://uthash.sourceforge.net/utstring + */ +#ifndef UTSTRING_H +#define UTSTRING_H + +#define UTSTRING_VERSION 1.9.6 + +#ifdef __GNUC__ +#define _UNUSED_ __attribute__ ((__unused__)) +#else +#define _UNUSED_ +#endif + +#include <stdlib.h> +#include <string.h> +#include <stdarg.h> +#define oom() exit(-1) + +typedef struct { + char *d; + size_t n; /* allocd size */ + size_t i; /* index of first unused byte */ +} UT_string; + +#define utstring_reserve(s,amt) \ +do { \ + if (((s)->n - (s)->i) < (size_t)(amt)) { \ + (s)->d = (char*)realloc((s)->d, (s)->n + amt); \ + if ((s)->d == NULL) oom(); \ + (s)->n += amt; \ + } \ +} while(0) + +#define utstring_init(s) \ +do { \ + (s)->n = 0; (s)->i = 0; (s)->d = NULL; \ + utstring_reserve(s,100); \ + (s)->d[0] = '\0'; \ +} while(0) + +#define utstring_done(s) \ +do { \ + if ((s)->d != NULL) free((s)->d); \ + (s)->n = 0; \ +} while(0) + +#define utstring_free(s) \ +do { \ + utstring_done(s); \ + free(s); \ +} while(0) + +#define utstring_new(s) \ +do { \ + s = (UT_string*)calloc(sizeof(UT_string),1); \ + if (!s) oom(); \ + utstring_init(s); \ +} while(0) + +#define utstring_renew(s) \ +do { \ + if (s) { \ + utstring_clear(s); \ + } else { \ + utstring_new(s); \ + } \ +} while(0) + +#define utstring_clear(s) \ +do { \ + (s)->i = 0; \ + (s)->d[0] = '\0'; \ +} while(0) + +#define utstring_bincpy(s,b,l) \ +do { \ + utstring_reserve((s),(l)+1); \ + if (l) memcpy(&(s)->d[(s)->i], b, l); \ + (s)->i += l; \ + (s)->d[(s)->i]='\0'; \ +} while(0) + +#define utstring_concat(dst,src) \ +do { \ + utstring_reserve((dst),((src)->i)+1); \ + if ((src)->i) memcpy(&(dst)->d[(dst)->i], (src)->d, (src)->i); \ + (dst)->i += (src)->i; \ + (dst)->d[(dst)->i]='\0'; \ +} while(0) + +#define utstring_len(s) ((unsigned)((s)->i)) + +#define utstring_body(s) ((s)->d) + +_UNUSED_ static void utstring_printf_va(UT_string *s, const char *fmt, va_list ap) { + int n; + va_list cp; + while (1) { +#ifdef _WIN32 + cp = ap; +#else + va_copy(cp, ap); +#endif + n = vsnprintf (&s->d[s->i], s->n-s->i, fmt, cp); + va_end(cp); + + if ((n > -1) && (n < (int)(s->n-s->i))) { + s->i += n; + return; + } + + /* Else try again with more space. */ + if (n > -1) utstring_reserve(s,n+1); /* exact */ + else utstring_reserve(s,(s->n)*2); /* 2x */ + } +} +_UNUSED_ static void utstring_printf(UT_string *s, const char *fmt, ...) { + va_list ap; + va_start(ap,fmt); + utstring_printf_va(s,fmt,ap); + va_end(ap); +} + +#endif /* UTSTRING_H */ diff --git a/src/modules/systemlib/visibility.h b/src/modules/systemlib/visibility.h new file mode 100644 index 000000000..2c6adc062 --- /dev/null +++ b/src/modules/systemlib/visibility.h @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file visibility.h + * + * Definitions controlling symbol naming and visibility. + * + * This file is normally included automatically by the build system. + */ + +#ifndef __SYSTEMLIB_VISIBILITY_H +#define __SYSTEMLIB_VISIBILITY_H + +#ifdef __EXPORT +# undef __EXPORT +#endif +#define __EXPORT __attribute__ ((visibility ("default"))) + +#ifdef __PRIVATE +# undef __PRIVATE +#endif +#define __PRIVATE __attribute__ ((visibility ("hidden"))) + +#ifdef __cplusplus +# define __BEGIN_DECLS extern "C" { +# define __END_DECLS } +#else +# define __BEGIN_DECLS +# define __END_DECLS +#endif +#endif
\ No newline at end of file diff --git a/src/modules/test/foo.c b/src/modules/test/foo.c new file mode 100644 index 000000000..ff6af031f --- /dev/null +++ b/src/modules/test/foo.c @@ -0,0 +1,4 @@ +int test_main(void) +{ + return 0; +}
\ No newline at end of file diff --git a/src/modules/test/module.mk b/src/modules/test/module.mk new file mode 100644 index 000000000..abf80eedf --- /dev/null +++ b/src/modules/test/module.mk @@ -0,0 +1,4 @@ + +MODULE_NAME = test +SRCS = foo.c + diff --git a/src/modules/uORB/module.mk b/src/modules/uORB/module.mk new file mode 100644 index 000000000..5ec31ab01 --- /dev/null +++ b/src/modules/uORB/module.mk @@ -0,0 +1,44 @@ +############################################################################ +# +# 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 uORB +# + +MODULE_COMMAND = uorb + +# XXX probably excessive, 2048 should be sufficient +MODULE_STACKSIZE = 4096 + +SRCS = uORB.cpp \ + objects_common.cpp diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp new file mode 100644 index 000000000..ae5fc6c61 --- /dev/null +++ b/src/modules/uORB/objects_common.cpp @@ -0,0 +1,174 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file objects_common.cpp + * + * Common object definitions without a better home. + */ + +#include <nuttx/config.h> + +#include <drivers/drv_orb_dev.h> + +#include <drivers/drv_mag.h> +ORB_DEFINE(sensor_mag, struct mag_report); + +#include <drivers/drv_accel.h> +ORB_DEFINE(sensor_accel, struct accel_report); + +#include <drivers/drv_gyro.h> +ORB_DEFINE(sensor_gyro, struct gyro_report); + +#include <drivers/drv_baro.h> +ORB_DEFINE(sensor_baro, struct baro_report); + +#include <drivers/drv_range_finder.h> +ORB_DEFINE(sensor_range_finder, struct range_finder_report); + +#include <drivers/drv_pwm_output.h> +ORB_DEFINE(output_pwm, struct pwm_output_values); + +#include <drivers/drv_rc_input.h> +ORB_DEFINE(input_rc, struct rc_input_values); + +#include "topics/vehicle_attitude.h" +ORB_DEFINE(vehicle_attitude, struct vehicle_attitude_s); + +#include "topics/sensor_combined.h" +ORB_DEFINE(sensor_combined, struct sensor_combined_s); + +#include "topics/vehicle_gps_position.h" +ORB_DEFINE(vehicle_gps_position, struct vehicle_gps_position_s); + +#include "topics/home_position.h" +ORB_DEFINE(home_position, struct home_position_s); + +#include "topics/vehicle_status.h" +ORB_DEFINE(vehicle_status, struct vehicle_status_s); + +#include "topics/battery_status.h" +ORB_DEFINE(battery_status, struct battery_status_s); + +#include "topics/vehicle_global_position.h" +ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s); + +#include "topics/vehicle_local_position.h" +ORB_DEFINE(vehicle_local_position, struct vehicle_local_position_s); + +#include "topics/vehicle_vicon_position.h" +ORB_DEFINE(vehicle_vicon_position, struct vehicle_vicon_position_s); + +#include "topics/vehicle_rates_setpoint.h" +ORB_DEFINE(vehicle_rates_setpoint, struct vehicle_rates_setpoint_s); + +#include "topics/rc_channels.h" +ORB_DEFINE(rc_channels, struct rc_channels_s); + +#include "topics/vehicle_command.h" +ORB_DEFINE(vehicle_command, struct vehicle_command_s); + +#include "topics/vehicle_local_position_setpoint.h" +ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoint_s); + +#include "topics/vehicle_bodyframe_speed_setpoint.h" +ORB_DEFINE(vehicle_bodyframe_speed_setpoint, struct vehicle_bodyframe_speed_setpoint_s); + +#include "topics/vehicle_global_position_setpoint.h" +ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setpoint_s); + +#include "topics/vehicle_global_position_set_triplet.h" +ORB_DEFINE(vehicle_global_position_set_triplet, struct vehicle_global_position_set_triplet_s); + +#include "topics/mission.h" +ORB_DEFINE(mission, struct mission_s); + +#include "topics/vehicle_attitude_setpoint.h" +ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s); + +#include "topics/manual_control_setpoint.h" +ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s); + +#include "topics/offboard_control_setpoint.h" +ORB_DEFINE(offboard_control_setpoint, struct offboard_control_setpoint_s); + +#include "topics/optical_flow.h" +ORB_DEFINE(optical_flow, struct optical_flow_s); + +#include "topics/filtered_bottom_flow.h" +ORB_DEFINE(filtered_bottom_flow, struct filtered_bottom_flow_s); + +#include "topics/omnidirectional_flow.h" +ORB_DEFINE(omnidirectional_flow, struct omnidirectional_flow_s); + +#include "topics/airspeed.h" +ORB_DEFINE(airspeed, struct airspeed_s); + +#include "topics/differential_pressure.h" +ORB_DEFINE(differential_pressure, struct differential_pressure_s); + +#include "topics/subsystem_info.h" +ORB_DEFINE(subsystem_info, struct subsystem_info_s); + +/* actuator controls, as requested by controller */ +#include "topics/actuator_controls.h" +ORB_DEFINE(actuator_controls_0, struct actuator_controls_s); +ORB_DEFINE(actuator_controls_1, struct actuator_controls_s); +ORB_DEFINE(actuator_controls_2, struct actuator_controls_s); +ORB_DEFINE(actuator_controls_3, struct actuator_controls_s); +ORB_DEFINE(actuator_armed, struct actuator_armed_s); + +/* actuator controls, as set by actuators / mixers after limiting */ +#include "topics/actuator_controls_effective.h" +ORB_DEFINE(actuator_controls_effective_0, struct actuator_controls_effective_s); +ORB_DEFINE(actuator_controls_effective_1, struct actuator_controls_effective_s); +ORB_DEFINE(actuator_controls_effective_2, struct actuator_controls_effective_s); +ORB_DEFINE(actuator_controls_effective_3, struct actuator_controls_effective_s); + +#include "topics/actuator_outputs.h" +ORB_DEFINE(actuator_outputs_0, struct actuator_outputs_s); +ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s); +ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s); +ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s); + +#include "topics/telemetry_status.h" +ORB_DEFINE(telemetry_status, struct telemetry_status_s); + +#include "topics/debug_key_value.h" +ORB_DEFINE(debug_key_value, struct debug_key_value_s); + +#include "topics/navigation_capabilities.h" +ORB_DEFINE(navigation_capabilities, struct navigation_capabilities_s); + +#include "topics/esc_status.h" +ORB_DEFINE(esc_status, struct esc_status_s); diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h new file mode 100644 index 000000000..b7c4196c0 --- /dev/null +++ b/src/modules/uORB/topics/actuator_controls.h @@ -0,0 +1,78 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file actuator_controls.h + * + * Actuator control topics - mixer inputs. + * + * Values published to these topics are the outputs of the vehicle control + * system, and are expected to be mixed and used to drive the actuators + * (servos, speed controls, etc.) that operate the vehicle. + * + * Each topic can be published by a single controller + */ + +#ifndef TOPIC_ACTUATOR_CONTROLS_H +#define TOPIC_ACTUATOR_CONTROLS_H + +#include <stdint.h> +#include "../uORB.h" + +#define NUM_ACTUATOR_CONTROLS 8 +#define NUM_ACTUATOR_CONTROL_GROUPS 4 /**< for sanity checking */ + +struct actuator_controls_s { + uint64_t timestamp; + float control[NUM_ACTUATOR_CONTROLS]; +}; + +/* actuator control sets; this list can be expanded as more controllers emerge */ +ORB_DECLARE(actuator_controls_0); +ORB_DECLARE(actuator_controls_1); +ORB_DECLARE(actuator_controls_2); +ORB_DECLARE(actuator_controls_3); + +/* control sets with pre-defined applications */ +#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) + +/** global 'actuator output is live' control. */ +struct actuator_armed_s { + bool armed; /**< Set to true if system is armed */ + bool ready_to_arm; /**< Set to true if system is ready to be armed */ + bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ +}; + +ORB_DECLARE(actuator_armed); + +#endif
\ No newline at end of file diff --git a/src/modules/uORB/topics/actuator_controls_effective.h b/src/modules/uORB/topics/actuator_controls_effective.h new file mode 100644 index 000000000..088c4fc8f --- /dev/null +++ b/src/modules/uORB/topics/actuator_controls_effective.h @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file actuator_controls_effective.h + * + * Actuator control topics - mixer inputs. + * + * Values published to these topics are the outputs of the vehicle control + * system and mixing process; they are the control-scale values that are + * then fed to the actual actuator driver. + * + * Each topic can be published by a single controller + */ + +#ifndef TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H +#define TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H + +#include <stdint.h> +#include "../uORB.h" +#include "actuator_controls.h" + +#define NUM_ACTUATOR_CONTROLS_EFFECTIVE NUM_ACTUATOR_CONTROLS +#define NUM_ACTUATOR_CONTROL_GROUPS_EFFECTIVE NUM_ACTUATOR_CONTROL_GROUPS /**< for sanity checking */ + +struct actuator_controls_effective_s { + uint64_t timestamp; + float control_effective[NUM_ACTUATOR_CONTROLS_EFFECTIVE]; +}; + +/* actuator control sets; this list can be expanded as more controllers emerge */ +ORB_DECLARE(actuator_controls_effective_0); +ORB_DECLARE(actuator_controls_effective_1); +ORB_DECLARE(actuator_controls_effective_2); +ORB_DECLARE(actuator_controls_effective_3); + +/* control sets with pre-defined applications */ +#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE ORB_ID(actuator_controls_effective_0) + +#endif /* TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H */
\ No newline at end of file diff --git a/src/modules/uORB/topics/actuator_outputs.h b/src/modules/uORB/topics/actuator_outputs.h new file mode 100644 index 000000000..bbe429073 --- /dev/null +++ b/src/modules/uORB/topics/actuator_outputs.h @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file actuator_outputs.h + * + * Actuator output values. + * + * Values published to these topics are the outputs of the control mixing + * system as sent to the actuators (servos, motors, etc.) that operate + * the vehicle. + * + * Each topic can be published by a single output driver. + */ + +#ifndef TOPIC_ACTUATOR_OUTPUTS_H +#define TOPIC_ACTUATOR_OUTPUTS_H + +#include <stdint.h> +#include "../uORB.h" + +#define NUM_ACTUATOR_OUTPUTS 16 +#define NUM_ACTUATOR_OUTPUT_GROUPS 4 /**< for sanity checking */ + +struct actuator_outputs_s { + uint64_t timestamp; /**< output timestamp in us since system boot */ + float output[NUM_ACTUATOR_OUTPUTS]; /**< output data, in natural output units */ + int noutputs; /**< valid outputs */ +}; + +/* actuator output sets; this list can be expanded as more drivers emerge */ +ORB_DECLARE(actuator_outputs_0); +ORB_DECLARE(actuator_outputs_1); +ORB_DECLARE(actuator_outputs_2); +ORB_DECLARE(actuator_outputs_3); + +/* output sets with pre-defined applications */ +#define ORB_ID_VEHICLE_CONTROLS ORB_ID(actuator_outputs_0) + +#endif
\ No newline at end of file diff --git a/src/modules/uORB/topics/airspeed.h b/src/modules/uORB/topics/airspeed.h new file mode 100644 index 000000000..a3da3758f --- /dev/null +++ b/src/modules/uORB/topics/airspeed.h @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * 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 airspeed.h + * + * Definition of airspeed topic + */ + +#ifndef TOPIC_AIRSPEED_H_ +#define TOPIC_AIRSPEED_H_ + +#include "../uORB.h" +#include <stdint.h> + +/** + * @addtogroup topics + * @{ + */ + +/** + * Airspeed + */ +struct airspeed_s { + 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 */ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(airspeed); + +#endif diff --git a/src/modules/uORB/topics/battery_status.h b/src/modules/uORB/topics/battery_status.h new file mode 100644 index 000000000..c40d0d4e5 --- /dev/null +++ b/src/modules/uORB/topics/battery_status.h @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * 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 battery_status.h + * + * Definition of the battery status uORB topic. + */ + +#ifndef BATTERY_STATUS_H_ +#define BATTERY_STATUS_H_ + +#include "../uORB.h" +#include <stdint.h> + +/** + * @addtogroup topics + * @{ + */ + +/** + * Battery voltages and status + */ +struct battery_status_s { + uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ + float voltage_v; /**< Battery voltage in volts, filtered */ + float current_a; /**< Battery current in amperes, filtered, -1 if unknown */ + float discharged_mah; /**< Discharged amount in mAh, filtered, -1 if unknown */ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(battery_status); + +#endif
\ No newline at end of file diff --git a/src/modules/uORB/topics/debug_key_value.h b/src/modules/uORB/topics/debug_key_value.h new file mode 100644 index 000000000..a9d1b83fd --- /dev/null +++ b/src/modules/uORB/topics/debug_key_value.h @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * 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 + * 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 debug_key_value.h + * Definition of the debug named value uORB topic. Allows to send a 10-char key + * with a float as value. + */ + +#ifndef TOPIC_DEBUG_KEY_VALUE_H_ +#define TOPIC_DEBUG_KEY_VALUE_H_ + +#include <stdint.h> +#include <stdbool.h> +#include "../uORB.h" + +/** + * @addtogroup topics + */ + +/** + * Key/value pair for debugging + */ +struct debug_key_value_s { + + /* + * Actual data, this is specific to the type of data which is stored in this struct + * A line containing L0GME will be added by the Python logging code generator to the + * logged dataset. + */ + uint32_t timestamp_ms; /**< in milliseconds since system start */ + char key[10]; /**< max. 10 characters as key / name */ + float value; /**< the value to send as debug output */ + +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(debug_key_value); + +#endif diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h new file mode 100644 index 000000000..1ffeda764 --- /dev/null +++ b/src/modules/uORB/topics/differential_pressure.h @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * 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 differential_pressure.h + * + * Definition of differential pressure topic + */ + +#ifndef TOPIC_DIFFERENTIAL_PRESSURE_H_ +#define TOPIC_DIFFERENTIAL_PRESSURE_H_ + +#include "../uORB.h" +#include <stdint.h> + +/** + * @addtogroup topics + * @{ + */ + +/** + * Differential pressure. + */ +struct differential_pressure_s { + uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ + uint16_t differential_pressure_pa; /**< Differential pressure reading */ + uint16_t 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 */ + +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(differential_pressure); + +#endif diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h new file mode 100644 index 000000000..e67a39e1e --- /dev/null +++ b/src/modules/uORB/topics/esc_status.h @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: @author Marco Bauer <marco@wtns.de> + * + * 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 esc_status.h + * Definition of the esc_status uORB topic. + * + * Published the state machine and the system status bitfields + * (see SYS_STATUS mavlink message), published only by commander app. + * + * All apps should write to subsystem_info: + * + * (any app) --> subsystem_info (published) --> (commander app state machine) --> esc_status --> (mavlink app) + */ + +#ifndef ESC_STATUS_H_ +#define ESC_STATUS_H_ + +#include <stdint.h> +#include <stdbool.h> +#include "../uORB.h" + +/** + * @addtogroup topics @{ + */ + +/** + * The number of ESCs supported. + * Current (Q2/2013) we support 8 ESCs, + */ +#define CONNECTED_ESC_MAX 8 + +enum ESC_VENDOR { + ESC_VENDOR_GENERIC = 0, /**< generic ESC */ + ESC_VENDOR_MIKROKOPTER /**< Mikrokopter */ +}; + +enum ESC_CONNECTION_TYPE { + ESC_CONNECTION_TYPE_PPM = 0, /**< Traditional PPM ESC */ + ESC_CONNECTION_TYPE_SERIAL, /**< Serial Bus connected ESC */ + ESC_CONNECTION_TYPE_ONESHOOT, /**< One Shoot PPM */ + ESC_CONNECTION_TYPE_I2C, /**< I2C */ + ESC_CONNECTION_TYPE_CAN /**< CAN-Bus */ +}; + +/** + * + */ +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 */ + uint16_t esc_version; /**< Version of current ESC - if supported */ + uint16_t esc_voltage; /**< Voltage measured from current ESC - if supported */ + uint16_t esc_current; /**< Current measured from current ESC (100mA steps) - if supported */ + uint16_t esc_rpm; /**< RPM measured from current ESC - if supported */ + uint16_t esc_temperature; /**< Temperature measured from current ESC - if supported */ + float esc_setpoint; /**< setpoint of current ESC */ + uint16_t esc_setpoint_raw; /**< setpoint of current ESC (Value sent to ESC) */ + uint16_t esc_state; /**< State of ESC - depend on Vendor */ + uint16_t esc_errorcount; /**< Number of reported errors by ESC - if supported */ + } esc[CONNECTED_ESC_MAX]; + +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +//ORB_DECLARE(esc_status); +ORB_DECLARE_OPTIONAL(esc_status); + +#endif diff --git a/src/modules/uORB/topics/filtered_bottom_flow.h b/src/modules/uORB/topics/filtered_bottom_flow.h new file mode 100644 index 000000000..ab6de2613 --- /dev/null +++ b/src/modules/uORB/topics/filtered_bottom_flow.h @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Samuel Zihlmann <samuezih@ee.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 filtered_bottom_flow.h + * Definition of the filtered bottom flow uORB topic. + */ + +#ifndef TOPIC_FILTERED_BOTTOM_FLOW_H_ +#define TOPIC_FILTERED_BOTTOM_FLOW_H_ + +#include <stdint.h> +#include <stdbool.h> +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * Filtered bottom flow in bodyframe. + */ +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 */ + float sumy; /**< Integrated bodyframe y flow in meters */ + + float vx; /**< Flow bodyframe x speed, m/s */ + float vy; /**< Flow bodyframe y Speed, m/s */ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(filtered_bottom_flow); + +#endif diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h new file mode 100644 index 000000000..7e1b82a0f --- /dev/null +++ b/src/modules/uORB/topics/home_position.h @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * Copyright (C) 2012-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 home_position.h + * Definition of the GPS home position uORB topic. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +#ifndef TOPIC_HOME_POSITION_H_ +#define TOPIC_HOME_POSITION_H_ + +#include <stdint.h> +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * GPS home position in WGS84 coordinates. + */ +struct home_position_s +{ + uint64_t timestamp; /**< Timestamp (microseconds since system boot) */ + uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp from the gps module */ + + int32_t lat; /**< Latitude in 1E7 degrees */ + int32_t lon; /**< Longitude in 1E7 degrees */ + int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL */ + float eph_m; /**< GPS HDOP horizontal dilution of position in m */ + float epv_m; /**< GPS VDOP horizontal dilution of position in m */ + float s_variance_m_s; /**< speed accuracy estimate m/s */ + float p_variance_m; /**< position accuracy estimate m */ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(home_position); + +#endif diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h new file mode 100644 index 000000000..261a8a4ad --- /dev/null +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * Copyright (C) 2008-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 + * 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 manual_control_setpoint.h + * Definition of the manual_control_setpoint uORB topic. + */ + +#ifndef TOPIC_MANUAL_CONTROL_SETPOINT_H_ +#define TOPIC_MANUAL_CONTROL_SETPOINT_H_ + +#include <stdint.h> +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +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 manual_override_switch; /**< manual override mode (mandatory) */ + float auto_mode_switch; /**< auto mode switch (mandatory) */ + + /** + * Any of the channels below may not be available and be set to NaN + * to indicate that it does not contain valid data. + */ + float manual_mode_switch; /**< manual mode (man, sas, alt) switch (optional) */ + float manual_sas_switch; /**< sas mode (rates / attitude) switch (optional) */ + float return_to_launch_switch; /**< return to launch switch (0 = disabled, 1 = enabled) */ + float auto_offboard_input_switch; /**< controller setpoint source (0 = onboard, 1 = offboard) */ + + 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 */ + +}; /**< manual control inputs */ + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(manual_control_setpoint); + +#endif diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h new file mode 100644 index 000000000..253f444b3 --- /dev/null +++ b/src/modules/uORB/topics/mission.h @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * 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> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mission_item.h + * Definition of one mission item. + */ + +#ifndef TOPIC_MISSION_H_ +#define TOPIC_MISSION_H_ + +#include <stdint.h> +#include <stdbool.h> +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +enum NAV_CMD { + NAV_CMD_WAYPOINT = 0, + NAV_CMD_LOITER_TURN_COUNT, + NAV_CMD_LOITER_TIME_LIMIT, + NAV_CMD_LOITER_UNLIMITED, + NAV_CMD_RETURN_TO_LAUNCH, + NAV_CMD_LAND, + NAV_CMD_TAKEOFF +}; + +/** + * Global position setpoint in WGS84 coordinates. + * + * 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 +{ + bool altitude_is_relative; /**< true if altitude is relative from start point */ + double lat; /**< latitude in degrees * 1E7 */ + double lon; /**< longitude in degrees * 1E7 */ + float altitude; /**< altitude in meters */ + float yaw; /**< in radians NED -PI..+PI */ + float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover */ + uint8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */ + enum NAV_CMD nav_cmd; /**< true if loitering is enabled */ + float param1; + float param2; + float param3; + float param4; +}; + +struct mission_s +{ + struct mission_item_s *items; + unsigned count; +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(mission); + +#endif diff --git a/src/modules/uORB/topics/navigation_capabilities.h b/src/modules/uORB/topics/navigation_capabilities.h new file mode 100644 index 000000000..6a3e811e3 --- /dev/null +++ b/src/modules/uORB/topics/navigation_capabilities.h @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * 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 navigation_capabilities.h + * + * Definition of navigation capabilities uORB topic. + */ + +#ifndef TOPIC_NAVIGATION_CAPABILITIES_H_ +#define TOPIC_NAVIGATION_CAPABILITIES_H_ + +#include "../uORB.h" +#include <stdint.h> + +/** + * @addtogroup topics + * @{ + */ + +/** + * Airspeed + */ +struct navigation_capabilities_s { + float turn_distance; /**< the optimal distance to a waypoint to switch to the next */ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(navigation_capabilities); + +#endif diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h new file mode 100644 index 000000000..2e895c59c --- /dev/null +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -0,0 +1,94 @@ +/**************************************************************************** + * + * Copyright (C) 2008-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 + * 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 offboard_control_setpoint.h + * Definition of the manual_control_setpoint uORB topic. + */ + +#ifndef TOPIC_OFFBOARD_CONTROL_SETPOINT_H_ +#define TOPIC_OFFBOARD_CONTROL_SETPOINT_H_ + +#include <stdint.h> +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * 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 +{ + OFFBOARD_CONTROL_MODE_DIRECT = 0, + OFFBOARD_CONTROL_MODE_DIRECT_RATES = 1, + OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE = 2, + OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY = 3, + OFFBOARD_CONTROL_MODE_DIRECT_POSITION = 4, + OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 5, + OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 6, + OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 7, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */ +}; + +struct offboard_control_setpoint_s { + uint64_t timestamp; + + enum OFFBOARD_CONTROL_MODE mode; /**< The current control inputs mode */ + bool armed; /**< Armed flag set, yes / no */ + float p1; /**< ailerons roll / roll rate input */ + float p2; /**< elevator / pitch / pitch rate */ + float p3; /**< rudder / yaw rate / yaw */ + float p4; /**< throttle / collective thrust / altitude */ + + float override_mode_switch; + + float aux1_cam_pan_flaps; + float aux2_cam_tilt; + float aux3_cam_zoom; + float aux4_cam_roll; + +}; /**< offboard control inputs */ +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(offboard_control_setpoint); + +#endif diff --git a/src/modules/uORB/topics/omnidirectional_flow.h b/src/modules/uORB/topics/omnidirectional_flow.h new file mode 100644 index 000000000..8f4be3b3f --- /dev/null +++ b/src/modules/uORB/topics/omnidirectional_flow.h @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * 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 + * 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 optical_flow.h + * Definition of the optical flow uORB topic. + */ + +#ifndef TOPIC_OMNIDIRECTIONAL_FLOW_H_ +#define TOPIC_OMNIDIRECTIONAL_FLOW_H_ + +#include <stdint.h> +#include <stdbool.h> +#include "../uORB.h" + +/** + * @addtogroup topics + */ + +/** + * Omnidirectional optical flow in NED body frame in SI units. + * + * @see http://en.wikipedia.org/wiki/International_System_of_Units + */ +struct omnidirectional_flow_s { + + uint64_t timestamp; /**< in microseconds since system start */ + + uint16_t left[10]; /**< Left flow, in decipixels */ + uint16_t right[10]; /**< Right flow, in decipixels */ + float front_distance_m; /**< Altitude / distance to object front in meters */ + uint8_t quality; /**< Quality of the measurement, 0: bad quality, 255: maximum quality */ + uint8_t sensor_id; /**< id of the sensor emitting the flow value */ + +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(omnidirectional_flow); + +#endif diff --git a/src/modules/uORB/topics/optical_flow.h b/src/modules/uORB/topics/optical_flow.h new file mode 100644 index 000000000..98f0e3fa2 --- /dev/null +++ b/src/modules/uORB/topics/optical_flow.h @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * 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 + * 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 optical_flow.h + * Definition of the optical flow uORB topic. + */ + +#ifndef TOPIC_OPTICAL_FLOW_H_ +#define TOPIC_OPTICAL_FLOW_H_ + +#include <stdint.h> +#include <stdbool.h> +#include "../uORB.h" + +/** + * @addtogroup topics + */ + +/** + * Optical flow in NED body frame in SI units. + * + * @see http://en.wikipedia.org/wiki/International_System_of_Units + */ +struct optical_flow_s { + + uint64_t timestamp; /**< in microseconds since system start */ + + 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 */ + float flow_comp_y_m; /**< speed over ground in meters, rotation-compensated */ + float ground_distance_m; /**< Altitude / distance to ground in meters */ + uint8_t quality; /**< Quality of the measurement, 0: bad quality, 255: maximum quality */ + uint8_t sensor_id; /**< id of the sensor emitting the flow value */ + +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(optical_flow); + +#endif diff --git a/src/modules/uORB/topics/parameter_update.h b/src/modules/uORB/topics/parameter_update.h new file mode 100644 index 000000000..300e895c7 --- /dev/null +++ b/src/modules/uORB/topics/parameter_update.h @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file parameter_update.h + * Notification about a parameter update. + */ + +#ifndef TOPIC_PARAMETER_UPDATE_H +#define TOPIC_PARAMETER_UPDATE_H + +#include <stdint.h> +#include "../uORB.h" + +struct parameter_update_s { + /** time at which the latest parameter was updated */ + uint64_t timestamp; +}; + +ORB_DECLARE(parameter_update); + +#endif
\ No newline at end of file diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h new file mode 100644 index 000000000..9dd54df91 --- /dev/null +++ b/src/modules/uORB/topics/rc_channels.h @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: @author Nils Wenzler <wenzlern@student.ethz.ch> + * @author Ivan Ovinnikov <oivan@student.ethz.ch> + * @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 rc_channels.h + * Definition of the rc_channels uORB topic. + */ + +#ifndef RC_CHANNELS_H_ +#define RC_CHANNELS_H_ + +#include <stdint.h> +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * The number of RC channel inputs supported. + * Current (Q1/2013) radios support up to 18 channels, + * leaving at a sane value of 14. + */ +#define RC_CHANNELS_MAX 14 + +/** + * 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, + OVERRIDE = 4, + AUTO_MODE = 5, + MANUAL_MODE = 6, + SAS_MODE = 7, + RTL = 8, + OFFBOARD_MODE = 9, + FLAPS = 10, + AUX_1 = 11, + AUX_2 = 12, + AUX_3 = 13, + AUX_4 = 14, + AUX_5 = 15, + RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ +}; + +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_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 */ +}; /**< radio control channels. */ + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(rc_channels); + +#endif diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h new file mode 100644 index 000000000..9a76b5182 --- /dev/null +++ b/src/modules/uORB/topics/sensor_combined.h @@ -0,0 +1,117 @@ +/**************************************************************************** + * + * Copyright (C) 2008-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> + * + * 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 sensor_combined.h + * Definition of the sensor_combined uORB topic. + */ + +#ifndef SENSOR_COMBINED_H_ +#define SENSOR_COMBINED_H_ + +#include <stdint.h> +#include <stdbool.h> +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +enum MAGNETOMETER_MODE { + MAGNETOMETER_MODE_NORMAL = 0, + MAGNETOMETER_MODE_POSITIVE_BIAS, + MAGNETOMETER_MODE_NEGATIVE_BIAS +}; + +/** + * Sensor readings in raw and SI-unit form. + * + * These values are read from the sensors. Raw values are in sensor-specific units, + * the scaled values are in SI-units, as visible from the ending of the variable + * or the comments. The use of the SI fields is in general advised, as these fields + * are scaled and offset-compensated where possible and do not change with board + * revisions and sensor updates. + * + */ +struct sensor_combined_s { + + /* + * Actual data, this is specific to the type of data which is stored in this struct + * A line containing L0GME will be added by the Python logging code generator to the + * logged dataset. + */ + + /* NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only */ + + uint64_t timestamp; /**< Timestamp in microseconds since boot */ + + int16_t gyro_raw[3]; /**< Raw sensor values of angular velocity */ + uint16_t gyro_counter; /**< Number of raw measurments taken */ + float gyro_rad_s[3]; /**< Angular velocity in radian per seconds */ + + int16_t accelerometer_raw[3]; /**< Raw acceleration in NED body frame */ + uint32_t accelerometer_counter; /**< Number of raw acc measurements taken */ + float accelerometer_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 */ + int accelerometer_mode; /**< Accelerometer measurement mode */ + float accelerometer_range_m_s2; /**< Accelerometer measurement range in m/s^2 */ + + int16_t magnetometer_raw[3]; /**< Raw magnetic field in NED body frame */ + float magnetometer_ga[3]; /**< Magnetic field in NED body frame, in Gauss */ + int magnetometer_mode; /**< Magnetometer measurement mode */ + float magnetometer_range_ga; /**< ± measurement range in Gauss */ + float magnetometer_cuttoff_freq_hz; /**< Internal analog low pass frequency of sensor */ + uint32_t magnetometer_counter; /**< Number of raw mag measurements taken */ + + 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 mcu_temp_celcius; /**< Internal temperature measurement of MCU */ + uint32_t baro_counter; /**< Number of raw baro measurements taken */ + + float differential_pressure_pa; /**< Airspeed sensor differential pressure */ + uint32_t differential_pressure_counter; /**< Number of raw differential pressure measurements taken */ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(sensor_combined); + +#endif diff --git a/src/modules/uORB/topics/subsystem_info.h b/src/modules/uORB/topics/subsystem_info.h new file mode 100644 index 000000000..c415e832e --- /dev/null +++ b/src/modules/uORB/topics/subsystem_info.h @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> + * Thomas Gubler <thomasgubler@student.ethz.ch> + * 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 + * 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 subsystem_info.h + * Definition of the subsystem info topic. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + */ + +#ifndef TOPIC_SUBSYSTEM_INFO_H_ +#define TOPIC_SUBSYSTEM_INFO_H_ + +#include <stdint.h> +#include <stdbool.h> +#include "../uORB.h" + +/** + * @addtogroup topics + */ + +enum SUBSYSTEM_TYPE +{ + SUBSYSTEM_TYPE_GYRO = 1, + SUBSYSTEM_TYPE_ACC = 2, + SUBSYSTEM_TYPE_MAG = 4, + SUBSYSTEM_TYPE_ABSPRESSURE = 8, + SUBSYSTEM_TYPE_DIFFPRESSURE = 16, + SUBSYSTEM_TYPE_GPS = 32, + SUBSYSTEM_TYPE_OPTICALFLOW = 64, + SUBSYSTEM_TYPE_CVPOSITION = 128, + SUBSYSTEM_TYPE_LASERPOSITION = 256, + SUBSYSTEM_TYPE_EXTERNALGROUNDTRUTH = 512, + SUBSYSTEM_TYPE_ANGULARRATECONTROL = 1024, + SUBSYSTEM_TYPE_ATTITUDESTABILIZATION = 2048, + SUBSYSTEM_TYPE_YAWPOSITION = 4096, + SUBSYSTEM_TYPE_ALTITUDECONTROL = 16384, + SUBSYSTEM_TYPE_POSITIONCONTROL = 32768, + SUBSYSTEM_TYPE_MOTORCONTROL = 65536, + SUBSYSTEM_TYPE_RANGEFINDER = 131072 +}; + +/** + * State of individual sub systems + */ +struct subsystem_info_s { + bool present; + bool enabled; + bool ok; + + enum SUBSYSTEM_TYPE subsystem_type; +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(subsystem_info); + +#endif /* TOPIC_SUBSYSTEM_INFO_H_ */ + diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h new file mode 100644 index 000000000..f30852de5 --- /dev/null +++ b/src/modules/uORB/topics/telemetry_status.h @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * 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 telemetry_status.h + * + * Telemetry status topics - radio status outputs + */ + +#ifndef TOPIC_TELEMETRY_STATUS_H +#define TOPIC_TELEMETRY_STATUS_H + +#include <stdint.h> +#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 +}; + +struct telemetry_status_s { + uint64_t timestamp; + enum TELEMETRY_STATUS_RADIO_TYPE type; /**< type of the radio hardware */ + unsigned rssi; /**< local signal strength */ + unsigned remote_rssi; /**< remote signal strength */ + unsigned rxerrors; /**< receive errors */ + unsigned 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 */ +}; + +ORB_DECLARE(telemetry_status); + +#endif /* TOPIC_TELEMETRY_STATUS_H */
\ No newline at end of file diff --git a/src/modules/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h new file mode 100755 index 000000000..c31c81d0c --- /dev/null +++ b/src/modules/uORB/topics/vehicle_attitude.h @@ -0,0 +1,90 @@ +/**************************************************************************** + * + * Copyright (C) 2008-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> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vehicle_attitude.h + * Definition of the attitude uORB topic. + */ + +#ifndef VEHICLE_ATTITUDE_H_ +#define VEHICLE_ATTITUDE_H_ + +#include <stdint.h> +#include <stdbool.h> +#include "../uORB.h" + +/** + * @addtogroup topics + */ + +/** + * Attitude in NED body frame in SI units. + * + * @see http://en.wikipedia.org/wiki/International_System_of_Units + */ +struct vehicle_attitude_s { + + uint64_t timestamp; /**< in microseconds since system start */ + + /* This is similar to the mavlink message ATTITUDE, but for onboard use */ + + /** @warning roll, pitch and yaw have always to be valid, the rotation matrix and quaternion are optional */ + + float roll; /**< Roll angle (rad, Tait-Bryan, NED) */ + float pitch; /**< Pitch angle (rad, Tait-Bryan, NED) */ + float yaw; /**< Yaw angle (rad, Tait-Bryan, NED) */ + float rollspeed; /**< Roll angular speed (rad/s, Tait-Bryan, NED) */ + float pitchspeed; /**< Pitch angular speed (rad/s, Tait-Bryan, NED) */ + float yawspeed; /**< Yaw angular speed (rad/s, Tait-Bryan, NED) */ + float rollacc; /**< Roll angular accelration (rad/s, Tait-Bryan, NED) */ + float pitchacc; /**< Pitch angular acceleration (rad/s, Tait-Bryan, NED) */ + float yawacc; /**< Yaw angular acceleration (rad/s, Tait-Bryan, NED) */ + float rate_offsets[3]; /**< Offsets of the body angular rates from zero */ + float R[3][3]; /**< Rotation matrix body to world, (Tait-Bryan, NED) */ + float q[4]; /**< Quaternion (NED) */ + bool R_valid; /**< Rotation matrix valid */ + bool q_valid; /**< Quaternion valid */ + +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_attitude); + +#endif diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h new file mode 100644 index 000000000..a7cda34a8 --- /dev/null +++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h @@ -0,0 +1,78 @@ +/**************************************************************************** + * + * 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vehicle_attitude_setpoint.h + * Definition of the vehicle attitude setpoint uORB topic. + */ + +#ifndef TOPIC_VEHICLE_ATTITUDE_SETPOINT_H_ +#define TOPIC_VEHICLE_ATTITUDE_SETPOINT_H_ + +#include <stdint.h> +#include <stdbool.h> +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * vehicle attitude setpoint. + */ +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 */ + float pitch_body; /**< body angle in NED frame */ + float yaw_body; /**< body angle in NED frame */ + //float body_valid; /**< Set to true if body angles are valid */ + + float R_body[9]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */ + bool R_valid; /**< Set to true if rotation matrix is valid */ + + float thrust; /**< Thrust in Newton the power system should generate */ + +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_attitude_setpoint); + +#endif /* TOPIC_ARDRONE_CONTROL_H_ */ diff --git a/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h b/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h new file mode 100644 index 000000000..fbfab09f3 --- /dev/null +++ b/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Samuel Zihlmann <samuezih@ee.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 vehicle_bodyframe_speed_setpoint.h + * Definition of the bodyframe speed setpoint uORB topic. + */ + +#ifndef TOPIC_VEHICLE_BODYFRAME_SPEED_SETPOINT_H_ +#define TOPIC_VEHICLE_BODYFRAME_SPEED_SETPOINT_H_ + +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +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 */ + float vy; /**< in m/s */ +// float vz; /**< in m/s */ + float thrust_sp; + float yaw_sp; /**< in radian -PI +PI */ +}; /**< Speed in bodyframe to go to */ + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_bodyframe_speed_setpoint); + +#endif diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h new file mode 100644 index 000000000..fac571659 --- /dev/null +++ b/src/modules/uORB/topics/vehicle_command.h @@ -0,0 +1,140 @@ +/**************************************************************************** + * + * Copyright (C) 2008-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> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vehicle_command.h + * Definition of the vehicle command uORB topic. + */ + +#ifndef TOPIC_VEHICLE_COMMAND_H_ +#define TOPIC_VEHICLE_COMMAND_H_ + +#include <stdint.h> +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * Commands for commander app. + * + * 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_ENUM_END=401, /* | */ +}; + +/** + * Commands for commander app. + * + * 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, /* | */ +}; + + +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. */ + float param4; /**< Parameter 4, as defined by MAVLink VEHICLE_CMD enum. */ + float param5; /**< Parameter 5, as defined by MAVLink VEHICLE_CMD enum. */ + float param6; /**< Parameter 6, as defined by MAVLink VEHICLE_CMD enum. */ + float param7; /**< Parameter 7, as defined by MAVLink VEHICLE_CMD enum. */ + uint16_t command; /**< Command ID, as defined MAVLink by VEHICLE_CMD enum. */ + uint8_t target_system; /**< System which should execute the command */ + uint8_t target_component; /**< Component which should execute the command, 0 for all components */ + uint8_t source_system; /**< System sending the command */ + uint8_t source_component; /**< Component sending the command */ + uint8_t confirmation; /**< 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) */ +}; /**< command sent to vehicle */ + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_command); + + + +#endif diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h new file mode 100644 index 000000000..f036c7223 --- /dev/null +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * 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> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vehicle_global_position.h + * Definition of the global fused WGS84 position uORB topic. + */ + +#ifndef VEHICLE_GLOBAL_POSITION_T_H_ +#define VEHICLE_GLOBAL_POSITION_T_H_ + +#include <stdint.h> +#include <stdbool.h> +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + + /** + * Fused global position in WGS84. + * + * This struct contains the system's believ about its position. It is not the raw GPS + * measurement (@see vehicle_gps_position). This topic is usually published by the position + * 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 +{ + uint64_t timestamp; /**< time of this estimate, in microseconds since system start */ + uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ + bool valid; /**< true if position satisfies validity criteria of estimator */ + + int32_t lat; /**< Latitude in 1E7 degrees LOGME */ + int32_t lon; /**< Longitude in 1E7 degrees LOGME */ + float alt; /**< Altitude in meters LOGME */ + float relative_alt; /**< Altitude above home position in meters, LOGME */ + float vx; /**< Ground X Speed (Latitude), m/s in ENU LOGME */ + float vy; /**< Ground Y Speed (Longitude), m/s in ENU LOGME */ + float vz; /**< Ground Z Speed (Altitude), m/s in ENU LOGME */ + float hdg; /**< Compass heading in radians -PI..+PI. */ + +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_global_position); + +#endif diff --git a/src/modules/uORB/topics/vehicle_global_position_set_triplet.h b/src/modules/uORB/topics/vehicle_global_position_set_triplet.h new file mode 100644 index 000000000..318abba89 --- /dev/null +++ b/src/modules/uORB/topics/vehicle_global_position_set_triplet.h @@ -0,0 +1,78 @@ +/**************************************************************************** + * + * 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> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vehicle_global_position_setpoint.h + * Definition of the global WGS84 position setpoint uORB topic. + */ + +#ifndef TOPIC_VEHICLE_GLOBAL_POSITION_SET_TRIPLET_H_ +#define TOPIC_VEHICLE_GLOBAL_POSITION_SET_TRIPLET_H_ + +#include <stdint.h> +#include <stdbool.h> +#include "../uORB.h" + +#include "vehicle_global_position_setpoint.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * Global position setpoint triplet in WGS84 coordinates. + * + * This are the three next waypoints (or just the next two or one). + */ +struct vehicle_global_position_set_triplet_s +{ + bool previous_valid; + bool next_valid; + + struct vehicle_global_position_setpoint_s previous; + struct vehicle_global_position_setpoint_s current; + struct vehicle_global_position_setpoint_s next; +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_global_position_set_triplet); + +#endif diff --git a/src/modules/uORB/topics/vehicle_global_position_setpoint.h b/src/modules/uORB/topics/vehicle_global_position_setpoint.h new file mode 100644 index 000000000..3ae3ff28c --- /dev/null +++ b/src/modules/uORB/topics/vehicle_global_position_setpoint.h @@ -0,0 +1,84 @@ +/**************************************************************************** + * + * 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> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vehicle_global_position_setpoint.h + * Definition of the global WGS84 position setpoint uORB topic. + */ + +#ifndef TOPIC_VEHICLE_GLOBAL_POSITION_SETPOINT_H_ +#define TOPIC_VEHICLE_GLOBAL_POSITION_SETPOINT_H_ + +#include <stdint.h> +#include <stdbool.h> +#include "../uORB.h" +#include "mission.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * Global position setpoint in WGS84 coordinates. + * + * 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 vehicle_global_position_setpoint_s +{ + bool altitude_is_relative; /**< true if altitude is relative from start point */ + int32_t lat; /**< latitude in degrees * 1E7 */ + int32_t lon; /**< longitude in degrees * 1E7 */ + float altitude; /**< altitude in meters */ + float yaw; /**< in radians NED -PI..+PI */ + float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover */ + uint8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */ + enum NAV_CMD nav_cmd; /**< true if loitering is enabled */ + float param1; + float param2; + float param3; + float param4; +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_global_position_setpoint); + +#endif diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h new file mode 100644 index 000000000..0a7fb4e9d --- /dev/null +++ b/src/modules/uORB/topics/vehicle_gps_position.h @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * 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> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vehicle_gps_position.h + * Definition of the GPS WGS84 uORB topic. + */ + +#ifndef TOPIC_VEHICLE_GPS_H_ +#define TOPIC_VEHICLE_GPS_H_ + +#include <stdint.h> +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * GPS position in WGS84 coordinates. + */ +struct vehicle_gps_position_s +{ + uint64_t timestamp_position; /**< Timestamp for position information */ + int32_t lat; /**< Latitude in 1E7 degrees */ + int32_t lon; /**< Longitude in 1E7 degrees */ + int32_t alt; /**< Altitude in 1E3 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 */ + float c_variance_rad; /**< course accuracy estimate rad */ + uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ + + float eph_m; /**< GPS HDOP horizontal dilution of position in m */ + float epv_m; /**< GPS VDOP horizontal dilution of position in m */ + + uint64_t timestamp_velocity; /**< Timestamp for velocity informations */ + float vel_m_s; /**< GPS ground speed (m/s) */ + float vel_n_m_s; /**< GPS ground speed in m/s */ + float vel_e_m_s; /**< GPS ground speed in m/s */ + float vel_d_m_s; /**< GPS ground speed in m/s */ + float cog_rad; /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */ + bool vel_ned_valid; /**< Flag to indicate if NED speed is valid */ + + uint64_t timestamp_time; /**< Timestamp for time information */ + uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module */ + + uint64_t timestamp_satellites; /**< Timestamp for sattelite information */ + uint8_t satellites_visible; /**< Number of satellites visible. If unknown, set to 255 */ + uint8_t satellite_prn[20]; /**< Global satellite ID */ + uint8_t satellite_used[20]; /**< 0: Satellite not used, 1: used for localization */ + uint8_t satellite_elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ + uint8_t satellite_azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ + uint8_t satellite_snr[20]; /**< Signal to noise ratio of satellite */ + bool satellite_info_available; /**< 0 for no info, 1 for info available */ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_gps_position); + +#endif diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h new file mode 100644 index 000000000..76eddeacd --- /dev/null +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vehicle_local_position.h + * Definition of the local fused NED position uORB topic. + */ + +#ifndef TOPIC_VEHICLE_LOCAL_POSITION_H_ +#define TOPIC_VEHICLE_LOCAL_POSITION_H_ + +#include <stdint.h> +#include <stdbool.h> +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * Fused local position in NED. + */ +struct vehicle_local_position_s +{ + uint64_t timestamp; /**< time of this estimate, in microseconds since system start */ + bool valid; /**< true if position satisfies validity criteria of estimator */ + + float x; /**< X positin in meters in NED earth-fixed frame */ + float y; /**< X positin in meters in NED earth-fixed frame */ + float z; /**< Z positin in meters in NED earth-fixed frame (negative altitude) */ + float absolute_alt; /**< Altitude as defined by pressure / GPS, LOGME */ + float vx; /**< Ground X Speed (Latitude), m/s in NED LOGME */ + float vy; /**< Ground Y Speed (Longitude), m/s in NED LOGME */ + float vz; /**< Ground Z Speed (Altitude), m/s in NED LOGME */ + float hdg; /**< Compass heading in radians -PI..+PI. */ + + // TODO Add covariances here + + /* Reference position in GPS / WGS84 frame */ + uint64_t home_timestamp;/**< Time when home position was set */ + int32_t home_lat; /**< Latitude in 1E7 degrees LOGME */ + int32_t home_lon; /**< Longitude in 1E7 degrees LOGME */ + float home_alt; /**< Altitude in meters LOGME */ + float home_hdg; /**< Compass heading in radians -PI..+PI. */ + +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_local_position); + +#endif diff --git a/src/modules/uORB/topics/vehicle_local_position_setpoint.h b/src/modules/uORB/topics/vehicle_local_position_setpoint.h new file mode 100644 index 000000000..d24d81e3a --- /dev/null +++ b/src/modules/uORB/topics/vehicle_local_position_setpoint.h @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * 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> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vehicle_local_position_setpoint.h + * Definition of the local NED position setpoint uORB topic. + */ + +#ifndef TOPIC_VEHICLE_LOCAL_POSITION_SETPOINT_H_ +#define TOPIC_VEHICLE_LOCAL_POSITION_SETPOINT_H_ + +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +struct vehicle_local_position_setpoint_s +{ + float x; /**< in meters NED */ + float y; /**< in meters NED */ + float z; /**< in meters NED */ + float yaw; /**< in radians NED -PI..+PI */ +}; /**< Local position in NED frame to go to */ + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_local_position_setpoint); + +#endif diff --git a/src/modules/uORB/topics/vehicle_rates_setpoint.h b/src/modules/uORB/topics/vehicle_rates_setpoint.h new file mode 100644 index 000000000..46e62c4b7 --- /dev/null +++ b/src/modules/uORB/topics/vehicle_rates_setpoint.h @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vehicle_rates_setpoint.h + * Definition of the vehicle rates setpoint topic + */ + +#ifndef TOPIC_VEHICLE_RATES_SETPOINT_H_ +#define TOPIC_VEHICLE_RATES_SETPOINT_H_ + +#include <stdint.h> +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ +struct vehicle_rates_setpoint_s +{ + uint64_t timestamp; /**< in microseconds since system start */ + + float roll; /**< body angular rates in NED frame */ + float pitch; /**< body angular rates in NED frame */ + float yaw; /**< body angular rates in NED frame */ + float thrust; /**< thrust normalized to 0..1 */ + +}; /**< vehicle_rates_setpoint */ + + /** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_rates_setpoint); + +#endif diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h new file mode 100644 index 000000000..c7c1048f6 --- /dev/null +++ b/src/modules/uORB/topics/vehicle_status.h @@ -0,0 +1,226 @@ +/**************************************************************************** + * + * 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> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vehicle_status.h + * Definition of the vehicle_status uORB topic. + * + * Published the state machine and the system status bitfields + * (see SYS_STATUS mavlink message), published only by commander app. + * + * All apps should write to subsystem_info: + * + * (any app) --> subsystem_info (published) --> (commander app state machine) --> vehicle_status --> (mavlink app) + */ + +#ifndef VEHICLE_STATUS_H_ +#define VEHICLE_STATUS_H_ + +#include <stdint.h> +#include <stdbool.h> +#include "../uORB.h" + +/** + * @addtogroup topics @{ + */ + +/* State Machine */ +typedef enum +{ + SYSTEM_STATE_PREFLIGHT = 0, + SYSTEM_STATE_STANDBY = 1, + SYSTEM_STATE_GROUND_READY = 2, + SYSTEM_STATE_MANUAL = 3, + SYSTEM_STATE_STABILIZED = 4, + SYSTEM_STATE_AUTO = 5, + SYSTEM_STATE_MISSION_ABORT = 6, + SYSTEM_STATE_EMCY_LANDING = 7, + SYSTEM_STATE_EMCY_CUTOFF = 8, + SYSTEM_STATE_GROUND_ERROR = 9, + SYSTEM_STATE_REBOOT= 10, + +} commander_state_machine_t; + +enum VEHICLE_MODE_FLAG { + VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, + VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, + VEHICLE_MODE_FLAG_HIL_ENABLED = 32, + VEHICLE_MODE_FLAG_STABILIZED_ENABLED = 16, + VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8, + VEHICLE_MODE_FLAG_AUTO_ENABLED = 4, + VEHICLE_MODE_FLAG_TEST_ENABLED = 2, + VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1 +}; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */ + +enum VEHICLE_FLIGHT_MODE { + VEHICLE_FLIGHT_MODE_MANUAL = 0, /**< direct manual control, exact mode determined by VEHICLE_MANUAL_CONTROL_MODE */ + VEHICLE_FLIGHT_MODE_STAB, /**< attitude or rate stabilization plus velocity or position stabilization */ + VEHICLE_FLIGHT_MODE_HOLD, /**< hold current position (hover or loiter around position when switched) */ + VEHICLE_FLIGHT_MODE_AUTO /**< attitude or rate stabilization plus absolute position control and waypoints */ +}; + +enum VEHICLE_MANUAL_CONTROL_MODE { + VEHICLE_MANUAL_CONTROL_MODE_DIRECT = 0, /**< no attitude control, direct stick input mixing (only fixed wing) */ + VEHICLE_MANUAL_CONTROL_MODE_RATES, /**< body rates control mode */ + VEHICLE_MANUAL_CONTROL_MODE_SAS /**< stability augmented system (SAS) mode */ +}; + +enum VEHICLE_MANUAL_SAS_MODE { + VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS = 0, /**< roll, pitch and yaw absolute */ + VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE, /**< roll and pitch absolute, yaw rate */ + VEHICLE_MANUAL_SAS_MODE_SIMPLE, /**< simple mode (includes altitude hold) */ + VEHICLE_MANUAL_SAS_MODE_ALTITUDE /**< altitude hold */ +}; + +/** + * 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, /* | */ +}; + +enum VEHICLE_BATTERY_WARNING { + VEHICLE_BATTERY_WARNING_NONE = 0, /**< no battery low voltage warning active */ + VEHICLE_BATTERY_WARNING_WARNING, /**< warning of low voltage 1. stage */ + VEHICLE_BATTERY_WARNING_ALERT /**< aleting of low voltage 2. stage */ +}; + + +/** + * state machine / state of vehicle. + * + * Encodes the complete system state and is set by the commander app. + */ +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 */ + uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ + uint64_t failsave_lowlevel_start_time; /**< time when the lowlevel failsafe flag was set */ + //uint64_t failsave_highlevel_begin; TO BE COMPLETED + + commander_state_machine_t state_machine; /**< current flight state, main state machine */ + enum VEHICLE_FLIGHT_MODE flight_mode; /**< current flight mode, as defined by mode switch */ + enum VEHICLE_MANUAL_CONTROL_MODE manual_control_mode; /**< current attitude control mode, as defined by VEHICLE_ATTITUDE_MODE enum */ + enum VEHICLE_MANUAL_SAS_MODE manual_sas_mode; /**< current stabilization mode */ + int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ + int32_t system_id; /**< system id, inspired by MAVLink's system ID field */ + int32_t component_id; /**< subsystem / component id, inspired by MAVLink's component ID field */ + + /* system flags - these represent the state predicates */ + + bool flag_system_armed; /**< true is motors / actuators are armed */ + bool flag_control_manual_enabled; /**< true if manual input is mixed in */ + bool flag_control_offboard_enabled; /**< true if offboard control input is on */ + bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */ + + bool flag_control_rates_enabled; /**< true if rates are stabilized */ + bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ + bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ + bool flag_control_position_enabled; /**< true if position is controlled */ + + bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */ + bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */ + bool flag_preflight_accel_calibration; + bool flag_preflight_airspeed_calibration; + + bool rc_signal_found_once; + bool rc_signal_lost; /**< true if RC reception is terminally lost */ + bool rc_signal_cutting_off; /**< true if RC reception is weak / cutting off */ + uint64_t rc_signal_lost_interval; /**< interval in microseconds since when no RC signal is available */ + + bool offboard_control_signal_found_once; + bool offboard_control_signal_lost; + bool offboard_control_signal_weak; + uint64_t offboard_control_signal_lost_interval; /**< interval in microseconds without an offboard control message */ + + bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ + //bool failsave_highlevel; + + /* see SYS_STATUS mavlink message for the following */ + uint32_t onboard_control_sensors_present; + uint32_t onboard_control_sensors_enabled; + uint32_t onboard_control_sensors_health; + float load; + float voltage_battery; + float current_battery; + float battery_remaining; + enum VEHICLE_BATTERY_WARNING battery_warning; /**< current battery warning mode, as defined by VEHICLE_BATTERY_WARNING enum */ + uint16_t drop_rate_comm; + uint16_t errors_comm; + uint16_t errors_count1; + uint16_t errors_count2; + uint16_t errors_count3; + uint16_t errors_count4; + + bool flag_global_position_valid; /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */ + bool flag_local_position_valid; + bool flag_vector_flight_mode_ok; /**< position estimation, battery voltage and other critical subsystems are good for autonomous flight */ + bool flag_auto_flight_mode_ok; /**< conditions of vector flight mode apply plus a valid takeoff position lock has been aquired */ + bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ + bool flag_valid_launch_position; /**< indicates a valid launch position */ + bool flag_valid_home_position; /**< indicates a valid home position (a valid home position is not always a valid launch) */ + bool flag_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_status); + +#endif diff --git a/src/modules/uORB/topics/vehicle_vicon_position.h b/src/modules/uORB/topics/vehicle_vicon_position.h new file mode 100644 index 000000000..0822fa89a --- /dev/null +++ b/src/modules/uORB/topics/vehicle_vicon_position.h @@ -0,0 +1,78 @@ +/**************************************************************************** + * + * 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vehicle_vicon_position.h + * Definition of the raw VICON Motion Capture position + */ + +#ifndef TOPIC_VEHICLE_VICON_POSITION_H_ +#define TOPIC_VEHICLE_VICON_POSITION_H_ + +#include <stdint.h> +#include <stdbool.h> +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * Fused local position in NED. + */ +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 */ + + float x; /**< X positin in meters in NED earth-fixed frame */ + float y; /**< X positin in meters in NED earth-fixed frame */ + float z; /**< Z positin in meters in NED earth-fixed frame (negative altitude) */ + float roll; + float pitch; + float yaw; + + // TODO Add covariances here + +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_vicon_position); + +#endif diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp new file mode 100644 index 000000000..7abbf42ae --- /dev/null +++ b/src/modules/uORB/uORB.cpp @@ -0,0 +1,1006 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file uORB.cpp + * A lightweight object broker. + */ + +#include <nuttx/config.h> + +#include <drivers/device/device.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <string.h> +#include <stdlib.h> +#include <fcntl.h> +#include <poll.h> +#include <errno.h> +#include <stdio.h> +#include <math.h> +#include <unistd.h> + +#include <nuttx/arch.h> +#include <nuttx/wqueue.h> +#include <nuttx/clock.h> + +#include <drivers/drv_hrt.h> + +#include <drivers/drv_orb_dev.h> + +#include "uORB.h" + +/** + * Utility functions. + */ +namespace +{ + +static const unsigned orb_maxpath = 64; + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +enum Flavor { + PUBSUB, + PARAM +}; + +int +node_mkpath(char *buf, Flavor f, const struct orb_metadata *meta) +{ + unsigned len; + + len = snprintf(buf, orb_maxpath, "/%s/%s", + (f == PUBSUB) ? "obj" : "param", + meta->o_name); + + if (len >= orb_maxpath) + return -ENAMETOOLONG; + + return OK; +} + +} + +/** + * Per-object device instance. + */ +class ORBDevNode : public device::CDev +{ +public: + ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path); + ~ORBDevNode(); + + virtual int open(struct file *filp); + virtual int close(struct file *filp); + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + static ssize_t publish(const orb_metadata *meta, orb_advert_t handle, const void *data); + +protected: + virtual pollevent_t poll_state(struct file *filp); + virtual void poll_notify_one(struct pollfd *fds, pollevent_t events); + +private: + struct SubscriberData { + unsigned generation; /**< last generation the subscriber has seen */ + unsigned update_interval; /**< if nonzero minimum interval between updates */ + struct hrt_call update_call; /**< deferred wakeup call if update_period is nonzero */ + void *poll_priv; /**< saved copy of fds->f_priv while poll is active */ + bool update_reported; /**< true if we have reported the update via poll/check */ + }; + + const struct orb_metadata *_meta; /**< object metadata information */ + uint8_t *_data; /**< allocated object buffer */ + hrt_abstime _last_update; /**< time the object was last updated */ + volatile unsigned _generation; /**< object generation count */ + pid_t _publisher; /**< if nonzero, current publisher */ + + SubscriberData *filp_to_sd(struct file *filp) { + SubscriberData *sd = (SubscriberData *)(filp->f_priv); + return sd; + } + + /** + * Perform a deferred update for a rate-limited subscriber. + */ + void update_deferred(); + + /** + * Bridge from hrt_call to update_deferred + * + * void *arg ORBDevNode pointer for which the deferred update is performed. + */ + static void update_deferred_trampoline(void *arg); + + /** + * Check whether a topic appears updated to a subscriber. + * + * @param sd The subscriber for whom to check. + * @return True if the topic should appear updated to the subscriber + */ + bool appears_updated(SubscriberData *sd); +}; + +ORBDevNode::ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path) : + CDev(name, path), + _meta(meta), + _data(nullptr), + _last_update(0), + _generation(0), + _publisher(0) +{ + // enable debug() calls + _debug_enabled = true; +} + +ORBDevNode::~ORBDevNode() +{ + if (_data != nullptr) + delete[] _data; +} + +int +ORBDevNode::open(struct file *filp) +{ + int ret; + + /* is this a publisher? */ + if (filp->f_oflags == O_WRONLY) { + + /* become the publisher if we can */ + lock(); + + if (_publisher == 0) { + _publisher = getpid(); + ret = OK; + + } else { + ret = -EBUSY; + } + + unlock(); + + /* now complete the open */ + if (ret == OK) { + ret = CDev::open(filp); + + /* open failed - not the publisher anymore */ + if (ret != OK) + _publisher = 0; + } + + return ret; + } + + /* is this a new subscriber? */ + if (filp->f_oflags == O_RDONLY) { + + /* allocate subscriber data */ + SubscriberData *sd = new SubscriberData; + + if (nullptr == sd) + return -ENOMEM; + + memset(sd, 0, sizeof(*sd)); + + /* default to no pending update */ + sd->generation = _generation; + + filp->f_priv = (void *)sd; + + ret = CDev::open(filp); + + if (ret != OK) + free(sd); + + return ret; + } + + /* can only be pub or sub, not both */ + return -EINVAL; +} + +int +ORBDevNode::close(struct file *filp) +{ + /* is this the publisher closing? */ + if (getpid() == _publisher) { + _publisher = 0; + + } else { + SubscriberData *sd = filp_to_sd(filp); + + if (sd != nullptr) + delete sd; + } + + return CDev::close(filp); +} + +ssize_t +ORBDevNode::read(struct file *filp, char *buffer, size_t buflen) +{ + SubscriberData *sd = (SubscriberData *)filp_to_sd(filp); + + /* if the object has not been written yet, return zero */ + if (_data == nullptr) + return 0; + + /* if the caller's buffer is the wrong size, that's an error */ + if (buflen != _meta->o_size) + return -EIO; + + /* + * Perform an atomic copy & state update + */ + irqstate_t flags = irqsave(); + + /* if the caller doesn't want the data, don't give it to them */ + if (nullptr != buffer) + memcpy(buffer, _data, _meta->o_size); + + /* track the last generation that the file has seen */ + sd->generation = _generation; + + /* + * Clear the flag that indicates that an update has been reported, as + * we have just collected it. + */ + sd->update_reported = false; + + irqrestore(flags); + + return _meta->o_size; +} + +ssize_t +ORBDevNode::write(struct file *filp, const char *buffer, size_t buflen) +{ + /* + * Writes are legal from interrupt context as long as the + * object has already been initialised from thread context. + * + * Writes outside interrupt context will allocate the object + * if it has not yet been allocated. + * + * Note that filp will usually be NULL. + */ + if (nullptr == _data) { + if (!up_interrupt_context()) { + + lock(); + + /* re-check size */ + if (nullptr == _data) + _data = new uint8_t[_meta->o_size]; + + unlock(); + } + + /* failed or could not allocate */ + if (nullptr == _data) + return -ENOMEM; + } + + /* If write size does not match, that is an error */ + if (_meta->o_size != buflen) + return -EIO; + + /* Perform an atomic copy. */ + irqstate_t flags = irqsave(); + memcpy(_data, buffer, _meta->o_size); + irqrestore(flags); + + /* update the timestamp and generation count */ + _last_update = hrt_absolute_time(); + _generation++; + + /* notify any poll waiters */ + poll_notify(POLLIN); + + return _meta->o_size; +} + +int +ORBDevNode::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + SubscriberData *sd = filp_to_sd(filp); + + switch (cmd) { + case ORBIOCLASTUPDATE: + *(hrt_abstime *)arg = _last_update; + return OK; + + case ORBIOCUPDATED: + *(bool *)arg = appears_updated(sd); + return OK; + + case ORBIOCSETINTERVAL: + sd->update_interval = arg; + return OK; + + case ORBIOCGADVERTISER: + *(uintptr_t *)arg = (uintptr_t)this; + return OK; + + default: + /* give it to the superclass */ + return CDev::ioctl(filp, cmd, arg); + } +} + +ssize_t +ORBDevNode::publish(const orb_metadata *meta, orb_advert_t handle, const void *data) +{ + ORBDevNode *devnode = (ORBDevNode *)handle; + int ret; + + /* this is a bit risky, since we are trusting the handle in order to deref it */ + if (devnode->_meta != meta) { + errno = EINVAL; + return ERROR; + } + + /* call the devnode write method with no file pointer */ + ret = devnode->write(nullptr, (const char *)data, meta->o_size); + + if (ret < 0) + return ERROR; + + if (ret != (int)meta->o_size) { + errno = EIO; + return ERROR; + } + + return OK; +} + +pollevent_t +ORBDevNode::poll_state(struct file *filp) +{ + SubscriberData *sd = filp_to_sd(filp); + + /* + * If the topic appears updated to the subscriber, say so. + */ + if (appears_updated(sd)) + return POLLIN; + + return 0; +} + +void +ORBDevNode::poll_notify_one(struct pollfd *fds, pollevent_t events) +{ + SubscriberData *sd = filp_to_sd((struct file *)fds->priv); + + /* + * If the topic looks updated to the subscriber, go ahead and notify them. + */ + if (appears_updated(sd)) + CDev::poll_notify_one(fds, events); +} + +bool +ORBDevNode::appears_updated(SubscriberData *sd) +{ + /* assume it doesn't look updated */ + bool ret = false; + + /* avoid racing between interrupt and non-interrupt context calls */ + irqstate_t state = irqsave(); + + /* check if this topic has been published yet, if not bail out */ + if (_data == nullptr) { + ret = false; + goto out; + } + + /* + * If the subscriber's generation count matches the update generation + * count, there has been no update from their perspective; if they + * don't match then we might have a visible update. + */ + while (sd->generation != _generation) { + + /* + * Handle non-rate-limited subscribers. + */ + if (sd->update_interval == 0) { + ret = true; + break; + } + + /* + * If we have previously told the subscriber that there is data, + * and they have not yet collected it, continue to tell them + * that there has been an update. This mimics the non-rate-limited + * behaviour where checking / polling continues to report an update + * until the topic is read. + */ + if (sd->update_reported) { + ret = true; + break; + } + + /* + * If the interval timer is still running, the topic should not + * appear updated, even though at this point we know that it has. + * We have previously been through here, so the subscriber + * must have collected the update we reported, otherwise + * update_reported would still be true. + */ + if (!hrt_called(&sd->update_call)) + break; + + /* + * Make sure that we don't consider the topic to be updated again + * until the interval has passed once more by restarting the interval + * timer and thereby re-scheduling a poll notification at that time. + */ + hrt_call_after(&sd->update_call, + sd->update_interval, + &ORBDevNode::update_deferred_trampoline, + (void *)this); + + /* + * Remember that we have told the subscriber that there is data. + */ + sd->update_reported = true; + ret = true; + + break; + } + +out: + irqrestore(state); + + /* consider it updated */ + return ret; +} + +void +ORBDevNode::update_deferred() +{ + /* + * Instigate a poll notification; any subscribers whose intervals have + * expired will be woken. + */ + poll_notify(POLLIN); +} + +void +ORBDevNode::update_deferred_trampoline(void *arg) +{ + ORBDevNode *node = (ORBDevNode *)arg; + + node->update_deferred(); +} + +/** + * Master control device for ObjDev. + * + * Used primarily to create new objects via the ORBIOCCREATE + * ioctl. + */ +class ORBDevMaster : public device::CDev +{ +public: + ORBDevMaster(Flavor f); + ~ORBDevMaster(); + + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); +private: + Flavor _flavor; +}; + +ORBDevMaster::ORBDevMaster(Flavor f) : + CDev((f == PUBSUB) ? "obj_master" : "param_master", + (f == PUBSUB) ? TOPIC_MASTER_DEVICE_PATH : PARAM_MASTER_DEVICE_PATH), + _flavor(f) +{ + // enable debug() calls + _debug_enabled = true; + +} + +ORBDevMaster::~ORBDevMaster() +{ +} + +int +ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + int ret; + + switch (cmd) { + case ORBIOCADVERTISE: { + const struct orb_metadata *meta = (const struct orb_metadata *)arg; + const char *objname; + char nodepath[orb_maxpath]; + ORBDevNode *node; + + /* construct a path to the node - this also checks the node name */ + ret = node_mkpath(nodepath, _flavor, meta); + + if (ret != OK) + return ret; + + /* driver wants a permanent copy of the node name, so make one here */ + objname = strdup(meta->o_name); + + if (objname == nullptr) + return -ENOMEM; + + /* construct the new node */ + node = new ORBDevNode(meta, objname, nodepath); + + /* initialise the node - this may fail if e.g. a node with this name already exists */ + if (node != nullptr) + ret = node->init(); + + /* if we didn't get a device, that's bad */ + if (node == nullptr) + return -ENOMEM; + + /* if init failed, discard the node and its name */ + if (ret != OK) { + delete node; + free((void *)objname); + } + + return ret; + } + + default: + /* give it to the superclass */ + return CDev::ioctl(filp, cmd, arg); + } +} + + +/** + * Local functions in support of the shell command. + */ + +namespace +{ + +ORBDevMaster *g_dev; + +struct orb_test { + int val; +}; + +ORB_DEFINE(orb_test, struct orb_test); + +int +test_fail(const char *fmt, ...) +{ + va_list ap; + + fprintf(stderr, "FAIL: "); + va_start(ap, fmt); + vfprintf(stderr, fmt, ap); + va_end(ap); + fprintf(stderr, "\n"); + fflush(stderr); + return ERROR; +} + +int +test_note(const char *fmt, ...) +{ + va_list ap; + + fprintf(stderr, "note: "); + va_start(ap, fmt); + vfprintf(stderr, fmt, ap); + va_end(ap); + fprintf(stderr, "\n"); + fflush(stderr); + return OK; +} + +ORB_DECLARE(sensor_combined); + +int +test() +{ + struct orb_test t, u; + int pfd, sfd; + bool updated; + + t.val = 0; + pfd = orb_advertise(ORB_ID(orb_test), &t); + + if (pfd < 0) + return test_fail("advertise failed: %d", errno); + + test_note("publish handle 0x%08x", pfd); + sfd = orb_subscribe(ORB_ID(orb_test)); + + if (sfd < 0) + return test_fail("subscribe failed: %d", errno); + + test_note("subscribe fd %d", sfd); + u.val = 1; + + if (OK != orb_copy(ORB_ID(orb_test), sfd, &u)) + return test_fail("copy(1) failed: %d", errno); + + if (u.val != t.val) + return test_fail("copy(1) mismatch: %d expected %d", u.val, t.val); + + if (OK != orb_check(sfd, &updated)) + return test_fail("check(1) failed"); + + if (updated) + return test_fail("spurious updated flag"); + + t.val = 2; + test_note("try publish"); + + if (OK != orb_publish(ORB_ID(orb_test), pfd, &t)) + return test_fail("publish failed"); + + if (OK != orb_check(sfd, &updated)) + return test_fail("check(2) failed"); + + if (!updated) + return test_fail("missing updated flag"); + + if (OK != orb_copy(ORB_ID(orb_test), sfd, &u)) + return test_fail("copy(2) failed: %d", errno); + + if (u.val != t.val) + return test_fail("copy(2) mismatch: %d expected %d", u.val, t.val); + + orb_unsubscribe(sfd); + close(pfd); + +#if 0 + /* this is a hacky test that exploits the sensors app to test rate-limiting */ + + sfd = orb_subscribe(ORB_ID(sensor_combined)); + + hrt_abstime start, end; + unsigned count; + + start = hrt_absolute_time(); + count = 0; + + do { + orb_check(sfd, &updated); + + if (updated) { + orb_copy(ORB_ID(sensor_combined), sfd, nullptr); + count++; + } + } while (count < 100); + + end = hrt_absolute_time(); + test_note("full-speed, 100 updates in %llu", end - start); + + orb_set_interval(sfd, 10); + + start = hrt_absolute_time(); + count = 0; + + do { + orb_check(sfd, &updated); + + if (updated) { + orb_copy(ORB_ID(sensor_combined), sfd, nullptr); + count++; + } + } while (count < 100); + + end = hrt_absolute_time(); + test_note("100Hz, 100 updates in %llu", end - start); + + orb_unsubscribe(sfd); +#endif + + return test_note("PASS"); +} + +int +info() +{ + return OK; +} + + +} // namespace + +/* + * uORB server 'main'. + */ +extern "C" { __EXPORT int uorb_main(int argc, char *argv[]); } + +int +uorb_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + * + * XXX it would be nice to have a wrapper for this... + */ + if (!strcmp(argv[1], "start")) { + + if (g_dev != nullptr) { + fprintf(stderr, "[uorb] already loaded\n"); + /* user wanted to start uorb, its already running, no error */ + return 0; + } + + /* create the driver */ + g_dev = new ORBDevMaster(PUBSUB); + + if (g_dev == nullptr) { + fprintf(stderr, "[uorb] driver alloc failed\n"); + return -ENOMEM; + } + + if (OK != g_dev->init()) { + fprintf(stderr, "[uorb] driver init failed\n"); + delete g_dev; + g_dev = nullptr; + return -EIO; + } + + printf("[uorb] ready\n"); + return OK; + } + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) + return test(); + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "status")) + return info(); + + fprintf(stderr, "unrecognised command, try 'start', 'test' or 'status'\n"); + return -EINVAL; +} + +/* + * Library functions. + */ +namespace +{ + +void debug(const char *fmt, ...) +{ + va_list ap; + + va_start(ap, fmt); + vfprintf(stderr, fmt, ap); + va_end(ap); + fprintf(stderr, "\n"); + fflush(stderr); + usleep(100000); +} + +/** + * Advertise a node; don't consider it an error if the node has + * already been advertised. + * + * @todo verify that the existing node is the same as the one + * we tried to advertise. + */ +int +node_advertise(const struct orb_metadata *meta) +{ + int fd = -1; + int ret = ERROR; + + /* open the control device */ + fd = open(TOPIC_MASTER_DEVICE_PATH, 0); + + if (fd < 0) + goto out; + + /* advertise the object */ + ret = ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)meta); + + /* it's OK if it already exists */ + if ((OK != ret) && (EEXIST == errno)) + ret = OK; + +out: + + if (fd >= 0) + close(fd); + + return ret; +} + +/** + * Common implementation for orb_advertise and orb_subscribe. + * + * Handles creation of the object and the initial publication for + * advertisers. + */ +int +node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool advertiser) +{ + char path[orb_maxpath]; + int fd, ret; + + /* + * If meta is null, the object was not defined, i.e. it is not + * known to the system. We can't advertise/subscribe such a thing. + */ + if (nullptr == meta) { + errno = ENOENT; + return ERROR; + } + + /* + * Advertiser must publish an initial value. + */ + if (advertiser && (data == nullptr)) { + errno = EINVAL; + return ERROR; + } + + /* + * Generate the path to the node and try to open it. + */ + ret = node_mkpath(path, f, meta); + + if (ret != OK) { + errno = -ret; + return ERROR; + } + + /* open the path as either the advertiser or the subscriber */ + fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY); + + /* we may need to advertise the node... */ + if (fd < 0) { + + /* try to create the node */ + ret = node_advertise(meta); + + /* on success, try the open again */ + if (ret == OK) + fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY); + } + + if (fd < 0) { + errno = EIO; + return ERROR; + } + + /* everything has been OK, we can return the handle now */ + return fd; +} + +} // namespace + +orb_advert_t +orb_advertise(const struct orb_metadata *meta, const void *data) +{ + int result, fd; + orb_advert_t advertiser; + + /* open the node as an advertiser */ + fd = node_open(PUBSUB, meta, data, true); + if (fd == ERROR) + return ERROR; + + /* get the advertiser handle and close the node */ + result = ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser); + close(fd); + if (result == ERROR) + return ERROR; + + /* the advertiser must perform an initial publish to initialise the object */ + result= orb_publish(meta, advertiser, data); + if (result == ERROR) + return ERROR; + + return advertiser; +} + +int +orb_subscribe(const struct orb_metadata *meta) +{ + return node_open(PUBSUB, meta, nullptr, false); +} + +int +orb_unsubscribe(int handle) +{ + return close(handle); +} + +int +orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) +{ + return ORBDevNode::publish(meta, handle, data); +} + +int +orb_copy(const struct orb_metadata *meta, int handle, void *buffer) +{ + int ret; + + ret = read(handle, buffer, meta->o_size); + + if (ret < 0) + return ERROR; + + if (ret != (int)meta->o_size) { + errno = EIO; + return ERROR; + } + + return OK; +} + +int +orb_check(int handle, bool *updated) +{ + return ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated); +} + +int +orb_stat(int handle, uint64_t *time) +{ + return ioctl(handle, ORBIOCLASTUPDATE, (unsigned long)(uintptr_t)time); +} + +int +orb_set_interval(int handle, unsigned interval) +{ + return ioctl(handle, ORBIOCSETINTERVAL, interval * 1000); +} + diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h new file mode 100644 index 000000000..82ff46ad2 --- /dev/null +++ b/src/modules/uORB/uORB.h @@ -0,0 +1,264 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef _UORB_UORB_H +#define _UORB_UORB_H + +/** + * @file uORB.h + * API for the uORB lightweight object broker. + */ + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> + +// Hack until everything is using this header +#include <systemlib/visibility.h> + +/** + * Object metadata. + */ +struct orb_metadata { + const char *o_name; /**< unique object name */ + const size_t o_size; /**< object size */ +}; + +typedef const struct orb_metadata *orb_id_t; + +/** + * Generates a pointer to the uORB metadata structure for + * a given topic. + * + * The topic must have been declared previously in scope + * with ORB_DECLARE(). + * + * @param _name The name of the topic. + */ +#define ORB_ID(_name) &__orb_##_name + +/** + * Declare (prototype) the uORB metadata for a topic. + * + * Note that optional topics are declared weak; this allows a potential + * subscriber to attempt to subscribe to a topic that is not known to the + * system at runtime. The ORB_ID() macro will return NULL/nullptr for + * such a topic, and attempts to advertise or subscribe to it will + * return -1/ENOENT (see below). + * + * @param _name The name of the topic. + */ +#if defined(__cplusplus) +# define ORB_DECLARE(_name) extern "C" const struct orb_metadata __orb_##_name __EXPORT +# define ORB_DECLARE_OPTIONAL(_name) extern "C" const struct orb_metadata __orb_##_name __EXPORT __attribute__((weak)) +#else +# define ORB_DECLARE(_name) extern const struct orb_metadata __orb_##_name __EXPORT +# define ORB_DECLARE_OPTIONAL(_name) extern const struct orb_metadata __orb_##_name __EXPORT __attribute__((weak)) +#endif + +/** + * Define (instantiate) the uORB metadata for a topic. + * + * The uORB metadata is used to help ensure that updates and + * copies are accessing the right data. + * + * Note that there must be no more than one instance of this macro + * for each topic. + * + * @param _name The name of the topic. + * @param _struct The structure the topic provides. + */ +#define ORB_DEFINE(_name, _struct) \ + const struct orb_metadata __orb_##_name = { \ + #_name, \ + sizeof(_struct) \ + }; struct hack + +__BEGIN_DECLS + +/** + * ORB topic advertiser handle. + * + * Advertiser handles are global; once obtained they can be shared freely + * and do not need to be closed or released. + * + * This permits publication from interrupt context and other contexts where + * a file-descriptor-based handle would not otherwise be in scope for the + * publisher. + */ +typedef intptr_t orb_advert_t; + +/** + * Advertise as the publisher of a topic. + * + * This performs the initial advertisement of a topic; it creates the topic + * node in /obj if required and publishes the initial data. + * + * Any number of advertisers may publish to a topic; publications are atomic + * but co-ordination between publishers is not provided by the ORB. + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @param data A pointer to the initial data to be published. + * For topics updated by interrupt handlers, the advertisement + * must be performed from non-interrupt context. + * @return ERROR on error, otherwise returns a handle + * that can be used to publish to the topic. + * If the topic in question is not known (due to an + * ORB_DEFINE with no corresponding ORB_DECLARE) + * this function will return -1 and set errno to ENOENT. + */ +extern orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) __EXPORT; + +/** + * Publish new data to a topic. + * + * The data is atomically published to the topic and any waiting subscribers + * will be notified. Subscribers that are not waiting can check the topic + * for updates using orb_check and/or orb_stat. + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @handle The handle returned from orb_advertise. + * @param data A pointer to the data to be published. + * @return OK on success, ERROR otherwise with errno set accordingly. + */ +extern int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) __EXPORT; + +/** + * Subscribe to a topic. + * + * The returned value is a file descriptor that can be passed to poll() + * in order to wait for updates to a topic, as well as topic_read, + * orb_check and orb_stat. + * + * Subscription will succeed even if the topic has not been advertised; + * in this case the topic will have a timestamp of zero, it will never + * signal a poll() event, checking will always return false and it cannot + * be copied. When the topic is subsequently advertised, poll, check, + * stat and copy calls will react to the initial publication that is + * performed as part of the advertisement. + * + * Subscription will fail if the topic is not known to the system, i.e. + * there is nothing in the system that has declared the topic and thus it + * can never be published. + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @return ERROR on error, otherwise returns a handle + * that can be used to read and update the topic. + * If the topic in question is not known (due to an + * ORB_DEFINE_OPTIONAL with no corresponding ORB_DECLARE) + * this function will return -1 and set errno to ENOENT. + */ +extern int orb_subscribe(const struct orb_metadata *meta) __EXPORT; + +/** + * Unsubscribe from a topic. + * + * @param handle A handle returned from orb_subscribe. + * @return OK on success, ERROR otherwise with errno set accordingly. + */ +extern int orb_unsubscribe(int handle) __EXPORT; + +/** + * Fetch data from a topic. + * + * This is the only operation that will reset the internal marker that + * indicates that a topic has been updated for a subscriber. Once poll + * or check return indicating that an updaet is available, this call + * must be used to update the subscription. + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @param handle A handle returned from orb_subscribe. + * @param buffer Pointer to the buffer receiving the data, or NULL + * if the caller wants to clear the updated flag without + * using the data. + * @return OK on success, ERROR otherwise with errno set accordingly. + */ +extern int orb_copy(const struct orb_metadata *meta, int handle, void *buffer) __EXPORT; + +/** + * Check whether a topic has been published to since the last orb_copy. + * + * This check can be used to determine whether to copy the topic when + * not using poll(), or to avoid the overhead of calling poll() when the + * topic is likely to have updated. + * + * Updates are tracked on a per-handle basis; this call will continue to + * return true until orb_copy is called using the same handle. This interface + * should be preferred over calling orb_stat due to the race window between + * stat and copy that can lead to missed updates. + * + * @param handle A handle returned from orb_subscribe. + * @param updated Set to true if the topic has been updated since the + * last time it was copied using this handle. + * @return OK if the check was successful, ERROR otherwise with + * errno set accordingly. + */ +extern int orb_check(int handle, bool *updated) __EXPORT; + +/** + * Return the last time that the topic was updated. + * + * @param handle A handle returned from orb_subscribe. + * @param time Returns the absolute time that the topic was updated, or zero if it has + * never been updated. Time is measured in microseconds. + * @return OK on success, ERROR otherwise with errno set accordingly. + */ +extern int orb_stat(int handle, uint64_t *time) __EXPORT; + +/** + * Set the minimum interval between which updates are seen for a subscription. + * + * If this interval is set, the subscriber will not see more than one update + * within the period. + * + * Specifically, the first time an update is reported to the subscriber a timer + * is started. The update will continue to be reported via poll and orb_check, but + * once fetched via orb_copy another update will not be reported until the timer + * expires. + * + * This feature can be used to pace a subscriber that is watching a topic that + * would otherwise update too quickly. + * + * @param handle A handle returned from orb_subscribe. + * @param interval An interval period in milliseconds. + * @return OK on success, ERROR otherwise with ERRNO set accordingly. + */ +extern int orb_set_interval(int handle, unsigned interval) __EXPORT; + +__END_DECLS + +#endif /* _UORB_UORB_H */ |