mirror of https://github.com/ArduPilot/ardupilot
SITL: raise the ADC noise level to 8 bits when flying
this is about the level of noise of a aircraft that has a lot of vibration
This commit is contained in:
parent
cd4fcb694e
commit
efe5c0142c
|
@ -157,8 +157,11 @@ static void sitl_fdm_input(void)
|
|||
|
||||
}
|
||||
|
||||
// used for noise generation in the ADC code
|
||||
bool sitl_motor_running = false;
|
||||
|
||||
/*
|
||||
send RC outputs to simulator for a quadcopter
|
||||
send RC outputs to simulator
|
||||
*/
|
||||
static void sitl_simulator_output(void)
|
||||
{
|
||||
|
@ -192,6 +195,10 @@ static void sitl_simulator_output(void)
|
|||
// the registers are 2x the PWM value
|
||||
pwm[i] = (*reg[i])/2;
|
||||
}
|
||||
|
||||
// use pwm3 as a proxy for if the motor is running
|
||||
sitl_motor_running = (pwm[2]>1200);
|
||||
|
||||
sendto(sitl_fd, (void*)pwm, sizeof(pwm), MSG_DONTWAIT, (const sockaddr *)&rcout_addr, sizeof(rcout_addr));
|
||||
}
|
||||
|
||||
|
|
|
@ -8,10 +8,14 @@
|
|||
|
||||
#include <stdlib.h>
|
||||
|
||||
#define NOISE_BITS 4
|
||||
#define NOISE_BITS 8
|
||||
|
||||
static inline float noise_generator(void)
|
||||
{
|
||||
extern bool sitl_motor_running;
|
||||
if (!sitl_motor_running) {
|
||||
return 0;
|
||||
}
|
||||
float noise = ((unsigned)random()) & ((1<<NOISE_BITS)-1);
|
||||
noise -= 0.5*(1<<NOISE_BITS);
|
||||
return noise;
|
||||
|
|
Loading…
Reference in New Issue