diff --git a/libraries/Desktop/support/sitl.cpp b/libraries/Desktop/support/sitl.cpp index e2207ac1bb..5e6ecfdbae 100644 --- a/libraries/Desktop/support/sitl.cpp +++ b/libraries/Desktop/support/sitl.cpp @@ -158,7 +158,8 @@ static void sitl_fdm_input(void) } // used for noise generation in the ADC code -bool sitl_motor_running = false; +// motor speed in revolutions per second +float sitl_motor_speed[4] = {0,0,0,0}; /* send RC outputs to simulator @@ -196,8 +197,15 @@ static void sitl_simulator_output(void) pwm[i] = (*reg[i])/2; } - // use pwm3 as a proxy for if the motor is running - sitl_motor_running = (pwm[2]>1200); + if (!desktop_state.quadcopter) { + // 400kV motor, 16V + sitl_motor_speed[0] = ((pwm[2]-1000)/1000.0) * 400 * 16 / 60.0; + } else { + // 850kV motor, 16V + for (i=0; i<4; i++) { + sitl_motor_speed[i] = ((pwm[i]-1000)/1000.0) * 850 * 12 / 60.0; + } + } 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 95ee7038f5..65b56315ab 100644 --- a/libraries/Desktop/support/sitl_adc.h +++ b/libraries/Desktop/support/sitl_adc.h @@ -7,18 +7,36 @@ #define _SITL_ADC_H #include +#include -#define NOISE_BITS 8 +static float noise_scale[8] = { 240, 400, 500, 200, 400, 400, 2000, 200 }; -static inline float noise_generator(void) +// generate a random float between -1 and 1 +static double rand_float(void) { - extern bool sitl_motor_running; - if (!sitl_motor_running) { + float ret = ((unsigned)random()) % 2000000; + return (ret - 1.0e6) / 1.0e6; +} + +static inline float noise_generator(uint8_t chan) +{ + extern float sitl_motor_speed[4]; + extern long unsigned int micros(void); + uint8_t i; + float noise = 0; + uint8_t noise_count=0; + double t = micros() / 1.0e6; + for (i=0; i<4; i++) { + if (sitl_motor_speed[i] > 0.0) { + float n = rand_float() * noise_scale[chan]; + noise += sin(fmod(t * sitl_motor_speed[i] * 2 * 3.14 + i, 2*3.14)) * n; + noise_count++; + } + } + if (noise_count == 0) { return 0; } - float noise = ((unsigned)random()) & ((1<= 0x1000) { next_value = 0xFFF; }