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

SITL: cleanup RC control for rover support 

added vehicle type, and setup right initial throttle
This commit is contained in:
Andrew Tridgell 2012-11-28 08:56:39 +11:00
parent 919d0ac8fd
commit 471ed9429a
4 changed files with 41 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/Desktop/support/desktop.h
View File

@ -4,10 +4,16 @@
#include <unistd.h> #include <unistd.h>
#include <sys/time.h> #include <sys/time.h>
enum vehicle_type {
ArduCopter,
APMrover2,
ArduPlane
};
struct desktop_info { struct desktop_info {
bool slider; // slider switch state, True means CLI mode bool slider; // slider switch state, True means CLI mode
struct timeval sketch_start_time; struct timeval sketch_start_time;
bool quadcopter; // use quadcopter outputs enum vehicle_type vehicle;
unsigned framerate; unsigned framerate;
float initial_height; float initial_height;
bool console_mode; bool console_mode;

12
libraries/Desktop/support/main.cpp
View File

@ -67,12 +67,22 @@ int main(int argc, char * const argv[])
} }
} }
printf("Starting sketch '%s'\n", SKETCH);
if (strcmp(SKETCH, "ArduCopter") == 0) { if (strcmp(SKETCH, "ArduCopter") == 0) {
desktop_state.quadcopter = true; desktop_state.vehicle = ArduCopter;
if (desktop_state.framerate == 0) { if (desktop_state.framerate == 0) {
desktop_state.framerate = 200; 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 { } else {
desktop_state.vehicle = ArduPlane;
if (desktop_state.framerate == 0) { if (desktop_state.framerate == 0) {
desktop_state.framerate = 50; desktop_state.framerate = 50;
} }

31
libraries/Desktop/support/sitl.cpp
View File

@ -144,8 +144,7 @@ static void sitl_fdm_input(void)
} }
// used for noise generation in the ADC code // used for noise generation in the ADC code
// motor speed in revolutions per second bool sitl_motors_on;
float sitl_motor_speed[4] = {0,0,0,0};
/* /*
send RC outputs to simulator send RC outputs to simulator
@ -169,14 +168,18 @@ static void sitl_simulator_output(void)
for (i=0; i<11; i++) { for (i=0; i<11; i++) {
(*reg[i]) = 1000*2; (*reg[i]) = 1000*2;
} }
if (!desktop_state.quadcopter) { if (desktop_state.vehicle == ArduPlane) {
(*reg[0]) = (*reg[1]) = (*reg[3]) = 1500*2; (*reg[0]) = (*reg[1]) = (*reg[3]) = 1500*2;
(*reg[7]) = 1800*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 // output at chosen framerate
if (millis() - last_update < 1000/desktop_state.framerate) { if (last_update != 0 && millis() - last_update < 1000/desktop_state.framerate) {
return; return;
} }
last_update = millis(); 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 // add in engine multiplier
if (control.pwm[2] > 1000) { if (control.pwm[2] > 1000) {
control.pwm[2] = ((control.pwm[2]-1000) * sitl.engine_mul) + 1000; control.pwm[2] = ((control.pwm[2]-1000) * sitl.engine_mul) + 1000;
if (control.pwm[2] > 2000) control.pwm[2] = 2000; if (control.pwm[2] > 2000) control.pwm[2] = 2000;
} }
sitl_motors_on = ((control.pwm[2]-1000)/1000.0) > 0;
// 400kV motor, 16V } else if (desktop_state.vehicle == APMrover2) {
sitl_motor_speed[0] = ((control.pwm[2]-1000)/1000.0) * 400 * 16 / 60.0; // 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 { } else {
// 850kV motor, 16V sitl_motors_on = false;
for (i=0; i<4; i++) { 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;
}
} }
} }

11
libraries/Desktop/support/sitl_adc.cpp
View File

@ -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; const float _accel_scale = 9.80665 / 423.8;
double adc[7]; double adc[7];
double p, q, r; double p, q, r;
extern float sitl_motor_speed[4]; extern bool sitl_motors_on;
bool motors_on = false;
SITL::convert_body_frame(roll, pitch, SITL::convert_body_frame(roll, pitch,
rollRate, pitchRate, yawRate, rollRate, pitchRate, yawRate,
&p, &q, &r); &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 // minimum noise levels are 2 bits
float accel_noise = _accel_scale*2; float accel_noise = _accel_scale*2;
float gyro_noise = _gyro_gain_y*2; float gyro_noise = _gyro_gain_y*2;
if (motors_on) { if (sitl_motors_on) {
// add extra noise when the motors are on // add extra noise when the motors are on
accel_noise += sitl.accel_noise; accel_noise += sitl.accel_noise;
gyro_noise += ToRad(sitl.gyro_noise); gyro_noise += ToRad(sitl.gyro_noise);