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

Math cleanup

Browse Source
This commit is contained in:
sambas 2013-04-27 16:30:02 +03:00
parent 9fec3f5567
commit 5f3e0c3e1d
9 changed files with 32 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
flight/libraries/WorldMagModel.c
View File

@ -696,7 +696,7 @@ int WMM_RotateMagneticVector(WMMtype_CoordSpherical * CoordSpherical,
*/ */
{ {
/* Difference between the spherical and Geodetic latitudes */ /* Difference between the spherical and Geodetic latitudes */
float Psi = (M_PI_F / 180) * (CoordSpherical->phig - CoordGeodetic->phi); float Psi = DEG2RAD(CoordSpherical->phig - CoordGeodetic->phi);
/* Rotate spherical field components to the Geodeitic system */ /* Rotate spherical field components to the Geodeitic system */
MagneticResultsGeo->Bz = MagneticResultsSph->Bx * sinf(Psi) + MagneticResultsSph->Bz * cosf(Psi); MagneticResultsGeo->Bz = MagneticResultsSph->Bx * sinf(Psi) + MagneticResultsSph->Bz * cosf(Psi);

3
flight/libraries/insgps_helper.c
View File

@ -5,9 +5,6 @@
#include "insgps.h" #include "insgps.h"
#include "CoordinateConversions.h" #include "CoordinateConversions.h"
#define DEG_TO_RAD (M_PI / 180.0)
#define RAD_TO_DEG (180.0 / M_PI)
#define INSGPS_GPS_TIMEOUT 2 /* 2 seconds triggers reinit of position */ #define INSGPS_GPS_TIMEOUT 2 /* 2 seconds triggers reinit of position */
#define INSGPS_GPS_MINSAT 6 /* 2 seconds triggers reinit of position */ #define INSGPS_GPS_MINSAT 6 /* 2 seconds triggers reinit of position */
#define INSGPS_GPS_MINPDOP 3.5 /* minimum PDOP for postition updates */ #define INSGPS_GPS_MINPDOP 3.5 /* minimum PDOP for postition updates */

5
flight/libraries/math/pid.c
View File

@ -30,8 +30,7 @@
#include "openpilot.h" #include "openpilot.h"
#include "pid.h" #include "pid.h"
#include <pios_math.h>
#define F_PI ((float) M_PI)
//! Private method //! Private method
static float bound(float val, float range); static float bound(float val, float range);
@ -121,7 +120,7 @@ void pid_zero(struct pid *pid)
*/ */
void pid_configure_derivative(float cutoff, float g) void pid_configure_derivative(float cutoff, float g)
{ {
deriv_tau = 1.0f / (2 * F_PI * cutoff); deriv_tau = 1.0f / (2 * M_PI_F * cutoff);
deriv_gamma = g; deriv_gamma = g;
} }

4
flight/libraries/math/sin_lookup.c
View File

@ -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 / M_PI_F; int degrees = RAD2DEG(angle);
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 / M_PI_F; int degrees = RAD2DEG(angle);
return cos_lookup_deg(degrees); return cos_lookup_deg(degrees);
} }

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

@ -300,9 +300,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 / M_PI_F; attitudeActual.Roll = RAD2DEG(atan2f(-accelsData.y, -accelsData.z));
attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / M_PI_F; attitudeActual.Pitch = RAD2DEG(atan2f(accelsData.x, -accelsData.z));
attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / M_PI_F; attitudeActual.Yaw = RAD2DEG(atan2f(-magData.y, magData.x));
RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1); RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
AttitudeActualSet(&attitudeActual); AttitudeActualSet(&attitudeActual);
@ -646,9 +646,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 / M_PI_F; attitudeActual.Roll = RAD2DEG(atan2f(-accelsData.y, -accelsData.z));
attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / M_PI_F; attitudeActual.Pitch = RAD2DEG(atan2f(accelsData.x, -accelsData.z));
attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / M_PI_F; attitudeActual.Yaw = RAD2DEG(atan2f(-magData.y, magData.x));
RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1); RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
AttitudeActualSet(&attitudeActual); AttitudeActualSet(&attitudeActual);
@ -690,9 +690,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 / M_PI_F; attitudeActual.Roll = RAD2DEG(atan2f(-accelsData.y, -accelsData.z));
attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / M_PI_F; attitudeActual.Pitch = RAD2DEG(atan2f(accelsData.x, -accelsData.z));
attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / M_PI_F; attitudeActual.Yaw = RAD2DEG(atan2f(-magData.y, magData.x));
RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1); RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
AttitudeActualSet(&attitudeActual); AttitudeActualSet(&attitudeActual);
@ -792,8 +792,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 * M_PI_F / 180.0f); vel[0] = gpsData.Groundspeed * cosf(DEG2RAD(gpsData.Heading));
vel[1] = gpsData.Groundspeed * sinf(gpsData.Heading * M_PI_F / 180.0f); vel[1] = gpsData.Groundspeed * sinf(DEG2RAD(gpsData.Heading));
vel[2] = 0; vel[2] = 0;
} }
// Transform the GPS position into NED coordinates // Transform the GPS position into NED coordinates
@ -886,12 +886,11 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
* @returns 0 for success, -1 for failure * @returns 0 for success, -1 for failure
*/ */
float T[3]; float T[3];
const float DEG2RAD = 3.141592653589793f / 180.0f;
static int32_t getNED(GPSPositionData * gpsPosition, float * NED) static int32_t getNED(GPSPositionData * gpsPosition, float * NED)
{ {
float dL[3] = {(gpsPosition->Latitude - homeLocation.Latitude) / 10.0e6f * DEG2RAD, float dL[3] = { DEG2RAD((gpsPosition->Latitude - homeLocation.Latitude) / 10.0e6f),
(gpsPosition->Longitude - homeLocation.Longitude) / 10.0e6f * DEG2RAD, DEG2RAD((gpsPosition->Longitude - homeLocation.Longitude) / 10.0e6f),
(gpsPosition->Altitude + gpsPosition->GeoidSeparation - homeLocation.Altitude)}; (gpsPosition->Altitude + gpsPosition->GeoidSeparation - homeLocation.Altitude) };
NED[0] = T[0] * dL[0]; NED[0] = T[0] * dL[0];
NED[1] = T[1] * dL[1]; NED[1] = T[1] * dL[1];
@ -925,7 +924,7 @@ static void settingsUpdatedCb(UAVObjEvent * ev)
HomeLocationGet(&homeLocation); HomeLocationGet(&homeLocation);
// Compute matrix to convert deltaLLA to NED // Compute matrix to convert deltaLLA to NED
float lat, alt; float lat, alt;
lat = homeLocation.Latitude / 10.0e6f * DEG2RAD; lat = DEG2RAD(homeLocation.Latitude / 10.0e6f);
alt = homeLocation.Altitude; alt = homeLocation.Altitude;
T[0] = alt+6.378137E6f; T[0] = alt+6.378137E6f;

5
flight/modules/PathPlanner/pathplanner.c
View File

@ -30,6 +30,7 @@
*/ */
#include "openpilot.h" #include "openpilot.h"
#include <pios_math.h>
#include "paths.h" #include "paths.h"
#include "flightstatus.h" #include "flightstatus.h"
@ -46,8 +47,6 @@
#define STACK_SIZE_BYTES 1024 #define STACK_SIZE_BYTES 1024
#define TASK_PRIORITY (tskIDLE_PRIORITY+1) #define TASK_PRIORITY (tskIDLE_PRIORITY+1)
#define MAX_QUEUE_SIZE 2 #define MAX_QUEUE_SIZE 2
#define F_PI 3.141526535897932f
#define RAD2DEG (180.0f/F_PI)
#define PATH_PLANNER_UPDATE_RATE_MS 20 #define PATH_PLANNER_UPDATE_RATE_MS 20
// Private types // Private types
@ -477,7 +476,7 @@ static uint8_t conditionPointingTowardsNext() {
float angle2 = atan2f(velocity.North,velocity.East); float angle2 = atan2f(velocity.North,velocity.East);
// calculate the absolute angular difference // calculate the absolute angular difference
angle1 = fabsf(RAD2DEG * (angle1 - angle2)); angle1 = fabsf(RAD2DEG(angle1 - angle2));
while (angle1>360) angle1-=360; while (angle1>360) angle1-=360;
if (angle1 <= pathAction.ConditionParameters[0]) { if (angle1 <= pathAction.ConditionParameters[0]) {

22
flight/modules/VtolPathFollower/vtolpathfollower.c
View File

@ -47,6 +47,7 @@
*/ */
#include "openpilot.h" #include "openpilot.h"
#include <pios_math.h>
#include "paths.h" #include "paths.h"
#include "vtolpathfollower.h" #include "vtolpathfollower.h"
@ -79,9 +80,6 @@
#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 DEG2RAD (F_PI/180.0f)
#define RAD2DEG(rad) ((rad)*(180.0f/F_PI))
// Private types // Private types
@ -338,8 +336,8 @@ static void updatePOIBearing()
// don't try to move any closer // don't try to move any closer
if (poiRadius >= 3.0f || changeRadius > 0) { if (poiRadius >= 3.0f || changeRadius > 0) {
if (pathAngle != 0 || changeRadius != 0) { if (pathAngle != 0 || changeRadius != 0) {
pathDesired.End[PATHDESIRED_END_NORTH] = poi.North + (poiRadius * cosf((pathAngle + yaw - 180.0f) * DEG2RAD)); pathDesired.End[PATHDESIRED_END_NORTH] = poi.North + (poiRadius * cosf(DEG2RAD(pathAngle + yaw - 180.0f)));
pathDesired.End[PATHDESIRED_END_EAST] = poi.East + (poiRadius * sinf((pathAngle + yaw - 180.0f) * DEG2RAD)); pathDesired.End[PATHDESIRED_END_EAST] = poi.East + (poiRadius * sinf(DEG2RAD(pathAngle + yaw - 180.0f)));
pathDesired.StartingVelocity = 1; pathDesired.StartingVelocity = 1;
pathDesired.EndingVelocity = 0; pathDesired.EndingVelocity = 0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
@ -481,13 +479,13 @@ void updateEndpointVelocity()
GPSPositionGet(&gpsPosition); GPSPositionGet(&gpsPosition);
HomeLocationData homeLocation; HomeLocationData homeLocation;
HomeLocationGet(&homeLocation); HomeLocationGet(&homeLocation);
float lat = homeLocation.Latitude / 10.0e6f * DEG2RAD; float lat = DEG2RAD(homeLocation.Latitude / 10.0e6f);
float alt = homeLocation.Altitude; float alt = homeLocation.Altitude;
float T[3] = float T[3] =
{ alt + 6.378137E6f, cosf(lat) * (alt + 6.378137E6f), -1.0f }; { alt + 6.378137E6f, cosf(lat) * (alt + 6.378137E6f), -1.0f };
float NED[3] = float NED[3] =
{ T[0] * ((gpsPosition.Latitude - homeLocation.Latitude) / 10.0e6f * DEG2RAD), T[1] { T[0] * (DEG2RAD((gpsPosition.Latitude - homeLocation.Latitude) / 10.0e6f)), T[1]
* ((gpsPosition.Longitude - homeLocation.Longitude) / 10.0e6f * DEG2RAD), T[2] * (DEG2RAD((gpsPosition.Longitude - homeLocation.Longitude) / 10.0e6f)), T[2]
* ((gpsPosition.Altitude + gpsPosition.GeoidSeparation - homeLocation.Altitude)) }; * ((gpsPosition.Altitude + gpsPosition.GeoidSeparation - homeLocation.Altitude)) };
northPos = NED[0]; northPos = NED[0];
@ -610,8 +608,8 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
{ {
GPSPositionData gpsPosition; GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition); GPSPositionGet(&gpsPosition);
northVel = gpsPosition.Groundspeed * cosf(gpsPosition.Heading * F_PI / 180.0f); northVel = gpsPosition.Groundspeed * cosf(DEG2RAD(gpsPosition.Heading));
eastVel = gpsPosition.Groundspeed * sinf(gpsPosition.Heading * F_PI / 180.0f); eastVel = gpsPosition.Groundspeed * sinf(DEG2RAD(gpsPosition.Heading));
downVel = velocityActual.Down; downVel = velocityActual.Down;
} }
break; break;
@ -656,9 +654,9 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
// Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the // Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the
// craft should move similarly for 5 deg roll versus 5 deg pitch // craft should move similarly for 5 deg roll versus 5 deg pitch
stabDesired.Pitch = bound(-northCommand * cosf(attitudeActual.Yaw * M_PI / 180) + -eastCommand * sinf(attitudeActual.Yaw * M_PI / 180), stabDesired.Pitch = bound(-northCommand * cosf(DEG2RAD(attitudeActual.Yaw)) + -eastCommand * sinf(DEG2RAD(attitudeActual.Yaw)),
-vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch); -vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
stabDesired.Roll = bound(-northCommand * sinf(attitudeActual.Yaw * M_PI / 180) + eastCommand * cosf(attitudeActual.Yaw * M_PI / 180), stabDesired.Roll = bound(-northCommand * sinf(DEG2RAD(attitudeActual.Yaw)) + eastCommand * cosf(DEG2RAD(attitudeActual.Yaw)),
-vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch); -vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
if (vtolpathfollowerSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) { if (vtolpathfollowerSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) {

1
flight/pios/common/pios_l3gd20.c
View File

@ -67,7 +67,6 @@ static int32_t PIOS_L3GD20_ReleaseBus();
volatile bool l3gd20_configured = false; volatile bool l3gd20_configured = false;
/* Local Variables */ /* Local Variables */
#define DEG_TO_RAD (M_PI / 180.0)
/** /**
* @brief Allocate a new device * @brief Allocate a new device

2
flight/pios/common/pios_mpu6000.c
View File

@ -64,8 +64,6 @@ static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const * cfg);
static int32_t PIOS_MPU6000_SetReg(uint8_t address, uint8_t buffer); static int32_t PIOS_MPU6000_SetReg(uint8_t address, uint8_t buffer);
static int32_t PIOS_MPU6000_GetReg(uint8_t address); static int32_t PIOS_MPU6000_GetReg(uint8_t address);
#define DEG_TO_RAD (M_PI / 180.0)
#define GRAV 9.81f #define GRAV 9.81f
/** /**