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

added new FBW test to develop new nav functions 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1489 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-01-11 21:16:00 +00:00
parent a80b4fbb69
commit 521d7ab424
1 changed files with 116 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

144
ArduCopterMega/test.pde
View File

@ -3,6 +3,7 @@
static int8_t test_radio_pwm(uint8_t argc, const Menu::arg *argv);
static int8_t test_radio(uint8_t argc, const Menu::arg *argv);
static int8_t test_stabilize(uint8_t argc, const Menu::arg *argv);
static int8_t test_fbw(uint8_t argc, const Menu::arg *argv);
static int8_t test_gps(uint8_t argc, const Menu::arg *argv);
static int8_t test_adc(uint8_t argc, const Menu::arg *argv);
static int8_t test_imu(uint8_t argc, const Menu::arg *argv);
@ -41,6 +42,7 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
{"pwm", test_radio_pwm},
{"radio", test_radio},
{"stabilize", test_stabilize},
{"fbw", test_fbw},
{"gps", test_gps},
{"adc", test_adc},
{"imu", test_imu},
@ -233,6 +235,105 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
}
}
static int8_t
test_fbw(uint8_t argc, const Menu::arg *argv)
{
static byte ts_num;
print_hit_enter();
delay(1000);
// setup the radio
// ---------------
init_rc_in();
control_mode = FBW;
//Serial.printf_P(PSTR("pid_stabilize_roll.kP: %4.4f\n"), pid_stabilize_roll.kP());
//Serial.printf_P(PSTR("max_stabilize_dampener:%d\n\n "), max_stabilize_dampener);
motor_armed = true;
trim_radio();
nav_yaw = 8000;
scaleLongDown = 1;
while(1){
// 50 hz
if (millis() - fast_loopTimer > 19) {
deltaMiliSeconds = millis() - fast_loopTimer;
fast_loopTimer = millis();
G_Dt = (float)deltaMiliSeconds / 1000.f;
if(compass_enabled){
medium_loopCounter++;
if(medium_loopCounter == 5){
compass.read(); // Read magnetometer
compass.calculate(roll, pitch); // Calculate heading
medium_loopCounter = 0;
}
}
// for trim features
read_trim_switch();
// Filters radio input - adjust filters in the radio.pde file
// ----------------------------------------------------------
read_radio();
// IMU
// ---
read_AHRS();
// allow us to zero out sensors with control switches
if(rc_5.control_in < 600){
roll_sensor = pitch_sensor = 0;
}
// custom code/exceptions for flight modes
// ---------------------------------------
//update_current_flight_mode();
// write out the servo PWM values
// ------------------------------
//set_servos_4();
ts_num++;
if (ts_num > 10){
dTnav = 200;
//next_WP.lat = random(-3000, 3000);
//next_WP.lng = random(-3000, 3000);
next_WP.lat = 3000;
next_WP.lng = 3000;
GPS.longitude = 0;
GPS.latitude = 0;
calc_nav();
ts_num = 0;
Serial.printf_P(PSTR(" ys:%ld, ny:%ld, ye:%ld, n_lat %ld, n_lon %ld -- n_pit %ld, n_rll %ld\n"),
dcm.yaw_sensor,
nav_yaw,
yaw_error,
nav_lat,
nav_lon,
nav_pitch,
nav_roll);
}
//r: 0, p:0 -- ny:8000, ys:2172, ye:0, n_lat 0, n_lon 0 -- n_pit 0, n_rll 0
// R: 1417, L: 1453 F: 1453 B: 1417
//Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)deltaMiliSeconds, ((int)roll_sensor/100), ((int)pitch_sensor/100), ((uint16_t)yaw_sensor/100));
//Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)deltaMiliSeconds, ((int)roll_sensor/100), ((int)pitch_sensor/100), ((uint16_t)yaw_sensor/100));
if(Serial.available() > 0){
return (0);
}
}
}
}
static int8_t
test_adc(uint8_t argc, const Menu::arg *argv)
{
@ -381,34 +482,21 @@ test_dcm(uint8_t argc, const Menu::arg *argv)
Matrix3f temp_t = dcm.get_dcm_transposed();
Serial.printf_P(PSTR("dcm\n"
"%d \t %d \t %d \n"
"%d \t %d \t %d \n"
"%d \t %d \t %d \n\n"),
(int)(temp.a.x * 100), (int)(temp.a.y * 100), (int)(temp.a.z * 100),
(int)(temp.b.x * 100), (int)(temp.b.y * 100), (int)(temp.b.z * 100),
(int)(temp.c.x * 100), (int)(temp.c.y * 100), (int)(temp.c.z * 100));
Serial.printf_P(PSTR("dcm T\n"
"%d \t %d \t %d \n"
"%d \t %d \t %d \n"
"%d \t %d \t %d \n\n"),
(int)(temp_t.a.x * 100), (int)(temp_t.a.y * 100), (int)(temp_t.a.z * 100),
(int)(temp_t.b.x * 100), (int)(temp_t.b.y * 100), (int)(temp_t.b.z * 100),
(int)(temp_t.c.x * 100), (int)(temp_t.c.y * 100), (int)(temp_t.c.z * 100));
"%4.4f \t %4.4f \t %4.4f \n"
"%4.4f \t %4.4f \t %4.4f \n"
"%4.4f \t %4.4f \t %4.4f \n\n"),
temp.a.x, temp.a.y, temp.a.z,
temp.b.x, temp.b.y, temp.b.z,
temp.c.x, temp.c.y, temp.c.z);
int _pitch = degrees(-asin(temp.c.x));
int _roll = degrees(atan2(temp.c.y, temp.c.z));
int _yaw = degrees(atan2(temp.b.x, temp.a.x));
Serial.printf_P(PSTR( "angles\n"
"%d \t %d \t %d\n\n"),
_pitch, _roll, _yaw);
int _pitch_t = degrees(-asin(temp_t.c.x));
int _roll_t = degrees(atan2(temp_t.c.y, temp_t.c.z));
int _yaw_t = degrees(atan2(temp_t.b.x, temp_t.a.x));
Serial.printf_P(PSTR( "angles_t\n"
"%d \t %d \t %d\n\n"),
_pitch_t, _roll_t, _yaw_t);
_pitch,
_roll,
_yaw);
//_out_vector = _cam_vector * temp;
//Serial.printf_P(PSTR( "cam\n"
@ -651,13 +739,13 @@ test_nav_out(uint8_t argc, const Menu::arg *argv)
while(1){
delay(50);
bearing_error += 100;
bearing_error = wrap_360(bearing_error);
calc_nav_pid();
calc_nav_pitch();
calc_nav_roll();
//bearing_error += 100;
//bearing_error = wrap_360(bearing_error);
//calc_nav_pid();
//calc_nav_pitch();
//calc_nav_roll();
Serial.printf("error %ld,\troll %ld,\tpitch %ld\n", bearing_error, nav_roll, nav_pitch);
//Serial.printf("error %ld,\troll %ld,\tpitch %ld\n", bearing_error, nav_roll, nav_pitch);
if(Serial.available() > 0){
return (0);