Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 18:38:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

desktop: normalise roll and pitch rates

Browse Source
This commit is contained in:
Andrew Tridgell 2011-11-19 10:07:11 +11:00 committed by Pat Hickey
parent 0ef727d707
commit 7e64cfcc93
4 changed files with 25 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/Desktop/support/sitl.cpp
View File

@ -126,9 +126,9 @@ static void sitl_fgear_input(void)
sitl_update_gps(d.fg_pkt.latitude, d.fg_pkt.longitude, sitl_update_gps(d.fg_pkt.latitude, d.fg_pkt.longitude,
ft2m(d.fg_pkt.altitude), ft2m(d.fg_pkt.altitude),
ft2m(d.fg_pkt.speedN), ft2m(d.fg_pkt.speedE)); ft2m(d.fg_pkt.speedN), ft2m(d.fg_pkt.speedE));
sitl_update_adc(d.fg_pkt.rollDeg, d.fg_pkt.pitchDeg, d.fg_pkt.yawDeg, kt2mps(d.fg_pkt.airspeed)); sitl_update_adc(d.fg_pkt.rollDeg, d.fg_pkt.pitchDeg, d.fg_pkt.heading, kt2mps(d.fg_pkt.airspeed));
sitl_update_barometer(ft2m(d.fg_pkt.altitude)); sitl_update_barometer(ft2m(d.fg_pkt.altitude));
sitl_update_compass(d.fg_pkt.heading, d.fg_pkt.rollDeg, d.fg_pkt.pitchDeg, d.fg_pkt.yawDeg); sitl_update_compass(d.fg_pkt.heading, d.fg_pkt.rollDeg, d.fg_pkt.pitchDeg, d.fg_pkt.heading);
count++; count++;
sim_input_count++; sim_input_count++;
if (millis() - last_report > 1000) { if (millis() - last_report > 1000) {

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

@ -57,7 +57,7 @@ void sitl_update_adc(float roll, float pitch, float yaw, float airspeed)
float rollRate, pitchRate, yawRate; float rollRate, pitchRate, yawRate;
static uint32_t last_update; static uint32_t last_update;
static float last_roll, last_pitch, last_yaw; static float last_roll, last_pitch, last_yaw;
uint32_t delta_t; unsigned long delta_t;
/* map roll/pitch/yaw to values the accelerometer would see */ /* map roll/pitch/yaw to values the accelerometer would see */
xAccel = sin(ToRad(pitch)); xAccel = sin(ToRad(pitch));
@ -73,12 +73,15 @@ void sitl_update_adc(float roll, float pitch, float yaw, float airspeed)
rollRate = 0; rollRate = 0;
pitchRate = 0; pitchRate = 0;
yawRate = 0; yawRate = 0;
delta_t = millis(); delta_t = micros();
} else { } else {
delta_t = millis() - last_update; delta_t = micros() - last_update;
rollRate = 1000.0 * (roll - last_roll) / delta_t; rollRate = 1.0e6 * (roll - last_roll) / delta_t;
pitchRate = 1000.0 * (pitch - last_pitch) / delta_t; pitchRate = 1.0e6 * (pitch - last_pitch) / delta_t;
yawRate = 1000.0 * (yaw - last_yaw) / delta_t; yawRate = 1.0e6 * (yaw - last_yaw) / delta_t;
rollRate = normalise(rollRate, -180, 180);
pitchRate = normalise(pitchRate, -180, 180);
yawRate = normalise(yawRate, -180, 180);
} }
last_update += delta_t; last_update += delta_t;
@ -109,8 +112,8 @@ void sitl_update_adc(float roll, float pitch, float yaw, float airspeed)
#if 0 #if 0
extern AP_DCM dcm; extern AP_DCM dcm;
printf("roll=%6.1f pitch=%6.1f dcm_roll=%6.1f dcm_pitch=%6.1f rollRate=%6.3f pitchRate=%6.3f\n", printf("roll=%6.1f / %6.1f pitch=%6.1f / %6.1f rollRate=%6.1f pitchRate=%6.1f\n",
roll, pitch, dcm.roll_sensor/100.0, dcm.pitch_sensor/100.0, rollRate, pitchRate); roll, dcm.roll_sensor/100.0, pitch, dcm.pitch_sensor/100.0, rollRate, pitchRate);
#endif #endif
} }

11
libraries/Desktop/support/util.cpp
View File

@ -79,3 +79,14 @@ void set_nonblocking(int fd)
unsigned v = fcntl(fd, F_GETFL, 0); unsigned v = fcntl(fd, F_GETFL, 0);
fcntl(fd, F_SETFL, v | O_NONBLOCK); fcntl(fd, F_SETFL, v | O_NONBLOCK);
} }
double normalise(double v, double min, double max)
{
while (v < min) {
v += (max - min);
}
while (v > max) {
v -= (max - min);
}
return v;
}

1
libraries/Desktop/support/util.h
View File

@ -8,3 +8,4 @@ void swap_doubles(double *d, unsigned count);
float swap_float(float f); float swap_float(float f);
void swap_floats(float *f, unsigned count); void swap_floats(float *f, unsigned count);
void set_nonblocking(int fd); void set_nonblocking(int fd);
double normalise(double v, double min, double max);