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

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 */
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 */
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 "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_MINSAT 6 /* 2 seconds triggers reinit of position */
#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 "pid.h"
#define F_PI ((float) M_PI)
#include <pios_math.h>
//! Private method
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)
{
deriv_tau = 1.0f / (2 * F_PI * cutoff);
deriv_tau = 1.0f / (2 * M_PI_F * cutoff);
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)
{
int degrees = angle * 180.0f / M_PI_F;
int degrees = RAD2DEG(angle);
return sin_lookup_deg(degrees);
}
@ -138,6 +138,6 @@ float sin_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);
}

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

@ -300,9 +300,9 @@ static int32_t updateAttitudeComplementary(bool first_run)
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
init = 0;
attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / M_PI_F;
attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / M_PI_F;
attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / M_PI_F;
attitudeActual.Roll = RAD2DEG(atan2f(-accelsData.y, -accelsData.z));
attitudeActual.Pitch = RAD2DEG(atan2f(accelsData.x, -accelsData.z));
attitudeActual.Yaw = RAD2DEG(atan2f(-magData.y, magData.x));
RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
AttitudeActualSet(&attitudeActual);
@ -646,9 +646,9 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
// Set initial attitude
AttitudeActualData attitudeActual;
attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / M_PI_F;
attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / M_PI_F;
attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / M_PI_F;
attitudeActual.Roll = RAD2DEG(atan2f(-accelsData.y, -accelsData.z));
attitudeActual.Pitch = RAD2DEG(atan2f(accelsData.x, -accelsData.z));
attitudeActual.Yaw = RAD2DEG(atan2f(-magData.y, magData.x));
RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
AttitudeActualSet(&attitudeActual);
@ -690,9 +690,9 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
// Set initial attitude
AttitudeActualData attitudeActual;
attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / M_PI_F;
attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / M_PI_F;
attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / M_PI_F;
attitudeActual.Roll = RAD2DEG(atan2f(-accelsData.y, -accelsData.z));
attitudeActual.Pitch = RAD2DEG(atan2f(accelsData.x, -accelsData.z));
attitudeActual.Yaw = RAD2DEG(atan2f(-magData.y, magData.x));
RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
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
sensors |= HORIZ_SENSORS;
vel[0] = gpsData.Groundspeed * cosf(gpsData.Heading * M_PI_F / 180.0f);
vel[1] = gpsData.Groundspeed * sinf(gpsData.Heading * M_PI_F / 180.0f);
vel[0] = gpsData.Groundspeed * cosf(DEG2RAD(gpsData.Heading));
vel[1] = gpsData.Groundspeed * sinf(DEG2RAD(gpsData.Heading));
vel[2] = 0;
}
// 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
*/
float T[3];
const float DEG2RAD = 3.141592653589793f / 180.0f;
static int32_t getNED(GPSPositionData * gpsPosition, float * NED)
{
float dL[3] = {(gpsPosition->Latitude - homeLocation.Latitude) / 10.0e6f * DEG2RAD,
(gpsPosition->Longitude - homeLocation.Longitude) / 10.0e6f * DEG2RAD,
(gpsPosition->Altitude + gpsPosition->GeoidSeparation - homeLocation.Altitude)};
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];
@ -925,7 +924,7 @@ static void settingsUpdatedCb(UAVObjEvent * ev)
HomeLocationGet(&homeLocation);
// Compute matrix to convert deltaLLA to NED
float lat, alt;
lat = homeLocation.Latitude / 10.0e6f * DEG2RAD;
lat = DEG2RAD(homeLocation.Latitude / 10.0e6f);
alt = homeLocation.Altitude;
T[0] = alt+6.378137E6f;

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

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

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

@ -47,6 +47,7 @@
*/
#include "openpilot.h"
#include <pios_math.h>
#include "paths.h"
#include "vtolpathfollower.h"
@ -79,9 +80,6 @@
#define MAX_QUEUE_SIZE 4
#define STACK_SIZE_BYTES 1548
#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
@ -338,8 +336,8 @@ static void updatePOIBearing()
// don't try to move any closer
if (poiRadius >= 3.0f || 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_EAST] = poi.East + (poiRadius * sinf((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(DEG2RAD(pathAngle + yaw - 180.0f)));
pathDesired.StartingVelocity = 1;
pathDesired.EndingVelocity = 0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
@ -481,13 +479,13 @@ void updateEndpointVelocity()
GPSPositionGet(&gpsPosition);
HomeLocationData homeLocation;
HomeLocationGet(&homeLocation);
float lat = homeLocation.Latitude / 10.0e6f * DEG2RAD;
float lat = DEG2RAD(homeLocation.Latitude / 10.0e6f);
float alt = homeLocation.Altitude;
float T[3] =
{ alt + 6.378137E6f, cosf(lat) * (alt + 6.378137E6f), -1.0f };
float NED[3] =
{ T[0] * ((gpsPosition.Latitude - homeLocation.Latitude) / 10.0e6f * DEG2RAD), T[1]
* ((gpsPosition.Longitude - homeLocation.Longitude) / 10.0e6f * DEG2RAD), T[2]
{ T[0] * (DEG2RAD((gpsPosition.Latitude - homeLocation.Latitude) / 10.0e6f)), T[1]
* (DEG2RAD((gpsPosition.Longitude - homeLocation.Longitude) / 10.0e6f)), T[2]
* ((gpsPosition.Altitude + gpsPosition.GeoidSeparation - homeLocation.Altitude)) };
northPos = NED[0];
@ -610,8 +608,8 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
{
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
northVel = gpsPosition.Groundspeed * cosf(gpsPosition.Heading * F_PI / 180.0f);
eastVel = gpsPosition.Groundspeed * sinf(gpsPosition.Heading * F_PI / 180.0f);
northVel = gpsPosition.Groundspeed * cosf(DEG2RAD(gpsPosition.Heading));
eastVel = gpsPosition.Groundspeed * sinf(DEG2RAD(gpsPosition.Heading));
downVel = velocityActual.Down;
}
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
// 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);
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);
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;
/* Local Variables */
#define DEG_TO_RAD (M_PI / 180.0)
/**
* @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_GetReg(uint8_t address);
#define DEG_TO_RAD (M_PI / 180.0)
#define GRAV 9.81f
/**