rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

OP-908 added pios_math.h and pios_helper.h to contains all the various math and general macros/constants. Fixed remaining constants still defined as double.

Browse Source 
+review OPReview-436
This commit is contained in:
Alessio Morale 2013-04-09 01:07:59 +02:00
parent 71de38f2d5
commit cc6d19e40a
13 changed files with 150 additions and 86 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
flight/Libraries/CoordinateConversions.c
View File

@ -29,11 +29,10 @@
#include <math.h> #include <math.h>
#include <stdint.h> #include <stdint.h>
#include <pios_math.h>
#include "CoordinateConversions.h" #include "CoordinateConversions.h"
#define F_PI ((float)M_PI) #define MIN_ALLOWABLE_MAGNITUDE 1e-30f
#define RAD2DEG (180.0f/ F_PI)
#define DEG2RAD (F_PI /180.0f)
// ****** convert Lat,Lon,Alt to ECEF ************ // ****** convert Lat,Lon,Alt to ECEF ************
void LLA2ECEF(float LLA[3], float ECEF[3]) void LLA2ECEF(float LLA[3], float ECEF[3])
@ -43,10 +42,10 @@ void LLA2ECEF(float LLA[3], float ECEF[3])
float sinLat, sinLon, cosLat, cosLon; float sinLat, sinLon, cosLat, cosLon;
float N; float N;
sinLat = sinf(DEG2RAD * LLA[0]); sinLat = sinf(DEG2RAD(LLA[0]));
sinLon = sinf(DEG2RAD * LLA[1]); sinLon = sinf(DEG2RAD(LLA[1]));
cosLat = cosf(DEG2RAD * LLA[0]); cosLat = cosf(DEG2RAD(LLA[0]));
cosLon = cosf(DEG2RAD * LLA[1]); cosLon = cosf(DEG2RAD(LLA[1]));
N = a / sqrtf(1.0f - e * e * sinLat * sinLat); //prime vertical radius of curvature N = a / sqrtf(1.0f - e * e * sinLat * sinLat); //prime vertical radius of curvature
@ -75,8 +74,8 @@ uint16_t ECEF2LLA(float ECEF[3], float LLA[3])
#define MAX_ITER 10 // should not take more than 5 for valid coordinates #define MAX_ITER 10 // should not take more than 5 for valid coordinates
#define ACCURACY 1.0e-11f // used to be e-14, but we don't need sub micrometer exact calculations #define ACCURACY 1.0e-11f // used to be e-14, but we don't need sub micrometer exact calculations
LLA[1] = RAD2DEG * atan2f(y, x); LLA[1] = RAD2DEG(atan2f(y, x));
Lat = DEG2RAD * LLA[0]; Lat = DEG2RAD(LLA[0]);
esLat = e * sinf(Lat); esLat = e * sinf(Lat);
N = a / sqrtf(1 - esLat * esLat); N = a / sqrtf(1 - esLat * esLat);
NplusH = N + LLA[2]; NplusH = N + LLA[2];
@ -93,7 +92,7 @@ uint16_t ECEF2LLA(float ECEF[3], float LLA[3])
iter += 1; iter += 1;
} }
LLA[0] = RAD2DEG * Lat; LLA[0] = RAD2DEG(Lat);
LLA[2] = NplusH - N; LLA[2] = NplusH - N;
return (iter < MAX_ITER); return (iter < MAX_ITER);
@ -104,10 +103,10 @@ void RneFromLLA(float LLA[3], float Rne[3][3])
{ {
float sinLat, sinLon, cosLat, cosLon; float sinLat, sinLon, cosLat, cosLon;
sinLat = (float)sinf(DEG2RAD * LLA[0]); sinLat = (float)sinf(DEG2RAD(LLA[0]));
sinLon = (float)sinf(DEG2RAD * LLA[1]); sinLon = (float)sinf(DEG2RAD(LLA[1]));
cosLat = (float)cosf(DEG2RAD * LLA[0]); cosLat = (float)cosf(DEG2RAD(LLA[0]));
cosLon = (float)cosf(DEG2RAD * LLA[1]); cosLon = (float)cosf(DEG2RAD(LLA[1]));
Rne[0][0] = -sinLat * cosLon; Rne[0][0] = -sinLat * cosLon;
Rne[0][1] = -sinLat * sinLon; Rne[0][1] = -sinLat * sinLon;
@ -135,9 +134,9 @@ void Quaternion2RPY(const float q[4], float rpy[3])
R23 = 2.0f * (q[2] * q[3] + q[0] * q[1]); R23 = 2.0f * (q[2] * q[3] + q[0] * q[1]);
R33 = q0s - q1s - q2s + q3s; R33 = q0s - q1s - q2s + q3s;
rpy[1] = RAD2DEG * asinf(-R13); // pitch always between -pi/2 to pi/2 rpy[1] = RAD2DEG(asinf(-R13)); // pitch always between -pi/2 to pi/2
rpy[2] = RAD2DEG * atan2f(R12, R11); rpy[2] = RAD2DEG(atan2f(R12, R11));
rpy[0] = RAD2DEG * atan2f(R23, R33); rpy[0] = RAD2DEG(atan2f(R23, R33));
//TODO: consider the cases where |R13| ~= 1, |pitch| ~= pi/2 //TODO: consider the cases where |R13| ~= 1, |pitch| ~= pi/2
} }
@ -148,9 +147,9 @@ void RPY2Quaternion(const float rpy[3], float q[4])
float phi, theta, psi; float phi, theta, psi;
float cphi, sphi, ctheta, stheta, cpsi, spsi; float cphi, sphi, ctheta, stheta, cpsi, spsi;
phi = DEG2RAD * rpy[0] / 2; phi = DEG2RAD(rpy[0] / 2);
theta = DEG2RAD * rpy[1] / 2; theta = DEG2RAD(rpy[1] / 2);
psi = DEG2RAD * rpy[2] / 2; psi = DEG2RAD(rpy[2] / 2);
cphi = cosf(phi); cphi = cosf(phi);
sphi = sinf(phi); sphi = sinf(phi);
ctheta = cosf(theta); ctheta = cosf(theta);
@ -294,13 +293,13 @@ uint8_t RotFrom2Vectors(const float v1b[3], const float v1e[3], const float v2b[
// The first rows of rot matrices chosen in direction of v1 // The first rows of rot matrices chosen in direction of v1
mag = VectorMagnitude(v1b); mag = VectorMagnitude(v1b);
if (fabsf(mag) < 1e-30f) if (fabsf(mag) < MIN_ALLOWABLE_MAGNITUDE)
return (-1); return (-1);
for (i=0;i<3;i++) for (i=0;i<3;i++)
Rib[0][i]=v1b[i]/mag; Rib[0][i]=v1b[i]/mag;
mag = VectorMagnitude(v1e); mag = VectorMagnitude(v1e);
if (fabsf(mag) < 1e-30f) if (fabsf(mag) < MIN_ALLOWABLE_MAGNITUDE)
return (-1); return (-1);
for (i=0;i<3;i++) for (i=0;i<3;i++)
Rie[0][i]=v1e[i]/mag; Rie[0][i]=v1e[i]/mag;
@ -308,14 +307,14 @@ uint8_t RotFrom2Vectors(const float v1b[3], const float v1e[3], const float v2b[
// The second rows of rot matrices chosen in direction of v1xv2 // The second rows of rot matrices chosen in direction of v1xv2
CrossProduct(v1b,v2b,&Rib[1][0]); CrossProduct(v1b,v2b,&Rib[1][0]);
mag = VectorMagnitude(&Rib[1][0]); mag = VectorMagnitude(&Rib[1][0]);
if (fabsf(mag) < 1e-30f) if (fabsf(mag) < MIN_ALLOWABLE_MAGNITUDE)
return (-1); return (-1);
for (i=0;i<3;i++) for (i=0;i<3;i++)
Rib[1][i]=Rib[1][i]/mag; Rib[1][i]=Rib[1][i]/mag;
CrossProduct(v1e,v2e,&Rie[1][0]); CrossProduct(v1e,v2e,&Rie[1][0]);
mag = VectorMagnitude(&Rie[1][0]); mag = VectorMagnitude(&Rie[1][0]);
if (fabsf(mag) < 1e-30f) if (fabsf(mag) < MIN_ALLOWABLE_MAGNITUDE)
return (-1); return (-1);
for (i=0;i<3;i++) for (i=0;i<3;i++)
Rie[1][i]=Rie[1][i]/mag; Rie[1][i]=Rie[1][i]/mag;

4
flight/Libraries/inc/WMMInternal.h
View File

@ -26,6 +26,7 @@
#ifndef WMMINTERNAL_H_ #ifndef WMMINTERNAL_H_
#define WMMINTERNAL_H_ #define WMMINTERNAL_H_
#include <pios_math.h>
// internal constants // internal constants
#define TRUE ((uint16_t)1) #define TRUE ((uint16_t)1)
@ -35,9 +36,6 @@
#define NUMTERMS 91 // ((WMM_MAX_MODEL_DEGREES+1)*(WMM_MAX_MODEL_DEGREES+2)/2); #define NUMTERMS 91 // ((WMM_MAX_MODEL_DEGREES+1)*(WMM_MAX_MODEL_DEGREES+2)/2);
#define NUMPCUP 92 // NUMTERMS +1 #define NUMPCUP 92 // NUMTERMS +1
#define NUMPCUPS 13 // WMM_MAX_MODEL_DEGREES +1 #define NUMPCUPS 13 // WMM_MAX_MODEL_DEGREES +1
#define M_PI_F ((float)M_PI)
#define RAD2DEG(rad) ((rad)*(180.0f/M_PI_F))
#define DEG2RAD(deg) ((deg)*(M_PI_F/180.0f))
// internal structure definitions // internal structure definitions
typedef struct { typedef struct {

8
flight/Libraries/math/sin_lookup.c
View File

@ -32,8 +32,8 @@
#include "math.h" #include "math.h"
#include "stdbool.h" #include "stdbool.h"
#include "stdint.h" #include "stdint.h"
#include <pios_math.h>
#define F_PI ((float)M_PI)
#define FLASH_TABLE #define FLASH_TABLE
#ifdef FLASH_TABLE #ifdef FLASH_TABLE
/** Version of the code which precomputes the lookup table in flash **/ /** Version of the code which precomputes the lookup table in flash **/
@ -82,7 +82,7 @@ int sin_lookup_initalize()
return -1; return -1;
for(uint32_t i = 0; i < 180; i++) for(uint32_t i = 0; i < 180; i++)
sin_table[i] = sinf((float)i * 2 * F_PI / 360.0f); sin_table[i] = sinf((float)i * 2 * M_PI_F / 360.0f);
return 0; return 0;
} }
@ -127,7 +127,7 @@ float cos_lookup_deg(float angle)
*/ */
float sin_lookup_rad(float angle) float sin_lookup_rad(float angle)
{ {
int degrees = angle * 180.0f / F_PI; int degrees = angle * 180.0f / M_PI_F;
return sin_lookup_deg(degrees); return sin_lookup_deg(degrees);
} }
@ -138,6 +138,6 @@ float sin_lookup_rad(float angle)
*/ */
float cos_lookup_rad(float angle) float cos_lookup_rad(float angle)
{ {
int degrees = angle * 180.0f / F_PI; int degrees = angle * 180.0f / M_PI_F;
return cos_lookup_deg(degrees); return cos_lookup_deg(degrees);
} }

6
flight/Libraries/paths.c
View File

@ -94,7 +94,7 @@ static void path_endpoint( float * start_point, float * end_point, float * cur_p
dist_diff = sqrtf( diff_north * diff_north + diff_east * diff_east ); dist_diff = sqrtf( diff_north * diff_north + diff_east * diff_east );
dist_path = sqrtf( path_north * path_north + path_east * path_east ); dist_path = sqrtf( path_north * path_north + path_east * path_east );
if(dist_diff < 1e-6 ) { if(dist_diff < 1e-6f ) {
status->fractional_progress = 1; status->fractional_progress = 1;
status->error = 0; status->error = 0;
status->path_direction[0] = status->path_direction[1] = 0; status->path_direction[0] = status->path_direction[1] = 0;
@ -135,7 +135,7 @@ static void path_vector( float * start_point, float * end_point, float * cur_poi
dot = path_north * diff_north + path_east * diff_east; dot = path_north * diff_north + path_east * diff_east;
dist_path = sqrtf( path_north * path_north + path_east * path_east ); dist_path = sqrtf( path_north * path_north + path_east * path_east );
if(dist_path < 1e-6) { if(dist_path < 1e-6f) {
// if the path is too short, we cannot determine vector direction. // if the path is too short, we cannot determine vector direction.
// Fly towards the endpoint to prevent flying away, // Fly towards the endpoint to prevent flying away,
// but assume progress=1 either way. // but assume progress=1 either way.
@ -188,7 +188,7 @@ static void path_circle(float * start_point, float * end_point, float * cur_poin
radius = sqrtf( radius_north * radius_north + radius_east * radius_east ); radius = sqrtf( radius_north * radius_north + radius_east * radius_east );
cradius = sqrtf( diff_north * diff_north + diff_east * diff_east ); cradius = sqrtf( diff_north * diff_north + diff_east * diff_east );
if (cradius < 1e-6) { if (cradius < 1e-6f) {
// cradius is zero, just fly somewhere and make sure correction is still a normal // cradius is zero, just fly somewhere and make sure correction is still a normal
status->fractional_progress = 1; status->fractional_progress = 1;
status->error = radius; status->error = radius;

7
flight/Modules/Airspeed/revolution/gps_airspeed.c
View File

@ -35,7 +35,7 @@
#include "gpsvelocity.h" #include "gpsvelocity.h"
#include "attitudeactual.h" #include "attitudeactual.h"
#include "CoordinateConversions.h" #include "CoordinateConversions.h"
#include <pios_math.h>
// Private constants // Private constants
@ -44,9 +44,6 @@
#define SAMPLING_DELAY_MS_GPS 100 //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 #define GPS_AIRSPEED_TIME_CONSTANT_MS 500.0f //Needs to be settable in a UAVO
#define F_PI 3.141526535897932f
#define DEG2RAD (F_PI/180.0f)
// Private types // Private types
struct GPSGlobals { struct GPSGlobals {
float RbeCol1_old[3]; float RbeCol1_old[3];
@ -117,7 +114,7 @@ void gps_airspeedGet(float *v_air_GPS)
float cosDiff=(Rbe[0][0]*gps->RbeCol1_old[0]) + (Rbe[0][1]*gps->RbeCol1_old[1]) + (Rbe[0][2]*gps->RbeCol1_old[2]); 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 there's more than a 5 degree difference between two fuselage measurements, then we have sufficient delta to continue.
if (fabs(cosDiff) < cos(5.0f*DEG2RAD)) { if (fabsf(cosDiff) < cosf(DEG2RAD(5.0f))) {
GPSVelocityData gpsVelData; GPSVelocityData gpsVelData;
GPSVelocityGet(&gpsVelData); GPSVelocityGet(&gpsVelData);

61
flight/Modules/Attitude/revolution/attitude.c
View File

@ -70,14 +70,13 @@
#include "revosettings.h" #include "revosettings.h"
#include "velocityactual.h" #include "velocityactual.h"
#include "CoordinateConversions.h" #include "CoordinateConversions.h"
#include <pios_math.h>
// Private constants // Private constants
#define STACK_SIZE_BYTES 2048 #define STACK_SIZE_BYTES 2048
#define TASK_PRIORITY (tskIDLE_PRIORITY+3) #define TASK_PRIORITY (tskIDLE_PRIORITY+3)
#define FAILSAFE_TIMEOUT_MS 10 #define FAILSAFE_TIMEOUT_MS 10
#define F_PI 3.14159265358979323846f #define PI_MOD(x) (fmodf(x + M_PI_F, M_PI_F * 2) - M_PI_F)
#define PI_MOD(x) (fmodf(x + F_PI, F_PI * 2) - F_PI)
// low pass filter configuration to calculate offset // low pass filter configuration to calculate offset
// of barometric altitude sensor // of barometric altitude sensor
@ -303,9 +302,9 @@ static int32_t updateAttitudeComplementary(bool first_run)
AttitudeActualData attitudeActual; AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual); AttitudeActualGet(&attitudeActual);
init = 0; init = 0;
attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI; attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / M_PI_F;
attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI; attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / M_PI_F;
attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / F_PI; attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / M_PI_F;
RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1); RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
AttitudeActualSet(&attitudeActual); AttitudeActualSet(&attitudeActual);
@ -415,10 +414,10 @@ static int32_t updateAttitudeComplementary(bool first_run)
// Work out time derivative from INSAlgo writeup // Work out time derivative from INSAlgo writeup
// Also accounts for the fact that gyros are in deg/s // Also accounts for the fact that gyros are in deg/s
float qdot[4]; float qdot[4];
qdot[0] = (-q[1] * gyrosData.x - q[2] * gyrosData.y - q[3] * gyrosData.z) * dT * F_PI / 180 / 2; qdot[0] = (-q[1] * gyrosData.x - q[2] * gyrosData.y - q[3] * gyrosData.z) * dT * M_PI_F / 180 / 2;
qdot[1] = (q[0] * gyrosData.x - q[3] * gyrosData.y + q[2] * gyrosData.z) * dT * F_PI / 180 / 2; qdot[1] = (q[0] * gyrosData.x - q[3] * gyrosData.y + q[2] * gyrosData.z) * dT * M_PI_F / 180 / 2;
qdot[2] = (q[3] * gyrosData.x + q[0] * gyrosData.y - q[1] * gyrosData.z) * dT * F_PI / 180 / 2; qdot[2] = (q[3] * gyrosData.x + q[0] * gyrosData.y - q[1] * gyrosData.z) * dT * M_PI_F / 180 / 2;
qdot[3] = (-q[2] * gyrosData.x + q[1] * gyrosData.y + q[0] * gyrosData.z) * dT * F_PI / 180 / 2; qdot[3] = (-q[2] * gyrosData.x + q[1] * gyrosData.y + q[0] * gyrosData.z) * dT * M_PI_F / 180 / 2;
// Take a time step // Take a time step
q[0] = q[0] + qdot[0]; q[0] = q[0] + qdot[0];
@ -641,7 +640,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
INSSetBaroVar(revoCalibration.baro_var); INSSetBaroVar(revoCalibration.baro_var);
// Initialize the gyro bias from the settings // Initialize the gyro bias from the settings
float gyro_bias[3] = {gyrosBias.x * F_PI / 180.0f, gyrosBias.y * F_PI / 180.0f, gyrosBias.z * F_PI / 180.0f}; float gyro_bias[3] = {gyrosBias.x * M_PI_F / 180.0f, gyrosBias.y * M_PI_F / 180.0f, gyrosBias.z * M_PI_F / 180.0f};
INSSetGyroBias(gyro_bias); INSSetGyroBias(gyro_bias);
xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS); xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS);
@ -649,9 +648,9 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
// Set initial attitude // Set initial attitude
AttitudeActualData attitudeActual; AttitudeActualData attitudeActual;
attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI; attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / M_PI_F;
attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI; attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / M_PI_F;
attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / F_PI; attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / M_PI_F;
RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1); RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
AttitudeActualSet(&attitudeActual); AttitudeActualSet(&attitudeActual);
@ -676,7 +675,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
INSSetMagNorth(homeLocation.Be); INSSetMagNorth(homeLocation.Be);
// Initialize the gyro bias from the settings // Initialize the gyro bias from the settings
float gyro_bias[3] = {gyrosBias.x * F_PI / 180.0f, gyrosBias.y * F_PI / 180.0f, gyrosBias.z * F_PI / 180.0f}; float gyro_bias[3] = {gyrosBias.x * M_PI_F / 180.0f, gyrosBias.y * M_PI_F / 180.0f, gyrosBias.z * M_PI_F / 180.0f};
INSSetGyroBias(gyro_bias); INSSetGyroBias(gyro_bias);
GPSPositionData gpsPosition; GPSPositionData gpsPosition;
@ -693,9 +692,9 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
// Set initial attitude // Set initial attitude
AttitudeActualData attitudeActual; AttitudeActualData attitudeActual;
attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI; attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / M_PI_F;
attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI; attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / M_PI_F;
attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / F_PI; attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / M_PI_F;
RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1); RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
AttitudeActualSet(&attitudeActual); AttitudeActualSet(&attitudeActual);
@ -711,9 +710,9 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f; dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f;
GyrosBiasGet(&gyrosBias); GyrosBiasGet(&gyrosBias);
float gyros[3] = {(gyrosData.x + gyrosBias.x) * F_PI / 180.0f, float gyros[3] = {(gyrosData.x + gyrosBias.x) * M_PI_F / 180.0f,
(gyrosData.y + gyrosBias.y) * F_PI / 180.0f, (gyrosData.y + gyrosBias.y) * M_PI_F / 180.0f,
(gyrosData.z + gyrosBias.z) * F_PI / 180.0f}; (gyrosData.z + gyrosBias.z) * M_PI_F / 180.0f};
INSStatePrediction(gyros, &accelsData.x, dT); INSStatePrediction(gyros, &accelsData.x, dT);
AttitudeActualData attitude; AttitudeActualData attitude;
@ -750,18 +749,18 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
// If the gyro bias setting was updated we should reset // If the gyro bias setting was updated we should reset
// the state estimate of the EKF // the state estimate of the EKF
if(gyroBiasSettingsUpdated) { if(gyroBiasSettingsUpdated) {
float gyro_bias[3] = {gyrosBias.x * F_PI / 180.0f, gyrosBias.y * F_PI / 180.0f, gyrosBias.z * F_PI / 180.0f}; float gyro_bias[3] = {gyrosBias.x * M_PI_F / 180.0f, gyrosBias.y * M_PI_F / 180.0f, gyrosBias.z * M_PI_F / 180.0f};
INSSetGyroBias(gyro_bias); INSSetGyroBias(gyro_bias);
gyroBiasSettingsUpdated = false; gyroBiasSettingsUpdated = false;
} }
// Because the sensor module remove the bias we need to add it // Because the sensor module remove the bias we need to add it
// back in here so that the INS algorithm can track it correctly // back in here so that the INS algorithm can track it correctly
float gyros[3] = {gyrosData.x * F_PI / 180.0f, gyrosData.y * F_PI / 180.0f, gyrosData.z * F_PI / 180.0f}; float gyros[3] = {gyrosData.x * M_PI_F / 180.0f, gyrosData.y * M_PI_F / 180.0f, gyrosData.z * M_PI_F / 180.0f};
if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) { if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) {
gyros[0] += gyrosBias.x * F_PI / 180.0f; gyros[0] += gyrosBias.x * M_PI_F / 180.0f;
gyros[1] += gyrosBias.y * F_PI / 180.0f; gyros[1] += gyrosBias.y * M_PI_F / 180.0f;
gyros[2] += gyrosBias.z * F_PI / 180.0f; gyros[2] += gyrosBias.z * M_PI_F / 180.0f;
} }
// Advance the state estimate // Advance the state estimate
@ -795,8 +794,8 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
if (0) { // Old code to take horizontal velocity from GPS Position update if (0) { // Old code to take horizontal velocity from GPS Position update
sensors |= HORIZ_SENSORS; sensors |= HORIZ_SENSORS;
vel[0] = gpsData.Groundspeed * cosf(gpsData.Heading * F_PI / 180.0f); vel[0] = gpsData.Groundspeed * cosf(gpsData.Heading * M_PI_F / 180.0f);
vel[1] = gpsData.Groundspeed * sinf(gpsData.Heading * F_PI / 180.0f); vel[1] = gpsData.Groundspeed * sinf(gpsData.Heading * M_PI_F / 180.0f);
vel[2] = 0; vel[2] = 0;
} }
// Transform the GPS position into NED coordinates // Transform the GPS position into NED coordinates
@ -870,9 +869,9 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
// Copy the gyro bias into the UAVO except when it was updated // Copy the gyro bias into the UAVO except when it was updated
// from the settings during the calculation, then consume it // from the settings during the calculation, then consume it
// next cycle // next cycle
gyrosBias.x = Nav.gyro_bias[0] * 180.0f / F_PI; gyrosBias.x = Nav.gyro_bias[0] * 180.0f / M_PI_F;
gyrosBias.y = Nav.gyro_bias[1] * 180.0f / F_PI; gyrosBias.y = Nav.gyro_bias[1] * 180.0f / M_PI_F;
gyrosBias.z = Nav.gyro_bias[2] * 180.0f / F_PI; gyrosBias.z = Nav.gyro_bias[2] * 180.0f / M_PI_F;
GyrosBiasSet(&gyrosBias); GyrosBiasSet(&gyrosBias);
} }

17
flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c
View File

@ -66,14 +66,11 @@
#include "velocitydesired.h" #include "velocitydesired.h"
#include "velocityactual.h" #include "velocityactual.h"
#include "CoordinateConversions.h" #include "CoordinateConversions.h"
#include <pios_math.h>
// Private constants // Private constants
#define MAX_QUEUE_SIZE 4 #define MAX_QUEUE_SIZE 4
#define STACK_SIZE_BYTES 1548 #define STACK_SIZE_BYTES 1548
#define TASK_PRIORITY (tskIDLE_PRIORITY+2) #define TASK_PRIORITY (tskIDLE_PRIORITY+2)
#define F_PI 3.14159265358979323846f
#define RAD2DEG (180.0f/F_PI)
#define DEG2RAD (F_PI/180.0f)
#define GEE 9.81f #define GEE 9.81f
// Private types // Private types
@ -293,7 +290,7 @@ static void updatePathVelocity()
break; break;
} }
// make sure groundspeed is not zero // make sure groundspeed is not zero
if (groundspeed<1e-2) groundspeed=1e-2; if (groundspeed<1e-2f) groundspeed=1e-2f;
// calculate velocity - can be zero if waypoints are too close // calculate velocity - can be zero if waypoints are too close
VelocityDesiredData velocityDesired; VelocityDesiredData velocityDesired;
@ -313,8 +310,8 @@ static void updatePathVelocity()
// difference between correction_direction and velocityactual >90 degrees and // difference between correction_direction and velocityactual >90 degrees and
// difference between path_direction and velocityactual >90 degrees ( 4th sector, facing away from eerything ) // difference between path_direction and velocityactual >90 degrees ( 4th sector, facing away from eerything )
// fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore) // fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore)
float angle1=RAD2DEG * ( atan2f(progress.path_direction[1],progress.path_direction[0]) - atan2f(velocityActual.East,velocityActual.North)); float angle1=RAD2DEG( atan2f(progress.path_direction[1],progress.path_direction[0]) - atan2f(velocityActual.East,velocityActual.North));
float angle2=RAD2DEG * ( atan2f(progress.correction_direction[1],progress.correction_direction[0]) - atan2f(velocityActual.East,velocityActual.North)); float angle2=RAD2DEG( atan2f(progress.correction_direction[1],progress.correction_direction[0]) - atan2f(velocityActual.East,velocityActual.North));
if (angle1<-180.0f) angle1+=360.0f; if (angle1<-180.0f) angle1+=360.0f;
if (angle1>180.0f) angle1-=360.0f; if (angle1>180.0f) angle1-=360.0f;
if (angle2<-180.0f) angle2+=360.0f; if (angle2<-180.0f) angle2+=360.0f;
@ -470,7 +467,7 @@ static uint8_t updateFixedDesiredAttitude()
result = 0; result = 0;
} }
if (indicatedAirspeedActual<1e-6) { if (indicatedAirspeedActual<1e-6f) {
// prevent division by zero, abort without controlling anything. This guidance mode is not suited for takeoff or touchdown, or handling stationary planes // prevent division by zero, abort without controlling anything. This guidance mode is not suited for takeoff or touchdown, or handling stationary planes
// also we cannot handle planes flying backwards, lets just wait until the nose drops // also we cannot handle planes flying backwards, lets just wait until the nose drops
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1; fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1;
@ -583,8 +580,8 @@ static uint8_t updateFixedDesiredAttitude()
/** /**
* Compute desired roll command * Compute desired roll command
*/ */
if (groundspeedDesired> 1e-6) { if (groundspeedDesired> 1e-6f) {
bearingError = RAD2DEG * (atan2f(velocityDesired.East,velocityDesired.North) - atan2f(velocityActual.East,velocityActual.North)); bearingError = RAD2DEG(atan2f(velocityDesired.East,velocityDesired.North) - atan2f(velocityActual.East,velocityActual.North));
} else { } else {
// if we are not supposed to move, run in a circle // if we are not supposed to move, run in a circle
bearingError = -90.0f; bearingError = -90.0f;

2
flight/Modules/PathPlanner/pathplanner.c
View File

@ -329,7 +329,7 @@ static uint8_t conditionTimeOut() {
toWaypoint = waypointActive.Index; toWaypoint = waypointActive.Index;
toStarttime = PIOS_DELAY_GetRaw(); toStarttime = PIOS_DELAY_GetRaw();
} }
if (PIOS_DELAY_DiffuS(toStarttime) >= 1e6 * pathAction.ConditionParameters[0]) { if (PIOS_DELAY_DiffuS(toStarttime) >= 1e6f * pathAction.ConditionParameters[0]) {
// make sure we reinitialize even if the same waypoint comes twice // make sure we reinitialize even if the same waypoint comes twice
toWaypoint = 0xFFFF; toWaypoint = 0xFFFF;
return true; return true;

2
flight/PiOS/Common/pios_mpxv.c
View File

@ -77,7 +77,7 @@ float PIOS_MPXV_CalcAirspeed(PIOS_MPXV_descriptor *desc,uint16_t measurement)
} }
//Compute calibrated airspeed, as per http://en.wikipedia.org/wiki/Calibrated_airspeed //Compute calibrated airspeed, as per http://en.wikipedia.org/wiki/Calibrated_airspeed
float calibratedAirspeed = A0*sqrt(5.0f*(pow(Qc/P0+1.0f,POWER)-1.0f)); float calibratedAirspeed = A0*sqrtf(5.0f*(powf(Qc/P0+1.0f,POWER)-1.0f));
//Upper bound airspeed. No need to lower bound it, that comes from Qc //Upper bound airspeed. No need to lower bound it, that comes from Qc
if (calibratedAirspeed > desc->maxSpeed) { //in [m/s] if (calibratedAirspeed > desc->maxSpeed) { //in [m/s]

4
flight/PiOS/Common/pios_rfm22b.c
View File

@ -964,12 +964,12 @@ static void PIOS_RFM22B_Task(void *parameters)
static void rfm22_setDatarate(struct pios_rfm22b_dev * rfm22b_dev, enum rfm22b_datarate datarate, bool data_whitening) static void rfm22_setDatarate(struct pios_rfm22b_dev * rfm22b_dev, enum rfm22b_datarate datarate, bool data_whitening)
{ {
uint32_t datarate_bps = data_rate[datarate]; uint32_t datarate_bps = data_rate[datarate];
rfm22b_dev->max_packet_time = (uint16_t)((float)(PIOS_PH_MAX_PACKET * 8 * 1000) / (float)(datarate_bps) + 0.5); rfm22b_dev->max_packet_time = (uint16_t)((float)(PIOS_PH_MAX_PACKET * 8 * 1000) / (float)(datarate_bps) + 0.5f);
if (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) if (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED)
{ {
// Generate a pseudo-random number from 0-8 to add to the delay // Generate a pseudo-random number from 0-8 to add to the delay
uint8_t random = PIOS_CRC_updateByte(0, (uint8_t)(xTaskGetTickCount() & 0xff)) & 0x03; uint8_t random = PIOS_CRC_updateByte(0, (uint8_t)(xTaskGetTickCount() & 0xff)) & 0x03;
rfm22b_dev->max_ack_delay = (uint16_t)((float)(sizeof(PHPacketHeader) * 8 * 1000) / (float)(datarate_bps) + 0.5) * 4 + random; rfm22b_dev->max_ack_delay = (uint16_t)((float)(sizeof(PHPacketHeader) * 8 * 1000) / (float)(datarate_bps) + 0.5f) * 4 + random;
} }
else else
rfm22b_dev->max_ack_delay = CONNECT_ATTEMPT_PERIOD_MS; rfm22b_dev->max_ack_delay = CONNECT_ATTEMPT_PERIOD_MS;

40
flight/PiOS/inc/pios_helpers.h Normal file
View File

@ -0,0 +1,40 @@
/**
******************************************************************************
*
* @file pios_helpers.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Header for helper functions/macro definitions
*
* @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_HELPERS_H_
#define PIOS_HELPERS_H_
/**
* @brief return the number of elements contained in the array x.
* @param[in] x the array
* @return number of elements in x.
*
*/
#define NELEMENTS(x) (sizeof(x) / sizeof((x)[0]))
#endif // PIOS_HELPERS_H_

35
flight/PiOS/inc/pios_math.h Normal file
View File

@ -0,0 +1,35 @@
/**
******************************************************************************
*
* @file pios_math.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Header for math functions/macro definitions
*
* @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_MATH_H_
#define PIOS_MATH_H_
#define M_PI_F ((float)M_PI)
#define RAD2DEG(rad) ((rad)*(180.0f/M_PI_F))
#define DEG2RAD(deg) ((deg)*(M_PI_F/180.0f))
#endif // PIOS_MATH_H_

3
flight/PiOS/pios.h
View File

@ -34,8 +34,7 @@
#ifndef PIOS_H #ifndef PIOS_H
#define PIOS_H #define PIOS_H
/* Used in pios_*.* files macro */ #include <pios_helpers.h>
#define NELEMENTS(x) (sizeof(x) / sizeof((x)[0]))
#ifdef USE_SIM_POSIX #ifdef USE_SIM_POSIX
/* SimPosix version of this file. This will probably be removed later */ /* SimPosix version of this file. This will probably be removed later */