diff --git a/libraries/Desktop/support/desktop.h b/libraries/Desktop/support/desktop.h index 814117b996..b6ff35e4f7 100644 --- a/libraries/Desktop/support/desktop.h +++ b/libraries/Desktop/support/desktop.h @@ -4,10 +4,16 @@ #include #include +enum vehicle_type { + ArduCopter, + APMrover2, + ArduPlane +}; + struct desktop_info { bool slider; // slider switch state, True means CLI mode struct timeval sketch_start_time; - bool quadcopter; // use quadcopter outputs + enum vehicle_type vehicle; unsigned framerate; float initial_height; bool console_mode; diff --git a/libraries/Desktop/support/main.cpp b/libraries/Desktop/support/main.cpp index 2cf78b335a..7869fe7aff 100644 --- a/libraries/Desktop/support/main.cpp +++ b/libraries/Desktop/support/main.cpp @@ -67,12 +67,22 @@ int main(int argc, char * const argv[]) } } + printf("Starting sketch '%s'\n", SKETCH); + if (strcmp(SKETCH, "ArduCopter") == 0) { - desktop_state.quadcopter = true; + desktop_state.vehicle = ArduCopter; if (desktop_state.framerate == 0) { desktop_state.framerate = 200; } + } else if (strcmp(SKETCH, "APMrover2") == 0) { + desktop_state.vehicle = APMrover2; + if (desktop_state.framerate == 0) { + desktop_state.framerate = 50; + } + // set right default throttle for rover (allowing for reverse) + ICR4.set(2, 1500); } else { + desktop_state.vehicle = ArduPlane; if (desktop_state.framerate == 0) { desktop_state.framerate = 50; } diff --git a/libraries/Desktop/support/sitl.cpp b/libraries/Desktop/support/sitl.cpp index 44e983b6eb..dd228dcf49 100644 --- a/libraries/Desktop/support/sitl.cpp +++ b/libraries/Desktop/support/sitl.cpp @@ -144,8 +144,7 @@ static void sitl_fdm_input(void) } // used for noise generation in the ADC code -// motor speed in revolutions per second -float sitl_motor_speed[4] = {0,0,0,0}; +bool sitl_motors_on; /* send RC outputs to simulator @@ -169,14 +168,18 @@ static void sitl_simulator_output(void) for (i=0; i<11; i++) { (*reg[i]) = 1000*2; } - if (!desktop_state.quadcopter) { + if (desktop_state.vehicle == ArduPlane) { (*reg[0]) = (*reg[1]) = (*reg[3]) = 1500*2; (*reg[7]) = 1800*2; } + if (desktop_state.vehicle == APMrover2) { + (*reg[0]) = (*reg[1]) = (*reg[2]) = (*reg[3]) = 1500*2; + (*reg[7]) = 1800*2; + } } // output at chosen framerate - if (millis() - last_update < 1000/desktop_state.framerate) { + if (last_update != 0 && millis() - last_update < 1000/desktop_state.framerate) { return; } last_update = millis(); @@ -189,19 +192,27 @@ static void sitl_simulator_output(void) } } - if (!desktop_state.quadcopter) { + if (desktop_state.vehicle == ArduPlane) { // add in engine multiplier if (control.pwm[2] > 1000) { control.pwm[2] = ((control.pwm[2]-1000) * sitl.engine_mul) + 1000; if (control.pwm[2] > 2000) control.pwm[2] = 2000; } - - // 400kV motor, 16V - sitl_motor_speed[0] = ((control.pwm[2]-1000)/1000.0) * 400 * 16 / 60.0; + sitl_motors_on = ((control.pwm[2]-1000)/1000.0) > 0; + } else if (desktop_state.vehicle == APMrover2) { + // add in engine multiplier + if (control.pwm[2] != 1500) { + control.pwm[2] = ((control.pwm[2]-1500) * sitl.engine_mul) + 1500; + if (control.pwm[2] > 2000) control.pwm[2] = 2000; + if (control.pwm[2] < 1000) control.pwm[2] = 1000; + } + sitl_motors_on = ((control.pwm[2]-1500)/500.0) != 0; } else { - // 850kV motor, 16V + sitl_motors_on = false; for (i=0; i<4; i++) { - sitl_motor_speed[i] = ((control.pwm[i]-1000)/1000.0) * 850 * 12 / 60.0; + if ((control.pwm[i]-1000)/1000.0 > 0) { + sitl_motors_on = true; + } } } diff --git a/libraries/Desktop/support/sitl_adc.cpp b/libraries/Desktop/support/sitl_adc.cpp index e71e0b5c8f..fcd7e82f3a 100644 --- a/libraries/Desktop/support/sitl_adc.cpp +++ b/libraries/Desktop/support/sitl_adc.cpp @@ -91,23 +91,16 @@ void sitl_update_adc(float roll, float pitch, float yaw, // Relative to earth const float _accel_scale = 9.80665 / 423.8; double adc[7]; double p, q, r; - extern float sitl_motor_speed[4]; - bool motors_on = false; + extern bool sitl_motors_on; SITL::convert_body_frame(roll, pitch, rollRate, pitchRate, yawRate, &p, &q, &r); - for (uint8_t i=0; i<4; i++) { - if (sitl_motor_speed[i] > 0.0) { - motors_on = true; - } - } - // minimum noise levels are 2 bits float accel_noise = _accel_scale*2; float gyro_noise = _gyro_gain_y*2; - if (motors_on) { + if (sitl_motors_on) { // add extra noise when the motors are on accel_noise += sitl.accel_noise; gyro_noise += ToRad(sitl.gyro_noise);