diff --git a/libraries/Desktop/support/sitl.cpp b/libraries/Desktop/support/sitl.cpp index 8e4d35c904..e2207ac1bb 100644 --- a/libraries/Desktop/support/sitl.cpp +++ b/libraries/Desktop/support/sitl.cpp @@ -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)); } diff --git a/libraries/Desktop/support/sitl_adc.h b/libraries/Desktop/support/sitl_adc.h index a467a20b7f..95ee7038f5 100644 --- a/libraries/Desktop/support/sitl_adc.h +++ b/libraries/Desktop/support/sitl_adc.h @@ -8,10 +8,14 @@ #include -#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<