Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

desktop: floating point precision changes from Justin Beech

This commit is contained in:
Andrew Tridgell 2012-01-11 21:30:48 +11:00
parent f35411ac45
commit 49cf409c23
3 changed files with 31 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/Desktop/support/desktop.h
View File

@ -20,11 +20,11 @@ void sitl_setup(void);
int sitl_gps_pipe(void);
ssize_t sitl_gps_read(int fd, void *buf, size_t count);
void sitl_update_compass(float heading, float roll, float pitch, float yaw);
void sitl_update_gps(float latitude, float longitude, float altitude,
float speedN, float speedE, bool have_lock);
void sitl_update_gps(double latitude, double longitude, float altitude,
double speedN, double speedE, bool have_lock);
void sitl_update_adc(float roll, float pitch, float yaw,
float rollRate, float pitchRate, float yawRate,
float xAccel, float yAccel, float zAccel,
double rollRate, double pitchRate, double yawRate,
double xAccel, double yAccel, double zAccel,
float airspeed);
void sitl_setup_adc(void);
void sitl_update_barometer(float altitude);

30
libraries/Desktop/support/sitl_adc.cpp
View File

@ -40,10 +40,28 @@ static uint16_t airspeed_sensor(float airspeed)
average rates to cope with slow update rates.
inputs are in degrees
phi - roll
theta - pitch
psi - true heading
alpha - angle of attack
beta - side slip
phidot - roll rate
thetadot - pitch rate
psidot - yaw rate
v_north - north velocity in local/body frame
v_east - east velocity in local/body frame
v_down - down velocity in local/body frame
A_X_pilot - X accel in body frame
A_Y_pilot - Y accel in body frame
A_Z_pilot - Z accel in body frame
Note: doubles on high prec. stuff are preserved until the last moment
*/
void sitl_update_adc(float roll, float pitch, float yaw,
float rollRate, float pitchRate, float yawRate,
float xAccel, float yAccel, float zAccel,
void sitl_update_adc(float roll, float pitch, float yaw, // Relative to earth
double rollRate, double pitchRate,double yawRate, // Local to plane
double xAccel, double yAccel, double zAccel, // Local to plane
float airspeed)
{
static const uint8_t sensor_map[6] = { 1, 2, 0, 4, 5, 6 };
@ -55,9 +73,9 @@ void sitl_update_adc(float roll, float pitch, float yaw,
const float _gyro_gain_y = ToRad(0.41);
const float _gyro_gain_z = ToRad(0.41);
const float _accel_scale = 9.80665 / 423.8;
float adc[7];
float phi, theta, phiDot, thetaDot, psiDot;
float p, q, r;
double adc[7];
double phi, theta, phiDot, thetaDot, psiDot;
double p, q, r;
/* convert the angular velocities from earth frame to
body frame. Thanks to James Goppert for the formula

6
libraries/Desktop/support/sitl_gps.cpp
View File

@ -78,8 +78,8 @@ static void gps_send(uint8_t msgid, uint8_t *buf, uint16_t size)
/*
possibly send a new GPS UBLOX packet
*/
void sitl_update_gps(float latitude, float longitude, float altitude,
float speedN, float speedE, bool have_lock)
void sitl_update_gps(double latitude, double longitude, float altitude,
double speedN, double speedE, bool have_lock)
{
struct ubx_nav_posllh {
uint32_t time; // GPS msToW
@ -113,7 +113,7 @@ void sitl_update_gps(float latitude, float longitude, float altitude,
const uint8_t MSG_POSLLH = 0x2;
const uint8_t MSG_STATUS = 0x3;
const uint8_t MSG_VELNED = 0x12;
float lon_scale;
double lon_scale;
// 4Hz
if (millis() - gps_state.last_update < 250) {