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

desktop: normalise roll and pitch rates

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,
ft2m(d.fg_pkt.altitude),
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_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++;
sim_input_count++;
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;
static uint32_t last_update;
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 */
xAccel = sin(ToRad(pitch));
@ -73,12 +73,15 @@ void sitl_update_adc(float roll, float pitch, float yaw, float airspeed)
rollRate = 0;
pitchRate = 0;
yawRate = 0;
delta_t = millis();
delta_t = micros();
} else {
delta_t = millis() - last_update;
rollRate = 1000.0 * (roll - last_roll) / delta_t;
pitchRate = 1000.0 * (pitch - last_pitch) / delta_t;
yawRate = 1000.0 * (yaw - last_yaw) / delta_t;
delta_t = micros() - last_update;
rollRate = 1.0e6 * (roll - last_roll) / delta_t;
pitchRate = 1.0e6 * (pitch - last_pitch) / 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;
@ -109,8 +112,8 @@ void sitl_update_adc(float roll, float pitch, float yaw, float airspeed)
#if 0
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",
roll, pitch, dcm.roll_sensor/100.0, dcm.pitch_sensor/100.0, rollRate, pitchRate);
printf("roll=%6.1f / %6.1f pitch=%6.1f / %6.1f rollRate=%6.1f pitchRate=%6.1f\n",
roll, dcm.roll_sensor/100.0, pitch, dcm.pitch_sensor/100.0, rollRate, pitchRate);
#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);
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);
void swap_floats(float *f, unsigned count);
void set_nonblocking(int fd);
double normalise(double v, double min, double max);