rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

Merge branch 'next' into corvuscorax/OP-1156_pathfollower-unification

Browse Source
This commit is contained in:
Corvus Corax 2014-08-16 13:58:40 +02:00
commit 8daa5dfa12
59 changed files with 2232 additions and 2354 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
flight/libraries/CoordinateConversions.c
View File

@ -180,7 +180,7 @@ void RPY2Quaternion(const float rpy[3], float q[4])
// ** Find Rbe, that rotates a vector from earth fixed to body frame, from quaternion **
void Quaternion2R(float q[4], float Rbe[3][3])
{
float q0s = q[0] * q[0], q1s = q[1] * q[1], q2s = q[2] * q[2], q3s = q[3] * q[3];
const float q0s = q[0] * q[0], q1s = q[1] * q[1], q2s = q[2] * q[2], q3s = q[3] * q[3];
Rbe[0][0] = q0s + q1s - q2s - q3s;
Rbe[0][1] = 2 * (q[1] * q[2] + q[0] * q[3]);
@ -193,6 +193,61 @@ void Quaternion2R(float q[4], float Rbe[3][3])
Rbe[2][2] = q0s - q1s - q2s + q3s;
}
// ** Find first row of Rbe, that rotates a vector from earth fixed to body frame, from quaternion **
// ** This vector corresponds to the fuselage/roll vector xB **
void QuaternionC2xB(const float q0, const float q1, const float q2, const float q3, float x[3])
{
const float q0s = q0 * q0, q1s = q1 * q1, q2s = q2 * q2, q3s = q3 * q3;
x[0] = q0s + q1s - q2s - q3s;
x[1] = 2 * (q1 * q2 + q0 * q3);
x[2] = 2 * (q1 * q3 - q0 * q2);
}
void Quaternion2xB(const float q[4], float x[3])
{
QuaternionC2xB(q[0], q[1], q[2], q[3], x);
}
// ** Find second row of Rbe, that rotates a vector from earth fixed to body frame, from quaternion **
// ** This vector corresponds to the spanwise/pitch vector yB **
void QuaternionC2yB(const float q0, const float q1, const float q2, const float q3, float y[3])
{
const float q0s = q0 * q0, q1s = q1 * q1, q2s = q2 * q2, q3s = q3 * q3;
y[0] = 2 * (q1 * q2 - q0 * q3);
y[1] = q0s - q1s + q2s - q3s;
y[2] = 2 * (q2 * q3 + q0 * q1);
}
void Quaternion2yB(const float q[4], float y[3])
{
QuaternionC2yB(q[0], q[1], q[2], q[3], y);
}
// ** Find third row of Rbe, that rotates a vector from earth fixed to body frame, from quaternion **
// ** This vector corresponds to the vertical/yaw vector zB **
void QuaternionC2zB(const float q0, const float q1, const float q2, const float q3, float z[3])
{
const float q0s = q0 * q0, q1s = q1 * q1, q2s = q2 * q2, q3s = q3 * q3;
z[0] = 2 * (q1 * q3 + q0 * q2);
z[1] = 2 * (q2 * q3 - q0 * q1);
z[2] = q0s - q1s - q2s + q3s;
}
void Quaternion2zB(const float q[4], float z[3])
{
QuaternionC2zB(q[0], q[1], q[2], q[3], z);
}
// ****** Express LLA in a local NED Base Frame ********
void LLA2Base(int32_t LLAi[3], double BaseECEF[3], float Rne[3][3], float NED[3])
{

15
flight/libraries/inc/CoordinateConversions.h
View File

@ -50,6 +50,21 @@ void RPY2Quaternion(const float rpy[3], float q[4]);
// ** Find Rbe, that rotates a vector from earth fixed to body frame, from quaternion **
void Quaternion2R(float q[4], float Rbe[3][3]);
// ** Find first row of Rbe, that rotates a vector from earth fixed to body frame, from quaternion **
// ** This vector corresponds to the fuselage/roll vector xB **
void QuaternionC2xB(const float q0, const float q1, const float q2, const float q3, float x[3]);
void Quaternion2xB(const float q[4], float x[3]);
// ** Find second row of Rbe, that rotates a vector from earth fixed to body frame, from quaternion **
// ** This vector corresponds to the spanwise/pitch vector yB **
void QuaternionC2yB(const float q0, const float q1, const float q2, const float q3, float y[3]);
void Quaternion2yB(const float q[4], float y[3]);
// ** Find third row of Rbe, that rotates a vector from earth fixed to body frame, from quaternion **
// ** This vector corresponds to the vertical/yaw vector zB **
void QuaternionC2zB(const float q0, const float q1, const float q2, const float q3, float z[3]);
void Quaternion2zB(const float q[4], float z[3]);
// ****** Express LLA in a local NED Base Frame ********
void LLA2Base(int32_t LLAi[3], double BaseECEF[3], float Rne[3][3], float NED[3]);

100
flight/libraries/math/butterworth.c Normal file
View File

@ -0,0 +1,100 @@
/**
******************************************************************************
* @addtogroup OpenPilot Math Utilities
* @{
* @addtogroup Butterworth low pass filter
* @{
*
* @file butterworth.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Direct form two of a second order Butterworth low pass filter
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "openpilot.h"
#include "math.h"
#include "butterworth.h"
/**
* Initialization function for coefficients of a second order Butterworth biquadratic filter in direct from 2.
* Note that b1 = 2 * b0 and b2 = b0 is use here and in the sequel.
* @param[in] ff Cut-off frequency ratio
* @param[out] filterPtr Pointer to filter coefficients
* @returns Nothing
*/
void InitButterWorthDF2Filter(const float ff, struct ButterWorthDF2Filter *filterPtr)
{
const float ita = 1.0f / tanf(M_PI_F * ff);
const float b0 = 1.0f / (1.0f + M_SQRT2_F * ita + ita * ita);
const float a1 = 2.0f * b0 * (ita * ita - 1.0f);
const float a2 = -b0 * (1.0f - M_SQRT2_F * ita + ita * ita);
filterPtr->b0 = b0;
filterPtr->a1 = a1;
filterPtr->a2 = a2;
}
/**
* Initialization function for intermediate values of a second order Butterworth biquadratic filter in direct from 2.
* Obtained by solving a linear equation system.
* @param[in] x0 Prescribed value
* @param[in] filterPtr Pointer to filter coefficients
* @param[out] wn1Ptr Pointer to first intermediate value
* @param[out] wn2Ptr Pointer to second intermediate value
* @returns Nothing
*/
void InitButterWorthDF2Values(const float x0, const struct ButterWorthDF2Filter *filterPtr, float *wn1Ptr, float *wn2Ptr)
{
const float b0 = filterPtr->b0;
const float a1 = filterPtr->a1;
const float a2 = filterPtr->a2;
const float a11 = 2.0f + a1;
const float a12 = 1.0f + a2;
const float a21 = 2.0f + a1 * a1 + a2;
const float a22 = 1.0f + a1 * a2;
const float det = a11 * a22 - a12 * a21;
const float rhs1 = x0 / b0 - x0;
const float rhs2 = x0 / b0 - x0 + a1 * x0;
*wn1Ptr = (a22 * rhs1 - a12 * rhs2) / det;
*wn2Ptr = (-a21 * rhs1 + a11 * rhs2) / det;
}
/**
* Second order Butterworth biquadratic filter in direct from 2, such that only two values wn1=w[n-1] and wn2=w[n-2] need to be stored.
* Function takes care of updating the values wn1 and wn2.
* @param[in] xn New raw value
* @param[in] filterPtr Pointer to filter coefficients
* @param[out] wn1Ptr Pointer to first intermediate value
* @param[out] wn2Ptr Pointer to second intermediate value
* @returns Filtered value
*/
float FilterButterWorthDF2(const float xn, const struct ButterWorthDF2Filter *filterPtr, float *wn1Ptr, float *wn2Ptr)
{
const float wn = xn + filterPtr->a1 * (*wn1Ptr) + filterPtr->a2 * (*wn2Ptr);
const float val = filterPtr->b0 * (wn + 2.0f * (*wn1Ptr) + (*wn2Ptr));
*wn2Ptr = *wn1Ptr;
*wn1Ptr = wn;
return val;
}

29
flight/modules/Attitude/revolution/inc/attitude.h → flight/libraries/math/butterworth.h
View File

@ -1,13 +1,13 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @addtogroup OpenPilot Math Utilities
* @{
* @addtogroup Attitude Attitude Module
* @addtogroup Butterworth low pass filter
* @{
*
* @file attitude.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
* @brief Acquires sensor data and fuses it into attitude estimate for CC
* @file butterworth.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Direct form two of a second order Butterworth low pass filter
*
* @see The GNU Public License (GPL) Version 3
*
@ -27,11 +27,20 @@
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef ATTITUDE_H
#define ATTITUDE_H
#include "openpilot.h"
#ifndef BUTTERWORTH_H
#define BUTTERWORTH_H
int32_t AttitudeInitialize(void);
// Coefficients of second order Butterworth biquadratic filter in direct from 2
struct ButterWorthDF2Filter {
float b0;
float a1;
float a2;
};
#endif // ATTITUDE_H
// Function declarations
void InitButterWorthDF2Filter(const float ff, struct ButterWorthDF2Filter *filterPtr);
void InitButterWorthDF2Values(const float x0, const struct ButterWorthDF2Filter *filterPtr, float *wn1Ptr, float *wn2Ptr);
float FilterButterWorthDF2(const float xn, const struct ButterWorthDF2Filter *filterPtr, float *wn1Ptr, float *wn2Ptr);
#endif

14
flight/modules/Airspeed/airspeed.c
View File

@ -44,7 +44,7 @@
#include "baro_airspeed_ms4525do.h"
#include "baro_airspeed_etasv3.h"
#include "baro_airspeed_mpxv.h"
#include "gps_airspeed.h"
#include "imu_airspeed.h"
#include "airspeedalarm.h"
#include "taskinfo.h"
@ -99,7 +99,7 @@ int32_t AirspeedInitialize()
HwSettingsInitialize();
uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
HwSettingsOptionalModulesGet(optionalModules);
HwSettingsOptionalModulesArrayGet(optionalModules);
if (optionalModules[HWSETTINGS_OPTIONALMODULES_AIRSPEED] == HWSETTINGS_OPTIONALMODULES_ENABLED) {
@ -136,7 +136,7 @@ MODULE_INITCALL(AirspeedInitialize, AirspeedStart);
static void airspeedTask(__attribute__((unused)) void *parameters)
{
AirspeedSettingsUpdatedCb(AirspeedSettingsHandle());
bool gpsAirspeedInitialized = false;
bool imuAirspeedInitialized = false;
AirspeedSensorData airspeedData;
AirspeedSensorGet(&airspeedData);
@ -164,9 +164,9 @@ static void airspeedTask(__attribute__((unused)) void *parameters)
AirspeedSensorSet(&airspeedData);
break;
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GROUNDSPEEDBASEDWINDESTIMATION:
if (!gpsAirspeedInitialized) {
gpsAirspeedInitialized = true;
gps_airspeedInitialize();
if (!imuAirspeedInitialized) {
imuAirspeedInitialized = true;
imu_airspeedInitialize(&airspeedSettings);
}
break;
}
@ -192,7 +192,7 @@ static void airspeedTask(__attribute__((unused)) void *parameters)
break;
#endif
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GROUNDSPEEDBASEDWINDESTIMATION:
gps_airspeedGet(&airspeedData, &airspeedSettings);
imu_airspeedGet(&airspeedData, &airspeedSettings);
break;
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_NONE:
// no need to check so often until a sensor is enabled

170
flight/modules/Airspeed/gps_airspeed.c
View File

@ -1,170 +0,0 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup AirspeedModule Airspeed Module
* @brief Use GPS data to estimate airspeed
* @{
*
* @file gps_airspeed.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Airspeed module, handles temperature and pressure readings from BMP085
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "openpilot.h"
#include "velocitystate.h"
#include "attitudestate.h"
#include "airspeedsensor.h"
#include "airspeedsettings.h"
#include "gps_airspeed.h"
#include "CoordinateConversions.h"
#include "airspeedalarm.h"
#include <pios_math.h>
// Private constants
#define GPS_AIRSPEED_BIAS_KP 0.1f // Needs to be settable in a UAVO
#define GPS_AIRSPEED_BIAS_KI 0.1f // Needs to be settable in a UAVO
#define SAMPLING_DELAY_MS_GPS 100 // Needs to be settable in a UAVO
#define GPS_AIRSPEED_TIME_CONSTANT_MS 500.0f // Needs to be settable in a UAVO
// Private types
struct GPSGlobals {
float RbeCol1_old[3];
float gpsVelOld_N;
float gpsVelOld_E;
float gpsVelOld_D;
float oldAirspeed;
};
// Private variables
static struct GPSGlobals *gps;
// Private functions
/*
* Initialize function loads first data sets, and allocates memory for structure.
*/
void gps_airspeedInitialize()
{
// This method saves memory in case we don't use the GPS module.
gps = (struct GPSGlobals *)pios_malloc(sizeof(struct GPSGlobals));
// GPS airspeed calculation variables
VelocityStateInitialize();
VelocityStateData gpsVelData;
VelocityStateGet(&gpsVelData);
gps->gpsVelOld_N = gpsVelData.North;
gps->gpsVelOld_E = gpsVelData.East;
gps->gpsVelOld_D = gpsVelData.Down;
gps->oldAirspeed = 0.0f;
AttitudeStateData attData;
AttitudeStateGet(&attData);
float Rbe[3][3];
float q[4] = { attData.q1, attData.q2, attData.q3, attData.q4 };
// Calculate rotation matrix
Quaternion2R(q, Rbe);
gps->RbeCol1_old[0] = Rbe[0][0];
gps->RbeCol1_old[1] = Rbe[0][1];
gps->RbeCol1_old[2] = Rbe[0][2];
}
/*
* Calculate airspeed as a function of GPS groundspeed and vehicle attitude.
* From "IMU Wind Estimation (Theory)", by William Premerlani.
* The idea is that V_gps=V_air+V_wind. If we assume wind constant, =>
* V_gps_2-V_gps_1 = (V_air_2+V_wind_2) -(V_air_1+V_wind_1) = V_air_2 - V_air_1.
* If we assume airspeed constant, => V_gps_2-V_gps_1 = |V|*(f_2 - f1),
* where "f" is the fuselage vector in earth coordinates.
* We then solve for |V| = |V_gps_2-V_gps_1|/ |f_2 - f1|.
*/
void gps_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *airspeedSettings)
{
float Rbe[3][3];
{ // Scoping to save memory. We really just need Rbe.
AttitudeStateData attData;
AttitudeStateGet(&attData);
float q[4] = { attData.q1, attData.q2, attData.q3, attData.q4 };
// Calculate rotation matrix
Quaternion2R(q, Rbe);
}
// Calculate the cos(angle) between the two fuselage basis vectors
float cosDiff = (Rbe[0][0] * gps->RbeCol1_old[0]) + (Rbe[0][1] * gps->RbeCol1_old[1]) + (Rbe[0][2] * gps->RbeCol1_old[2]);
// If there's more than a 5 degree difference between two fuselage measurements, then we have sufficient delta to continue.
if (fabsf(cosDiff) < cosf(DEG2RAD(5.0f))) {
VelocityStateData gpsVelData;
VelocityStateGet(&gpsVelData);
if (gpsVelData.North * gpsVelData.North + gpsVelData.East * gpsVelData.East + gpsVelData.Down * gpsVelData.Down < 1.0f) {
airspeedData->CalibratedAirspeed = 0;
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR);
return; // do not calculate if gps velocity is insufficient...
}
// Calculate the norm^2 of the difference between the two GPS vectors
float normDiffGPS2 = powf(gpsVelData.North - gps->gpsVelOld_N, 2.0f) + powf(gpsVelData.East - gps->gpsVelOld_E, 2.0f) + powf(gpsVelData.Down - gps->gpsVelOld_D, 2.0f);
// Calculate the norm^2 of the difference between the two fuselage vectors
float normDiffAttitude2 = powf(Rbe[0][0] - gps->RbeCol1_old[0], 2.0f) + powf(Rbe[0][1] - gps->RbeCol1_old[1], 2.0f) + powf(Rbe[0][2] - gps->RbeCol1_old[2], 2.0f);
// Airspeed magnitude is the ratio between the two difference norms
float airspeed = sqrtf(normDiffGPS2 / normDiffAttitude2);
if (!IS_REAL(airspeedData->CalibratedAirspeed)) {
airspeedData->CalibratedAirspeed = 0;
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR);
} else {
// need a low pass filter to filter out spikes in non coordinated maneuvers
airspeedData->CalibratedAirspeed = (1.0f - airspeedSettings->GroundSpeedBasedEstimationLowPassAlpha) * gps->oldAirspeed + airspeedSettings->GroundSpeedBasedEstimationLowPassAlpha * airspeed;
gps->oldAirspeed = airspeedData->CalibratedAirspeed;
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
AirspeedAlarm(SYSTEMALARMS_ALARM_OK);
}
// Save old variables for next pass
gps->gpsVelOld_N = gpsVelData.North;
gps->gpsVelOld_E = gpsVelData.East;
gps->gpsVelOld_D = gpsVelData.Down;
gps->RbeCol1_old[0] = Rbe[0][0];
gps->RbeCol1_old[1] = Rbe[0][1];
gps->RbeCol1_old[2] = Rbe[0][2];
}
}
/**
* @}
* @}
*/

306
flight/modules/Airspeed/imu_airspeed.c Normal file
View File

@ -0,0 +1,306 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup AirspeedModule Airspeed Module
* @brief Use attitude and velocity data to estimate airspeed
* @{
*
* @file imu_airspeed.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief IMU based airspeed calculation
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "openpilot.h"
#include "velocitystate.h"
#include "attitudestate.h"
#include "airspeedsensor.h"
#include "airspeedsettings.h"
#include "imu_airspeed.h"
#include "CoordinateConversions.h"
#include "butterworth.h"
#include <pios_math.h>
// Private constants
#define EPS 1e-6f
#define EPS_REORIENTATION 1e-10f
#define EPS_VELOCITY 1.f
// Private types
// structure with smoothed fuselage orientation, ground speed, wind vector and their changes in time
struct IMUGlobals {
// Butterworth filters
struct ButterWorthDF2Filter filter;
struct ButterWorthDF2Filter prefilter;
float ff, ffV;
// storage variables for Butterworth filter
float pn1, pn2;
float yn1, yn2;
float v1n1, v1n2;
float v2n1, v2n2;
float v3n1, v3n2;
float Vw1n1, Vw1n2;
float Vw2n1, Vw2n2;
float Vw3n1, Vw3n2;
float Vw1, Vw2, Vw3;
// storage variables for derivative calculation
float pOld, yOld;
float v1Old, v2Old, v3Old;
};
// Private variables
static struct IMUGlobals *imu;
// Private functions
// a simple square inline function based on multiplication faster than powf(x,2.0f)
static inline float Sq(float x)
{
return x * x;
}
// ****** find pitch, yaw from quaternion ********
static void Quaternion2PY(const float q0, const float q1, const float q2, const float q3, float *pPtr, float *yPtr, bool principalArg)
{
float R13, R11, R12;
const float q0s = q0 * q0;
const float q1s = q1 * q1;
const float q2s = q2 * q2;
const float q3s = q3 * q3;
R13 = 2.0f * (q1 * q3 - q0 * q2);
R11 = q0s + q1s - q2s - q3s;
R12 = 2.0f * (q1 * q2 + q0 * q3);
*pPtr = asinf(-R13); // pitch always between -pi/2 to pi/2
const float y_ = atan2f(R12, R11);
// use old yaw contained in y to add multiples of 2pi to have a continuous yaw if user does not want the principal argument
// else simply copy atan2 result into result
if (principalArg) {
*yPtr = y_;
} else {
// calculate needed mutliples of 2pi to avoid jumps
// number of cycles accumulated in old yaw
const int32_t cycles = (int32_t)(*yPtr / M_2PI_F);
// look for a jump by substracting the modulus, i.e. there is maximally one jump.
// take slightly less than 2pi, because the jump will always be lower than 2pi
const int32_t mod = (int32_t)((y_ - (*yPtr - cycles * M_2PI_F)) / (M_2PI_F * 0.8f));
*yPtr = y_ + M_2PI_F * (cycles - mod);
}
}
static void PY2xB(const float p, const float y, float x[3])
{
const float cosp = cosf(p);
x[0] = cosp * cosf(y);
x[1] = cosp * sinf(y);
x[2] = -sinf(p);
}
static void PY2DeltaxB(const float p, const float y, const float xB[3], float x[3])
{
const float cosp = cosf(p);
x[0] = xB[0] - cosp * cosf(y);
x[1] = xB[1] - cosp * sinf(y);
x[2] = xB[2] - -sinf(p);
}
/*
* Initialize function loads first data sets, and allocates memory for structure.
*/
void imu_airspeedInitialize(const AirspeedSettingsData *airspeedSettings)
{
// pre-filter frequency rate
const float ff = (float)(airspeedSettings->SamplePeriod) / 1000.0f / airspeedSettings->IMUBasedEstimationLowPassPeriod1;
// filter frequency rate
const float ffV = (float)(airspeedSettings->SamplePeriod) / 1000.0f / airspeedSettings->IMUBasedEstimationLowPassPeriod2;
// This method saves memory in case we don't use the module.
imu = (struct IMUGlobals *)pios_malloc(sizeof(struct IMUGlobals));
// airspeed calculation variables
VelocityStateInitialize();
VelocityStateData velData;
VelocityStateGet(&velData);
AttitudeStateData attData;
AttitudeStateGet(&attData);
// initialize filters for given ff and ffV
InitButterWorthDF2Filter(ffV, &(imu->filter));
InitButterWorthDF2Filter(ff, &(imu->prefilter));
imu->ffV = ffV;
imu->ff = ff;
// get pitch and yaw from quarternion; principal argument for yaw
Quaternion2PY(attData.q1, attData.q2, attData.q3, attData.q4, &(imu->pOld), &(imu->yOld), true);
InitButterWorthDF2Values(imu->pOld, &(imu->prefilter), &(imu->pn1), &(imu->pn2));
InitButterWorthDF2Values(imu->yOld, &(imu->prefilter), &(imu->yn1), &(imu->yn2));
// use current NED speed as vOld vector and as initial value for filter
imu->v1Old = velData.North;
imu->v2Old = velData.East;
imu->v3Old = velData.Down;
InitButterWorthDF2Values(imu->v1Old, &(imu->prefilter), &(imu->v1n1), &(imu->v1n2));
InitButterWorthDF2Values(imu->v2Old, &(imu->prefilter), &(imu->v2n1), &(imu->v2n2));
InitButterWorthDF2Values(imu->v3Old, &(imu->prefilter), &(imu->v3n1), &(imu->v3n2));
// initial guess for windspeed is zero
imu->Vw3 = imu->Vw2 = imu->Vw1 = 0.0f;
InitButterWorthDF2Values(0.0f, &(imu->filter), &(imu->Vw1n1), &(imu->Vw1n2));
imu->Vw3n1 = imu->Vw2n1 = imu->Vw1n1;
imu->Vw3n2 = imu->Vw2n2 = imu->Vw1n2;
}
/*
* Calculate airspeed as a function of groundspeed and vehicle attitude.
* Adapted from "IMU Wind Estimation (Theory)", by William Premerlani.
* The idea is that V_gps=V_air+V_wind. If we assume wind constant, =>
* V_gps_2-V_gps_1 = (V_air_2+V_wind_2) -(V_air_1+V_wind_1) = V_air_2 - V_air_1.
* If we assume airspeed constant, => V_gps_2-V_gps_1 = |V|*(f_2 - f1),
* where "f" is the fuselage vector in earth coordinates.
* We then solve for |V| = |V_gps_2-V_gps_1|/ |f_2 - f1|.
* Adapted to: |V| = (V_gps_2-V_gps_1) dot (f2_-f_1) / |f_2 - f1|^2.
*
* See OP-1317 imu_wind_estimation.pdf for details on the adaptation
* Need a low pass filter to filter out spikes in non coordinated maneuvers
* A two step Butterworth second order filter is used. In the first step fuselage vector xB
* and ground speed vector Vel are filtered. The fuselage vector is filtered through its pitch
* and yaw to keep a unit length. After building the differenced dxB and dVel are produced and
* the airspeed calculated. The calculated airspeed is filtered again with a Butterworth filter
*/
void imu_airspeedGet(AirspeedSensorData *airspeedData, const AirspeedSettingsData *airspeedSettings)
{
// pre-filter frequency rate
const float ff = (float)(airspeedSettings->SamplePeriod) / 1000.0f / airspeedSettings->IMUBasedEstimationLowPassPeriod1;
// filter frequency rate
const float ffV = (float)(airspeedSettings->SamplePeriod) / 1000.0f / airspeedSettings->IMUBasedEstimationLowPassPeriod2;
// check for a change in filter frequency rate. if yes, then actualize filter constants and intermediate values
if (fabsf(ffV - imu->ffV) > EPS) {
InitButterWorthDF2Filter(ffV, &(imu->filter));
InitButterWorthDF2Values(imu->Vw1, &(imu->filter), &(imu->Vw1n1), &(imu->Vw1n2));
InitButterWorthDF2Values(imu->Vw2, &(imu->filter), &(imu->Vw2n1), &(imu->Vw2n2));
InitButterWorthDF2Values(imu->Vw3, &(imu->filter), &(imu->Vw3n1), &(imu->Vw3n2));
}
if (fabsf(ff - imu->ff) > EPS) {
InitButterWorthDF2Filter(ff, &(imu->prefilter));
InitButterWorthDF2Values(imu->pOld, &(imu->prefilter), &(imu->pn1), &(imu->pn2));
InitButterWorthDF2Values(imu->yOld, &(imu->prefilter), &(imu->yn1), &(imu->yn2));
InitButterWorthDF2Values(imu->v1Old, &(imu->prefilter), &(imu->v1n1), &(imu->v1n2));
InitButterWorthDF2Values(imu->v2Old, &(imu->prefilter), &(imu->v2n1), &(imu->v2n2));
InitButterWorthDF2Values(imu->v3Old, &(imu->prefilter), &(imu->v3n1), &(imu->v3n2));
}
float normVel2;
float normDiffAttitude2;
float dvdtDotdfdt;
float xB[3];
// get values and conduct smoothing of ground speed and orientation independently of the calculation of airspeed
{ // Scoping to save memory
AttitudeStateData attData;
AttitudeStateGet(&attData);
VelocityStateData velData;
VelocityStateGet(&velData);
float p = imu->pOld, y = imu->yOld;
float dxB[3];
// get pitch and roll Euler angles from quaternion
// do not calculate the principlal argument of yaw, i.e. use old yaw to add multiples of 2pi to have a continuous yaw
Quaternion2PY(attData.q1, attData.q2, attData.q3, attData.q4, &p, &y, false);
// filter pitch and roll Euler angles instead of fuselage vector to guarantee a unit length at all times
p = FilterButterWorthDF2(p, &(imu->prefilter), &(imu->pn1), &(imu->pn2));
y = FilterButterWorthDF2(y, &(imu->prefilter), &(imu->yn1), &(imu->yn2));
// transform pitch and yaw into fuselage vector xB and xBold
PY2xB(p, y, xB);
// calculate change in fuselage vector by substraction of old value
PY2DeltaxB(imu->pOld, imu->yOld, xB, dxB);
// filter ground speed from VelocityState
const float fv1n = FilterButterWorthDF2(velData.North, &(imu->prefilter), &(imu->v1n1), &(imu->v1n2));
const float fv2n = FilterButterWorthDF2(velData.East, &(imu->prefilter), &(imu->v2n1), &(imu->v2n2));
const float fv3n = FilterButterWorthDF2(velData.Down, &(imu->prefilter), &(imu->v3n1), &(imu->v3n2));
// calculate norm of ground speed
normVel2 = Sq(fv1n) + Sq(fv2n) + Sq(fv3n);
// calculate norm of orientation change
normDiffAttitude2 = Sq(dxB[0]) + Sq(dxB[1]) + Sq(dxB[2]);
// cauclate scalar product between groundspeed change and orientation change
dvdtDotdfdt = (fv1n - imu->v1Old) * dxB[0] + (fv2n - imu->v2Old) * dxB[1] + (fv3n - imu->v3Old) * dxB[2];
// actualise old values
imu->pOld = p;
imu->yOld = y;
imu->v1Old = fv1n;
imu->v2Old = fv2n;
imu->v3Old = fv3n;
}
// Some reorientation needed to be able to calculate airspeed, calculate only for sufficient velocity
// a negative scalar product is a clear sign that we are not really able to calculate the airspeed
// NOTE: normVel2 check against EPS_VELOCITY might make problems during hovering maneuvers in fixed wings
if (normDiffAttitude2 > EPS_REORIENTATION && normVel2 > EPS_VELOCITY && dvdtDotdfdt > 0.f) {
// Airspeed modulus: |v| = dv/dt * dxB/dt / |dxB/dt|^2
// airspeed is always REAL because normDiffAttitude2 > EPS_REORIENTATION > 0 and REAL dvdtDotdfdt
const float airspeed = dvdtDotdfdt / normDiffAttitude2;
// groundspeed = airspeed + wind ---> wind = groundspeed - airspeed
const float wind[3] = { imu->v1Old - xB[0] * airspeed,
imu->v2Old - xB[1] * airspeed,
imu->v3Old - xB[2] * airspeed };
// filter raw wind
imu->Vw1 = FilterButterWorthDF2(wind[0], &(imu->filter), &(imu->Vw1n1), &(imu->Vw1n2));
imu->Vw2 = FilterButterWorthDF2(wind[1], &(imu->filter), &(imu->Vw2n1), &(imu->Vw2n2));
imu->Vw3 = FilterButterWorthDF2(wind[2], &(imu->filter), &(imu->Vw3n1), &(imu->Vw3n2));
} // else leave wind estimation unchanged
{ // Scoping to save memory
// airspeed = groundspeed - wind
const float Vair[3] = {
imu->v1Old - imu->Vw1,
imu->v2Old - imu->Vw2,
imu->v3Old - imu->Vw3
};
// project airspeed into fuselage vector
airspeedData->CalibratedAirspeed = Vair[0] * xB[0] + Vair[1] * xB[1] + Vair[2] * xB[2];
}
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
AlarmsClear(SYSTEMALARMS_ALARM_AIRSPEED);
}
/**
* @}
* @}
*/

14
flight/modules/Airspeed/inc/gps_airspeed.h → flight/modules/Airspeed/inc/imu_airspeed.h
View File

@ -3,10 +3,10 @@
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup AirspeedModule Airspeed Module
* @brief Calculate airspeed as a function of the difference between sequential GPS velocity and attitude measurements
* @brief Calculate airspeed as a function of the difference between sequential ground velocity and attitude measurements
* @{
*
* @file gps_airspeed.h
* @file imu_airspeed.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Airspeed module, reads temperature and pressure from BMP085
*
@ -28,13 +28,13 @@
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef GPS_AIRSPEED_H
#define GPS_AIRSPEED_H
#ifndef IMU_AIRSPEED_H
#define IMU_AIRSPEED_H
void gps_airspeedInitialize();
void gps_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *airspeedSettings);
void imu_airspeedInitialize(const AirspeedSettingsData *airspeedSettings);
void imu_airspeedGet(AirspeedSensorData *airspeedData, const AirspeedSettingsData *airspeedSettings);
#endif // GPS_AIRSPEED_H
#endif // IMU_AIRSPEED_H
/**
* @}

1348
flight/modules/Attitude/revolution/attitude.c
View File

@ -1,1348 +0,0 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup Attitude Copter Control Attitude Estimation
* @brief Acquires sensor data and computes attitude estimate
* Specifically updates the the @ref AttitudeState "AttitudeState" and @ref AttitudeRaw "AttitudeRaw" settings objects
* @{
*
* @file attitude.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Module to handle all comms to the AHRS on a periodic basis.
*
* @see The GNU Public License (GPL) Version 3
*
******************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/**
* Input objects: None, takes sensor data via pios
* Output objects: @ref AttitudeRaw @ref AttitudeState
*
* This module computes an attitude estimate from the sensor data
*
* The module executes in its own thread.
*
* UAVObjects are automatically generated by the UAVObjectGenerator from
* the object definition XML file.
*
* Modules have no API, all communication to other modules is done through UAVObjects.
* However modules may use the API exposed by shared libraries.
* See the OpenPilot wiki for more details.
* http://www.openpilot.org/OpenPilot_Application_Architecture
*
*/
#include <openpilot.h>
#include <pios_struct_helper.h>
#include "attitude.h"
#include "accelsensor.h"
#include "accelstate.h"
#include "airspeedsensor.h"
#include "airspeedstate.h"
#include "attitudestate.h"
#include "attitudesettings.h"
#include "barosensor.h"
#include "flightstatus.h"
#include "gpspositionsensor.h"
#include "gpsvelocitysensor.h"
#include "gyrostate.h"
#include "gyrosensor.h"
#include "homelocation.h"
#include "magsensor.h"
#include "magstate.h"
#include "positionstate.h"
#include "ekfconfiguration.h"
#include "ekfstatevariance.h"
#include "revocalibration.h"
#include "revosettings.h"
#include "velocitystate.h"
#include "taskinfo.h"
#include "CoordinateConversions.h"
// Private constants
#define STACK_SIZE_BYTES 2048
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
#define FAILSAFE_TIMEOUT_MS 10
#define CALIBRATION_DELAY 4000
#define CALIBRATION_DURATION 6000
// low pass filter configuration to calculate offset
// of barometric altitude sensor
// reasoning: updates at: 10 Hz, tau= 300 s settle time
// exp(-(1/f) / tau ) ~=~ 0.9997
#define BARO_OFFSET_LOWPASS_ALPHA 0.9997f
// simple IAS to TAS aproximation - 2% increase per 1000ft
// since we do not have flowing air temperature information
#define IAS2TAS(alt) (1.0f + (0.02f * (alt) / 304.8f))
// Private types
// Private variables
static xTaskHandle attitudeTaskHandle;
static xQueueHandle gyroQueue;
static xQueueHandle accelQueue;
static xQueueHandle magQueue;
static xQueueHandle airspeedQueue;
static xQueueHandle baroQueue;
static xQueueHandle gpsQueue;
static xQueueHandle gpsVelQueue;
static AttitudeSettingsData attitudeSettings;
static HomeLocationData homeLocation;
static RevoCalibrationData revoCalibration;
static EKFConfigurationData ekfConfiguration;
static RevoSettingsData revoSettings;
static FlightStatusData flightStatus;
const uint32_t SENSOR_QUEUE_SIZE = 10;
static bool volatile variance_error = true;
static bool volatile initialization_required = true;
static uint32_t volatile running_algorithm = 0xffffffff; // we start with no algorithm running
static float rollPitchBiasRate = 0;
// Accel filtering
static float accel_alpha = 0;
static bool accel_filter_enabled = false;
static float accels_filtered[3];
static float grot_filtered[3];
// Private functions
static void AttitudeTask(void *parameters);
static int32_t updateAttitudeComplementary(bool first_run);
static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode);
static void settingsUpdatedCb(UAVObjEvent *objEv);
static int32_t getNED(GPSPositionSensorData *gpsPosition, float *NED);
static void magOffsetEstimation(MagSensorData *mag);
// check for invalid values
static inline bool invalid(float data)
{
if (isnan(data) || isinf(data)) {
return true;
}
return false;
}
// check for invalid variance values
static inline bool invalid_var(float data)
{
if (invalid(data)) {
return true;
}
if (data < 1e-15f) { // var should not be close to zero. And not negative either.
return true;
}
return false;
}
/**
* API for sensor fusion algorithms:
* Configure(xQueueHandle gyro, xQueueHandle accel, xQueueHandle mag, xQueueHandle baro)
* Stores all the queues the algorithm will pull data from
* FinalizeSensors() -- before saving the sensors modifies them based on internal state (gyro bias)
* Update() -- queries queues and updates the attitude estiamte
*/
/**
* Initialise the module. Called before the start function
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AttitudeInitialize(void)
{
GyroSensorInitialize();
GyroStateInitialize();
AccelSensorInitialize();
AccelStateInitialize();
MagSensorInitialize();
MagStateInitialize();
AirspeedSensorInitialize();
AirspeedStateInitialize();
BaroSensorInitialize();
GPSPositionSensorInitialize();
GPSVelocitySensorInitialize();
AttitudeSettingsInitialize();
AttitudeStateInitialize();
PositionStateInitialize();
VelocityStateInitialize();
RevoSettingsInitialize();
RevoCalibrationInitialize();
EKFConfigurationInitialize();
EKFStateVarianceInitialize();
FlightStatusInitialize();
// Initialize this here while we aren't setting the homelocation in GPS
HomeLocationInitialize();
// Initialize quaternion
AttitudeStateData attitude;
AttitudeStateGet(&attitude);
attitude.q1 = 1.0f;
attitude.q2 = 0.0f;
attitude.q3 = 0.0f;
attitude.q4 = 0.0f;
AttitudeStateSet(&attitude);
AttitudeSettingsConnectCallback(&settingsUpdatedCb);
RevoSettingsConnectCallback(&settingsUpdatedCb);
RevoCalibrationConnectCallback(&settingsUpdatedCb);
HomeLocationConnectCallback(&settingsUpdatedCb);
EKFConfigurationConnectCallback(&settingsUpdatedCb);
FlightStatusConnectCallback(&settingsUpdatedCb);
return 0;
}
/**
* Start the task. Expects all objects to be initialized by this point.
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AttitudeStart(void)
{
// Create the queues for the sensors
gyroQueue = xQueueCreate(1, sizeof(UAVObjEvent));
accelQueue = xQueueCreate(1, sizeof(UAVObjEvent));
magQueue = xQueueCreate(1, sizeof(UAVObjEvent));
airspeedQueue = xQueueCreate(1, sizeof(UAVObjEvent));
baroQueue = xQueueCreate(1, sizeof(UAVObjEvent));
gpsQueue = xQueueCreate(1, sizeof(UAVObjEvent));
gpsVelQueue = xQueueCreate(1, sizeof(UAVObjEvent));
// Start main task
xTaskCreate(AttitudeTask, "Attitude", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &attitudeTaskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ATTITUDE, attitudeTaskHandle);
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE);
#endif
GyroSensorConnectQueue(gyroQueue);
AccelSensorConnectQueue(accelQueue);
MagSensorConnectQueue(magQueue);
AirspeedSensorConnectQueue(airspeedQueue);
BaroSensorConnectQueue(baroQueue);
GPSPositionSensorConnectQueue(gpsQueue);
GPSVelocitySensorConnectQueue(gpsVelQueue);
return 0;
}
MODULE_INITCALL(AttitudeInitialize, AttitudeStart);
/**
* Module thread, should not return.
*/
static void AttitudeTask(__attribute__((unused)) void *parameters)
{
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
// Force settings update to make sure rotation loaded
settingsUpdatedCb(NULL);
// Wait for all the sensors be to read
vTaskDelay(100);
// Main task loop - TODO: make it run as delayed callback
while (1) {
int32_t ret_val = -1;
bool first_run = false;
if (initialization_required) {
initialization_required = false;
first_run = true;
}
// This function blocks on data queue
switch (running_algorithm) {
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY:
ret_val = updateAttitudeComplementary(first_run);
break;
case REVOSETTINGS_FUSIONALGORITHM_INS13GPSOUTDOOR:
ret_val = updateAttitudeINSGPS(first_run, true);
break;
case REVOSETTINGS_FUSIONALGORITHM_INS13INDOOR:
ret_val = updateAttitudeINSGPS(first_run, false);
break;
default:
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
break;
}
if (ret_val != 0) {
initialization_required = true;
}
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
#endif
}
}
static inline void apply_accel_filter(const float *raw, float *filtered)
{
if (accel_filter_enabled) {
filtered[0] = filtered[0] * accel_alpha + raw[0] * (1 - accel_alpha);
filtered[1] = filtered[1] * accel_alpha + raw[1] * (1 - accel_alpha);
filtered[2] = filtered[2] * accel_alpha + raw[2] * (1 - accel_alpha);
} else {
filtered[0] = raw[0];
filtered[1] = raw[1];
filtered[2] = raw[2];
}
}
float accel_mag;
float qmag;
float attitudeDt;
float mag_err[3];
static int32_t updateAttitudeComplementary(bool first_run)
{
UAVObjEvent ev;
GyroSensorData gyroSensorData;
GyroStateData gyroStateData;
AccelSensorData accelSensorData;
static int32_t timeval;
float dT;
static uint8_t init = 0;
static float gyro_bias[3] = { 0, 0, 0 };
static bool magCalibrated = true;
static uint32_t initStartupTime = 0;
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
if (xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE ||
xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE) {
// When one of these is updated so should the other
// Do not set attitude timeout warnings in simulation mode
if (!AttitudeStateReadOnly()) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING);
return -1;
}
}
AccelSensorGet(&accelSensorData);
// TODO: put in separate filter
AccelStateData accelState;
accelState.x = accelSensorData.x;
accelState.y = accelSensorData.y;
accelState.z = accelSensorData.z;
AccelStateSet(&accelState);
// During initialization and
if (first_run) {
#if defined(PIOS_INCLUDE_HMC5883)
// To initialize we need a valid mag reading
if (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) != pdTRUE) {
return -1;
}
MagSensorData magData;
MagSensorGet(&magData);
#else
MagSensorData magData;
magData.x = 100.0f;
magData.y = 0.0f;
magData.z = 0.0f;
#endif
float magBias[3];
RevoCalibrationmag_biasArrayGet(magBias);
// don't trust Mag for initial orientation if it has not been calibrated
if (magBias[0] < 1e-6f && magBias[1] < 1e-6f && magBias[2] < 1e-6f) {
magCalibrated = false;
magData.x = 100.0f;
magData.y = 0.0f;
magData.z = 0.0f;
}
AttitudeStateData attitudeState;
AttitudeStateGet(&attitudeState);
init = 0;
// Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly,
// so pseudo "north" vector can be estimated even if the board is not level
attitudeState.Roll = atan2f(-accelSensorData.y, -accelSensorData.z);
float zn = cosf(attitudeState.Roll) * magData.z + sinf(attitudeState.Roll) * magData.y;
float yn = cosf(attitudeState.Roll) * magData.y - sinf(attitudeState.Roll) * magData.z;
// rotate accels z vector according to roll
float azn = cosf(attitudeState.Roll) * accelSensorData.z + sinf(attitudeState.Roll) * accelSensorData.y;
attitudeState.Pitch = atan2f(accelSensorData.x, -azn);
float xn = cosf(attitudeState.Pitch) * magData.x + sinf(attitudeState.Pitch) * zn;
attitudeState.Yaw = atan2f(-yn, xn);
// TODO: This is still a hack
// Put this in a proper generic function in CoordinateConversion.c
// should take 4 vectors: g (0,0,-9.81), accels, Be (or 1,0,0 if no home loc) and magnetometers (or 1,0,0 if no mags)
// should calculate the rotation in 3d space using proper cross product math
// SUBTODO: formulate the math required
attitudeState.Roll = RAD2DEG(attitudeState.Roll);
attitudeState.Pitch = RAD2DEG(attitudeState.Pitch);
attitudeState.Yaw = RAD2DEG(attitudeState.Yaw);
RPY2Quaternion(&attitudeState.Roll, &attitudeState.q1);
AttitudeStateSet(&attitudeState);
timeval = PIOS_DELAY_GetRaw();
// wait calibration_delay only at powerup
if (xTaskGetTickCount() < 3000) {
initStartupTime = 0;
} else {
initStartupTime = xTaskGetTickCount() - CALIBRATION_DELAY;
}
// Zero gyro bias
// This is really needed after updating calibration settings.
gyro_bias[0] = 0.0f;
gyro_bias[1] = 0.0f;
gyro_bias[2] = 0.0f;
return 0;
}
if ((xTaskGetTickCount() - initStartupTime < CALIBRATION_DURATION + CALIBRATION_DELAY) &&
(xTaskGetTickCount() - initStartupTime > CALIBRATION_DELAY)) {
// For first CALIBRATION_DURATION seconds after CALIBRATION_DELAY from startup
// Zero gyro bias assuming it is steady, smoothing the gyro input value applying rollPitchBiasRate.
attitudeSettings.AccelKp = 1.0f;
attitudeSettings.AccelKi = 0.0f;
attitudeSettings.YawBiasRate = 0.23f;
accel_filter_enabled = false;
rollPitchBiasRate = 0.01f;
attitudeSettings.MagKp = magCalibrated ? 1.0f : 0.0f;
init = 0;
} else if ((attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
attitudeSettings.AccelKp = 1.0f;
attitudeSettings.AccelKi = 0.0f;
attitudeSettings.YawBiasRate = 0.23f;
accel_filter_enabled = false;
rollPitchBiasRate = 0.01f;
attitudeSettings.MagKp = magCalibrated ? 1.0f : 0.0f;
init = 0;
} else if (init == 0) {
// Reload settings (all the rates)
AttitudeSettingsGet(&attitudeSettings);
rollPitchBiasRate = 0.0f;
if (accel_alpha > 0.0f) {
accel_filter_enabled = true;
}
init = 1;
}
GyroSensorGet(&gyroSensorData);
gyroStateData.x = gyroSensorData.x;
gyroStateData.y = gyroSensorData.y;
gyroStateData.z = gyroSensorData.z;
// Compute the dT using the cpu clock
dT = PIOS_DELAY_DiffuS(timeval) / 1000000.0f;
timeval = PIOS_DELAY_GetRaw();
float q[4];
AttitudeStateData attitudeState;
AttitudeStateGet(&attitudeState);
float grot[3];
float accel_err[3];
// Get the current attitude estimate
quat_copy(&attitudeState.q1, q);
// Apply smoothing to accel values, to reduce vibration noise before main calculations.
apply_accel_filter((const float *)&accelSensorData.x, accels_filtered);
// Rotate gravity to body frame and cross with accels
grot[0] = -(2.0f * (q[1] * q[3] - q[0] * q[2]));
grot[1] = -(2.0f * (q[2] * q[3] + q[0] * q[1]));
grot[2] = -(q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
apply_accel_filter(grot, grot_filtered);
CrossProduct((const float *)accels_filtered, (const float *)grot_filtered, accel_err);
// Account for accel magnitude
accel_mag = accels_filtered[0] * accels_filtered[0] + accels_filtered[1] * accels_filtered[1] + accels_filtered[2] * accels_filtered[2];
accel_mag = sqrtf(accel_mag);
float grot_mag;
if (accel_filter_enabled) {
grot_mag = sqrtf(grot_filtered[0] * grot_filtered[0] + grot_filtered[1] * grot_filtered[1] + grot_filtered[2] * grot_filtered[2]);
} else {
grot_mag = 1.0f;
}
// TODO! check grot_mag & accel vector magnitude values for correctness.
accel_err[0] /= (accel_mag * grot_mag);
accel_err[1] /= (accel_mag * grot_mag);
accel_err[2] /= (accel_mag * grot_mag);
if (xQueueReceive(magQueue, &ev, 0) != pdTRUE) {
// Rotate gravity to body frame and cross with accels
float brot[3];
float Rbe[3][3];
MagSensorData mag;
Quaternion2R(q, Rbe);
MagSensorGet(&mag);
// TODO: separate filter!
if (revoCalibration.MagBiasNullingRate > 0) {
magOffsetEstimation(&mag);
}
MagStateData mags;
mags.x = mag.x;
mags.y = mag.y;
mags.z = mag.z;
MagStateSet(&mags);
// If the mag is producing bad data don't use it (normally bad calibration)
if (!isnan(mag.x) && !isinf(mag.x) && !isnan(mag.y) && !isinf(mag.y) && !isnan(mag.z) && !isinf(mag.z)) {
rot_mult(Rbe, homeLocation.Be, brot);
float mag_len = sqrtf(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z);
mag.x /= mag_len;
mag.y /= mag_len;
mag.z /= mag_len;
float bmag = sqrtf(brot[0] * brot[0] + brot[1] * brot[1] + brot[2] * brot[2]);
brot[0] /= bmag;
brot[1] /= bmag;
brot[2] /= bmag;
// Only compute if neither vector is null
if (bmag < 1.0f || mag_len < 1.0f) {
mag_err[0] = mag_err[1] = mag_err[2] = 0.0f;
} else {
CrossProduct((const float *)&mag.x, (const float *)brot, mag_err);
}
}
} else {
mag_err[0] = mag_err[1] = mag_err[2] = 0.0f;
}
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
// Correct rates based on integral coefficient
gyroStateData.x -= gyro_bias[0];
gyroStateData.y -= gyro_bias[1];
gyroStateData.z -= gyro_bias[2];
gyro_bias[0] -= accel_err[0] * attitudeSettings.AccelKi - (gyroStateData.x) * rollPitchBiasRate;
gyro_bias[1] -= accel_err[1] * attitudeSettings.AccelKi - (gyroStateData.y) * rollPitchBiasRate;
gyro_bias[2] -= -mag_err[2] * attitudeSettings.MagKi - (gyroStateData.z) * rollPitchBiasRate;
// save gyroscope state
GyroStateSet(&gyroStateData);
// Correct rates based on proportional coefficient
gyroStateData.x += accel_err[0] * attitudeSettings.AccelKp / dT;
gyroStateData.y += accel_err[1] * attitudeSettings.AccelKp / dT;
gyroStateData.z += accel_err[2] * attitudeSettings.AccelKp / dT + mag_err[2] * attitudeSettings.MagKp / dT;
// Work out time derivative from INSAlgo writeup
// Also accounts for the fact that gyros are in deg/s
float qdot[4];
qdot[0] = DEG2RAD(-q[1] * gyroStateData.x - q[2] * gyroStateData.y - q[3] * gyroStateData.z) * dT / 2;
qdot[1] = DEG2RAD(q[0] * gyroStateData.x - q[3] * gyroStateData.y + q[2] * gyroStateData.z) * dT / 2;
qdot[2] = DEG2RAD(q[3] * gyroStateData.x + q[0] * gyroStateData.y - q[1] * gyroStateData.z) * dT / 2;
qdot[3] = DEG2RAD(-q[2] * gyroStateData.x + q[1] * gyroStateData.y + q[0] * gyroStateData.z) * dT / 2;
// Take a time step
q[0] = q[0] + qdot[0];
q[1] = q[1] + qdot[1];
q[2] = q[2] + qdot[2];
q[3] = q[3] + qdot[3];
if (q[0] < 0.0f) {
q[0] = -q[0];
q[1] = -q[1];
q[2] = -q[2];
q[3] = -q[3];
}
// Renomalize
qmag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
q[0] = q[0] / qmag;
q[1] = q[1] / qmag;
q[2] = q[2] / qmag;
q[3] = q[3] / qmag;
// If quaternion has become inappropriately short or is nan reinit.
// THIS SHOULD NEVER ACTUALLY HAPPEN
if ((fabsf(qmag) < 1.0e-3f) || isnan(qmag)) {
q[0] = 1.0f;
q[1] = 0.0f;
q[2] = 0.0f;
q[3] = 0.0f;
}
quat_copy(q, &attitudeState.q1);
// Convert into eueler degrees (makes assumptions about RPY order)
Quaternion2RPY(&attitudeState.q1, &attitudeState.Roll);
AttitudeStateSet(&attitudeState);
// Flush these queues for avoid errors
xQueueReceive(baroQueue, &ev, 0);
if (xQueueReceive(gpsQueue, &ev, 0) == pdTRUE && homeLocation.Set == HOMELOCATION_SET_TRUE) {
float NED[3];
// Transform the GPS position into NED coordinates
GPSPositionSensorData gpsPosition;
GPSPositionSensorGet(&gpsPosition);
getNED(&gpsPosition, NED);
PositionStateData positionState;
PositionStateGet(&positionState);
positionState.North = NED[0];
positionState.East = NED[1];
positionState.Down = NED[2];
PositionStateSet(&positionState);
}
if (xQueueReceive(gpsVelQueue, &ev, 0) == pdTRUE) {
// Transform the GPS position into NED coordinates
GPSVelocitySensorData gpsVelocity;
GPSVelocitySensorGet(&gpsVelocity);
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
velocityState.North = gpsVelocity.North;
velocityState.East = gpsVelocity.East;
velocityState.Down = gpsVelocity.Down;
VelocityStateSet(&velocityState);
}
if (xQueueReceive(airspeedQueue, &ev, 0) == pdTRUE) {
// Calculate true airspeed from indicated airspeed
AirspeedSensorData airspeedSensor;
AirspeedSensorGet(&airspeedSensor);
AirspeedStateData airspeed;
AirspeedStateGet(&airspeed);
PositionStateData positionState;
PositionStateGet(&positionState);
if (airspeedSensor.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE) {
// we have airspeed available
airspeed.CalibratedAirspeed = airspeedSensor.CalibratedAirspeed;
airspeed.TrueAirspeed = (airspeedSensor.TrueAirspeed < 0.f) ? airspeed.CalibratedAirspeed *IAS2TAS(homeLocation.Altitude - positionState.Down) : airspeedSensor.TrueAirspeed;
AirspeedStateSet(&airspeed);
}
}
if (!init && flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
} else if (variance_error) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
}
return 0;
}
#include "insgps.h"
int32_t ins_failed = 0;
extern struct NavStruct Nav;
int32_t init_stage = 0;
/**
* @brief Use the INSGPS fusion algorithm in either indoor or outdoor mode (use GPS)
* @params[in] first_run This is the first run so trigger reinitialization
* @params[in] outdoor_mode If true use the GPS for position, if false weakly pull to (0,0)
* @return 0 for success, -1 for failure
*/
static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
{
UAVObjEvent ev;
GyroSensorData gyroSensorData;
AccelSensorData accelSensorData;
MagStateData magData;
AirspeedSensorData airspeedData;
BaroSensorData baroData;
GPSPositionSensorData gpsData;
GPSVelocitySensorData gpsVelData;
static bool mag_updated = false;
static bool baro_updated;
static bool airspeed_updated;
static bool gps_updated;
static bool gps_vel_updated;
static bool value_error = false;
static float baroOffset = 0.0f;
static uint32_t ins_last_time = 0;
static bool inited;
float NED[3] = { 0.0f, 0.0f, 0.0f };
float vel[3] = { 0.0f, 0.0f, 0.0f };
float zeros[3] = { 0.0f, 0.0f, 0.0f };
// Perform the update
uint16_t sensors = 0;
float dT;
// Wait until the gyro and accel object is updated, if a timeout then go to failsafe
if ((xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE) ||
(xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE)) {
// Do not set attitude timeout warnings in simulation mode
if (!AttitudeStateReadOnly()) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING);
return -1;
}
}
if (inited) {
mag_updated = 0;
baro_updated = 0;
airspeed_updated = 0;
gps_updated = 0;
gps_vel_updated = 0;
}
if (first_run) {
inited = false;
init_stage = 0;
mag_updated = 0;
baro_updated = 0;
airspeed_updated = 0;
gps_updated = 0;
gps_vel_updated = 0;
ins_last_time = PIOS_DELAY_GetRaw();
return 0;
}
mag_updated |= (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE);
baro_updated |= xQueueReceive(baroQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE;
airspeed_updated |= xQueueReceive(airspeedQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE;
// Check if we are running simulation
if (!GPSPositionSensorReadOnly()) {
gps_updated |= (xQueueReceive(gpsQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode;
} else {
gps_updated |= pdTRUE && outdoor_mode;
}
if (!GPSVelocitySensorReadOnly()) {
gps_vel_updated |= (xQueueReceive(gpsVelQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode;
} else {
gps_vel_updated |= pdTRUE && outdoor_mode;
}
// Get most recent data
GyroSensorGet(&gyroSensorData);
AccelSensorGet(&accelSensorData);
// TODO: separate filter!
if (mag_updated) {
MagSensorData mags;
MagSensorGet(&mags);
if (revoCalibration.MagBiasNullingRate > 0) {
magOffsetEstimation(&mags);
}
magData.x = mags.x;
magData.y = mags.y;
magData.z = mags.z;
MagStateSet(&magData);
} else {
MagStateGet(&magData);
}
BaroSensorGet(&baroData);
AirspeedSensorGet(&airspeedData);
GPSPositionSensorGet(&gpsData);
GPSVelocitySensorGet(&gpsVelData);
// TODO: put in separate filter
AccelStateData accelState;
accelState.x = accelSensorData.x;
accelState.y = accelSensorData.y;
accelState.z = accelSensorData.z;
AccelStateSet(&accelState);
value_error = false;
// safety checks
if (invalid(gyroSensorData.x) ||
invalid(gyroSensorData.y) ||
invalid(gyroSensorData.z) ||
invalid(accelSensorData.x) ||
invalid(accelSensorData.y) ||
invalid(accelSensorData.z)) {
// cannot run process update, raise error!
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
return 0;
}
if (invalid(magData.x) ||
invalid(magData.y) ||
invalid(magData.z)) {
// magnetometers can be ignored for a while
mag_updated = false;
value_error = true;
}
// Don't require HomeLocation.Set to be true but at least require a mag configuration (allows easily
// switching between indoor and outdoor mode with Set = false)
if ((homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1] + homeLocation.Be[2] * homeLocation.Be[2] < 1e-5f)) {
mag_updated = false;
value_error = true;
}
if (invalid(baroData.Altitude)) {
baro_updated = false;
value_error = true;
}
if (invalid(airspeedData.CalibratedAirspeed)) {
airspeed_updated = false;
value_error = true;
}
if (invalid(gpsData.Altitude)) {
gps_updated = false;
value_error = true;
}
if (invalid_var(ekfConfiguration.R.GPSPosNorth) ||
invalid_var(ekfConfiguration.R.GPSPosEast) ||
invalid_var(ekfConfiguration.R.GPSPosDown) ||
invalid_var(ekfConfiguration.R.GPSVelNorth) ||
invalid_var(ekfConfiguration.R.GPSVelEast) ||
invalid_var(ekfConfiguration.R.GPSVelDown)) {
gps_updated = false;
value_error = true;
}
if (invalid(gpsVelData.North) ||
invalid(gpsVelData.East) ||
invalid(gpsVelData.Down)) {
gps_vel_updated = false;
value_error = true;
}
// Discard airspeed if sensor not connected
if (airspeedData.SensorConnected != AIRSPEEDSENSOR_SENSORCONNECTED_TRUE) {
airspeed_updated = false;
}
// Have a minimum requirement for gps usage
if ((gpsData.Satellites < 7) ||
(gpsData.PDOP > 4.0f) ||
(gpsData.Latitude == 0 && gpsData.Longitude == 0) ||
(homeLocation.Set != HOMELOCATION_SET_TRUE)) {
gps_updated = false;
gps_vel_updated = false;
}
if (!inited) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
} else if (value_error) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
} else if (variance_error) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
} else if (outdoor_mode && gpsData.Satellites < 7) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
}
dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f;
ins_last_time = PIOS_DELAY_GetRaw();
// This should only happen at start up or at mode switches
if (dT > 0.01f) {
dT = 0.01f;
} else if (dT <= 0.001f) {
dT = 0.001f;
}
if (!inited && mag_updated && baro_updated && (gps_updated || !outdoor_mode) && !variance_error) {
// Don't initialize until all sensors are read
if (init_stage == 0) {
// Reset the INS algorithm
INSGPSInit();
INSSetMagVar((float[3]) { ekfConfiguration.R.MagX,
ekfConfiguration.R.MagY,
ekfConfiguration.R.MagZ }
);
INSSetAccelVar((float[3]) { ekfConfiguration.Q.AccelX,
ekfConfiguration.Q.AccelY,
ekfConfiguration.Q.AccelZ }
);
INSSetGyroVar((float[3]) { ekfConfiguration.Q.GyroX,
ekfConfiguration.Q.GyroY,
ekfConfiguration.Q.GyroZ }
);
INSSetGyroBiasVar((float[3]) { ekfConfiguration.Q.GyroDriftX,
ekfConfiguration.Q.GyroDriftY,
ekfConfiguration.Q.GyroDriftZ }
);
INSSetBaroVar(ekfConfiguration.R.BaroZ);
// Initialize the gyro bias
float gyro_bias[3] = { 0.0f, 0.0f, 0.0f };
INSSetGyroBias(gyro_bias);
float pos[3] = { 0.0f, 0.0f, 0.0f };
if (outdoor_mode) {
GPSPositionSensorData gpsPosition;
GPSPositionSensorGet(&gpsPosition);
// Transform the GPS position into NED coordinates
getNED(&gpsPosition, pos);
// Initialize barometric offset to current GPS NED coordinate
baroOffset = -pos[2] - baroData.Altitude;
} else {
// Initialize barometric offset to homelocation altitude
baroOffset = -baroData.Altitude;
pos[2] = -(baroData.Altitude + baroOffset);
}
// xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS);
// MagSensorGet(&magData);
AttitudeStateData attitudeState;
AttitudeStateGet(&attitudeState);
// Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly,
// so pseudo "north" vector can be estimated even if the board is not level
attitudeState.Roll = atan2f(-accelSensorData.y, -accelSensorData.z);
float zn = cosf(attitudeState.Roll) * magData.z + sinf(attitudeState.Roll) * magData.y;
float yn = cosf(attitudeState.Roll) * magData.y - sinf(attitudeState.Roll) * magData.z;
// rotate accels z vector according to roll
float azn = cosf(attitudeState.Roll) * accelSensorData.z + sinf(attitudeState.Roll) * accelSensorData.y;
attitudeState.Pitch = atan2f(accelSensorData.x, -azn);
float xn = cosf(attitudeState.Pitch) * magData.x + sinf(attitudeState.Pitch) * zn;
attitudeState.Yaw = atan2f(-yn, xn);
// TODO: This is still a hack
// Put this in a proper generic function in CoordinateConversion.c
// should take 4 vectors: g (0,0,-9.81), accels, Be (or 1,0,0 if no home loc) and magnetometers (or 1,0,0 if no mags)
// should calculate the rotation in 3d space using proper cross product math
// SUBTODO: formulate the math required
attitudeState.Roll = RAD2DEG(attitudeState.Roll);
attitudeState.Pitch = RAD2DEG(attitudeState.Pitch);
attitudeState.Yaw = RAD2DEG(attitudeState.Yaw);
RPY2Quaternion(&attitudeState.Roll, &attitudeState.q1);
AttitudeStateSet(&attitudeState);
float q[4] = { attitudeState.q1, attitudeState.q2, attitudeState.q3, attitudeState.q4 };
INSSetState(pos, zeros, q, zeros, zeros);
INSResetP(cast_struct_to_array(ekfConfiguration.P, ekfConfiguration.P.AttitudeQ1));
} else {
// Run prediction a bit before any corrections
// Because the sensor module remove the bias we need to add it
// back in here so that the INS algorithm can track it correctly
float gyros[3] = { DEG2RAD(gyroSensorData.x), DEG2RAD(gyroSensorData.y), DEG2RAD(gyroSensorData.z) };
INSStatePrediction(gyros, &accelSensorData.x, dT);
AttitudeStateData attitude;
AttitudeStateGet(&attitude);
attitude.q1 = Nav.q[0];
attitude.q2 = Nav.q[1];
attitude.q3 = Nav.q[2];
attitude.q4 = Nav.q[3];
Quaternion2RPY(&attitude.q1, &attitude.Roll);
AttitudeStateSet(&attitude);
}
init_stage++;
if (init_stage > 10) {
inited = true;
}
return 0;
}
if (!inited) {
return 0;
}
// Because the sensor module remove the bias we need to add it
// back in here so that the INS algorithm can track it correctly
float gyros[3] = { DEG2RAD(gyroSensorData.x), DEG2RAD(gyroSensorData.y), DEG2RAD(gyroSensorData.z) };
// Advance the state estimate
INSStatePrediction(gyros, &accelSensorData.x, dT);
// Copy the attitude into the UAVO
AttitudeStateData attitude;
AttitudeStateGet(&attitude);
attitude.q1 = Nav.q[0];
attitude.q2 = Nav.q[1];
attitude.q3 = Nav.q[2];
attitude.q4 = Nav.q[3];
Quaternion2RPY(&attitude.q1, &attitude.Roll);
AttitudeStateSet(&attitude);
// Advance the covariance estimate
INSCovariancePrediction(dT);
if (mag_updated) {
sensors |= MAG_SENSORS;
}
if (baro_updated) {
sensors |= BARO_SENSOR;
}
INSSetMagNorth(homeLocation.Be);
if (gps_updated && outdoor_mode) {
INSSetPosVelVar((float[3]) { ekfConfiguration.R.GPSPosNorth,
ekfConfiguration.R.GPSPosEast,
ekfConfiguration.R.GPSPosDown },
(float[3]) { ekfConfiguration.R.GPSVelNorth,
ekfConfiguration.R.GPSVelEast,
ekfConfiguration.R.GPSVelDown }
);
sensors |= POS_SENSORS;
if (0) { // Old code to take horizontal velocity from GPS Position update
sensors |= HORIZ_SENSORS;
vel[0] = gpsData.Groundspeed * cosf(DEG2RAD(gpsData.Heading));
vel[1] = gpsData.Groundspeed * sinf(DEG2RAD(gpsData.Heading));
vel[2] = 0.0f;
}
// Transform the GPS position into NED coordinates
getNED(&gpsData, NED);
// Track barometric altitude offset with a low pass filter
baroOffset = BARO_OFFSET_LOWPASS_ALPHA * baroOffset +
(1.0f - BARO_OFFSET_LOWPASS_ALPHA)
* (-NED[2] - baroData.Altitude);
} else if (!outdoor_mode) {
INSSetPosVelVar((float[3]) { ekfConfiguration.FakeR.FakeGPSPosIndoor,
ekfConfiguration.FakeR.FakeGPSPosIndoor,
ekfConfiguration.FakeR.FakeGPSPosIndoor },
(float[3]) { ekfConfiguration.FakeR.FakeGPSVelIndoor,
ekfConfiguration.FakeR.FakeGPSVelIndoor,
ekfConfiguration.FakeR.FakeGPSVelIndoor }
);
vel[0] = vel[1] = vel[2] = 0.0f;
NED[0] = NED[1] = 0.0f;
NED[2] = -(baroData.Altitude + baroOffset);
sensors |= HORIZ_SENSORS | HORIZ_POS_SENSORS;
sensors |= POS_SENSORS | VERT_SENSORS;
}
if (gps_vel_updated && outdoor_mode) {
sensors |= HORIZ_SENSORS | VERT_SENSORS;
vel[0] = gpsVelData.North;
vel[1] = gpsVelData.East;
vel[2] = gpsVelData.Down;
}
// Copy the position into the UAVO
PositionStateData positionState;
PositionStateGet(&positionState);
positionState.North = Nav.Pos[0];
positionState.East = Nav.Pos[1];
positionState.Down = Nav.Pos[2];
PositionStateSet(&positionState);
// airspeed correction needs current positionState
if (airspeed_updated) {
// we have airspeed available
AirspeedStateData airspeed;
AirspeedStateGet(&airspeed);
airspeed.CalibratedAirspeed = airspeedData.CalibratedAirspeed;
airspeed.TrueAirspeed = (airspeedData.TrueAirspeed < 0.f) ? airspeed.CalibratedAirspeed *IAS2TAS(homeLocation.Altitude - positionState.Down) : airspeedData.TrueAirspeed;
AirspeedStateSet(&airspeed);
if (!gps_vel_updated && !gps_updated) {
// feed airspeed into EKF, treat wind as 1e2 variance
sensors |= HORIZ_SENSORS | VERT_SENSORS;
INSSetPosVelVar((float[3]) { ekfConfiguration.FakeR.FakeGPSPosIndoor,
ekfConfiguration.FakeR.FakeGPSPosIndoor,
ekfConfiguration.FakeR.FakeGPSPosIndoor },
(float[3]) { ekfConfiguration.FakeR.FakeGPSVelAirspeed,
ekfConfiguration.FakeR.FakeGPSVelAirspeed,
ekfConfiguration.FakeR.FakeGPSVelAirspeed }
);
// rotate airspeed vector into NED frame - airspeed is measured in X axis only
float R[3][3];
Quaternion2R(Nav.q, R);
float vtas[3] = { airspeed.TrueAirspeed, 0.0f, 0.0f };
rot_mult(R, vtas, vel);
}
}
/*
* TODO: Need to add a general sanity check for all the inputs to make sure their kosher
* although probably should occur within INS itself
*/
if (sensors) {
INSCorrection(&magData.x, NED, vel, (baroData.Altitude + baroOffset), sensors);
}
// Copy the velocity into the UAVO
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
velocityState.North = Nav.Vel[0];
velocityState.East = Nav.Vel[1];
velocityState.Down = Nav.Vel[2];
VelocityStateSet(&velocityState);
GyroStateData gyroState;
gyroState.x = RAD2DEG(gyros[0] - RAD2DEG(Nav.gyro_bias[0]));
gyroState.y = RAD2DEG(gyros[1] - RAD2DEG(Nav.gyro_bias[1]));
gyroState.z = RAD2DEG(gyros[2] - RAD2DEG(Nav.gyro_bias[2]));
GyroStateSet(&gyroState);
EKFStateVarianceData vardata;
EKFStateVarianceGet(&vardata);
INSGetP(cast_struct_to_array(vardata.P, vardata.P.AttitudeQ1));
EKFStateVarianceSet(&vardata);
return 0;
}
/**
* @brief Convert the GPS LLA position into NED coordinates
* @note this method uses a taylor expansion around the home coordinates
* to convert to NED which allows it to be done with all floating
* calculations
* @param[in] Current GPS coordinates
* @param[out] NED frame coordinates
* @returns 0 for success, -1 for failure
*/
float T[3];
static int32_t getNED(GPSPositionSensorData *gpsPosition, float *NED)
{
float dL[3] = { DEG2RAD((gpsPosition->Latitude - homeLocation.Latitude) / 10.0e6f),
DEG2RAD((gpsPosition->Longitude - homeLocation.Longitude) / 10.0e6f),
(gpsPosition->Altitude + gpsPosition->GeoidSeparation - homeLocation.Altitude) };
NED[0] = T[0] * dL[0];
NED[1] = T[1] * dL[1];
NED[2] = T[2] * dL[2];
return 0;
}
static void settingsUpdatedCb(UAVObjEvent *ev)
{
if (ev == NULL || ev->obj == FlightStatusHandle()) {
FlightStatusGet(&flightStatus);
}
if (ev == NULL || ev->obj == RevoCalibrationHandle()) {
RevoCalibrationGet(&revoCalibration);
}
// change of these settings require reinitialization of the EKF
// when an error flag has been risen, we also listen to flightStatus updates,
// since we are waiting for the system to get disarmed so we can reinitialize safely.
if (ev == NULL ||
ev->obj == EKFConfigurationHandle() ||
ev->obj == RevoSettingsHandle() ||
(variance_error == true && ev->obj == FlightStatusHandle())
) {
bool error = false;
EKFConfigurationGet(&ekfConfiguration);
int t;
for (t = 0; t < EKFCONFIGURATION_P_NUMELEM; t++) {
if (invalid_var(cast_struct_to_array(ekfConfiguration.P, ekfConfiguration.P.AttitudeQ1)[t])) {
error = true;
}
}
for (t = 0; t < EKFCONFIGURATION_Q_NUMELEM; t++) {
if (invalid_var(cast_struct_to_array(ekfConfiguration.Q, ekfConfiguration.Q.AccelX)[t])) {
error = true;
}
}
for (t = 0; t < EKFCONFIGURATION_R_NUMELEM; t++) {
if (invalid_var(cast_struct_to_array(ekfConfiguration.R, ekfConfiguration.R.BaroZ)[t])) {
error = true;
}
}
RevoSettingsGet(&revoSettings);
// Reinitialization of the EKF is not desired during flight.
// It will be delayed until the board is disarmed by raising the error flag.
// We will not prevent the initial initialization though, since the board could be in always armed mode.
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && !initialization_required) {
error = true;
}
if (error) {
variance_error = true;
} else {
// trigger reinitialization - possibly with new algorithm
running_algorithm = revoSettings.FusionAlgorithm;
variance_error = false;
initialization_required = true;
}
}
if (ev == NULL || ev->obj == HomeLocationHandle()) {
HomeLocationGet(&homeLocation);
// Compute matrix to convert deltaLLA to NED
float lat, alt;
lat = DEG2RAD(homeLocation.Latitude / 10.0e6f);
alt = homeLocation.Altitude;
T[0] = alt + 6.378137E6f;
T[1] = cosf(lat) * (alt + 6.378137E6f);
T[2] = -1.0f;
// TODO: convert positionState to new reference frame and gracefully update EKF state!
// needed for long range flights where the reference coordinate is adjusted in flight
}
if (ev == NULL || ev->obj == AttitudeSettingsHandle()) {
AttitudeSettingsGet(&attitudeSettings);
// Calculate accel filter alpha, in the same way as for gyro data in stabilization module.
const float fakeDt = 0.0015f;
if (attitudeSettings.AccelTau < 0.0001f) {
accel_alpha = 0; // not trusting this to resolve to 0
accel_filter_enabled = false;
} else {
accel_alpha = expf(-fakeDt / attitudeSettings.AccelTau);
accel_filter_enabled = true;
}
}
}
/**
* Perform an update of the @ref MagBias based on
* Magmeter Offset Cancellation: Theory and Implementation,
* revisited William Premerlani, October 14, 2011
*/
static void magOffsetEstimation(MagSensorData *mag)
{
#if 0
// Constants, to possibly go into a UAVO
static const float MIN_NORM_DIFFERENCE = 50;
static float B2[3] = { 0, 0, 0 };
MagBiasData magBias;
MagBiasGet(&magBias);
// Remove the current estimate of the bias
mag->x -= magBias.x;
mag->y -= magBias.y;
mag->z -= magBias.z;
// First call
if (B2[0] == 0 && B2[1] == 0 && B2[2] == 0) {
B2[0] = mag->x;
B2[1] = mag->y;
B2[2] = mag->z;
return;
}
float B1[3] = { mag->x, mag->y, mag->z };
float norm_diff = sqrtf(powf(B2[0] - B1[0], 2) + powf(B2[1] - B1[1], 2) + powf(B2[2] - B1[2], 2));
if (norm_diff > MIN_NORM_DIFFERENCE) {
float norm_b1 = sqrtf(B1[0] * B1[0] + B1[1] * B1[1] + B1[2] * B1[2]);
float norm_b2 = sqrtf(B2[0] * B2[0] + B2[1] * B2[1] + B2[2] * B2[2]);
float scale = cal.MagBiasNullingRate * (norm_b2 - norm_b1) / norm_diff;
float b_error[3] = { (B2[0] - B1[0]) * scale, (B2[1] - B1[1]) * scale, (B2[2] - B1[2]) * scale };
magBias.x += b_error[0];
magBias.y += b_error[1];
magBias.z += b_error[2];
MagBiasSet(&magBias);
// Store this value to compare against next update
B2[0] = B1[0]; B2[1] = B1[1]; B2[2] = B1[2];
}
#else // if 0
static float magBias[3] = { 0 };
// Remove the current estimate of the bias
mag->x -= magBias[0];
mag->y -= magBias[1];
mag->z -= magBias[2];
AttitudeStateData attitude;
AttitudeStateGet(&attitude);
const float Rxy = sqrtf(homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1]);
const float Rz = homeLocation.Be[2];
const float rate = revoCalibration.MagBiasNullingRate;
float Rot[3][3];
float B_e[3];
float xy[2];
float delta[3];
// Get the rotation matrix
Quaternion2R(&attitude.q1, Rot);
// Rotate the mag into the NED frame
B_e[0] = Rot[0][0] * mag->x + Rot[1][0] * mag->y + Rot[2][0] * mag->z;
B_e[1] = Rot[0][1] * mag->x + Rot[1][1] * mag->y + Rot[2][1] * mag->z;
B_e[2] = Rot[0][2] * mag->x + Rot[1][2] * mag->y + Rot[2][2] * mag->z;
float cy = cosf(DEG2RAD(attitude.Yaw));
float sy = sinf(DEG2RAD(attitude.Yaw));
xy[0] = cy * B_e[0] + sy * B_e[1];
xy[1] = -sy * B_e[0] + cy * B_e[1];
float xy_norm = sqrtf(xy[0] * xy[0] + xy[1] * xy[1]);
delta[0] = -rate * (xy[0] / xy_norm * Rxy - xy[0]);
delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]);
delta[2] = -rate * (Rz - B_e[2]);
if (!isnan(delta[0]) && !isinf(delta[0]) &&
!isnan(delta[1]) && !isinf(delta[1]) &&
!isnan(delta[2]) && !isinf(delta[2])) {
magBias[0] += delta[0];
magBias[1] += delta[1];
magBias[2] += delta[2];
}
#endif // if 0
}
/**
* @}
* @}
*/

30
flight/modules/Extensions/MagBaro/magbaro.c
View File

@ -64,7 +64,8 @@ static int32_t alt_ds_pres = 0;
static int alt_ds_count = 0;
#endif
#if defined(PIOS_INCLUDE_HMC5883)
#if defined(PIOS_INCLUDE_HMC5X83)
pios_hmc5x83_dev_t mag_handle = 0;
int32_t mag_test;
static float mag_bias[3] = { 0, 0, 0 };
static float mag_scale[3] = { 1, 1, 1 };
@ -108,7 +109,7 @@ int32_t MagBaroInitialize()
#endif
if (magbaroEnabled) {
#if defined(PIOS_INCLUDE_HMC5883)
#if defined(PIOS_INCLUDE_HMC5X83)
MagSensorInitialize();
#endif
@ -127,15 +128,16 @@ MODULE_INITCALL(MagBaroInitialize, MagBaroStart);
/**
* Module thread, should not return.
*/
#if defined(PIOS_INCLUDE_HMC5883)
static const struct pios_hmc5883_cfg pios_hmc5883_cfg = {
#ifdef PIOS_HMC5883_HAS_GPIOS
#if defined(PIOS_INCLUDE_HMC5X83)
static const struct pios_hmc5x83_cfg pios_hmc5x83_cfg = {
#ifdef PIOS_HMC5X83_HAS_GPIOS
.exti_cfg = 0,
#endif
.M_ODR = PIOS_HMC5883_ODR_15,
.Meas_Conf = PIOS_HMC5883_MEASCONF_NORMAL,
.Gain = PIOS_HMC5883_GAIN_1_9,
.Mode = PIOS_HMC5883_MODE_CONTINUOUS,
.M_ODR = PIOS_HMC5x83_ODR_15,
.Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL,
.Gain = PIOS_HMC5x83_GAIN_1_9,
.Mode = PIOS_HMC5x83_MODE_CONTINUOUS,
.Driver = &PIOS_HMC5x83_I2C_DRIVER,
};
#endif
@ -148,9 +150,9 @@ static void magbaroTask(__attribute__((unused)) void *parameters)
PIOS_BMP085_Init();
#endif
#if defined(PIOS_INCLUDE_HMC5883)
#if defined(PIOS_INCLUDE_HMC5X83)
MagSensorData mag;
PIOS_HMC5883_Init(&pios_hmc5883_cfg);
mag_handle = PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, PIOS_I2C_MAIN_ADAPTER, 0);
uint32_t mag_update_time = PIOS_DELAY_GetRaw();
#endif
@ -197,10 +199,10 @@ static void magbaroTask(__attribute__((unused)) void *parameters)
}
#endif /* if defined(PIOS_INCLUDE_BMP085) */
#if defined(PIOS_INCLUDE_HMC5883)
if (PIOS_HMC5883_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 100000) {
#if defined(PIOS_INCLUDE_HMC5X83)
if (PIOS_HMC5x83_NewDataAvailable(mag_handle) || PIOS_DELAY_DiffuS(mag_update_time) > 100000) {
int16_t values[3];
PIOS_HMC5883_ReadMag(values);
PIOS_HMC5x83_ReadMag(mag_handle, values);
float mags[3] = { (float)values[1] * mag_scale[0] - mag_bias[0],
(float)values[0] * mag_scale[1] - mag_bias[1],
-(float)values[2] * mag_scale[2] - mag_bias[2] };

4
flight/modules/GPS/NMEA.c
View File

@ -687,8 +687,8 @@ static bool nmeaProcessGPGSV(__attribute__((unused)) GPSPositionSensorData *GpsD
// Get sat info
gsv_partial.PRN[sat_index] = atoi(param[parIdx++]);
gsv_partial.Elevation[sat_index] = NMEA_real_to_float(param[parIdx++]);
gsv_partial.Azimuth[sat_index] = NMEA_real_to_float(param[parIdx++]);
gsv_partial.Elevation[sat_index] = atoi(param[parIdx++]);
gsv_partial.Azimuth[sat_index] = atoi(param[parIdx++]);
gsv_partial.SNR[sat_index] = atoi(param[parIdx++]);
#ifdef NMEA_DEBUG_GSV
DEBUG_MSG(" %d", gsv_partial.PRN[sat_index]);

104
flight/modules/GPS/UBX.c
View File

@ -32,10 +32,11 @@
#include "pios.h"
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
#include "inc/UBX.h"
#include "inc/GPS.h"
#include "UBX.h"
#include "GPS.h"
// If a PVT sentence is received in the last UBX_PVT_TIMEOUT (ms) timeframe it disables VELNED/POSLLH/SOL/TIMEUTC
#define UBX_PVT_TIMEOUT (1000)
// parse incoming character stream for messages in UBX binary format
int parse_ubx_stream(uint8_t c, char *gps_rx_buffer, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *gpsRxStats)
@ -251,6 +252,47 @@ void parse_ubx_nav_velned(struct UBX_NAV_VELNED *velned, GPSPositionSensorData *
}
}
void parse_ubx_nav_pvt(struct UBX_NAV_PVT *pvt, GPSPositionSensorData *GpsPosition)
{
GPSVelocitySensorData GpsVelocity;
check_msgtracker(pvt->iTOW, (ALL_RECEIVED));
GpsVelocity.North = (float)pvt->velN * 0.001f;
GpsVelocity.East = (float)pvt->velE * 0.001f;
GpsVelocity.Down = (float)pvt->velD * 0.001f;
GPSVelocitySensorSet(&GpsVelocity);
GpsPosition->Groundspeed = (float)pvt->gSpeed * 0.001f;
GpsPosition->Heading = (float)pvt->heading * 1.0e-5f;
GpsPosition->Altitude = (float)pvt->hMSL * 0.001f;
GpsPosition->GeoidSeparation = (float)(pvt->height - pvt->hMSL) * 0.001f;
GpsPosition->Latitude = pvt->lat;
GpsPosition->Longitude = pvt->lon;
GpsPosition->Satellites = pvt->numSV;
GpsPosition->PDOP = pvt->pDOP * 0.01f;
if (pvt->flags & PVT_FLAGS_GNSSFIX_OK) {
GpsPosition->Status = pvt->fixType == PVT_FIX_TYPE_3D ?
GPSPOSITIONSENSOR_STATUS_FIX3D : GPSPOSITIONSENSOR_STATUS_FIX2D;
} else {
GpsPosition->Status = GPSPOSITIONSENSOR_STATUS_NOFIX;
}
#if !defined(PIOS_GPS_MINIMAL)
if (pvt->valid & PVT_VALID_VALIDTIME) {
// Time is valid, set GpsTime
GPSTimeData GpsTime;
GpsTime.Year = pvt->year;
GpsTime.Month = pvt->month;
GpsTime.Day = pvt->day;
GpsTime.Hour = pvt->hour;
GpsTime.Minute = pvt->min;
GpsTime.Second = pvt->sec;
GPSTimeSet(&GpsTime);
}
#endif
}
#if !defined(PIOS_GPS_MINIMAL)
void parse_ubx_nav_timeutc(struct UBX_NAV_TIMEUTC *timeutc)
{
@ -283,8 +325,8 @@ void parse_ubx_nav_svinfo(struct UBX_NAV_SVINFO *svinfo)
svdata.SatsInView = 0;
for (chan = 0; chan < svinfo->numCh; chan++) {
if (svdata.SatsInView < GPSSATELLITES_PRN_NUMELEM) {
svdata.Azimuth[svdata.SatsInView] = (float)svinfo->sv[chan].azim;
svdata.Elevation[svdata.SatsInView] = (float)svinfo->sv[chan].elev;
svdata.Azimuth[svdata.SatsInView] = svinfo->sv[chan].azim;
svdata.Elevation[svdata.SatsInView] = svinfo->sv[chan].elev;
svdata.PRN[svdata.SatsInView] = svinfo->sv[chan].svid;
svdata.SNR[svdata.SatsInView] = svinfo->sv[chan].cno;
svdata.SatsInView++;
@ -292,8 +334,8 @@ void parse_ubx_nav_svinfo(struct UBX_NAV_SVINFO *svinfo)
}
// fill remaining slots (if any)
for (chan = svdata.SatsInView; chan < GPSSATELLITES_PRN_NUMELEM; chan++) {
svdata.Azimuth[chan] = (float)0.0f;
svdata.Elevation[chan] = (float)0.0f;
svdata.Azimuth[chan] = 0;
svdata.Elevation[chan] = 0;
svdata.PRN[chan] = 0;
svdata.SNR[chan] = 0;
}
@ -308,26 +350,51 @@ void parse_ubx_nav_svinfo(struct UBX_NAV_SVINFO *svinfo)
uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition)
{
uint32_t id = 0;
static uint32_t lastPvtTime = 0;
static bool ubxInitialized = false;
if (!ubxInitialized) {
// initialize dop values. If no DOP sentence is received it is safer to initialize them to a high value rather than 0.
GpsPosition->HDOP = 99.99f;
GpsPosition->PDOP = 99.99f;
GpsPosition->VDOP = 99.99f;
ubxInitialized = true;
}
// is it using PVT?
bool usePvt = (lastPvtTime) && (PIOS_DELAY_GetuSSince(lastPvtTime) < UBX_PVT_TIMEOUT * 1000);
switch (ubx->header.class) {
case UBX_CLASS_NAV:
if (!usePvt) {
// Set of messages to be decoded when not using pvt
switch (ubx->header.id) {
case UBX_ID_POSLLH:
parse_ubx_nav_posllh(&ubx->payload.nav_posllh, GpsPosition);
break;
case UBX_ID_SOL:
parse_ubx_nav_sol(&ubx->payload.nav_sol, GpsPosition);
break;
case UBX_ID_VELNED:
parse_ubx_nav_velned(&ubx->payload.nav_velned, GpsPosition);
break;
#if !defined(PIOS_GPS_MINIMAL)
case UBX_ID_TIMEUTC:
parse_ubx_nav_timeutc(&ubx->payload.nav_timeutc);
break;
#endif
}
}
// messages used always
switch (ubx->header.id) {
case UBX_ID_POSLLH:
parse_ubx_nav_posllh(&ubx->payload.nav_posllh, GpsPosition);
break;
case UBX_ID_DOP:
parse_ubx_nav_dop(&ubx->payload.nav_dop, GpsPosition);
break;
case UBX_ID_SOL:
parse_ubx_nav_sol(&ubx->payload.nav_sol, GpsPosition);
break;
case UBX_ID_VELNED:
parse_ubx_nav_velned(&ubx->payload.nav_velned, GpsPosition);
case UBX_ID_PVT:
parse_ubx_nav_pvt(&ubx->payload.nav_pvt, GpsPosition);
lastPvtTime = PIOS_DELAY_GetuS();
break;
#if !defined(PIOS_GPS_MINIMAL)
case UBX_ID_TIMEUTC:
parse_ubx_nav_timeutc(&ubx->payload.nav_timeutc);
break;
case UBX_ID_SVINFO:
parse_ubx_nav_svinfo(&ubx->payload.nav_svinfo);
break;
@ -335,6 +402,7 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi
}
break;
}
if (msgtracker.msg_received == ALL_RECEIVED) {
GPSPositionSensorSet(GpsPosition);
msgtracker.msg_received = NONE_RECEIVED;

55
flight/modules/GPS/inc/UBX.h
View File

@ -51,6 +51,7 @@
#define UBX_ID_VELNED 0x12
#define UBX_ID_TIMEUTC 0x21
#define UBX_ID_SVINFO 0x30
#define UBX_ID_PVT 0x07
// private structures
@ -156,6 +157,59 @@ struct UBX_NAV_TIMEUTC {
uint8_t valid; // Validity Flags
};
#define PVT_VALID_VALIDDATE 0x01
#define PVT_VALID_VALIDTIME 0x02
#define PVT_VALID_FULLYRESOLVED 0x04
#define PVT_FIX_TYPE_NO_FIX 0
#define PVT_FIX_TYPE_DEAD_RECKON 0x01 // Dead Reckoning only
#define PVT_FIX_TYPE_2D 0x02 // 2D-Fix
#define PVT_FIX_TYPE_3D 0x03 // 3D-Fix
#define PVT_FIX_TYPE_GNSS_DEAD_RECKON 0x04 // GNSS + dead reckoning combined
#define PVT_FIX_TYPE_TIME_ONLY 0x05 // Time only fix
#define PVT_FLAGS_GNSSFIX_OK (1 << 0)
#define PVT_FLAGS_DIFFSOLN (1 << 1)
#define PVT_FLAGS_PSMSTATE_ENABLED (1 << 2)
#define PVT_FLAGS_PSMSTATE_ACQUISITION (2 << 2)
#define PVT_FLAGS_PSMSTATE_TRACKING (3 << 2)
#define PVT_FLAGS_PSMSTATE_PO_TRACKING (4 << 2)
#define PVT_FLAGS_PSMSTATE_INACTIVE (5 << 2)
// PVT Navigation Position Velocity Time Solution
struct UBX_NAV_PVT {
uint32_t iTOW;
uint16_t year;
uint8_t month;
uint8_t day;
uint8_t hour;
uint8_t min;
uint8_t sec;
uint8_t valid;
uint32_t tAcc;
int32_t nano;
uint8_t fixType;
uint8_t flags;
uint8_t reserved1;
uint8_t numSV;
int32_t lon;
int32_t lat;
int32_t height;
int32_t hMSL;
uint32_t hAcc;
uint32_t vAcc;
int32_t velN;
int32_t velE;
int32_t velD;
int32_t gSpeed;
int32_t heading;
uint32_t sAcc;
uint32_t headingAcc;
uint16_t pDOP;
uint16_t reserved2;
uint32_t reserved3;
} __attribute__((packed));
// Space Vehicle (SV) Information
// Single SV information block
@ -198,6 +252,7 @@ typedef union {
struct UBX_NAV_DOP nav_dop;
struct UBX_NAV_SOL nav_sol;
struct UBX_NAV_VELNED nav_velned;
struct UBX_NAV_PVT nav_pvt;
#if !defined(PIOS_GPS_MINIMAL)
struct UBX_NAV_TIMEUTC nav_timeutc;
struct UBX_NAV_SVINFO nav_svinfo;

17
flight/modules/Sensors/sensors.c
View File

@ -82,6 +82,11 @@ static xTaskHandle sensorsTaskHandle;
RevoCalibrationData cal;
AccelGyroSettingsData agcal;
#ifdef PIOS_INCLUDE_HMC5X83
#include <pios_hmc5x83.h>
extern pios_hmc5x83_dev_t onboard_mag;
#endif
// These values are initialized by settings but can be updated by the attitude algorithm
static float mag_bias[3] = { 0, 0, 0 };
@ -200,8 +205,8 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
PIOS_DEBUG_Assert(0);
}
#if defined(PIOS_INCLUDE_HMC5883)
mag_test = PIOS_HMC5883_Test();
#if defined(PIOS_INCLUDE_HMC5X83)
mag_test = PIOS_HMC5x83_Test(onboard_mag);
#else
mag_test = 0;
#endif
@ -409,11 +414,11 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
// Because most crafts wont get enough information from gravity to zero yaw gyro, we try
// and make it average zero (weakly)
#if defined(PIOS_INCLUDE_HMC5883)
#if defined(PIOS_INCLUDE_HMC5X83)
MagSensorData mag;
if (PIOS_HMC5883_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 150000) {
if (PIOS_HMC5x83_NewDataAvailable(onboard_mag) || PIOS_DELAY_DiffuS(mag_update_time) > 150000) {
int16_t values[3];
PIOS_HMC5883_ReadMag(values);
PIOS_HMC5x83_ReadMag(onboard_mag, values);
float mags[3] = { (float)values[1] - mag_bias[0],
(float)values[0] - mag_bias[1],
-(float)values[2] - mag_bias[2] };
@ -428,7 +433,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
MagSensorSet(&mag);
mag_update_time = PIOS_DELAY_GetRaw();
}
#endif /* if defined(PIOS_INCLUDE_HMC5883) */
#endif /* if defined(PIOS_INCLUDE_HMC5X83) */
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);

2
flight/modules/StateEstimation/filtercf.c
View File

@ -221,7 +221,7 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float
// During initialization and
if (this->first_run) {
#if defined(PIOS_INCLUDE_HMC5883)
#if defined(PIOS_INCLUDE_HMC5X83)
// wait until mags have been updated
if (!this->magUpdated) {
return FILTERRESULT_ERROR;

42
flight/modules/TxPID/txpid.c
View File

@ -83,6 +83,7 @@
// Private functions
static void updatePIDs(UAVObjEvent *ev);
static uint8_t update(float *var, float val);
static uint8_t updateUint8(uint8_t *var, float val);
static float scale(float val, float inMin, float inMax, float outMin, float outMax);
/**
@ -226,6 +227,9 @@ static void updatePIDs(UAVObjEvent *ev)
case TXPIDSETTINGS_PIDS_ROLLRATEILIMIT:
needsUpdateBank |= update(&bank.RollRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_ROLLRATERESP:
needsUpdateBank |= update(&bank.ManualRate.Roll, value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEKP:
needsUpdateBank |= update(&bank.RollPI.Kp, value);
break;
@ -235,6 +239,9 @@ static void updatePIDs(UAVObjEvent *ev)
case TXPIDSETTINGS_PIDS_ROLLATTITUDEILIMIT:
needsUpdateBank |= update(&bank.RollPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDERESP:
needsUpdateBank |= updateUint8(&bank.RollMax, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEKP:
needsUpdateBank |= update(&bank.PitchRatePID.Kp, value);
break;
@ -247,6 +254,9 @@ static void updatePIDs(UAVObjEvent *ev)
case TXPIDSETTINGS_PIDS_PITCHRATEILIMIT:
needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATERESP:
needsUpdateBank |= update(&bank.ManualRate.Pitch, value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEKP:
needsUpdateBank |= update(&bank.PitchPI.Kp, value);
break;
@ -256,6 +266,9 @@ static void updatePIDs(UAVObjEvent *ev)
case TXPIDSETTINGS_PIDS_PITCHATTITUDEILIMIT:
needsUpdateBank |= update(&bank.PitchPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDERESP:
needsUpdateBank |= updateUint8(&bank.PitchMax, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKP:
needsUpdateBank |= update(&bank.RollRatePID.Kp, value);
needsUpdateBank |= update(&bank.PitchRatePID.Kp, value);
@ -272,6 +285,10 @@ static void updatePIDs(UAVObjEvent *ev)
needsUpdateBank |= update(&bank.RollRatePID.ILimit, value);
needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATERESP:
needsUpdateBank |= update(&bank.ManualRate.Roll, value);
needsUpdateBank |= update(&bank.ManualRate.Pitch, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKP:
needsUpdateBank |= update(&bank.RollPI.Kp, value);
needsUpdateBank |= update(&bank.PitchPI.Kp, value);
@ -284,6 +301,10 @@ static void updatePIDs(UAVObjEvent *ev)
needsUpdateBank |= update(&bank.RollPI.ILimit, value);
needsUpdateBank |= update(&bank.PitchPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDERESP:
needsUpdateBank |= updateUint8(&bank.RollMax, value);
needsUpdateBank |= updateUint8(&bank.PitchMax, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEKP:
needsUpdateBank |= update(&bank.YawRatePID.Kp, value);
break;
@ -296,6 +317,9 @@ static void updatePIDs(UAVObjEvent *ev)
case TXPIDSETTINGS_PIDS_YAWRATEILIMIT:
needsUpdateBank |= update(&bank.YawRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATERESP:
needsUpdateBank |= update(&bank.ManualRate.Yaw, value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEKP:
needsUpdateBank |= update(&bank.YawPI.Kp, value);
break;
@ -305,6 +329,9 @@ static void updatePIDs(UAVObjEvent *ev)
case TXPIDSETTINGS_PIDS_YAWATTITUDEILIMIT:
needsUpdateBank |= update(&bank.YawPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDERESP:
needsUpdateBank |= updateUint8(&bank.YawMax, value);
break;
case TXPIDSETTINGS_PIDS_GYROTAU:
needsUpdateStab |= update(&stab.GyroTau, value);
break;
@ -389,6 +416,21 @@ static uint8_t update(float *var, float val)
return 0;
}
/**
* Updates var using val if needed.
* \returns 1 if updated, 0 otherwise
*/
static uint8_t updateUint8(uint8_t *var, float val)
{
uint8_t roundedVal = (uint8_t)roundf(val);
if (*var != roundedVal) {
*var = roundedVal;
return 1;
}
return 0;
}
/**
* @}
*/

130
flight/pios/common/pios_debuglog.c
View File

@ -49,6 +49,9 @@ static xSemaphoreHandle mutex = 0;
#endif
static bool logging_enabled = false;
#define MAX_CONSECUTIVE_FAILS_COUNT 10
static bool log_is_full = false;
static uint8_t fails_count = 0;
static uint16_t flightnum = 0;
static uint16_t lognum = 0;
static DebugLogEntryData *buffer = 0;
@ -56,8 +59,16 @@ static DebugLogEntryData *buffer = 0;
static DebugLogEntryData staticbuffer;
#endif
/* Private Function Prototypes */
#define LOG_ENTRY_MAX_DATA_SIZE (sizeof(((DebugLogEntryData *)0)->Data))
#define LOG_ENTRY_HEADER_SIZE (sizeof(DebugLogEntryData) - LOG_ENTRY_MAX_DATA_SIZE)
// build the obj_id as a DEBUGLOGENTRY ID with least significant byte zeroed and filled with flight number
#define LOG_GET_FLIGHT_OBJID(x) ((DEBUGLOGENTRY_OBJID & ~0xFF) | (x & 0xFF))
static uint32_t used_buffer_space = 0;
/* Private Function Prototypes */
static void enqueue_data(uint32_t objid, uint16_t instid, size_t size, uint8_t *data);
static bool write_current_buffer();
/**
* @brief Initialize the log facility
*/
@ -75,9 +86,12 @@ void PIOS_DEBUGLOG_Initialize()
return;
}
mutexlock();
lognum = 0;
flightnum = 0;
while (PIOS_FLASHFS_ObjLoad(pios_user_fs_id, flightnum * 256, lognum, (uint8_t *)buffer, sizeof(DebugLogEntryData)) == 0) {
lognum = 0;
flightnum = 0;
fails_count = 0;
used_buffer_space = 0;
log_is_full = false;
while (PIOS_FLASHFS_ObjLoad(pios_user_fs_id, LOG_GET_FLIGHT_OBJID(flightnum), lognum, (uint8_t *)buffer, sizeof(DebugLogEntryData)) == 0) {
flightnum++;
}
mutexunlock();
@ -90,6 +104,11 @@ void PIOS_DEBUGLOG_Initialize()
*/
void PIOS_DEBUGLOG_Enable(uint8_t enabled)
{
// increase the flight num as soon as logging is disabled
if (logging_enabled && !enabled) {
flightnum++;
lognum = 0;
}
logging_enabled = enabled;
}
@ -103,30 +122,13 @@ void PIOS_DEBUGLOG_Enable(uint8_t enabled)
*/
void PIOS_DEBUGLOG_UAVObject(uint32_t objid, uint16_t instid, size_t size, uint8_t *data)
{
if (!logging_enabled || !buffer) {
if (!logging_enabled || !buffer || log_is_full) {
return;
}
mutexlock();
buffer->Flight = flightnum;
#if defined(PIOS_INCLUDE_FREERTOS)
buffer->FlightTime = xTaskGetTickCount() * portTICK_RATE_MS;
#else
buffer->FlightTime = 0;
#endif
buffer->Entry = lognum;
buffer->Type = DEBUGLOGENTRY_TYPE_UAVOBJECT;
buffer->ObjectID = objid;
buffer->InstanceID = instid;
if (size > sizeof(buffer->Data)) {
size = sizeof(buffer->Data);
}
buffer->Size = size;
memset(buffer->Data, 0xff, sizeof(buffer->Data));
memcpy(buffer->Data, data, size);
if (PIOS_FLASHFS_ObjSave(pios_user_fs_id, flightnum * 256, lognum, (uint8_t *)buffer, sizeof(DebugLogEntryData)) == 0) {
lognum++;
}
enqueue_data(objid, instid, size, data);
mutexunlock();
}
/**
@ -137,27 +139,30 @@ void PIOS_DEBUGLOG_UAVObject(uint32_t objid, uint16_t instid, size_t size, uint8
*/
void PIOS_DEBUGLOG_Printf(char *format, ...)
{
if (!logging_enabled || !buffer) {
if (!logging_enabled || !buffer || log_is_full) {
return;
}
va_list args;
va_start(args, format);
mutexlock();
// flush any pending buffer before writing debug text
if (used_buffer_space) {
write_current_buffer();
}
memset(buffer->Data, 0xff, sizeof(buffer->Data));
vsnprintf((char *)buffer->Data, sizeof(buffer->Data), (char *)format, args);
buffer->Flight = flightnum;
#if defined(PIOS_INCLUDE_FREERTOS)
buffer->FlightTime = xTaskGetTickCount() * portTICK_RATE_MS;
#else
buffer->FlightTime = 0;
#endif
buffer->FlightTime = PIOS_DELAY_GetuS();
buffer->Entry = lognum;
buffer->Type = DEBUGLOGENTRY_TYPE_TEXT;
buffer->ObjectID = 0;
buffer->InstanceID = 0;
buffer->Size = strlen((const char *)buffer->Data);
if (PIOS_FLASHFS_ObjSave(pios_user_fs_id, flightnum * 256, lognum, (uint8_t *)buffer, sizeof(DebugLogEntryData)) == 0) {
if (PIOS_FLASHFS_ObjSave(pios_user_fs_id, LOG_GET_FLIGHT_OBJID(flightnum), lognum, (uint8_t *)buffer, sizeof(DebugLogEntryData)) == 0) {
lognum++;
}
mutexunlock();
@ -179,7 +184,7 @@ void PIOS_DEBUGLOG_Printf(char *format, ...)
int32_t PIOS_DEBUGLOG_Read(void *mybuffer, uint16_t flight, uint16_t inst)
{
PIOS_Assert(mybuffer);
return PIOS_FLASHFS_ObjLoad(pios_user_fs_id, flight * 256, inst, (uint8_t *)mybuffer, sizeof(DebugLogEntryData));
return PIOS_FLASHFS_ObjLoad(pios_user_fs_id, LOG_GET_FLIGHT_OBJID(flight), inst, (uint8_t *)mybuffer, sizeof(DebugLogEntryData));
}
/**
@ -214,11 +219,68 @@ void PIOS_DEBUGLOG_Format(void)
{
mutexlock();
PIOS_FLASHFS_Format(pios_user_fs_id);
lognum = 0;
flightnum = 0;
lognum = 0;
flightnum = 0;
log_is_full = false;
fails_count = 0;
used_buffer_space = 0;
mutexunlock();
}
void enqueue_data(uint32_t objid, uint16_t instid, size_t size, uint8_t *data)
{
DebugLogEntryData *entry;
// start a new block
if (!used_buffer_space) {
entry = buffer;
memset(buffer->Data, 0xff, sizeof(buffer->Data));
used_buffer_space += size;
} else {
// if an instance is being filled and there is enough space, does enqueues new data.
if (used_buffer_space + size + LOG_ENTRY_HEADER_SIZE > LOG_ENTRY_MAX_DATA_SIZE) {
buffer->Type = DEBUGLOGENTRY_TYPE_MULTIPLEUAVOBJECTS;
if (!write_current_buffer()) {
return;
}
entry = buffer;
memset(buffer->Data, 0xff, sizeof(buffer->Data));
used_buffer_space += size;
} else {
entry = (DebugLogEntryData *)&buffer->Data[used_buffer_space];
used_buffer_space += size + LOG_ENTRY_HEADER_SIZE;
}
}
entry->Flight = flightnum;
entry->FlightTime = PIOS_DELAY_GetuS();
entry->Entry = lognum;
entry->Type = DEBUGLOGENTRY_TYPE_UAVOBJECT;
entry->ObjectID = objid;
entry->InstanceID = instid;
if (size > sizeof(buffer->Data)) {
size = sizeof(buffer->Data);
}
entry->Size = size;
memcpy(entry->Data, data, size);
}
bool write_current_buffer()
{
// not enough space, write the block and start a new one
if (PIOS_FLASHFS_ObjSave(pios_user_fs_id, LOG_GET_FLIGHT_OBJID(flightnum), lognum, (uint8_t *)buffer, sizeof(DebugLogEntryData)) == 0) {
lognum++;
fails_count = 0;
used_buffer_space = 0;
} else {
if (fails_count++ > MAX_CONSECUTIVE_FAILS_COUNT) {
log_is_full = true;
}
return false;
}
return true;
}
/**
* @}
* @}

58
flight/pios/common/pios_flash_jedec.c
View File

@ -75,13 +75,16 @@ struct jedec_flash_dev {
enum pios_jedec_dev_magic magic;
};
#define FLASH_FAST_PRESCALER PIOS_SPI_PRESCALER_2
#define FLASH_PRESCALER PIOS_SPI_PRESCALER_4
// ! Private functions
static int32_t PIOS_Flash_Jedec_Validate(struct jedec_flash_dev *flash_dev);
static struct jedec_flash_dev *PIOS_Flash_Jedec_alloc(void);
static int32_t PIOS_Flash_Jedec_ReadID(struct jedec_flash_dev *flash_dev);
static int32_t PIOS_Flash_Jedec_ReadStatus(struct jedec_flash_dev *flash_dev);
static int32_t PIOS_Flash_Jedec_ClaimBus(struct jedec_flash_dev *flash_dev);
static int32_t PIOS_Flash_Jedec_ClaimBus(struct jedec_flash_dev *flash_dev, bool fast);
static int32_t PIOS_Flash_Jedec_ReleaseBus(struct jedec_flash_dev *flash_dev);
static int32_t PIOS_Flash_Jedec_WriteEnable(struct jedec_flash_dev *flash_dev);
static int32_t PIOS_Flash_Jedec_Busy(struct jedec_flash_dev *flash_dev);
@ -166,12 +169,12 @@ int32_t PIOS_Flash_Jedec_Init(uintptr_t *flash_id, uint32_t spi_id, uint32_t sla
* @brief Claim the SPI bus for flash use and assert CS pin
* @return 0 for sucess, -1 for failure to get semaphore
*/
static int32_t PIOS_Flash_Jedec_ClaimBus(struct jedec_flash_dev *flash_dev)
static int32_t PIOS_Flash_Jedec_ClaimBus(struct jedec_flash_dev *flash_dev, bool fast)
{
if (PIOS_SPI_ClaimBus(flash_dev->spi_id) < 0) {
return -1;
}
PIOS_SPI_SetClockSpeed(flash_dev->spi_id, fast ? FLASH_FAST_PRESCALER : FLASH_PRESCALER);
PIOS_SPI_RC_PinSet(flash_dev->spi_id, flash_dev->slave_num, 0);
flash_dev->claimed = true;
@ -209,7 +212,7 @@ static int32_t PIOS_Flash_Jedec_Busy(struct jedec_flash_dev *flash_dev)
*/
static int32_t PIOS_Flash_Jedec_WriteEnable(struct jedec_flash_dev *flash_dev)
{
if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) {
if (PIOS_Flash_Jedec_ClaimBus(flash_dev, true) != 0) {
return -1;
}
@ -226,7 +229,7 @@ static int32_t PIOS_Flash_Jedec_WriteEnable(struct jedec_flash_dev *flash_dev)
*/
static int32_t PIOS_Flash_Jedec_ReadStatus(struct jedec_flash_dev *flash_dev)
{
if (PIOS_Flash_Jedec_ClaimBus(flash_dev) < 0) {
if (PIOS_Flash_Jedec_ClaimBus(flash_dev, true) < 0) {
return -1;
}
@ -247,7 +250,7 @@ static int32_t PIOS_Flash_Jedec_ReadStatus(struct jedec_flash_dev *flash_dev)
*/
static int32_t PIOS_Flash_Jedec_ReadID(struct jedec_flash_dev *flash_dev)
{
if (PIOS_Flash_Jedec_ClaimBus(flash_dev) < 0) {
if (PIOS_Flash_Jedec_ClaimBus(flash_dev, true) < 0) {
return -2;
}
@ -354,7 +357,7 @@ static int32_t PIOS_Flash_Jedec_EraseSector(uintptr_t flash_id, uint32_t addr)
return ret;
}
if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) {
if (PIOS_Flash_Jedec_ClaimBus(flash_dev, true) != 0) {
return -1;
}
@ -368,7 +371,7 @@ static int32_t PIOS_Flash_Jedec_EraseSector(uintptr_t flash_id, uint32_t addr)
// Keep polling when bus is busy too
while (PIOS_Flash_Jedec_Busy(flash_dev) != 0) {
#if defined(FLASH_FREERTOS)
vTaskDelay(1);
vTaskDelay(2);
#endif
}
@ -394,7 +397,7 @@ static int32_t PIOS_Flash_Jedec_EraseChip(uintptr_t flash_id)
return ret;
}
if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) {
if (PIOS_Flash_Jedec_ClaimBus(flash_dev, true) != 0) {
return -1;
}
@ -455,16 +458,14 @@ static int32_t PIOS_Flash_Jedec_WriteData(uintptr_t flash_id, uint32_t addr, uin
if (((addr & 0xff) + len) > 0x100) {
return -3;
}
if ((ret = PIOS_Flash_Jedec_WriteEnable(flash_dev)) != 0) {
return ret;
}
/* Execute write page command and clock in address. Keep CS asserted */
if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) {
if (PIOS_Flash_Jedec_ClaimBus(flash_dev, true) != 0) {
return -1;
}
if (PIOS_SPI_TransferBlock(flash_dev->spi_id, out, NULL, sizeof(out), NULL) < 0) {
PIOS_Flash_Jedec_ReleaseBus(flash_dev);
return -1;
@ -486,7 +487,7 @@ static int32_t PIOS_Flash_Jedec_WriteData(uintptr_t flash_id, uint32_t addr, uin
#else
// Query status this way to prevent accel chip locking us out
if (PIOS_Flash_Jedec_ClaimBus(flash_dev) < 0) {
if (PIOS_Flash_Jedec_ClaimBus(flash_dev, true) < 0) {
return -1;
}
@ -536,13 +537,12 @@ static int32_t PIOS_Flash_Jedec_WriteChunks(uintptr_t flash_id, uint32_t addr, s
if (((addr & 0xff) + len) > 0x100) {
return -3;
}
if ((ret = PIOS_Flash_Jedec_WriteEnable(flash_dev)) != 0) {
return ret;
}
/* Execute write page command and clock in address. Keep CS asserted */
if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) {
if (PIOS_Flash_Jedec_ClaimBus(flash_dev, true) != 0) {
return -1;
}
@ -582,17 +582,29 @@ static int32_t PIOS_Flash_Jedec_ReadData(uintptr_t flash_id, uint32_t addr, uint
if (PIOS_Flash_Jedec_Validate(flash_dev) != 0) {
return -1;
}
if (PIOS_Flash_Jedec_ClaimBus(flash_dev) == -1) {
bool fast_read = flash_dev->cfg->fast_read != 0;
if (PIOS_Flash_Jedec_ClaimBus(flash_dev, fast_read) == -1) {
return -1;
}
/* Execute read command and clock in address. Keep CS asserted */
uint8_t out[] = { JEDEC_READ_DATA, (addr >> 16) & 0xff, (addr >> 8) & 0xff, addr & 0xff };
if (PIOS_SPI_TransferBlock(flash_dev->spi_id, out, NULL, sizeof(out), NULL) < 0) {
PIOS_Flash_Jedec_ReleaseBus(flash_dev);
return -2;
if (!fast_read) {
uint8_t out[] = { JEDEC_READ_DATA, (addr >> 16) & 0xff, (addr >> 8) & 0xff, addr & 0xff };
if (PIOS_SPI_TransferBlock(flash_dev->spi_id, out, NULL, sizeof(out), NULL) < 0) {
PIOS_Flash_Jedec_ReleaseBus(flash_dev);
return -2;
}
} else {
uint8_t cmdlen = flash_dev->cfg->fast_read_dummy_bytes + 4;
uint8_t out[cmdlen];
memset(out, 0x0, cmdlen);
out[0] = flash_dev->cfg->fast_read;
out[1] = (addr >> 16) & 0xff;
out[2] = (addr >> 8) & 0xff;
out[3] = addr & 0xff;
if (PIOS_SPI_TransferBlock(flash_dev->spi_id, out, NULL, cmdlen, NULL) < 0) {
PIOS_Flash_Jedec_ReleaseBus(flash_dev);
return -2;
}
}
/* Copy the transfer data to the buffer */

425
flight/pios/common/pios_hmc5883.c
View File

@ -1,425 +0,0 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_HMC5883 HMC5883 Functions
* @brief Deals with the hardware interface to the magnetometers
* @{
* @file pios_hmc5883.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief HMC5883 Magnetic Sensor Functions from AHRS
* @see The GNU Public License (GPL) Version 3
*
******************************************************************************
*/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "pios.h"
#ifdef PIOS_INCLUDE_HMC5883
/* Global Variables */
/* Local Types */
/* Local Variables */
volatile bool pios_hmc5883_data_ready;
static int32_t PIOS_HMC5883_Config(const struct pios_hmc5883_cfg *cfg);
static int32_t PIOS_HMC5883_Read(uint8_t address, uint8_t *buffer, uint8_t len);
static int32_t PIOS_HMC5883_Write(uint8_t address, uint8_t buffer);
static const struct pios_hmc5883_cfg *dev_cfg;
/**
* @brief Initialize the HMC5883 magnetometer sensor.
* @return none
*/
void PIOS_HMC5883_Init(const struct pios_hmc5883_cfg *cfg)
{
dev_cfg = cfg; // store config before enabling interrupt
#ifdef PIOS_HMC5883_HAS_GPIOS
PIOS_EXTI_Init(cfg->exti_cfg);
#endif
int32_t val = PIOS_HMC5883_Config(cfg);
PIOS_Assert(val == 0);
pios_hmc5883_data_ready = false;
}
/**
* @brief Initialize the HMC5883 magnetometer sensor
* \return none
* \param[in] PIOS_HMC5883_ConfigTypeDef struct to be used to configure sensor.
*
* CTRL_REGA: Control Register A
* Read Write
* Default value: 0x10
* 7:5 0 These bits must be cleared for correct operation.
* 4:2 DO2-DO0: Data Output Rate Bits
* DO2 | DO1 | DO0 | Minimum Data Output Rate (Hz)
* ------------------------------------------------------
* 0 | 0 | 0 | 0.75
* 0 | 0 | 1 | 1.5
* 0 | 1 | 0 | 3
* 0 | 1 | 1 | 7.5
* 1 | 0 | 0 | 15 (default)
* 1 | 0 | 1 | 30
* 1 | 1 | 0 | 75
* 1 | 1 | 1 | Not Used
* 1:0 MS1-MS0: Measurement Configuration Bits
* MS1 | MS0 | MODE
* ------------------------------
* 0 | 0 | Normal
* 0 | 1 | Positive Bias
* 1 | 0 | Negative Bias
* 1 | 1 | Not Used
*
* CTRL_REGB: Control RegisterB
* Read Write
* Default value: 0x20
* 7:5 GN2-GN0: Gain Configuration Bits.
* GN2 | GN1 | GN0 | Mag Input | Gain | Output Range
* | | | Range[Ga] | [LSB/mGa] |
* ------------------------------------------------------
* 0 | 0 | 0 | ±0.88Ga | 1370 | 0xF800–0x07FF (-2048:2047)
* 0 | 0 | 1 | ±1.3Ga (def) | 1090 | 0xF800–0x07FF (-2048:2047)
* 0 | 1 | 0 | ±1.9Ga | 820 | 0xF800–0x07FF (-2048:2047)
* 0 | 1 | 1 | ±2.5Ga | 660 | 0xF800–0x07FF (-2048:2047)
* 1 | 0 | 0 | ±4.0Ga | 440 | 0xF800–0x07FF (-2048:2047)
* 1 | 0 | 1 | ±4.7Ga | 390 | 0xF800–0x07FF (-2048:2047)
* 1 | 1 | 0 | ±5.6Ga | 330 | 0xF800–0x07FF (-2048:2047)
* 1 | 1 | 1 | ±8.1Ga | 230 | 0xF800–0x07FF (-2048:2047)
* |Not recommended|
*
* 4:0 CRB4-CRB: 0 This bit must be cleared for correct operation.
*
* _MODE_REG: Mode Register
* Read Write
* Default value: 0x02
* 7:2 0 These bits must be cleared for correct operation.
* 1:0 MD1-MD0: Mode Select Bits
* MS1 | MS0 | MODE
* ------------------------------
* 0 | 0 | Continuous-Conversion Mode.
* 0 | 1 | Single-Conversion Mode
* 1 | 0 | Negative Bias
* 1 | 1 | Sleep Mode
*/
static uint8_t CTRLB = 0x00;
static int32_t PIOS_HMC5883_Config(const struct pios_hmc5883_cfg *cfg)
{
uint8_t CTRLA = 0x00;
uint8_t MODE = 0x00;
CTRLB = 0;
CTRLA |= (uint8_t)(cfg->M_ODR | cfg->Meas_Conf);
CTRLB |= (uint8_t)(cfg->Gain);
MODE |= (uint8_t)(cfg->Mode);
// CRTL_REGA
if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_A, CTRLA) != 0) {
return -1;
}
// CRTL_REGB
if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_B, CTRLB) != 0) {
return -1;
}
// Mode register
if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, MODE) != 0) {
return -1;
}
return 0;
}
/**
* @brief Read current X, Z, Y values (in that order)
* \param[out] int16_t array of size 3 to store X, Z, and Y magnetometer readings
* \return 0 for success or -1 for failure
*/
int32_t PIOS_HMC5883_ReadMag(int16_t out[3])
{
pios_hmc5883_data_ready = false;
uint8_t buffer[6];
int32_t temp;
int32_t sensitivity;
if (PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_XMSB_REG, buffer, 6) != 0) {
return -1;
}
switch (CTRLB & 0xE0) {
case 0x00:
sensitivity = PIOS_HMC5883_Sensitivity_0_88Ga;
break;
case 0x20:
sensitivity = PIOS_HMC5883_Sensitivity_1_3Ga;
break;
case 0x40:
sensitivity = PIOS_HMC5883_Sensitivity_1_9Ga;
break;
case 0x60:
sensitivity = PIOS_HMC5883_Sensitivity_2_5Ga;
break;
case 0x80:
sensitivity = PIOS_HMC5883_Sensitivity_4_0Ga;
break;
case 0xA0:
sensitivity = PIOS_HMC5883_Sensitivity_4_7Ga;
break;
case 0xC0:
sensitivity = PIOS_HMC5883_Sensitivity_5_6Ga;
break;
case 0xE0:
sensitivity = PIOS_HMC5883_Sensitivity_8_1Ga;
break;
default:
PIOS_Assert(0);
}
for (int i = 0; i < 3; i++) {
temp = ((int16_t)((uint16_t)buffer[2 * i] << 8)
+ buffer[2 * i + 1]) * 1000 / sensitivity;
out[i] = temp;
}
// Data reads out as X,Z,Y
temp = out[2];
out[2] = out[1];
out[1] = temp;
// This should not be necessary but for some reason it is coming out of continuous conversion mode
PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, PIOS_HMC5883_MODE_CONTINUOUS);
return 0;
}
/**
* @brief Read the identification bytes from the HMC5883 sensor
* \param[out] uint8_t array of size 4 to store HMC5883 ID.
* \return 0 if successful, -1 if not
*/
uint8_t PIOS_HMC5883_ReadID(uint8_t out[4])
{
uint8_t retval = PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_IDA_REG, out, 3);
out[3] = '\0';
return retval;
}
/**
* @brief Tells whether new magnetometer readings are available
* \return true if new data is available
* \return false if new data is not available
*/
bool PIOS_HMC5883_NewDataAvailable(void)
{
return pios_hmc5883_data_ready;
}
/**
* @brief Reads one or more bytes into a buffer
* \param[in] address HMC5883 register address (depends on size)
* \param[out] buffer destination buffer
* \param[in] len number of bytes which should be read
* \return 0 if operation was successful
* \return -1 if error during I2C transfer
* \return -2 if unable to claim i2c device
*/
static int32_t PIOS_HMC5883_Read(uint8_t address, uint8_t *buffer, uint8_t len)
{
uint8_t addr_buffer[] = {
address,
};
const struct pios_i2c_txn txn_list[] = {
{
.info = __func__,
.addr = PIOS_HMC5883_I2C_ADDR,
.rw = PIOS_I2C_TXN_WRITE,
.len = sizeof(addr_buffer),
.buf = addr_buffer,
}
,
{
.info = __func__,
.addr = PIOS_HMC5883_I2C_ADDR,
.rw = PIOS_I2C_TXN_READ,
.len = len,
.buf = buffer,
}
};
return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list));
}
/**
* @brief Writes one or more bytes to the HMC5883
* \param[in] address Register address
* \param[in] buffer source buffer
* \return 0 if operation was successful
* \return -1 if error during I2C transfer
* \return -2 if unable to claim i2c device
*/
static int32_t PIOS_HMC5883_Write(uint8_t address, uint8_t buffer)
{
uint8_t data[] = {
address,
buffer,
};
const struct pios_i2c_txn txn_list[] = {
{
.info = __func__,
.addr = PIOS_HMC5883_I2C_ADDR,
.rw = PIOS_I2C_TXN_WRITE,
.len = sizeof(data),
.buf = data,
}
,
};
;
return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list));
}
/**
* @brief Run self-test operation. Do not call this during operational use!!
* \return 0 if success, -1 if test failed
*/
int32_t PIOS_HMC5883_Test(void)
{
int32_t failed = 0;
uint8_t registers[3] = { 0, 0, 0 };
uint8_t status;
uint8_t ctrl_a_read;
uint8_t ctrl_b_read;
uint8_t mode_read;
int16_t values[3];
/* Verify that ID matches (HMC5883 ID is null-terminated ASCII string "H43") */
char id[4];
PIOS_HMC5883_ReadID((uint8_t *)id);
if ((id[0] != 'H') || (id[1] != '4') || (id[2] != '3')) { // Expect H43
return -1;
}
/* Backup existing configuration */
if (PIOS_HMC5883_Read(PIOS_HMC5883_CONFIG_REG_A, registers, 3) != 0) {
return -1;
}
/* Stop the device and read out last value */
PIOS_DELAY_WaitmS(10);
if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, PIOS_HMC5883_MODE_IDLE) != 0) {
return -1;
}
if (PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_STATUS_REG, &status, 1) != 0) {
return -1;
}
if (PIOS_HMC5883_ReadMag(values) != 0) {
return -1;
}
/*
* Put HMC5883 into self test mode
* This is done by placing measurement config into positive (0x01) or negative (0x10) bias
* and then placing the mode register into single-measurement mode. This causes the HMC5883
* to create an artificial magnetic field of ~1.1 Gauss.
*
* If gain were PIOS_HMC5883_GAIN_2_5, for example, X and Y will read around +766 LSB
* (1.16 Ga * 660 LSB/Ga) and Z would read around +713 LSB (1.08 Ga * 660 LSB/Ga)
*
* Changing measurement config back to PIOS_HMC5883_MEASCONF_NORMAL will leave self-test mode.
*/
PIOS_DELAY_WaitmS(10);
if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_A, PIOS_HMC5883_MEASCONF_BIAS_POS | PIOS_HMC5883_ODR_15) != 0) {
return -1;
}
PIOS_DELAY_WaitmS(10);
if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_B, PIOS_HMC5883_GAIN_8_1) != 0) {
return -1;
}
PIOS_DELAY_WaitmS(10);
if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, PIOS_HMC5883_MODE_SINGLE) != 0) {
return -1;
}
/* Must wait for value to be updated */
PIOS_DELAY_WaitmS(200);
if (PIOS_HMC5883_ReadMag(values) != 0) {
return -1;
}
/*
if(abs(values[0] - 766) > 20)
failed |= 1;
if(abs(values[1] - 766) > 20)
failed |= 1;
if(abs(values[2] - 713) > 20)
failed |= 1;
*/
PIOS_HMC5883_Read(PIOS_HMC5883_CONFIG_REG_A, &ctrl_a_read, 1);
PIOS_HMC5883_Read(PIOS_HMC5883_CONFIG_REG_B, &ctrl_b_read, 1);
PIOS_HMC5883_Read(PIOS_HMC5883_MODE_REG, &mode_read, 1);
PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_STATUS_REG, &status, 1);
/* Restore backup configuration */
PIOS_DELAY_WaitmS(10);
if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_A, registers[0]) != 0) {
return -1;
}
PIOS_DELAY_WaitmS(10);
if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_B, registers[1]) != 0) {
return -1;
}
PIOS_DELAY_WaitmS(10);
if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, registers[2]) != 0) {
return -1;
}
return failed;
}
/**
* @brief IRQ Handler
*/
bool PIOS_HMC5883_IRQHandler(void)
{
pios_hmc5883_data_ready = true;
return false;
}
#endif /* PIOS_INCLUDE_HMC5883 */
/**
* @}
* @}
*/

557
flight/pios/common/pios_hmc5x83.c Normal file
View File

@ -0,0 +1,557 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_HMC5x83 HMC5x83 Functions
* @brief Deals with the hardware interface to the magnetometers
* @{
* @file pios_hmc5x83.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief HMC5x83 Magnetic Sensor Functions from AHRS
* @see The GNU Public License (GPL) Version 3
*
******************************************************************************
*/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "pios.h"
#include <pios_hmc5x83.h>
#include <pios_mem.h>
#ifdef PIOS_INCLUDE_HMC5X83
#define PIOS_HMC5X83_MAGIC 0x4d783833
/* Global Variables */
/* Local Types */
typedef struct {
uint32_t magic;
const struct pios_hmc5x83_cfg *cfg;
uint32_t port_id;
uint8_t slave_num;
uint8_t CTRLB;
volatile bool data_ready;
} pios_hmc5x83_dev_data_t;
static int32_t PIOS_HMC5x83_Config(pios_hmc5x83_dev_data_t *dev);
/**
* Allocate the device setting structure
* @return pios_hmc5x83_dev_data_t pointer to newly created structure
*/
pios_hmc5x83_dev_data_t *dev_alloc()
{
pios_hmc5x83_dev_data_t *dev = (pios_hmc5x83_dev_data_t *)pios_malloc(sizeof(pios_hmc5x83_dev_data_t));
PIOS_DEBUG_Assert(dev);
memset(dev, 0x00, sizeof(pios_hmc5x83_dev_data_t));
dev->magic = PIOS_HMC5X83_MAGIC;
return dev;
}
/**
* Validate a pios_hmc5x83_dev_t handler and return the related pios_hmc5x83_dev_data_t pointer
* @param dev device handler
* @return the device data structure
*/
pios_hmc5x83_dev_data_t *dev_validate(pios_hmc5x83_dev_t dev)
{
pios_hmc5x83_dev_data_t *dev_data = (pios_hmc5x83_dev_data_t *)dev;
PIOS_DEBUG_Assert(dev_data->magic == PIOS_HMC5X83_MAGIC);
return dev_data;
}
/**
* @brief Initialize the HMC5x83 magnetometer sensor.
* @return none
*/
pios_hmc5x83_dev_t PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_t port_id, uint8_t slave_num)
{
pios_hmc5x83_dev_data_t *dev = dev_alloc();
dev->cfg = cfg; // store config before enabling interrupt
dev->port_id = port_id;
dev->slave_num = slave_num;
#ifdef PIOS_HMC5X83_HAS_GPIOS
PIOS_EXTI_Init(cfg->exti_cfg);
#endif
int32_t val = PIOS_HMC5x83_Config(dev);
PIOS_Assert(val == 0);
dev->data_ready = false;
return (pios_hmc5x83_dev_t)dev;
}
/**
* @brief Initialize the HMC5x83 magnetometer sensor
* \return none
* \param[in] pios_hmc5x83_dev_data_t device config to be used.
* \param[in] PIOS_HMC5x83_ConfigTypeDef struct to be used to configure sensor.
*
* CTRL_REGA: Control Register A
* Read Write
* Default value: 0x10
* 7:5 0 These bits must be cleared for correct operation.
* 4:2 DO2-DO0: Data Output Rate Bits
* DO2 | DO1 | DO0 | Minimum Data Output Rate (Hz)
* ------------------------------------------------------
* 0 | 0 | 0 | 0.75
* 0 | 0 | 1 | 1.5
* 0 | 1 | 0 | 3
* 0 | 1 | 1 | 7.5
* 1 | 0 | 0 | 15 (default)
* 1 | 0 | 1 | 30
* 1 | 1 | 0 | 75
* 1 | 1 | 1 | Not Used
* 1:0 MS1-MS0: Measurement Configuration Bits
* MS1 | MS0 | MODE
* ------------------------------
* 0 | 0 | Normal
* 0 | 1 | Positive Bias
* 1 | 0 | Negative Bias
* 1 | 1 | Not Used
*
* CTRL_REGB: Control RegisterB
* Read Write
* Default value: 0x20
* 7:5 GN2-GN0: Gain Configuration Bits.
* GN2 | GN1 | GN0 | Mag Input | Gain | Output Range
* | | | Range[Ga] | [LSB/mGa] |
* ------------------------------------------------------
* 0 | 0 | 0 | ±0.88Ga | 1370 | 0xF800–0x07FF (-2048:2047)
* 0 | 0 | 1 | ±1.3Ga (def) | 1090 | 0xF800–0x07FF (-2048:2047)
* 0 | 1 | 0 | ±1.9Ga | 820 | 0xF800–0x07FF (-2048:2047)
* 0 | 1 | 1 | ±2.5Ga | 660 | 0xF800–0x07FF (-2048:2047)
* 1 | 0 | 0 | ±4.0Ga | 440 | 0xF800–0x07FF (-2048:2047)
* 1 | 0 | 1 | ±4.7Ga | 390 | 0xF800–0x07FF (-2048:2047)
* 1 | 1 | 0 | ±5.6Ga | 330 | 0xF800–0x07FF (-2048:2047)
* 1 | 1 | 1 | ±8.1Ga | 230 | 0xF800–0x07FF (-2048:2047)
* |Not recommended|
*
* 4:0 CRB4-CRB: 0 This bit must be cleared for correct operation.
*
* _MODE_REG: Mode Register
* Read Write
* Default value: 0x02
* 7:2 0 These bits must be cleared for correct operation.
* 1:0 MD1-MD0: Mode Select Bits
* MS1 | MS0 | MODE
* ------------------------------
* 0 | 0 | Continuous-Conversion Mode.
* 0 | 1 | Single-Conversion Mode
* 1 | 0 | Negative Bias
* 1 | 1 | Sleep Mode
*/
static int32_t PIOS_HMC5x83_Config(pios_hmc5x83_dev_data_t *dev)
{
uint8_t CTRLA = 0x00;
uint8_t MODE = 0x00;
const struct pios_hmc5x83_cfg *cfg = dev->cfg;
dev->CTRLB = 0;
CTRLA |= (uint8_t)(cfg->M_ODR | cfg->Meas_Conf);
CTRLA |= cfg->TempCompensation ? PIOS_HMC5x83_CTRLA_TEMP : 0;
dev->CTRLB |= (uint8_t)(cfg->Gain);
MODE |= (uint8_t)(cfg->Mode);
// CRTL_REGA
if (cfg->Driver->Write((pios_hmc5x83_dev_t)dev, PIOS_HMC5x83_CONFIG_REG_A, CTRLA) != 0) {
return -1;
}
// CRTL_REGB
if (cfg->Driver->Write((pios_hmc5x83_dev_t)dev, PIOS_HMC5x83_CONFIG_REG_B, dev->CTRLB) != 0) {
return -1;
}
// Mode register
if (cfg->Driver->Write((pios_hmc5x83_dev_t)dev, PIOS_HMC5x83_MODE_REG, MODE) != 0) {
return -1;
}
return 0;
}
/**
* @brief Read current X, Z, Y values (in that order)
* \param[in] dev device handler
* \param[out] int16_t array of size 3 to store X, Z, and Y magnetometer readings
* \return 0 for success or -1 for failure
*/
int32_t PIOS_HMC5x83_ReadMag(pios_hmc5x83_dev_t handler, int16_t out[3])
{
pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
dev->data_ready = false;
uint8_t buffer[6];
int32_t temp;
int32_t sensitivity;
if (dev->cfg->Driver->Read(handler, PIOS_HMC5x83_DATAOUT_XMSB_REG, buffer, 6) != 0) {
return -1;
}
switch (dev->CTRLB & 0xE0) {
case 0x00:
sensitivity = PIOS_HMC5x83_Sensitivity_0_88Ga;
break;
case 0x20:
sensitivity = PIOS_HMC5x83_Sensitivity_1_3Ga;
break;
case 0x40:
sensitivity = PIOS_HMC5x83_Sensitivity_1_9Ga;
break;
case 0x60:
sensitivity = PIOS_HMC5x83_Sensitivity_2_5Ga;
break;
case 0x80:
sensitivity = PIOS_HMC5x83_Sensitivity_4_0Ga;
break;
case 0xA0:
sensitivity = PIOS_HMC5x83_Sensitivity_4_7Ga;
break;
case 0xC0:
sensitivity = PIOS_HMC5x83_Sensitivity_5_6Ga;
break;
case 0xE0:
sensitivity = PIOS_HMC5x83_Sensitivity_8_1Ga;
break;
default:
PIOS_Assert(0);
}
for (int i = 0; i < 3; i++) {
temp = ((int16_t)((uint16_t)buffer[2 * i] << 8)
+ buffer[2 * i + 1]) * 1000 / sensitivity;
out[i] = temp;
}
// Data reads out as X,Z,Y
temp = out[2];
out[2] = out[1];
out[1] = temp;
// This should not be necessary but for some reason it is coming out of continuous conversion mode
dev->cfg->Driver->Write(handler, PIOS_HMC5x83_MODE_REG, PIOS_HMC5x83_MODE_CONTINUOUS);
return 0;
}
/**
* @brief Read the identification bytes from the HMC5x83 sensor
* \param[out] uint8_t array of size 4 to store HMC5x83 ID.
* \return 0 if successful, -1 if not
*/
uint8_t PIOS_HMC5x83_ReadID(pios_hmc5x83_dev_t handler, uint8_t out[4])
{
pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
uint8_t retval = dev->cfg->Driver->Read(handler, PIOS_HMC5x83_DATAOUT_IDA_REG, out, 3);
out[3] = '\0';
return retval;
}
/**
* @brief Tells whether new magnetometer readings are available
* \return true if new data is available
* \return false if new data is not available
*/
bool PIOS_HMC5x83_NewDataAvailable(pios_hmc5x83_dev_t handler)
{
pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
return dev->data_ready;
}
/**
* @brief Run self-test operation. Do not call this during operational use!!
* \return 0 if success, -1 if test failed
*/
int32_t PIOS_HMC5x83_Test(pios_hmc5x83_dev_t handler)
{
int32_t failed = 0;
uint8_t registers[3] = { 0, 0, 0 };
uint8_t status;
uint8_t ctrl_a_read;
uint8_t ctrl_b_read;
uint8_t mode_read;
int16_t values[3];
pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
/* Verify that ID matches (HMC5x83 ID is null-terminated ASCII string "H43") */
char id[4];
PIOS_HMC5x83_ReadID(handler, (uint8_t *)id);
if ((id[0] != 'H') || (id[1] != '4') || (id[2] != '3')) { // Expect H43
return -1;
}
/* Backup existing configuration */
if (dev->cfg->Driver->Read(handler, PIOS_HMC5x83_CONFIG_REG_A, registers, 3) != 0) {
return -1;
}
/* Stop the device and read out last value */
PIOS_DELAY_WaitmS(10);
if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_MODE_REG, PIOS_HMC5x83_MODE_IDLE) != 0) {
return -1;
}
if (dev->cfg->Driver->Read(handler, PIOS_HMC5x83_DATAOUT_STATUS_REG, &status, 1) != 0) {
return -1;
}
if (PIOS_HMC5x83_ReadMag(handler, values) != 0) {
return -1;
}
/*
* Put HMC5x83 into self test mode
* This is done by placing measurement config into positive (0x01) or negative (0x10) bias
* and then placing the mode register into single-measurement mode. This causes the HMC5x83
* to create an artificial magnetic field of ~1.1 Gauss.
*
* If gain were PIOS_HMC5x83_GAIN_2_5, for example, X and Y will read around +766 LSB
* (1.16 Ga * 660 LSB/Ga) and Z would read around +713 LSB (1.08 Ga * 660 LSB/Ga)
*
* Changing measurement config back to PIOS_HMC5x83_MEASCONF_NORMAL will leave self-test mode.
*/
PIOS_DELAY_WaitmS(10);
if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_CONFIG_REG_A, PIOS_HMC5x83_MEASCONF_BIAS_POS | PIOS_HMC5x83_ODR_15) != 0) {
return -1;
}
PIOS_DELAY_WaitmS(10);
if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_CONFIG_REG_B, PIOS_HMC5x83_GAIN_8_1) != 0) {
return -1;
}
PIOS_DELAY_WaitmS(10);
if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_MODE_REG, PIOS_HMC5x83_MODE_SINGLE) != 0) {
return -1;
}
/* Must wait for value to be updated */
PIOS_DELAY_WaitmS(200);
if (PIOS_HMC5x83_ReadMag(handler, values) != 0) {
return -1;
}
dev->cfg->Driver->Read(handler, PIOS_HMC5x83_CONFIG_REG_A, &ctrl_a_read, 1);
dev->cfg->Driver->Read(handler, PIOS_HMC5x83_CONFIG_REG_B, &ctrl_b_read, 1);
dev->cfg->Driver->Read(handler, PIOS_HMC5x83_MODE_REG, &mode_read, 1);
dev->cfg->Driver->Read(handler, PIOS_HMC5x83_DATAOUT_STATUS_REG, &status, 1);
/* Restore backup configuration */
PIOS_DELAY_WaitmS(10);
if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_CONFIG_REG_A, registers[0]) != 0) {
return -1;
}
PIOS_DELAY_WaitmS(10);
if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_CONFIG_REG_B, registers[1]) != 0) {
return -1;
}
PIOS_DELAY_WaitmS(10);
if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_MODE_REG, registers[2]) != 0) {
return -1;
}
return failed;
}
/**
* @brief IRQ Handler
*/
bool PIOS_HMC5x83_IRQHandler(pios_hmc5x83_dev_t handler)
{
pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
dev->data_ready = true;
return false;
}
#ifdef PIOS_INCLUDE_SPI
int32_t PIOS_HMC5x83_SPI_Read(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t *buffer, uint8_t len);
int32_t PIOS_HMC5x83_SPI_Write(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t buffer);
const struct pios_hmc5x83_io_driver PIOS_HMC5x83_SPI_DRIVER = {
.Read = PIOS_HMC5x83_SPI_Read,
.Write = PIOS_HMC5x83_SPI_Write,
};
static int32_t pios_hmc5x83_spi_claim_bus(pios_hmc5x83_dev_data_t *dev)
{
if (PIOS_SPI_ClaimBus(dev->port_id) < 0) {
return -1;
}
PIOS_SPI_RC_PinSet(dev->port_id, dev->slave_num, 0);
return 0;
}
static void pios_hmc5x83_spi_release_bus(pios_hmc5x83_dev_data_t *dev)
{
PIOS_SPI_RC_PinSet(dev->port_id, dev->slave_num, 1);
PIOS_SPI_ReleaseBus(dev->port_id);
}
/**
* @brief Reads one or more bytes into a buffer
* \param[in] address HMC5x83 register address (depends on size)
* \param[out] buffer destination buffer
* \param[in] len number of bytes which should be read
* \return 0 if operation was successful
* \return -1 if error during I2C transfer
* \return -2 if unable to claim i2c device
*/
int32_t PIOS_HMC5x83_SPI_Read(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t *buffer, uint8_t len)
{
pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
if (pios_hmc5x83_spi_claim_bus(dev) < 0) {
return -1;
}
memset(buffer, 0xA5, len);
PIOS_SPI_TransferByte(dev->port_id, address | PIOS_HMC5x83_SPI_AUTOINCR_FLAG | PIOS_HMC5x83_SPI_READ_FLAG);
// buffer[0] = address | PIOS_HMC5x83_SPI_AUTOINCR_FLAG | PIOS_HMC5x83_SPI_READ_FLAG;
/* Copy the transfer data to the buffer */
if (PIOS_SPI_TransferBlock(dev->port_id, NULL, buffer, len, NULL) < 0) {
pios_hmc5x83_spi_release_bus(dev);
return -3;
}
pios_hmc5x83_spi_release_bus(dev);
return 0;
}
/**
* @brief Writes one or more bytes to the HMC5x83
* \param[in] address Register address
* \param[in] buffer source buffer
* \return 0 if operation was successful
* \return -1 if error during I2C transfer
* \return -2 if unable to claim spi device
*/
int32_t PIOS_HMC5x83_SPI_Write(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t buffer)
{
pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
if (pios_hmc5x83_spi_claim_bus(dev) < 0) {
return -1;
}
uint8_t data[] = {
address | PIOS_HMC5x83_SPI_AUTOINCR_FLAG,
buffer,
};
if (PIOS_SPI_TransferBlock(dev->port_id, data, NULL, sizeof(data), NULL) < 0) {
pios_hmc5x83_spi_release_bus(dev);
return -2;
}
pios_hmc5x83_spi_release_bus(dev);
return 0;
}
#endif /* PIOS_INCLUDE_SPI */
#ifdef PIOS_INCLUDE_I2C
int32_t PIOS_HMC5x83_I2C_Read(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t *buffer, uint8_t len);
int32_t PIOS_HMC5x83_I2C_Write(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t buffer);
const struct pios_hmc5x83_io_driver PIOS_HMC5x83_I2C_DRIVER = {
.Read = PIOS_HMC5x83_I2C_Read,
.Write = PIOS_HMC5x83_I2C_Write,
};
/**
* @brief Reads one or more bytes into a buffer
* \param[in] address HMC5x83 register address (depends on size)
* \param[out] buffer destination buffer
* \param[in] len number of bytes which should be read
* \return 0 if operation was successful
* \return -1 if error during I2C transfer
* \return -2 if unable to claim i2c device
*/
int32_t PIOS_HMC5x83_I2C_Read(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t *buffer, uint8_t len)
{
pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
uint8_t addr_buffer[] = {
address,
};
const struct pios_i2c_txn txn_list[] = {
{
.info = __func__,
.addr = PIOS_HMC5x83_I2C_ADDR,
.rw = PIOS_I2C_TXN_WRITE,
.len = sizeof(addr_buffer),
.buf = addr_buffer,
}
,
{
.info = __func__,
.addr = PIOS_HMC5x83_I2C_ADDR,
.rw = PIOS_I2C_TXN_READ,
.len = len,
.buf = buffer,
}
};
return PIOS_I2C_Transfer(dev->port_id, txn_list, NELEMENTS(txn_list));
}
/**
* @brief Writes one or more bytes to the HMC5x83
* \param[in] address Register address
* \param[in] buffer source buffer
* \return 0 if operation was successful
* \return -1 if error during I2C transfer
* \return -2 if unable to claim i2c device
*/
int32_t PIOS_HMC5x83_I2C_Write(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t buffer)
{
pios_hmc5x83_dev_data_t *dev = dev_validate(handler);
uint8_t data[] = {
address,
buffer,
};
const struct pios_i2c_txn txn_list[] = {
{
.info = __func__,
.addr = PIOS_HMC5x83_I2C_ADDR,
.rw = PIOS_I2C_TXN_WRITE,
.len = sizeof(data),
.buf = data,
}
,
};
;
return PIOS_I2C_Transfer(dev->port_id, txn_list, NELEMENTS(txn_list));
}
#endif /* PIOS_INCLUDE_I2C */
#endif /* PIOS_INCLUDE_HMC5x83 */
/**
* @}
* @}
*/

53
flight/pios/inc/pios_flash_jedec_catalog.h
View File

@ -40,30 +40,65 @@ const struct pios_flash_jedec_cfg pios_flash_jedec_catalog[] =
.expect_manufacturer = JEDEC_MANUFACTURER_ST,
.expect_memorytype = 0x20,
.expect_capacity = 0x15,
.sector_erase = 0xD8,
.chip_erase = 0xC7,
.sector_erase = 0xD8,
.chip_erase = 0xC7,
.fast_read = 0x0B,
.fast_read_dummy_bytes = 1,
},
{ // m25px16
.expect_manufacturer = JEDEC_MANUFACTURER_ST,
.expect_memorytype = 0x71,
.expect_capacity = 0x15,
.sector_erase = 0xD8,
.chip_erase = 0xC7,
.sector_erase = 0xD8,
.chip_erase = 0xC7,
.fast_read = 0x0B,
.fast_read_dummy_bytes = 1,
},
{ // w25x
.expect_manufacturer = JEDEC_MANUFACTURER_WINBOND,
.expect_memorytype = 0x30,
.expect_capacity = 0x13,
.sector_erase = 0x20,
.chip_erase = 0x60
.sector_erase = 0x20,
.chip_erase = 0x60,
.fast_read = 0x0B,
.fast_read_dummy_bytes = 1,
},
{ // 25q16
.expect_manufacturer = JEDEC_MANUFACTURER_WINBOND,
.expect_memorytype = 0x40,
.expect_capacity = 0x15,
.sector_erase = 0x20,
.chip_erase = 0x60
}
.sector_erase = 0x20,
.chip_erase = 0x60,
.fast_read = 0x0B,
.fast_read_dummy_bytes = 1,
},
{ // 25q512
.expect_manufacturer = JEDEC_MANUFACTURER_MICRON,
.expect_memorytype = 0xBA,
.expect_capacity = 0x20,
.sector_erase = 0xD8,
.chip_erase = 0xC7,
.fast_read = 0x0B,
.fast_read_dummy_bytes = 1,
},
{ // 25q256
.expect_manufacturer = JEDEC_MANUFACTURER_NUMORIX,
.expect_memorytype = 0xBA,
.expect_capacity = 0x19,
.sector_erase = 0xD8,
.chip_erase = 0xC7,
.fast_read = 0x0B,
.fast_read_dummy_bytes = 1,
},
{ // 25q128
.expect_manufacturer = JEDEC_MANUFACTURER_MICRON,
.expect_memorytype = 0xBA,
.expect_capacity = 0x18,
.sector_erase = 0xD8,
.chip_erase = 0xC7,
.fast_read = 0x0B,
.fast_read_dummy_bytes = 1,
},
};
const uint32_t pios_flash_jedec_catalog_size = NELEMENTS(pios_flash_jedec_catalog);

14
flight/pios/inc/pios_flash_jedec_priv.h
View File

@ -36,15 +36,19 @@
extern const struct pios_flash_driver pios_jedec_flash_driver;
#define JEDEC_MANUFACTURER_ST 0x20
#define JEDEC_MANUFACTURER_MICRON 0x20
#define JEDEC_MANUFACTURER_NUMORIX 0x20
#define JEDEC_MANUFACTURER_MACRONIX 0xC2
#define JEDEC_MANUFACTURER_WINBOND 0xEF
struct pios_flash_jedec_cfg {
uint8_t expect_manufacturer;
uint8_t expect_memorytype;
uint8_t expect_capacity;
uint32_t sector_erase;
uint32_t chip_erase;
uint8_t expect_manufacturer;
uint8_t expect_memorytype;
uint8_t expect_capacity;
uint8_t sector_erase;
uint8_t chip_erase;
uint8_t fast_read;
uint8_t fast_read_dummy_bytes;
};
int32_t PIOS_Flash_Jedec_Init(uintptr_t *flash_id, uint32_t spi_id, uint32_t slave_num);

116
flight/pios/inc/pios_hmc5883.h
View File

@ -1,116 +0,0 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_HMC5883 HMC5883 Functions
* @brief Deals with the hardware interface to the magnetometers
* @{
*
* @file pios_hmc5883.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief HMC5883 functions header.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_HMC5883_H
#define PIOS_HMC5883_H
/* HMC5883 Addresses */
#define PIOS_HMC5883_I2C_ADDR 0x1E
#define PIOS_HMC5883_I2C_READ_ADDR 0x3D
#define PIOS_HMC5883_I2C_WRITE_ADDR 0x3C
#define PIOS_HMC5883_CONFIG_REG_A (uint8_t)0x00
#define PIOS_HMC5883_CONFIG_REG_B (uint8_t)0x01
#define PIOS_HMC5883_MODE_REG (uint8_t)0x02
#define PIOS_HMC5883_DATAOUT_XMSB_REG 0x03
#define PIOS_HMC5883_DATAOUT_XLSB_REG 0x04
#define PIOS_HMC5883_DATAOUT_ZMSB_REG 0x05
#define PIOS_HMC5883_DATAOUT_ZLSB_REG 0x06
#define PIOS_HMC5883_DATAOUT_YMSB_REG 0x07
#define PIOS_HMC5883_DATAOUT_YLSB_REG 0x08
#define PIOS_HMC5883_DATAOUT_STATUS_REG 0x09
#define PIOS_HMC5883_DATAOUT_IDA_REG 0x0A
#define PIOS_HMC5883_DATAOUT_IDB_REG 0x0B
#define PIOS_HMC5883_DATAOUT_IDC_REG 0x0C
/* Output Data Rate */
#define PIOS_HMC5883_ODR_0_75 0x00
#define PIOS_HMC5883_ODR_1_5 0x04
#define PIOS_HMC5883_ODR_3 0x08
#define PIOS_HMC5883_ODR_7_5 0x0C
#define PIOS_HMC5883_ODR_15 0x10
#define PIOS_HMC5883_ODR_30 0x14
#define PIOS_HMC5883_ODR_75 0x18
/* Measure configuration */
#define PIOS_HMC5883_MEASCONF_NORMAL 0x00
#define PIOS_HMC5883_MEASCONF_BIAS_POS 0x01
#define PIOS_HMC5883_MEASCONF_BIAS_NEG 0x02
/* Gain settings */
#define PIOS_HMC5883_GAIN_0_88 0x00
#define PIOS_HMC5883_GAIN_1_3 0x20
#define PIOS_HMC5883_GAIN_1_9 0x40
#define PIOS_HMC5883_GAIN_2_5 0x60
#define PIOS_HMC5883_GAIN_4_0 0x80
#define PIOS_HMC5883_GAIN_4_7 0xA0
#define PIOS_HMC5883_GAIN_5_6 0xC0
#define PIOS_HMC5883_GAIN_8_1 0xE0
/* Modes */
#define PIOS_HMC5883_MODE_CONTINUOUS 0x00
#define PIOS_HMC5883_MODE_SINGLE 0x01
#define PIOS_HMC5883_MODE_IDLE 0x02
#define PIOS_HMC5883_MODE_SLEEP 0x03
/* Sensitivity Conversion Values */
#define PIOS_HMC5883_Sensitivity_0_88Ga 1370 // LSB/Ga
#define PIOS_HMC5883_Sensitivity_1_3Ga 1090 // LSB/Ga
#define PIOS_HMC5883_Sensitivity_1_9Ga 820 // LSB/Ga
#define PIOS_HMC5883_Sensitivity_2_5Ga 660 // LSB/Ga
#define PIOS_HMC5883_Sensitivity_4_0Ga 440 // LSB/Ga
#define PIOS_HMC5883_Sensitivity_4_7Ga 390 // LSB/Ga
#define PIOS_HMC5883_Sensitivity_5_6Ga 330 // LSB/Ga
#define PIOS_HMC5883_Sensitivity_8_1Ga 230 // LSB/Ga --> NOT RECOMMENDED
struct pios_hmc5883_cfg {
#ifdef PIOS_HMC5883_HAS_GPIOS
const struct pios_exti_cfg *exti_cfg; /* Pointer to the EXTI configuration */
#endif
uint8_t M_ODR; /* OUTPUT DATA RATE --> here below the relative define (See datasheet page 11 for more details) */
uint8_t Meas_Conf; /* Measurement Configuration,: Normal, positive bias, or negative bias --> here below the relative define */
uint8_t Gain; /* Gain Configuration, select the full scale --> here below the relative define (See datasheet page 11 for more details) */
uint8_t Mode;
};
/* Public Functions */
extern void PIOS_HMC5883_Init(const struct pios_hmc5883_cfg *cfg);
extern bool PIOS_HMC5883_NewDataAvailable(void);
extern int32_t PIOS_HMC5883_ReadMag(int16_t out[3]);
extern uint8_t PIOS_HMC5883_ReadID(uint8_t out[4]);
extern int32_t PIOS_HMC5883_Test(void);
extern bool PIOS_HMC5883_IRQHandler();
#endif /* PIOS_HMC5883_H */
/**
* @}
* @}
*/

137
flight/pios/inc/pios_hmc5x83.h Normal file
View File

@ -0,0 +1,137 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_HMC5x83 HMC5x83 Functions
* @brief Deals with the hardware interface to the magnetometers
* @{
*
* @file pios_hmc5x83.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief HMC5x83 functions header.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_HMC5x83_H
#define PIOS_HMC5x83_H
#include <stdint.h>
/* HMC5x83 Addresses */
#define PIOS_HMC5x83_I2C_ADDR 0x1E
#define PIOS_HMC5x83_I2C_READ_ADDR 0x3D
#define PIOS_HMC5x83_I2C_WRITE_ADDR 0x3C
#define PIOS_HMC5x83_SPI_READ_FLAG 0x80
#define PIOS_HMC5x83_SPI_AUTOINCR_FLAG 0x40
#define PIOS_HMC5x83_CONFIG_REG_A (uint8_t)0x00
#define PIOS_HMC5x83_CONFIG_REG_B (uint8_t)0x01
#define PIOS_HMC5x83_MODE_REG (uint8_t)0x02
#define PIOS_HMC5x83_DATAOUT_XMSB_REG 0x03
#define PIOS_HMC5x83_DATAOUT_XLSB_REG 0x04
#define PIOS_HMC5x83_DATAOUT_ZMSB_REG 0x05
#define PIOS_HMC5x83_DATAOUT_ZLSB_REG 0x06
#define PIOS_HMC5x83_DATAOUT_YMSB_REG 0x07
#define PIOS_HMC5x83_DATAOUT_YLSB_REG 0x08
#define PIOS_HMC5x83_DATAOUT_STATUS_REG 0x09
#define PIOS_HMC5x83_DATAOUT_IDA_REG 0x0A
#define PIOS_HMC5x83_DATAOUT_IDB_REG 0x0B
#define PIOS_HMC5x83_DATAOUT_IDC_REG 0x0C
/* Output Data Rate */
#define PIOS_HMC5x83_ODR_0_75 0x00
#define PIOS_HMC5x83_ODR_1_5 0x04
#define PIOS_HMC5x83_ODR_3 0x08
#define PIOS_HMC5x83_ODR_7_5 0x0C
#define PIOS_HMC5x83_ODR_15 0x10
#define PIOS_HMC5x83_ODR_30 0x14
#define PIOS_HMC5x83_ODR_75 0x18
/* Measure configuration */
#define PIOS_HMC5x83_MEASCONF_NORMAL 0x00
#define PIOS_HMC5x83_MEASCONF_BIAS_POS 0x01
#define PIOS_HMC5x83_MEASCONF_BIAS_NEG 0x02
/* Gain settings */
#define PIOS_HMC5x83_GAIN_0_88 0x00
#define PIOS_HMC5x83_GAIN_1_3 0x20
#define PIOS_HMC5x83_GAIN_1_9 0x40
#define PIOS_HMC5x83_GAIN_2_5 0x60
#define PIOS_HMC5x83_GAIN_4_0 0x80
#define PIOS_HMC5x83_GAIN_4_7 0xA0
#define PIOS_HMC5x83_GAIN_5_6 0xC0
#define PIOS_HMC5x83_GAIN_8_1 0xE0
#define PIOS_HMC5x83_CTRLA_TEMP 0x40
/* Modes */
#define PIOS_HMC5x83_MODE_CONTINUOUS 0x00
#define PIOS_HMC5x83_MODE_SINGLE 0x01
#define PIOS_HMC5x83_MODE_IDLE 0x02
#define PIOS_HMC5x83_MODE_SLEEP 0x03
/* Sensitivity Conversion Values */
#define PIOS_HMC5x83_Sensitivity_0_88Ga 1370 // LSB/Ga
#define PIOS_HMC5x83_Sensitivity_1_3Ga 1090 // LSB/Ga
#define PIOS_HMC5x83_Sensitivity_1_9Ga 820 // LSB/Ga
#define PIOS_HMC5x83_Sensitivity_2_5Ga 660 // LSB/Ga
#define PIOS_HMC5x83_Sensitivity_4_0Ga 440 // LSB/Ga
#define PIOS_HMC5x83_Sensitivity_4_7Ga 390 // LSB/Ga
#define PIOS_HMC5x83_Sensitivity_5_6Ga 330 // LSB/Ga
#define PIOS_HMC5x83_Sensitivity_8_1Ga 230 // LSB/Ga --> NOT RECOMMENDED
typedef uintptr_t pios_hmc5x83_dev_t;
struct pios_hmc5x83_io_driver {
int32_t (*Write)(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t buffer);
int32_t (*Read)(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t *buffer, uint8_t len);
};
#ifdef PIOS_INCLUDE_SPI
extern const struct pios_hmc5x83_io_driver PIOS_HMC5x83_SPI_DRIVER;
#endif
#ifdef PIOS_INCLUDE_I2C
extern const struct pios_hmc5x83_io_driver PIOS_HMC5x83_I2C_DRIVER;
#endif
struct pios_hmc5x83_cfg {
#ifdef PIOS_HMC5X83_HAS_GPIOS
const struct pios_exti_cfg *exti_cfg; /* Pointer to the EXTI configuration */
#endif
uint8_t M_ODR; // OUTPUT DATA RATE --> here below the relative define (See datasheet page 11 for more details) */
uint8_t Meas_Conf; // Measurement Configuration,: Normal, positive bias, or negative bias --> here below the relative define */
uint8_t Gain; // Gain Configuration, select the full scale --> here below the relative define (See datasheet page 11 for more details) */
uint8_t Mode;
bool TempCompensation; // enable temperature sensor on HMC5983 for temperature gain compensation
const struct pios_hmc5x83_io_driver *Driver;
};
/* Public Functions */
extern pios_hmc5x83_dev_t PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_t port_id, uint8_t device_num);
extern bool PIOS_HMC5x83_NewDataAvailable(pios_hmc5x83_dev_t handler);
extern int32_t PIOS_HMC5x83_ReadMag(pios_hmc5x83_dev_t handler, int16_t out[3]);
extern uint8_t PIOS_HMC5x83_ReadID(pios_hmc5x83_dev_t handler, uint8_t out[4]);
extern int32_t PIOS_HMC5x83_Test(pios_hmc5x83_dev_t handler);
extern bool PIOS_HMC5x83_IRQHandler(pios_hmc5x83_dev_t handler);
#endif /* PIOS_HMC5x83_H */
/**
* @}
* @}
*/

2
flight/pios/inc/pios_math.h
View File

@ -40,6 +40,7 @@
#define M_2_SQRTPI_F 1.12837916709551257389615890312f /* 2/sqrt(pi) */
#define M_1_PI_F 0.31830988618379067153776752675f /* 1/pi */
#define M_2_PI_F 0.63661977236758134307553505349f /* 2/pi */
#define M_2PI_F 6.28318530717958647692528676656f /* 2pi */
#define M_LN10_F 2.30258509299404568401799145468f /* ln(10) */
#define M_LN2_F 0.69314718055994530941723212146f /* ln(2) */
#define M_LNPI_F 1.14472988584940017414342735135f /* ln(pi) */
@ -54,6 +55,7 @@
#define M_PI_D 3.14159265358979323846264338328d /* pi */
#define M_PI_2_D 1.57079632679489661923132169164d /* pi/2 */
#define M_PI_4_D 0.78539816339744830961566084582d /* pi/4 */
#define M_2PI_D 6.28318530717958647692528676656d /* 2pi */
#define M_SQRTPI_D 1.77245385090551602729816748334d /* sqrt(pi) */
#define M_2_SQRTPI_D 1.12837916709551257389615890312d /* 2/sqrt(pi) */
#define M_1_PI_D 0.31830988618379067153776752675d /* 1/pi */

8
flight/pios/pios.h
View File

@ -204,10 +204,10 @@
#include <pios_hmc5843.h>
#endif
#ifdef PIOS_INCLUDE_HMC5883
/* HMC5883 3-Axis Digital Compass */
/* #define PIOS_HMC5883_HAS_GPIOS */
#include <pios_hmc5883.h>
#ifdef PIOS_INCLUDE_HMC5X83
/* HMC5883/HMC5983 3-Axis Digital Compass */
/* #define PIOS_HMC5x83_HAS_GPIOS */
#include <pios_hmc5x83.h>
#endif
#ifdef PIOS_INCLUDE_BMP085

3
flight/pios/stm32f4xx/pios_spi.c
View File

@ -585,6 +585,9 @@ static int32_t SPI_DMA_TransferBlock(uint32_t spi_id, const uint8_t *send_buffer
/* Wait until all bytes have been transmitted/received */
while (DMA_GetCurrDataCounter(spi_dev->cfg->dma.rx.channel)) {
#if defined(PIOS_INCLUDE_FREERTOS)
vTaskDelay(0);
#endif
;
}

2
flight/targets/boards/coptercontrol/firmware/inc/pios_config.h
View File

@ -84,7 +84,7 @@
#define PIOS_INCLUDE_MPU6000
#define PIOS_MPU6000_ACCEL
/* #define PIOS_INCLUDE_HMC5843 */
/* #define PIOS_INCLUDE_HMC5883 */
/* #define PIOS_INCLUDE_HMC5X83 */
/* #define PIOS_HMC5883_HAS_GPIOS */
/* #define PIOS_INCLUDE_BMP085 */
/* #define PIOS_INCLUDE_MS5611 */

4
flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h
View File

@ -84,8 +84,8 @@
// #define PIOS_INCLUDE_MPU6000
// #define PIOS_MPU6000_ACCEL
/* #define PIOS_INCLUDE_HMC5843 */
// #define PIOS_INCLUDE_HMC5883
// #define PIOS_HMC5883_HAS_GPIOS
// #define PIOS_INCLUDE_HMC5X83
// #define PIOS_HMC5X83_HAS_GPIOS
/* #define PIOS_INCLUDE_BMP085 */
// #define PIOS_INCLUDE_MS5611
// #define PIOS_INCLUDE_MPXV

27
flight/targets/boards/discoveryf4bare/firmware/pios_board.c
View File

@ -90,10 +90,10 @@ void PIOS_ADC_DMC_irq_handler(void)
#endif /* if defined(PIOS_INCLUDE_ADC) */
#if defined(PIOS_INCLUDE_HMC5883)
#include "pios_hmc5883.h"
static const struct pios_exti_cfg pios_exti_hmc5883_cfg __exti_config = {
.vector = PIOS_HMC5883_IRQHandler,
#if defined(PIOS_INCLUDE_HMC5X83)
#include "pios_hmc5x83.h"
static const struct pios_exti_cfg pios_exti_hmc5x83_cfg __exti_config = {
.vector = PIOS_HMC5x83_IRQHandler,
.line = EXTI_Line7,
.pin = {
.gpio = GPIOB,
@ -123,14 +123,15 @@ static const struct pios_exti_cfg pios_exti_hmc5883_cfg __exti_config = {
},
};
static const struct pios_hmc5883_cfg pios_hmc5883_cfg = {
.exti_cfg = &pios_exti_hmc5883_cfg,
.M_ODR = PIOS_HMC5883_ODR_75,
.Meas_Conf = PIOS_HMC5883_MEASCONF_NORMAL,
.Gain = PIOS_HMC5883_GAIN_1_9,
.Mode = PIOS_HMC5883_MODE_CONTINUOUS,
static const struct pios_hmc5x83_cfg pios_hmc5x83_cfg = {
.exti_cfg = &pios_exti_hmc5x83_cfg,
.M_ODR = PIOS_HMC5x83_ODR_75,
.Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL,
.Gain = PIOS_HMC5x83_GAIN_1_9,
.Mode = PIOS_HMC5x83_MODE_CONTINUOUS,
.Driver = &PIOS_HMC5x83_I2C_DRIVER,
};
#endif /* PIOS_INCLUDE_HMC5883 */
#endif /* PIOS_INCLUDE_HMC5X83 */
/**
* Configuration for the MS5611 chip
@ -929,8 +930,8 @@ void PIOS_Board_Init(void)
PIOS_ADC_Init(&pios_adc_cfg);
#endif
#if defined(PIOS_INCLUDE_HMC5883)
PIOS_HMC5883_Init(&pios_hmc5883_cfg);
#if defined(PIOS_INCLUDE_HMC5X83)
PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_mag_pressure_adapter_id, 0);
#endif
#if defined(PIOS_INCLUDE_MS5611)

2
flight/targets/boards/oplinkmini/firmware/inc/pios_config.h
View File

@ -81,7 +81,7 @@
/* #define PIOS_INCLUDE_MPU6000 */
/* #define PIOS_MPU6000_ACCEL */
/* #define PIOS_INCLUDE_HMC5843 */
/* #define PIOS_INCLUDE_HMC5883 */
/* #define PIOS_INCLUDE_HMC5X83 */
/* #define PIOS_HMC5883_HAS_GPIOS */
/* #define PIOS_INCLUDE_BMP085 */
/* #define PIOS_INCLUDE_MS5611 */

4
flight/targets/boards/osd/firmware/inc/pios_config.h
View File

@ -81,8 +81,8 @@
/* #define PIOS_INCLUDE_MPU6000 */
/* #define PIOS_MPU6000_ACCEL */
/* #define PIOS_INCLUDE_HMC5843 */
#define PIOS_INCLUDE_HMC5883
/* #define PIOS_HMC5883_HAS_GPIOS */
#define PIOS_INCLUDE_HMC5X83
/* #define PIOS_HMC5X83_HAS_GPIOS */
#define PIOS_INCLUDE_BMP085
/* #define PIOS_INCLUDE_MS5611 */
/* #define PIOS_INCLUDE_MPXV */

4
flight/targets/boards/revolution/firmware/inc/pios_config.h
View File

@ -84,8 +84,8 @@
#define PIOS_INCLUDE_MPU6000
#define PIOS_MPU6000_ACCEL
/* #define PIOS_INCLUDE_HMC5843 */
#define PIOS_INCLUDE_HMC5883
#define PIOS_HMC5883_HAS_GPIOS
#define PIOS_INCLUDE_HMC5X83
#define PIOS_HMC5X83_HAS_GPIOS
/* #define PIOS_INCLUDE_BMP085 */
#define PIOS_INCLUDE_MS5611
#define PIOS_INCLUDE_MPXV

34
flight/targets/boards/revolution/firmware/pios_board.c
View File

@ -57,6 +57,7 @@
*/
#if defined(PIOS_INCLUDE_ADC)
#include "pios_adc_priv.h"
void PIOS_ADC_DMC_irq_handler(void);
void DMA2_Stream4_IRQHandler(void) __attribute__((alias("PIOS_ADC_DMC_irq_handler")));
@ -91,10 +92,16 @@ void PIOS_ADC_DMC_irq_handler(void)
#endif /* if defined(PIOS_INCLUDE_ADC) */
#if defined(PIOS_INCLUDE_HMC5883)
#include "pios_hmc5883.h"
static const struct pios_exti_cfg pios_exti_hmc5883_cfg __exti_config = {
.vector = PIOS_HMC5883_IRQHandler,
#if defined(PIOS_INCLUDE_HMC5X83)
#include "pios_hmc5x83.h"
pios_hmc5x83_dev_t onboard_mag = 0;
bool pios_board_internal_mag_handler()
{
return PIOS_HMC5x83_IRQHandler(onboard_mag);
}
static const struct pios_exti_cfg pios_exti_hmc5x83_cfg __exti_config = {
.vector = pios_board_internal_mag_handler,
.line = EXTI_Line7,
.pin = {
.gpio = GPIOB,
@ -124,14 +131,15 @@ static const struct pios_exti_cfg pios_exti_hmc5883_cfg __exti_config = {
},
};
static const struct pios_hmc5883_cfg pios_hmc5883_cfg = {
.exti_cfg = &pios_exti_hmc5883_cfg,
.M_ODR = PIOS_HMC5883_ODR_75,
.Meas_Conf = PIOS_HMC5883_MEASCONF_NORMAL,
.Gain = PIOS_HMC5883_GAIN_1_9,
.Mode = PIOS_HMC5883_MODE_CONTINUOUS,
static const struct pios_hmc5x83_cfg pios_hmc5x83_cfg = {
.exti_cfg = &pios_exti_hmc5x83_cfg,
.M_ODR = PIOS_HMC5x83_ODR_75,
.Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL,
.Gain = PIOS_HMC5x83_GAIN_1_9,
.Mode = PIOS_HMC5x83_MODE_CONTINUOUS,
.Driver = &PIOS_HMC5x83_I2C_DRIVER,
};
#endif /* PIOS_INCLUDE_HMC5883 */
#endif /* PIOS_INCLUDE_HMC5X83 */
/**
* Configuration for the MS5611 chip
@ -944,8 +952,8 @@ void PIOS_Board_Init(void)
PIOS_ADC_Init(&pios_adc_cfg);
#endif
#if defined(PIOS_INCLUDE_HMC5883)
PIOS_HMC5883_Init(&pios_hmc5883_cfg);
#if defined(PIOS_INCLUDE_HMC5X83)
onboard_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_mag_pressure_adapter_id, 0);
#endif
#if defined(PIOS_INCLUDE_MS5611)

4
flight/targets/boards/revoproto/firmware/inc/pios_config.h
View File

@ -81,8 +81,8 @@
#define PIOS_INCLUDE_MPU6000
#define PIOS_MPU6000_ACCEL
/* #define PIOS_INCLUDE_HMC5843 */
#define PIOS_INCLUDE_HMC5883
#define PIOS_HMC5883_HAS_GPIOS
#define PIOS_INCLUDE_HMC5X83
#define PIOS_HMC5X83_HAS_GPIOS
/* #define PIOS_INCLUDE_BMP085 */
#define PIOS_INCLUDE_MS5611
#define PIOS_INCLUDE_MPXV

33
flight/targets/boards/revoproto/firmware/pios_board.c
View File

@ -81,10 +81,16 @@ void PIOS_ADC_DMC_irq_handler(void)
#endif /* if defined(PIOS_INCLUDE_ADC) */
#if defined(PIOS_INCLUDE_HMC5883)
#include "pios_hmc5883.h"
static const struct pios_exti_cfg pios_exti_hmc5883_cfg __exti_config = {
.vector = PIOS_HMC5883_IRQHandler,
#if defined(PIOS_INCLUDE_HMC5X83)
pios_hmc5x83_dev_t onboard_mag = 0;
bool pios_board_internal_mag_handler()
{
return PIOS_HMC5x83_IRQHandler(onboard_mag);
}
#include "pios_hmc5x83.h"
static const struct pios_exti_cfg pios_exti_hmc5x83_cfg __exti_config = {
.vector = pios_board_internal_mag_handler,
.line = EXTI_Line5,
.pin = {
.gpio = GPIOB,
@ -114,14 +120,15 @@ static const struct pios_exti_cfg pios_exti_hmc5883_cfg __exti_config = {
},
};
static const struct pios_hmc5883_cfg pios_hmc5883_cfg = {
.exti_cfg = &pios_exti_hmc5883_cfg,
.M_ODR = PIOS_HMC5883_ODR_75,
.Meas_Conf = PIOS_HMC5883_MEASCONF_NORMAL,
.Gain = PIOS_HMC5883_GAIN_1_9,
.Mode = PIOS_HMC5883_MODE_CONTINUOUS,
static const struct pios_hmc5x83_cfg pios_hmc5x83_cfg = {
.exti_cfg = &pios_exti_hmc5x83_cfg,
.M_ODR = PIOS_HMC5x83_ODR_75,
.Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL,
.Gain = PIOS_HMC5x83_GAIN_1_9,
.Mode = PIOS_HMC5x83_MODE_CONTINUOUS,
.Driver = &PIOS_HMC5x83_I2C_DRIVER,
};
#endif /* PIOS_INCLUDE_HMC5883 */
#endif /* PIOS_INCLUDE_HMC5X83 */
/**
* Configuration for the MS5611 chip
@ -938,8 +945,8 @@ void PIOS_Board_Init(void)
PIOS_ADC_Init(&pios_adc_cfg);
#endif
#if defined(PIOS_INCLUDE_HMC5883)
PIOS_HMC5883_Init(&pios_hmc5883_cfg);
#if defined(PIOS_INCLUDE_HMC5X83)
onboard_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_pressure_adapter_id, 0);
#endif
#if defined(PIOS_INCLUDE_MS5611)

1
flight/targets/boards/simposix/firmware/Makefile
View File

@ -99,6 +99,7 @@ SRC += $(FLIGHTLIB)/sanitycheck.c
SRC += $(MATHLIB)/sin_lookup.c
SRC += $(MATHLIB)/pid.c
SRC += $(MATHLIB)/mathmisc.c
SRC += $(MATHLIB)/butterworth.c
SRC += $(PIOSCORECOMMON)/pios_task_monitor.c
SRC += $(PIOSCORECOMMON)/pios_dosfs_logfs.c

2
flight/targets/boards/simposix/firmware/inc/pios_config.h
View File

@ -63,7 +63,7 @@
/* Select the sensors to include */
// #define PIOS_INCLUDE_BMA180
// #define PIOS_INCLUDE_HMC5883
// #define PIOS_INCLUDE_HMC5X83
// #define PIOS_INCLUDE_MPU6000
// #define PIOS_MPU6000_ACCEL
// #define PIOS_INCLUDE_L3GD20

9
ground/openpilotgcs/copydata.pro
View File

@ -30,9 +30,6 @@ GCS_LIBRARY_PATH
libQt5Qml.so.5 \
libQt5DBus.so.5 \
libQt5QuickParticles.so.5 \
libicui18n.so.51 \
libicuuc.so.51 \
libicudata.so.51 \
libqgsttools_p.so.1
data_copy.commands += -@$(MKDIR) $$targetPath(\"$$GCS_QT_LIBRARY_PATH\") $$addNewline()
@ -142,9 +139,9 @@ GCS_LIBRARY_PATH
Qt5MultimediaWidgets$${DS}.dll \
Qt5Quick$${DS}.dll \
Qt5Qml$${DS}.dll \
icuin51.dll \
icudt51.dll \
icuuc51.dll
icuin52.dll \
icudt52.dll \
icuuc52.dll
# it is more robust to take the following DLLs from Qt rather than from MinGW
QT_DLLS += libgcc_s_dw2-1.dll \
libstdc++-6.dll \

65
ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_fr.ts
View File

@ -7934,14 +7934,12 @@ Useful if you have accidentally changed some settings.</source>
<translation>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Ceci ajuste le niveau de stabilité que votre véhicule aura en vol incliné (ex. vol en avançant) en mode Attitude. Ajouter une valeur d&apos;intégrale en mode Attitude lorsque une intégrale est présente en mode Rate n&apos;est pas recommandé.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</translation>
</message>
<message>
<location/>
<source>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;This sets the maximum value of the integral (KP) that is used in Rate mode.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</source>
<translation>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Ceci limite la valeur maximale de l&apos;intégrale (KP) qui est utilisée en mode Rate.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</translation>
<translation type="vanished">&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Ceci limite la valeur maximale de l&apos;intégrale (KP) qui est utilisée en mode Rate.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</translation>
</message>
<message>
<location/>
<source>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;This sets the maximum value of the integral (KP) that is used in AttitudeMode.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</source>
<translation>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Ceci limite la valeur maximale de l&apos;intégrale (KP) qui est utilisée en mode Attitude.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</translation>
<translation type="vanished">&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Ceci limite la valeur maximale de l&apos;intégrale (KP) qui est utilisée en mode Attitude.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</translation>
</message>
<message>
<location/>
@ -8234,6 +8232,16 @@ Useful if you have accidentally changed some settings.</source>
<source>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Motor response time to go from min thrust to max thrust. It allows thrust anticipation on entering/exiting inverted mode&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</source>
<translation>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Temps de réponse du moteur pour passer de la poussée minimum à la poussée maximum. Cela permet une anticipation lors de l&apos;entrée/sortie du mode inversé.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</translation>
</message>
<message>
<location/>
<source>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;This sets the maximum value of the integral (KI) that is used in Rate mode.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</source>
<translation>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Ceci limite la valeur maximale de l&apos;intégrale (KI) qui est utilisée en mode Rate.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</translation>
</message>
<message>
<location/>
<source>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;This sets the maximum value of the integral (KI) that is used in AttitudeMode.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</source>
<translation>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Ceci limite la valeur maximale de l&apos;intégrale (KI) qui est utilisée en mode Attitude.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</translation>
</message>
</context>
<context>
<name>TxPIDWidget</name>
@ -8946,7 +8954,7 @@ les données en cache</translation>
<translation>Diagramme de Connexion</translation>
</message>
<message>
<location line="+153"/>
<location line="+156"/>
<source>Save File</source>
<translation>Enregistrer Fichier</translation>
</message>
@ -9297,17 +9305,19 @@ p, li { white-space: pre-wrap; }
<translation>Hexacoptère</translation>
</message>
<message>
<location line="+2"/>
<location line="+8"/>
<location line="+1"/>
<source>Hexacopter Coax (Y6)</source>
<translation>Hexacoptère Coax (Y6)</translation>
</message>
<message>
<location line="+2"/>
<location line="-7"/>
<location line="+1"/>
<source>Hexacopter X</source>
<translation>Hexacoptère X</translation>
</message>
<message>
<location line="+2"/>
<location line="+1"/>
<source>Hexacopter H</source>
<translation>Hexacoptère H</translation>
@ -9392,7 +9402,7 @@ p, li { white-space: pre-wrap; }
</message>
<message>
<location/>
<location filename="../../../src/plugins/setupwizard/pages/outputcalibrationpage.cpp" line="+261"/>
<location filename="../../../src/plugins/setupwizard/pages/outputcalibrationpage.cpp" line="+267"/>
<location line="+76"/>
<location line="+28"/>
<location line="+19"/>
@ -10256,11 +10266,29 @@ p, li { white-space: pre-wrap; }
<context>
<name>ConfigMultiRotorWidget</name>
<message>
<location filename="../../../src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp" line="+713"/>
<location line="+179"/>
<location filename="../../../src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp" line="+696"/>
<location line="+24"/>
<location line="+31"/>
<location line="+31"/>
<location line="+25"/>
<location line="+24"/>
<location line="+24"/>
<location line="+41"/>
<location line="+200"/>
<location line="+70"/>
<source>Configuration OK</source>
<translation>Configuration OK</translation>
</message>
<message>
<location line="-302"/>
<source>&lt;font color=&apos;red&apos;&gt;ERROR: Assign a Yaw channel&lt;/font&gt;</source>
<translation>&lt;font color=&apos;red&apos;&gt;ERREUR : Veuillez affecter le canal de Yaw&lt;/font&gt;</translation>
</message>
<message>
<location line="+367"/>
<source>&lt;font color=&apos;red&apos;&gt;ERROR: Assign all %1 motor channels&lt;/font&gt;</source>
<translation>&lt;font color=&apos;red&apos;&gt;ERREUR : Veuillez affecter tous les %1 canaux moteurs&lt;/font&gt;</translation>
</message>
</context>
<context>
<name>ConfigCCHWWidget</name>
@ -10857,7 +10885,7 @@ Double clic sur la légende ou le tracé pour afficher/cacher la légende.</tran
</message>
<message>
<location line="+3"/>
<location line="+44"/>
<location line="+47"/>
<location line="+15"/>
<location line="+25"/>
<location line="+13"/>
@ -10865,7 +10893,7 @@ Double clic sur la légende ou le tracé pour afficher/cacher la légende.</tran
<translation>Inconnu</translation>
</message>
<message>
<location line="-92"/>
<location line="-95"/>
<source>Vehicle type: </source>
<translation>Type de véhicule : </translation>
</message>
@ -10904,6 +10932,11 @@ Double clic sur la légende ou le tracé pour afficher/cacher la légende.</tran
<source>Hexacopter Coax (Y6)</source>
<translation>Hexacoptère Coax (Y6)</translation>
</message>
<message>
<location line="+3"/>
<source>Hexacopter H</source>
<translation type="unfinished">Hexacoptère H</translation>
</message>
<message>
<location line="+3"/>
<source>Hexacopter X</source>
@ -11023,7 +11056,7 @@ Double clic sur la légende ou le tracé pour afficher/cacher la légende.</tran
<translation>Écriture paramètres matériels</translation>
</message>
<message>
<location line="+109"/>
<location line="+113"/>
<source>Writing actuator settings</source>
<translation>Écriture paramètres actionneurs</translation>
</message>
@ -11069,7 +11102,7 @@ Double clic sur la légende ou le tracé pour afficher/cacher la légende.</tran
<translation>Écriture contrôles manuels par défaut</translation>
</message>
<message>
<location line="+153"/>
<location line="+154"/>
<source>Preparing mixer settings</source>
<translation>Préparation des paramètres de mixage</translation>
</message>
@ -13938,7 +13971,7 @@ et même conduire au crash. A utiliser avec prudence.</translation>
<context>
<name>ConfigVehicleTypeWidget</name>
<message>
<location filename="../../../src/plugins/config/configvehicletypewidget.cpp" line="+129"/>
<location filename="../../../src/plugins/config/configvehicletypewidget.cpp" line="+130"/>
<source>Multirotor</source>
<translation>Multirotor</translation>
</message>
@ -13963,7 +13996,7 @@ et même conduire au crash. A utiliser avec prudence.</translation>
<translation type="unfinished">Personnalisé</translation>
</message>
<message>
<location line="+270"/>
<location line="+273"/>
<source>http://wiki.openpilot.org/x/44Cf</source>
<translatorcomment>Lien Wiki FR</translatorcomment>
<translation>http://wiki.openpilot.org/x/IIBqAQ</translation>

272
ground/openpilotgcs/src/plugins/config/configtxpidwidget.cpp
View File

@ -28,6 +28,10 @@
#include "configtxpidwidget.h"
#include "txpidsettings.h"
#include "hwsettings.h"
#include "stabilizationsettings.h"
#include "stabilizationsettingsbank1.h"
#include "stabilizationsettingsbank2.h"
#include "stabilizationsettingsbank3.h"
#include <extensionsystem/pluginmanager.h>
#include <coreplugin/generalsettings.h>
@ -51,16 +55,21 @@ ConfigTxPIDWidget::ConfigTxPIDWidget(QWidget *parent) : ConfigTaskWidget(parent)
connect(m_txpid->Apply, SIGNAL(clicked()), this, SLOT(applySettings()));
connect(m_txpid->Save, SIGNAL(clicked()), this, SLOT(saveSettings()));
addWidgetBinding("TxPIDSettings", "BankNumber", m_txpid->pidBank, 0, 1, true);
connect(m_txpid->PID1, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSpinBoxProperties(int)));
connect(m_txpid->PID2, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSpinBoxProperties(int)));
connect(m_txpid->PID3, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSpinBoxProperties(int)));
addWidgetBinding("TxPIDSettings", "PIDs", m_txpid->PID1, TxPIDSettings::PIDS_INSTANCE1);
addWidgetBinding("TxPIDSettings", "PIDs", m_txpid->PID2, TxPIDSettings::PIDS_INSTANCE2);
addWidgetBinding("TxPIDSettings", "PIDs", m_txpid->PID3, TxPIDSettings::PIDS_INSTANCE3);
addWidgetBinding("TxPIDSettings", "BankNumber", m_txpid->pidBank, 0, 1, true);
addWidgetBinding("TxPIDSettings", "Inputs", m_txpid->Input1, TxPIDSettings::INPUTS_INSTANCE1);
addWidgetBinding("TxPIDSettings", "Inputs", m_txpid->Input2, TxPIDSettings::INPUTS_INSTANCE2);
addWidgetBinding("TxPIDSettings", "Inputs", m_txpid->Input3, TxPIDSettings::INPUTS_INSTANCE3);
// It's important that the PIDx values are populated before the MinPIDx and MaxPIDx,
// otherwise the MinPIDx and MaxPIDx will be capped by the old spin box limits. The correct limits
// are set when updateSpinBoxProperties is called when the PIDx->currentTextChanged signal is sent.
// The binding order is reversed because the values are populated in reverse.
addWidgetBinding("TxPIDSettings", "MinPID", m_txpid->MinPID1, TxPIDSettings::MINPID_INSTANCE1);
addWidgetBinding("TxPIDSettings", "MinPID", m_txpid->MinPID2, TxPIDSettings::MINPID_INSTANCE2);
addWidgetBinding("TxPIDSettings", "MinPID", m_txpid->MinPID3, TxPIDSettings::MINPID_INSTANCE3);
@ -69,6 +78,10 @@ ConfigTxPIDWidget::ConfigTxPIDWidget(QWidget *parent) : ConfigTaskWidget(parent)
addWidgetBinding("TxPIDSettings", "MaxPID", m_txpid->MaxPID2, TxPIDSettings::MAXPID_INSTANCE2);
addWidgetBinding("TxPIDSettings", "MaxPID", m_txpid->MaxPID3, TxPIDSettings::MAXPID_INSTANCE3);
addWidgetBinding("TxPIDSettings", "PIDs", m_txpid->PID1, TxPIDSettings::PIDS_INSTANCE1);
addWidgetBinding("TxPIDSettings", "PIDs", m_txpid->PID2, TxPIDSettings::PIDS_INSTANCE2);
addWidgetBinding("TxPIDSettings", "PIDs", m_txpid->PID3, TxPIDSettings::PIDS_INSTANCE3);
addWidgetBinding("TxPIDSettings", "ThrottleRange", m_txpid->ThrottleMin, TxPIDSettings::THROTTLERANGE_MIN);
addWidgetBinding("TxPIDSettings", "ThrottleRange", m_txpid->ThrottleMax, TxPIDSettings::THROTTLERANGE_MAX);
@ -88,6 +101,257 @@ ConfigTxPIDWidget::~ConfigTxPIDWidget()
// Do nothing
}
static bool isResponsivenessOption(int pidOption)
{
switch (pidOption) {
case TxPIDSettings::PIDS_ROLLRATERESP:
case TxPIDSettings::PIDS_PITCHRATERESP:
case TxPIDSettings::PIDS_ROLLPITCHRATERESP:
case TxPIDSettings::PIDS_YAWRATERESP:
case TxPIDSettings::PIDS_ROLLATTITUDERESP:
case TxPIDSettings::PIDS_PITCHATTITUDERESP:
case TxPIDSettings::PIDS_ROLLPITCHATTITUDERESP:
case TxPIDSettings::PIDS_YAWATTITUDERESP:
return true;
default:
return false;
}
}
static bool isAttitudeOption(int pidOption)
{
switch (pidOption) {
case TxPIDSettings::PIDS_ROLLATTITUDEKP:
case TxPIDSettings::PIDS_PITCHATTITUDEKP:
case TxPIDSettings::PIDS_ROLLPITCHATTITUDEKP:
case TxPIDSettings::PIDS_YAWATTITUDEKP:
case TxPIDSettings::PIDS_ROLLATTITUDEKI:
case TxPIDSettings::PIDS_PITCHATTITUDEKI:
case TxPIDSettings::PIDS_ROLLPITCHATTITUDEKI:
case TxPIDSettings::PIDS_YAWATTITUDEKI:
case TxPIDSettings::PIDS_ROLLATTITUDEILIMIT:
case TxPIDSettings::PIDS_PITCHATTITUDEILIMIT:
case TxPIDSettings::PIDS_ROLLPITCHATTITUDEILIMIT:
case TxPIDSettings::PIDS_YAWATTITUDEILIMIT:
case TxPIDSettings::PIDS_ROLLATTITUDERESP:
case TxPIDSettings::PIDS_PITCHATTITUDERESP:
case TxPIDSettings::PIDS_ROLLPITCHATTITUDERESP:
case TxPIDSettings::PIDS_YAWATTITUDERESP:
return true;
default:
return false;
}
}
template <class StabilizationSettingsBankX>
static float defaultValueForPidOption(const StabilizationSettingsBankX *bank, int pidOption)
{
switch (pidOption) {
case TxPIDSettings::PIDS_DISABLED:
return 0.0f;
case TxPIDSettings::PIDS_ROLLRATEKP:
return bank->getRollRatePID_Kp();
case TxPIDSettings::PIDS_PITCHRATEKP:
return bank->getPitchRatePID_Kp();
case TxPIDSettings::PIDS_ROLLPITCHRATEKP:
return bank->getRollRatePID_Kp();
case TxPIDSettings::PIDS_YAWRATEKP:
return bank->getYawRatePID_Kp();
case TxPIDSettings::PIDS_ROLLRATEKI:
return bank->getRollRatePID_Ki();
case TxPIDSettings::PIDS_PITCHRATEKI:
return bank->getPitchRatePID_Ki();
case TxPIDSettings::PIDS_ROLLPITCHRATEKI:
return bank->getRollRatePID_Ki();
case TxPIDSettings::PIDS_YAWRATEKI:
return bank->getYawRatePID_Ki();
case TxPIDSettings::PIDS_ROLLRATEKD:
return bank->getRollRatePID_Kd();
case TxPIDSettings::PIDS_PITCHRATEKD:
return bank->getPitchRatePID_Kd();
case TxPIDSettings::PIDS_ROLLPITCHRATEKD:
return bank->getRollRatePID_Kd();
case TxPIDSettings::PIDS_YAWRATEKD:
return bank->getYawRatePID_Kd();
case TxPIDSettings::PIDS_ROLLRATEILIMIT:
return bank->getRollRatePID_ILimit();
case TxPIDSettings::PIDS_PITCHRATEILIMIT:
return bank->getPitchRatePID_ILimit();
case TxPIDSettings::PIDS_ROLLPITCHRATEILIMIT:
return bank->getRollRatePID_ILimit();
case TxPIDSettings::PIDS_YAWRATEILIMIT:
return bank->getYawRatePID_ILimit();
case TxPIDSettings::PIDS_ROLLRATERESP:
return bank->getManualRate_Roll();
case TxPIDSettings::PIDS_PITCHRATERESP:
return bank->getManualRate_Pitch();
case TxPIDSettings::PIDS_ROLLPITCHRATERESP:
return bank->getManualRate_Roll();
case TxPIDSettings::PIDS_YAWRATERESP:
return bank->getManualRate_Yaw();
case TxPIDSettings::PIDS_ROLLATTITUDEKP:
return bank->getRollPI_Kp();
case TxPIDSettings::PIDS_PITCHATTITUDEKP:
return bank->getPitchPI_Kp();
case TxPIDSettings::PIDS_ROLLPITCHATTITUDEKP:
return bank->getRollPI_Kp();
case TxPIDSettings::PIDS_YAWATTITUDEKP:
return bank->getYawPI_Kp();
case TxPIDSettings::PIDS_ROLLATTITUDEKI:
return bank->getRollPI_Ki();
case TxPIDSettings::PIDS_PITCHATTITUDEKI:
return bank->getPitchPI_Ki();
case TxPIDSettings::PIDS_ROLLPITCHATTITUDEKI:
return bank->getRollPI_Ki();
case TxPIDSettings::PIDS_YAWATTITUDEKI:
return bank->getYawPI_Ki();
case TxPIDSettings::PIDS_ROLLATTITUDEILIMIT:
return bank->getRollPI_ILimit();
case TxPIDSettings::PIDS_PITCHATTITUDEILIMIT:
return bank->getPitchPI_ILimit();
case TxPIDSettings::PIDS_ROLLPITCHATTITUDEILIMIT:
return bank->getRollPI_ILimit();
case TxPIDSettings::PIDS_YAWATTITUDEILIMIT:
return bank->getYawPI_ILimit();
case TxPIDSettings::PIDS_ROLLATTITUDERESP:
return (float)bank->getRollMax();
case TxPIDSettings::PIDS_PITCHATTITUDERESP:
return (float)bank->getPitchMax();
case TxPIDSettings::PIDS_ROLLPITCHATTITUDERESP:
return (float)bank->getRollMax();
case TxPIDSettings::PIDS_YAWATTITUDERESP:
return bank->getYawMax();
case -1: // The PID Option field was uninitialized.
return 0.0f;
default:
Q_ASSERT_X(false, "getDefaultValueForOption", "Incorrect PID option");
return 0.0f;
}
}
float ConfigTxPIDWidget::getDefaultValueForPidOption(int pidOption)
{
if (pidOption == TxPIDSettings::PIDS_GYROTAU) {
StabilizationSettings *stab = qobject_cast<StabilizationSettings *>(getObject(QString("StabilizationSettings")));
return stab->getGyroTau();
}
int pidBankIndex = m_txpid->pidBank->currentIndex();
if (pidBankIndex == -1) {
// The pidBank field was uninitilized.
return 0.0f;
}
int bankNumber = pidBankIndex + 1;
if (bankNumber == 1) {
StabilizationSettingsBank1 *bank = qobject_cast<StabilizationSettingsBank1 *>(getObject(QString("StabilizationSettingsBank1")));
return defaultValueForPidOption(bank, pidOption);
} else if (bankNumber == 2) {
StabilizationSettingsBank2 *bank = qobject_cast<StabilizationSettingsBank2 *>(getObject(QString("StabilizationSettingsBank2")));
return defaultValueForPidOption(bank, pidOption);
} else if (bankNumber == 3) {
StabilizationSettingsBank3 *bank = qobject_cast<StabilizationSettingsBank3 *>(getObject(QString("StabilizationSettingsBank3")));
return defaultValueForPidOption(bank, pidOption);
} else {
Q_ASSERT_X(false, "getDefaultValueForPidOption", "Incorrect bank number");
return 0.0f;
}
}
void ConfigTxPIDWidget::updateSpinBoxProperties(int selectedPidOption)
{
QObject *PIDx = sender();
QDoubleSpinBox *minPID;
QDoubleSpinBox *maxPID;
if (PIDx == m_txpid->PID1) {
minPID = m_txpid->MinPID1;
maxPID = m_txpid->MaxPID1;
} else if (PIDx == m_txpid->PID2) {
minPID = m_txpid->MinPID2;
maxPID = m_txpid->MaxPID2;
} else if (PIDx == m_txpid->PID3) {
minPID = m_txpid->MinPID3;
maxPID = m_txpid->MaxPID3;
} else {
Q_ASSERT_X(false, "updateSpinBoxProperties", "Incorrect sender object");
return;
}
// The ranges need to be setup before the values can be set,
// otherwise the value might be incorrectly capped.
if (isResponsivenessOption(selectedPidOption)) {
if (isAttitudeOption(selectedPidOption)) {
// Limit to 180 degrees.
minPID->setRange(0, 180);
maxPID->setRange(0, 180);
} else {
minPID->setRange(0, 999);
maxPID->setRange(0, 999);
}
minPID->setSingleStep(1);
maxPID->setSingleStep(1);
minPID->setDecimals(0);
maxPID->setDecimals(0);
} else {
minPID->setRange(0, 99.99);
maxPID->setRange(0, 99.99);
minPID->setSingleStep(0.000100);
maxPID->setSingleStep(0.000100);
minPID->setDecimals(6);
maxPID->setDecimals(6);
}
float value = getDefaultValueForPidOption(selectedPidOption);
minPID->setValue(value);
maxPID->setValue(value);
}
void ConfigTxPIDWidget::refreshValues()
{
HwSettings *hwSettings = HwSettings::GetInstance(getObjectManager());

2
ground/openpilotgcs/src/plugins/config/configtxpidwidget.h
View File

@ -40,6 +40,8 @@ private:
Ui_TxPIDWidget *m_txpid;
private slots:
void updateSpinBoxProperties(int selectedPidOption);
float getDefaultValueForPidOption(int pidOption);
void refreshValues();
void applySettings();
void saveSettings();

12
ground/openpilotgcs/src/plugins/config/stabilization.ui
View File

@ -16641,7 +16641,7 @@ border-radius: 5;</string>
<enum>Qt::StrongFocus</enum>
</property>
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;This sets the maximum value of the integral (KP) that is used in Rate mode.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;This sets the maximum value of the integral (KI) that is used in Rate mode.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="styleSheet">
<string notr="true"/>
@ -16694,7 +16694,7 @@ border-radius: 5;</string>
<enum>Qt::StrongFocus</enum>
</property>
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;This sets the maximum value of the integral (KP) that is used in Rate mode.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;This sets the maximum value of the integral (KI) that is used in Rate mode.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="styleSheet">
<string notr="true"/>
@ -17851,7 +17851,7 @@ font:bold;</string>
<enum>Qt::StrongFocus</enum>
</property>
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;This sets the maximum value of the integral (KP) that is used in AttitudeMode.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;This sets the maximum value of the integral (KI) that is used in AttitudeMode.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="styleSheet">
<string notr="true"/>
@ -17932,7 +17932,7 @@ font:bold;</string>
<enum>Qt::StrongFocus</enum>
</property>
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;This sets the maximum value of the integral (KP) that is used in AttitudeMode.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;This sets the maximum value of the integral (KI) that is used in AttitudeMode.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="styleSheet">
<string notr="true"/>
@ -18042,7 +18042,7 @@ font:bold;</string>
<enum>Qt::StrongFocus</enum>
</property>
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;This sets the maximum value of the integral (KP) that is used in AttitudeMode.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;This sets the maximum value of the integral (KI) that is used in AttitudeMode.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="styleSheet">
<string notr="true"/>
@ -18657,7 +18657,7 @@ font:bold;</string>
<enum>Qt::StrongFocus</enum>
</property>
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;This sets the maximum value of the integral (KP) that is used in Rate mode.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;This sets the maximum value of the integral (KI) that is used in Rate mode.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="styleSheet">
<string notr="true"/>

1
ground/openpilotgcs/src/plugins/coreplugin/coreplugin.pro
View File

@ -143,7 +143,6 @@ RESOURCES += core.qrc \
unix:!macx {
images.files = images/openpilot_logo_*.png
images.files = images/qtcreator_logo_*.png
images.path = /share/pixmaps
INSTALLS += images
}

BIN
ground/openpilotgcs/src/plugins/coreplugin/images/qtcreator_logo_32.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 1.4 KiB

17
ground/openpilotgcs/src/plugins/flightlog/FlightLogDialog.qml
View File

@ -7,7 +7,7 @@ import org.openpilot 1.0
import "functions.js" as Functions
Rectangle {
width: 600
width: 700
height: 400
id: root
ColumnLayout {
@ -59,11 +59,11 @@ Rectangle {
delegate:
Text {
verticalAlignment: Text.AlignVCenter
text: Functions.millisToTime(styleData.value)
text: Functions.microsToTime(styleData.value)
}
}
TableViewColumn {
role: "Type"; title: "Type"; width: 60;
role: "Type"; title: "Type"; width: 50;
delegate:
Text {
verticalAlignment: Text.AlignVCenter
@ -72,6 +72,7 @@ Rectangle {
case 0 : text: qsTr("Empty"); break;
case 1 : text: qsTr("Text"); break;
case 2 : text: qsTr("UAVO"); break;
case 3 : text: qsTr("UAVO(P)"); break;
default: text: qsTr("Unknown"); break;
}
}
@ -93,12 +94,16 @@ Rectangle {
spacing: 10
Text {
id: totalFlights
text: "<b>" + qsTr("Flights recorded: ") + "</b>" + (logStatus.Flight + 1)
text: "<b>" + qsTr("Flights recorded:") + "</b> " + (logStatus.Flight + 1)
}
Text {
id: totalSlots
text: "<b>" + qsTr("Slots used/free:") + "</b> " +
logStatus.UsedSlots + "/" + logStatus.FreeSlots
}
Text {
id: totalEntries
text: "<b>" + qsTr("Entries logged (free): ") + "</b>" +
logStatus.UsedSlots + " (" + logStatus.FreeSlots + ")"
text: "<b>" + qsTr("Entries downloaded:") + "</b> " + logManager.logEntriesCount
}
Rectangle {
Layout.fillHeight: true

42
ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.cpp
View File

@ -205,21 +205,47 @@ void FlightLogManager::retrieveLogs(int flightToRetrieve)
for (int flight = startFlight; flight <= endFlight; flight++) {
m_flightLogControl->setFlight(flight);
bool gotLast = false;
int entry = 0;
int slot = 0;
while (!gotLast) {
// Send request for loading flight entry on flight side and wait for ack/nack
m_flightLogControl->setEntry(entry);
m_flightLogControl->setEntry(slot);
if (updateHelper.doObjectAndWait(m_flightLogControl, UAVTALK_TIMEOUT) == UAVObjectUpdaterHelper::SUCCESS &&
requestHelper.doObjectAndWait(m_flightLogEntry, UAVTALK_TIMEOUT) == UAVObjectUpdaterHelper::SUCCESS) {
if (m_flightLogEntry->getType() != DebugLogEntry::TYPE_EMPTY) {
// Ok, we retrieved the entry, and it was the correct one. clone it and add it to the list
ExtendedDebugLogEntry *logEntry = new ExtendedDebugLogEntry();
logEntry->setData(m_flightLogEntry->getData(), m_objectManager);
m_logEntries << logEntry;
if (logEntry->getData().Type == DebugLogEntry::TYPE_MULTIPLEUAVOBJECTS) {
const quint32 total_len = sizeof(DebugLogEntry::DataFields);
const quint32 data_len = sizeof(((DebugLogEntry::DataFields *)0)->Data);
const quint32 header_len = total_len - data_len;
DebugLogEntry::DataFields fields;
quint32 start = logEntry->getData().Size;
// cycle until there is space for another object
while (start + header_len + 1 < data_len) {
memset(&fields, 0xFF, total_len);
memcpy(&fields, &logEntry->getData().Data[start], header_len);
// check wether a packed object is found
// note that empty data blocks are set as 0xFF in flight side to minimize flash wearing
// thus as soon as this read outside of used area, the test will fail as lenght would be 0xFFFF
quint32 toread = header_len + fields.Size;
if (!(toread + start > data_len)) {
memcpy(&fields, &logEntry->getData().Data[start], toread);
ExtendedDebugLogEntry *subEntry = new ExtendedDebugLogEntry();
subEntry->setData(fields, m_objectManager);
m_logEntries << subEntry;
}
start += toread;
}
}
// Increment to get next entry from flight side
entry++;
slot++;
} else {
// We are done, not more entries on this flight
gotLast = true;
@ -280,7 +306,7 @@ void FlightLogManager::exportToOPL(QString fileName)
ExtendedDebugLogEntry *entry = m_logEntries[currentEntry];
// Only log uavobjects
if (entry->getType() == ExtendedDebugLogEntry::TYPE_UAVOBJECT) {
if (entry->getType() == ExtendedDebugLogEntry::TYPE_UAVOBJECT || entry->getType() == ExtendedDebugLogEntry::TYPE_MULTIPLEUAVOBJECTS) {
// Set timestamp that should be logged for this entry
logFile.setNextTimeStamp(entry->getFlightTime() - adjustedBaseTime);
@ -615,7 +641,7 @@ QString ExtendedDebugLogEntry::getLogString()
{
if (getType() == DebugLogEntry::TYPE_TEXT) {
return QString((const char *)getData().Data);
} else if (getType() == DebugLogEntry::TYPE_UAVOBJECT) {
} else if (getType() == DebugLogEntry::TYPE_UAVOBJECT || getType() == DebugLogEntry::TYPE_MULTIPLEUAVOBJECTS) {
return m_object->toString().replace("\n", " ").replace("\t", " ");
} else {
return "";
@ -631,7 +657,7 @@ void ExtendedDebugLogEntry::toXML(QXmlStreamWriter *xmlWriter, quint32 baseTime)
if (getType() == DebugLogEntry::TYPE_TEXT) {
xmlWriter->writeAttribute("type", "text");
xmlWriter->writeTextElement("message", QString((const char *)getData().Data));
} else if (getType() == DebugLogEntry::TYPE_UAVOBJECT) {
} else if (getType() == DebugLogEntry::TYPE_UAVOBJECT || getType() == DebugLogEntry::TYPE_MULTIPLEUAVOBJECTS) {
xmlWriter->writeAttribute("type", "uavobject");
m_object->toXML(xmlWriter);
}
@ -644,7 +670,7 @@ void ExtendedDebugLogEntry::toCSV(QTextStream *csvStream, quint32 baseTime)
if (getType() == DebugLogEntry::TYPE_TEXT) {
data = QString((const char *)getData().Data);
} else if (getType() == DebugLogEntry::TYPE_UAVOBJECT) {
} else if (getType() == DebugLogEntry::TYPE_UAVOBJECT || getType() == DebugLogEntry::TYPE_MULTIPLEUAVOBJECTS) {
data = m_object->toString().replace("\n", "").replace("\t", "");
}
*csvStream << QString::number(getFlight() + 1) << '\t' << QString::number(getFlightTime() - baseTime) << '\t' << QString::number(getEntry()) << '\t' << data << '\n';
@ -654,7 +680,7 @@ void ExtendedDebugLogEntry::setData(const DebugLogEntry::DataFields &data, UAVOb
{
DebugLogEntry::setData(data);
if (getType() == DebugLogEntry::TYPE_UAVOBJECT) {
if (getType() == DebugLogEntry::TYPE_UAVOBJECT || getType() == DebugLogEntry::TYPE_MULTIPLEUAVOBJECTS) {
UAVDataObject *object = (UAVDataObject *)objectManager->getObject(getObjectID(), getInstanceID());
Q_ASSERT(object);
m_object = object->clone(getInstanceID());

11
ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.h
View File

@ -94,6 +94,10 @@ public slots:
setDirty(true);
if (m_setting != 1 && m_setting != 3) {
setPeriod(0);
} else {
if (!period()) {
setPeriod(500);
}
}
emit settingChanged(setting);
}
@ -178,7 +182,7 @@ class FlightLogManager : public QObject {
Q_PROPERTY(QStringList logSettings READ logSettings NOTIFY logSettingsChanged)
Q_PROPERTY(QStringList logStatuses READ logStatuses NOTIFY logStatusesChanged)
Q_PROPERTY(int loggingEnabled READ loggingEnabled WRITE setLoggingEnabled NOTIFY loggingEnabledChanged)
Q_PROPERTY(int logEntriesCount READ logEntriesCount NOTIFY logEntriesChanged)
public:
explicit FlightLogManager(QObject *parent = 0);
@ -240,7 +244,10 @@ public:
{
return m_loggingEnabled;
}
int logEntriesCount()
{
return m_logEntries.count();
}
signals:
void logEntriesChanged();
void flightEntriesChanged();

6
ground/openpilotgcs/src/plugins/flightlog/functions.js
View File

@ -11,6 +11,12 @@ function millisToTime(ms) {
return pad(hours, 2) + ":" + pad(minutes, 2) + ":" + pad(seconds, 2) + ":" + pad(msleft, 3);
}
function microsToTime(us) {
var ms = Math.floor(us / 1000);
return millisToTime(ms);
}
function pad(number, length) {
var str = '' + number;
while (str.length < length) {

4
ground/openpilotgcs/src/plugins/ophid/inc/ophid_usbmon.h
View File

@ -67,11 +67,7 @@ HIDAPI BOOL WINAPI HidD_FreePreparsedData(PHIDP_PREPARSED_DATA);
HIDAPI BOOL WINAPI HidD_FlushQueue(HANDLE);
HIDAPI BOOL WINAPI HidD_GetConfiguration(HANDLE, PHIDD_CONFIGURATION, ULONG);
HIDAPI BOOL WINAPI HidD_SetConfiguration(HANDLE, PHIDD_CONFIGURATION, ULONG);
HIDAPI BOOL WINAPI HidD_GetNumInputBuffers(HANDLE, PULONG);
HIDAPI BOOL WINAPI HidD_SetNumInputBuffers(HANDLE HidDeviceObject, ULONG);
HIDAPI BOOL WINAPI HidD_GetPhysicalDescriptor(HANDLE, PVOID, ULONG);
HIDAPI BOOL WINAPI HidD_GetManufacturerString(HANDLE, PVOID, ULONG);
HIDAPI BOOL WINAPI HidD_GetProductString(HANDLE, PVOID, ULONG);
HIDAPI BOOL WINAPI HidD_GetIndexedString(HANDLE, ULONG, PVOID, ULONG);
HIDAPI BOOL WINAPI HidD_GetSerialNumberString(HANDLE, PVOID, ULONG);

21
ground/openpilotgcs/src/plugins/serialconnection/serialplugin.cpp
View File

@ -117,12 +117,29 @@ bool sortPorts(const QSerialPortInfo &s1, const QSerialPortInfo &s2)
return s1.portName() < s2.portName();
}
QList<QSerialPortInfo> SerialConnection::availablePorts()
{
QList<QSerialPortInfo> ports = QSerialPortInfo::availablePorts();
#if QT_VERSION == 0x050301 && defined(Q_OS_WIN)
// workaround to QTBUG-39748 (https://bugreports.qt-project.org/browse/QTBUG-39748)
// Qt 5.3.1 reports spurious ports with an empty description...
QMutableListIterator<QSerialPortInfo> i(ports);
while (i.hasNext()) {
if (i.next().description().isEmpty()) {
i.remove();
}
}
#endif
return ports;
}
QList <Core::IConnection::device> SerialConnection::availableDevices()
{
QList <Core::IConnection::device> list;
if (enablePolling) {
QList<QSerialPortInfo> ports = QSerialPortInfo::availablePorts();
QList<QSerialPortInfo> ports = availablePorts();
// sort the list by port number (nice idea from PT_Dreamer :))
qSort(ports.begin(), ports.end(), sortPorts);
@ -143,7 +160,7 @@ QIODevice *SerialConnection::openDevice(const QString &deviceName)
if (serialHandle) {
closeDevice(deviceName);
}
QList<QSerialPortInfo> ports = QSerialPortInfo::availablePorts();
QList<QSerialPortInfo> ports = availablePorts();
foreach(QSerialPortInfo port, ports) {
if (port.portName() == deviceName) {
// don't specify a parent when constructing the QSerialPort as this object will be moved

2
ground/openpilotgcs/src/plugins/serialconnection/serialplugin.h
View File

@ -107,6 +107,8 @@ private:
SerialPluginConfiguration *m_config;
SerialPluginOptionsPage *m_optionspage;
QList<QSerialPortInfo> availablePorts();
protected slots:
void onEnumerationChanged();

2
ground/openpilotgcs/src/python.pri
View File

@ -15,7 +15,7 @@ OPENPILOT_TOOLS_DIR = $$(OPENPILOT_TOOLS_DIR)
PYTHON = \"$$ROOT_DIR/tools/$$PYTHON_DIR/python\"
} else {
# not found, hope it's in the path...
PYTHON = \"python\"
PYTHON = $$(PYTHON)
}
}

3
make/apps-defs.mk
View File

@ -64,7 +64,7 @@ SRC += $(PIOSCOMMON)/pios_etasv3.c
SRC += $(PIOSCOMMON)/pios_gcsrcvr.c
SRC += $(PIOSCOMMON)/pios_hcsr04.c
SRC += $(PIOSCOMMON)/pios_hmc5843.c
SRC += $(PIOSCOMMON)/pios_hmc5883.c
SRC += $(PIOSCOMMON)/pios_hmc5x83.c
SRC += $(PIOSCOMMON)/pios_i2c_esc.c
SRC += $(PIOSCOMMON)/pios_l3gd20.c
SRC += $(PIOSCOMMON)/pios_mpu6000.c
@ -109,6 +109,7 @@ SRC += $(FLIGHTLIB)/CoordinateConversions.c
SRC += $(MATHLIB)/sin_lookup.c
SRC += $(MATHLIB)/pid.c
SRC += $(MATHLIB)/mathmisc.c
SRC += $(MATHLIB)/butterworth.c
SRC += $(FLIGHTLIB)/printf-stdarg.c
## Modules

164
make/tools.mk
View File

@ -9,8 +9,8 @@
# Ready to use:
# arm_sdk_install
# qt_sdk_install
# mingw_install (Windows only - NOT USED for Qt-5.1.x)
# python_install (Windows only - NOT USED for Qt-5.1.x)
# mingw_install (Windows only - NOT USED for Qt-5.3.x)
# python_install (Windows only - NOT USED for Qt-5.3.x)
# nsis_install (Windows only)
# sdl_install (Windows only)
# openssl_install (Windows only)
@ -61,14 +61,14 @@ ifeq ($(UNAME), Linux)
ifeq ($(ARCH), x86_64)
ARM_SDK_URL := https://launchpad.net/gcc-arm-embedded/4.8/4.8-2014-q1-update/+download/gcc-arm-none-eabi-4_8-2014q1-20140314-linux.tar.bz2
ARM_SDK_MD5_URL:= https://launchpad.net/gcc-arm-embedded/4.8/4.8-2014-q1-update/+download/gcc-arm-none-eabi-4_8-2014q1-20140314-linux.tar.bz2/+md5
QT_SDK_URL := http://download.qt-project.org/official_releases/qt/5.2/5.2.1/qt-opensource-linux-x64-5.2.1.run
QT_SDK_MD5_URL := http://wiki.openpilot.org/download/attachments/18612236/qt-opensource-linux-x64-5.2.1.run.md5
QT_SDK_URL := http://download.qt-project.org/official_releases/qt/5.3/5.3.1/qt-opensource-linux-x64-5.3.1.run
QT_SDK_MD5_URL := http://download.qt-project.org/official_releases/qt/5.3/5.3.1/qt-opensource-linux-x64-5.3.1.run.md5
QT_SDK_ARCH := gcc_64
else
ARM_SDK_URL := https://launchpad.net/gcc-arm-embedded/4.8/4.8-2014-q1-update/+download/gcc-arm-none-eabi-4_8-2014q1-20140314-linux.tar.bz2
ARM_SDK_MD5_URL := https://launchpad.net/gcc-arm-embedded/4.8/4.8-2014-q1-update/+download/gcc-arm-none-eabi-4_8-2014q1-20140314-linux.tar.bz2/+md5
QT_SDK_URL := http://download.qt-project.org/official_releases/qt/5.2/5.2.1/qt-opensource-linux-x86-5.2.1.run
QT_SDK_MD5_URL := http://wiki.openpilot.org/download/attachments/18612236/qt-opensource-linux-x86-5.2.1.run.md5
QT_SDK_URL := http://download.qt-project.org/official_releases/qt/5.3/5.3.1/qt-opensource-linux-x86-5.3.1.run
QT_SDK_MD5_URL := http://download.qt-project.org/official_releases/qt/5.3/5.3.1/qt-opensource-linux-x86-5.3.1.run.md5
QT_SDK_ARCH := gcc
endif
UNCRUSTIFY_URL := http://wiki.openpilot.org/download/attachments/18612236/uncrustify-0.60.tar.gz
@ -76,15 +76,20 @@ ifeq ($(UNAME), Linux)
else ifeq ($(UNAME), Darwin)
ARM_SDK_URL := https://launchpad.net/gcc-arm-embedded/4.8/4.8-2014-q1-update/+download/gcc-arm-none-eabi-4_8-2014q1-20140314-mac.tar.bz2
ARM_SDK_MD5_URL:= https://launchpad.net/gcc-arm-embedded/4.8/4.8-2014-q1-update/+download/gcc-arm-none-eabi-4_8-2014q1-20140314-mac.tar.bz2/+md5
QT_SDK_URL := "Please install native Qt 5.1.x SDK using package manager"
QT_SDK_URL := http://download.qt-project.org/official_releases/qt/5.3/5.3.1/qt-opensource-mac-x64-clang-5.3.1.dmg
QT_SDK_MD5_URL := http://download.qt-project.org/official_releases/qt/5.3/5.3.1/qt-opensource-mac-x64-clang-5.3.1.dmg.md5
QT_SDK_ARCH := clang_64
QT_SDK_MAINTENANCE_TOOL := /Volumes/qt-opensource-mac-x64-clang-5.3.1/qt-opensource-mac-x64-clang-5.3.1.app/Contents/MacOS/qt-opensource-mac-x64-clang-5.3.1
QT_SDK_MOUNT_DIR := /Volumes/qt-opensource-mac-x64-clang-5.3.1
QT_SDK_INSTALLER_DAT := /Volumes/qt-opensource-mac-x64-clang-5.3.1/qt-opensource-mac-x64-clang-5.3.1.app/Contents/Resources/installer.dat
UNCRUSTIFY_URL := http://wiki.openpilot.org/download/attachments/18612236/uncrustify-0.60.tar.gz
DOXYGEN_URL := http://wiki.openpilot.org/download/attachments/18612236/doxygen-1.8.3.1.src.tar.gz
else ifeq ($(UNAME), Windows)
ARM_SDK_URL := https://launchpad.net/gcc-arm-embedded/4.8/4.8-2014-q1-update/+download/gcc-arm-none-eabi-4_8-2014q1-20140314-win32.zip
ARM_SDK_MD5_URL:= https://launchpad.net/gcc-arm-embedded/4.8/4.8-2014-q1-update/+download/gcc-arm-none-eabi-4_8-2014q1-20140314-win32.zip/+md5
QT_SDK_URL := http://download.qt-project.org/official_releases/qt/5.2/5.2.1/qt-opensource-windows-x86-mingw48_opengl-5.2.1.exe
QT_SDK_MD5_URL := http://wiki.openpilot.org/download/attachments/18612236/qt-opensource-windows-x86-mingw48_opengl-5.2.1.exe.md5
QT_SDK_ARCH := mingw48_32
QT_SDK_URL := http://download.qt-project.org/official_releases/qt/5.3/5.3.1/qt-opensource-windows-x86-mingw482_opengl-5.3.1.exe
QT_SDK_MD5_URL := http://download.qt-project.org/official_releases/qt/5.3/5.3.1/qt-opensource-windows-x86-mingw482_opengl-5.3.1.exe.md5
QT_SDK_ARCH := mingw482_32
NSIS_URL := http://wiki.openpilot.org/download/attachments/18612236/nsis-2.46-unicode.tar.bz2
SDL_URL := http://wiki.openpilot.org/download/attachments/18612236/SDL-devel-1.2.15-mingw32.tar.gz
OPENSSL_URL := http://wiki.openpilot.org/download/attachments/18612236/openssl-1.0.1e-win32.tar.bz2
@ -97,7 +102,7 @@ GTEST_URL := http://wiki.openpilot.org/download/attachments/18612236/gtest-1.6.0
# Changing PYTHON_DIR, also update it in ground/openpilotgcs/src/python.pri
ARM_SDK_DIR := $(TOOLS_DIR)/gcc-arm-none-eabi-4_8-2014q1
QT_SDK_DIR := $(TOOLS_DIR)/qt-5.2.1
QT_SDK_DIR := $(TOOLS_DIR)/qt-5.3.1
MINGW_DIR := $(QT_SDK_DIR)/Tools/mingw48_32
PYTHON_DIR := $(QT_SDK_DIR)/Tools/mingw48_32/opt/bin
NSIS_DIR := $(TOOLS_DIR)/nsis-2.46-unicode
@ -106,7 +111,15 @@ OPENSSL_DIR := $(TOOLS_DIR)/openssl-1.0.1e-win32
UNCRUSTIFY_DIR := $(TOOLS_DIR)/uncrustify-0.60
DOXYGEN_DIR := $(TOOLS_DIR)/doxygen-1.8.3.1
GTEST_DIR := $(TOOLS_DIR)/gtest-1.6.0
MESAWIN_DIR := $(TOOLS_DIR)/mesawin
ifeq ($(UNAME), Windows)
MINGW_DIR := $(QT_SDK_DIR)/Tools/$(QT_SDK_ARCH)
PYTHON_DIR := $(QT_SDK_DIR)/Tools/$(QT_SDK_ARCH)/opt/bin
NSIS_DIR := $(TOOLS_DIR)/nsis-2.46-unicode
SDL_DIR := $(TOOLS_DIR)/SDL-1.2.15
OPENSSL_DIR := $(TOOLS_DIR)/openssl-1.0.1e-win32
MESAWIN_DIR := $(TOOLS_DIR)/mesawin
endif
QT_SDK_PREFIX := $(QT_SDK_DIR)
@ -353,14 +366,14 @@ qt_sdk_install: qt_sdk_clean | $(DL_DIR) $(TOOLS_DIR)
$(V1) $(DL_DIR)/$(5) --dump-binary-data -o $(1)
# Extract packages under tool directory
$(V1) $(MKDIR) -p $$(call toprel, $(dir $(2)))
$(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.readme/1.0.0qt-project-url.7z" | grep -v Extracting
$(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt/1.0.0ThirdPartySoftware_Listing.7z" | grep -v Extracting
$(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.readme/1.0.0readme.7z" | grep -v Extracting
$(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.521.win32_mingw48.essentials/5.2.1mingw48_essentials.7z" | grep -v Extracting
$(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.521.win32_mingw48.essentials/5.2.1x32-4.8.0-release-posix-dwarf-rev2-runtime.7z" | grep -v Extracting
$(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.521.win32_mingw48.essentials/5.2.1icu_51_1_mingw_builds_4_8_0_posix_dwarf_32.7z" | grep -v Extracting
$(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.521.win32_mingw48.addons/5.2.1mingw48_addons.7z" | grep -v Extracting
$(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.tools.win32_mingw48/4.8.0-1-1x32-4.8.0-release-posix-dwarf-rev2.7z" | grep -v Extracting
$(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.readme/1.0.0-0qt-project-url.7z" | grep -v Extracting
$(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt/5.3.1ThirdPartySoftware_Listing.7z" | grep -v Extracting
$(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.readme/1.0.0-0readme.7z" | grep -v Extracting
$(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.53.win32_mingw482/5.3.1-0qt5_essentials.7z" | grep -v Extracting
$(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.53.win32_mingw482/5.3.1-0i686-4.8.2-release-posix-dwarf-rt_v3-rev3-runtime.7z" | grep -v Extracting
$(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.53.win32_mingw482/5.3.1-0icu_52_1_mingw_builds_32_4_8_2_posix_dwarf.7z" | grep -v Extracting
$(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.53.win32_mingw482/5.3.1-0qt5_addons.7z" | grep -v Extracting
$(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.tools.win32_mingw482/4.8.2i686-4.8.2-release-posix-dwarf-rt_v3-rev3.7z" | grep -v Extracting
# Run patcher
@$(ECHO)
@$(ECHO) "Executing QtPatch in" $$(call toprel, $(QT_SDK_PREFIX))
@ -420,15 +433,15 @@ qt_sdk_install: qt_sdk_clean | $(DL_DIR) $(TOOLS_DIR)
$(V1) $(DL_DIR)/$(5) --dump-binary-data -o $(1)
# Extract packages under tool directory
$(V1) $(MKDIR) -p $$(call toprel, $(dir $(2)))
$(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.readme/1.0.0qt-project-url.7z" | grep -v Extracting
$(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt/1.0.0ThirdPartySoftware_Listing.7z" | grep -v Extracting
$(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.readme/1.0.0readme.7z" | grep -v Extracting
$(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.521.$(6).essentials/5.2.1$(6)_qt5_essentials.7z" | grep -v Extracting
$(V1) if [ -f "$(1)/qt.521.$(6).essentials/5.2.1icu_51_1_ubuntu_11_10_64.7z" ]; then $(SEVENZIP) -y -o$(2) x "$(1)/qt.521.$(6).essentials/5.2.1icu_51_1_ubuntu_11_10_64.7z" | grep -v Extracting; fi
$(V1) if [ -f "$(1)/qt.521.$(6).essentials/5.2.1icu_51_1_ubuntu_11_10_32.7z" ]; then $(SEVENZIP) -y -o$(2) x "$(1)/qt.521.$(6).essentials/5.2.1icu_51_1_ubuntu_11_10_32.7z" | grep -v Extracting; fi
# $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.521.$(6).essentials/5.2.1icu_path_patcher.sh.7z" | grep -v Extracting
$(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.521.$(6).addons/5.2.1$(6)_qt5_addons.7z" | grep -v Extracting
# go to OpenPilot/tools/5.1.1/gcc_64 and call patcher.sh
$(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.readme/1.0.0-0qt-project-url.7z" | grep -v Extracting
$(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt/5.3.1ThirdPartySoftware_Listing.7z" | grep -v Extracting
$(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.readme/1.0.0-0readme.7z" | grep -v Extracting
$(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.53.$(6)/5.3.1-0qt5_essentials.7z" | grep -v Extracting
$(V1) if [ -f "$(1)/qt.53.$(6)/5.3.1-0icu_51_1_ubuntu_11_10_64.7z" ]; then $(SEVENZIP) -y -o$(2) x "$(1)/qt.53.$(6)/5.3.1-0icu_51_1_ubuntu_11_10_64.7z" | grep -v Extracting; fi
$(V1) if [ -f "$(1)/qt.53.$(6)/5.3.1-0icu_51_1_ubuntu_11_10_32.7z" ]; then $(SEVENZIP) -y -o$(2) x "$(1)/qt.53.$(6)/5.3.1-0icu_51_1_ubuntu_11_10_32.7z" | grep -v Extracting; fi
# $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.53.$(6).essentials/5.3.1icu_path_patcher.sh.7z" | grep -v Extracting
$(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.53.$(6)/5.3.1-0qt5_addons.7z" | grep -v Extracting
# go to OpenPilot/tools/5.3.1/gcc_64 and call patcher.sh
@$(ECHO)
@$(ECHO) "Running patcher in" $$(call toprel, $(QT_SDK_PREFIX))
$(V1) $(CD) $(QT_SDK_PREFIX)
@ -459,6 +472,79 @@ qt_sdk_distclean:
endef
##############################
#
# Mac QT install template
# $(1) = tool temp extract/build directory
# $(2) = tool install directory
# $(3) = tool distribution URL
# $(4) = tool distribution .md5 URL
# $(5) = tool distribution file
# $(6) = QT architecture
# $(7) = optional extra build recipes template
# $(8) = optional extra clean recipes template
#
##############################
define MAC_QT_INSTALL_TEMPLATE
.PHONY: $(addprefix qt_sdk_, install clean distclean)
qt_sdk_install: qt_sdk_clean | $(DL_DIR) $(TOOLS_DIR)
$(V1) if ! $(SEVENZIP) >/dev/null 2>&1; then \
$(ECHO) $(MSG_NOTICE) "Please install the p7zip for your distribution. i.e.: sudo brew install p7zip." && \
exit 1; \
fi
$(call DOWNLOAD_TEMPLATE,$(3),$(5),"$(4)")
# Mount .dmg file
$(V1) hdiutil attach -nobrowse $(DL_DIR)/$(5)
# Explode .dmg file into install packages
@$(ECHO) $(MSG_EXTRACTING) $$(call toprel, $(1))
$(V1) $(MKDIR) -p $$(call toprel, $(dir $(1)))
$(V1) $(QT_SDK_MAINTENANCE_TOOL) --dump-binary-data -i $(QT_SDK_INSTALLER_DAT) -o $(1)
# Extract packages under tool directory
$(V1) $(MKDIR) -p $$(call toprel, $(dir $(2)))
$(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.readme/1.0.0-0qt-project-url.7z" | grep -v Extracting
$(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt/5.3.1ThirdPartySoftware_Listing.7z" | grep -v Extracting
$(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.readme/1.0.0-0readme.7z" | grep -v Extracting
$(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.53.$(6)/5.3.1-0qt5_essentials.7z" | grep -v Extracting
# $(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.53.$(6).essentials/5.3.1icu_path_patcher.sh.7z" | grep -v Extracting
$(V1) $(SEVENZIP) -y -o$(2) x "$(1)/qt.53.$(6)/5.3.1-0qt5_addons.7z" | grep -v Extracting
# go to OpenPilot/tools/5.3.1/gcc_64 and call patcher.sh
@$(ECHO)
@$(ECHO) "Running patcher in" $$(call toprel, $(QT_SDK_PREFIX))
$(V1) $(CD) $(QT_SDK_PREFIX)
# $(V1) "$(QT_SDK_PREFIX)/patcher.sh" $(QT_SDK_PREFIX)
# call qmake patcher
@$(ECHO) "Executing QtPatch in" $$(call toprel, $(QT_SDK_PREFIX))
$(V1) $(QT_SDK_MAINTENANCE_TOOL) --runoperation QtPatch mac $(QT_SDK_PREFIX) qt5
#Unmount the .dmg file
$(V1) hdiutil detach $(QT_SDK_MOUNT_DIR)
# Execute post build templates
$(7)
# Clean up temporary files
@$(ECHO) $(MSG_CLEANING) $$(call toprel, $(1))
$(V1) [ ! -d "$(1)" ] || $(RM) -rf "$(1)"
qt_sdk_clean:
@$(ECHO) $(MSG_CLEANING) $$(call toprel, $(1))
$(V1) [ ! -d "$(1)" ] || $(RM) -rf "$(1)"
@$(ECHO) $(MSG_CLEANING) $$(call toprel, "$(2)")
$(V1) [ ! -d "$(2)" ] || $(RM) -rf "$(2)"
$(8)
qt_sdk_distclean:
@$(ECHO) $(MSG_DISTCLEANING) $$(call toprel, $(DL_DIR)/$(5))
$(V1) [ ! -f "$(DL_DIR)/$(5)" ] || $(RM) "$(DL_DIR)/$(5)"
$(V1) [ ! -f "$(DL_DIR)/$(5).md5" ] || $(RM) "$(DL_DIR)/$(5).md5"
endef
##############################
#
# ARM SDK
@ -496,13 +582,11 @@ endef
#
# Qt SDK
#
# Mac OS X: user should install native Qt SDK package
#
##############################
ifeq ($(UNAME), Windows)
QT_SDK_PREFIX := $(QT_SDK_DIR)/5.2.1/$(QT_SDK_ARCH)
QT_SDK_PREFIX := $(QT_SDK_DIR)/5.3/$(QT_SDK_ARCH)
# This additional configuration step should not be necessary
# but it is needed as a workaround to https://bugreports.qt-project.org/browse/QTBUG-33254
@ -517,10 +601,16 @@ QT_BUILD_DIR := $(BUILD_DIR)/QT_BUILD
else ifeq ($(UNAME), Linux)
QT_SDK_PREFIX := "$(QT_SDK_DIR)/5.2.1/$(QT_SDK_ARCH)"
QT_SDK_PREFIX := "$(QT_SDK_DIR)/5.3/$(QT_SDK_ARCH)"
QT_BUILD_DIR := $(BUILD_DIR)/QT_BUILD
$(eval $(call LINUX_QT_INSTALL_TEMPLATE,$(QT_BUILD_DIR),$(QT_SDK_DIR),$(QT_SDK_URL),$(QT_SDK_MD5_URL),$(notdir $(QT_SDK_URL)),$(QT_SDK_ARCH)))
else ifeq ($(UNAME), Darwin)
QT_SDK_PREFIX := "$(QT_SDK_DIR)/5.3/$(QT_SDK_ARCH)"
QT_BUILD_DIR := $(BUILD_DIR)/QT_BUILD
$(eval $(call MAC_QT_INSTALL_TEMPLATE,$(QT_BUILD_DIR),$(QT_SDK_DIR),$(QT_SDK_URL),$(QT_SDK_MD5_URL),$(notdir $(QT_SDK_URL)),$(QT_SDK_ARCH)))
else
QT_SDK_PREFIX := $(QT_SDK_DIR)
@ -528,7 +618,7 @@ QT_SDK_PREFIX := $(QT_SDK_DIR)
.PHONY: qt_sdk_install
qt_sdk_install:
@$(ECHO) $(MSG_NOTICE) --------------------------------------------------------
@$(ECHO) $(MSG_NOTICE) Please install native Qt 5.2.x SDK using package manager
@$(ECHO) $(MSG_NOTICE) Please install native Qt 5.3.x SDK using package manager
@$(ECHO) $(MSG_NOTICE) --------------------------------------------------------
.PHONY: qt_sdk_clean
@ -600,7 +690,11 @@ ifeq ($(shell [ -d "$(PYTHON_DIR)" ] && $(ECHO) "exists"), exists)
else
# not installed, hope it's in the path...
# $(info $(EMPTY) WARNING $(call toprel, $(PYTHON_DIR)) not found, using system PATH)
export PYTHON := python
ifeq ($(findstring Python 2,$(shell python --version)), Python 2)
export PYTHON := python
else
export PYTHON := python2
endif
endif
.PHONY: python_version

3
shared/uavobjectdefinition/airspeedsettings.xml
View File

@ -5,7 +5,8 @@
<field name="ZeroPoint" units="raw" type="uint16" elements="1" defaultvalue="0"/>
<field name="Scale" units="raw" type="float" elements="1" defaultvalue="1.0"/>
<field name="AirspeedSensorType" units="" type="enum" elements="1" options="PixHawkAirspeedMS4525DO,EagleTreeAirspeedV3,DIYDronesMPXV5004,DIYDronesMPXV7002,GroundSpeedBasedWindEstimation,None" defaultvalue="None"/>
<field name="GroundSpeedBasedEstimationLowPassAlpha" units="" type="float" elements="1" defaultvalue="0.08" />
<field name="IMUBasedEstimationLowPassPeriod1" units="s" type="float" elements="1" defaultvalue="0.5" />
<field name="IMUBasedEstimationLowPassPeriod2" units="s" type="float" elements="1" defaultvalue="10" />
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>

4
shared/uavobjectdefinition/debuglogentry.xml
View File

@ -2,9 +2,9 @@
<object name="DebugLogEntry" singleinstance="true" settings="false" category="System">
<description>Log Entry in Flash</description>
<field name="Flight" units="" type="uint16" elements="1" />
<field name="FlightTime" units="ms" type="uint32" elements="1" />
<field name="FlightTime" units="us" type="uint32" elements="1" />
<field name="Entry" units="" type="uint16" elements="1" />
<field name="Type" units="" type="enum" elements="1" options="Empty, Text, UAVObject" />
<field name="Type" units="" type="enum" elements="1" options="Empty, Text, UAVObject, MultipleUAVObjects" />
<field name="ObjectID" units="" type="uint32" elements="1"/>
<field name="InstanceID" units="" type="uint16" elements="1"/>
<field name="Size" units="" type="uint16" elements="1" />

4
shared/uavobjectdefinition/gpssatellites.xml
View File

@ -3,8 +3,8 @@
<description>Contains information about the GPS satellites in view from @ref GPSModule.</description>
<field name="SatsInView" units="" type="int8" elements="1"/>
<field name="PRN" units="" type="int8" elements="16"/>
<field name="Elevation" units="degrees" type="float" elements="16"/>
<field name="Azimuth" units="degrees" type="float" elements="16"/>
<field name="Elevation" units="degrees" type="int8" elements="16"/>
<field name="Azimuth" units="degrees" type="int16" elements="16"/>
<field name="SNR" units="" type="int8" elements="16"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>

15
shared/uavobjectdefinition/txpidsettings.xml
View File

@ -11,13 +11,14 @@
<field name="PIDs" units="option" type="enum"
elementnames="Instance1,Instance2,Instance3"
options="Disabled,
Roll Rate.Kp, Pitch Rate.Kp, Roll+Pitch Rate.Kp, Yaw Rate.Kp,
Roll Rate.Ki, Pitch Rate.Ki, Roll+Pitch Rate.Ki, Yaw Rate.Ki,
Roll Rate.Kd, Pitch Rate.Kd, Roll+Pitch Rate.Kd, Yaw Rate.Kd,
Roll Rate.ILimit, Pitch Rate.ILimit, Roll+Pitch Rate.ILimit, Yaw Rate.ILimit,
Roll Attitude.Kp, Pitch Attitude.Kp, Roll+Pitch Attitude.Kp, Yaw Attitude.Kp,
Roll Attitude.Ki, Pitch Attitude.Ki, Roll+Pitch Attitude.Ki, Yaw Attitude.Ki,
Roll Attitude.ILimit, Pitch Attitude.ILimit, Roll+Pitch Attitude.ILimit, Yaw Attitude.ILimit,
Roll Rate.Kp, Roll Rate.Ki, Roll Rate.Kd, Roll Rate.ILimit, Roll Rate.Resp,
Pitch Rate.Kp, Pitch Rate.Ki, Pitch Rate.Kd, Pitch Rate.ILimit, Pitch Rate.Resp,
Roll+Pitch Rate.Kp, Roll+Pitch Rate.Ki, Roll+Pitch Rate.Kd, Roll+Pitch Rate.ILimit, Roll+Pitch Rate.Resp,
Yaw Rate.Kp, Yaw Rate.Ki, Yaw Rate.Kd, Yaw Rate.ILimit, Yaw Rate.Resp,
Roll Attitude.Kp, Roll Attitude.Ki, Roll Attitude.ILimit, Roll Attitude.Resp,
Pitch Attitude.Kp, Pitch Attitude.Ki, Pitch Attitude.ILimit, Pitch Attitude.Resp,
Roll+Pitch Attitude.Kp, Roll+Pitch Attitude.Ki, Roll+Pitch Attitude.ILimit, Roll+Pitch Attitude.Resp,
Yaw Attitude.Kp, Yaw Attitude.Ki, Yaw Attitude.ILimit, Yaw Attitude.Resp,
GyroTau"
defaultvalue="Disabled"/>
<field name="MinPID" units="" type="float" elementnames="Instance1,Instance2,Instance3" defaultvalue="0"/>