mirror of https://github.com/ArduPilot/ardupilot
fixed FBW bugs
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1560 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
parent
b8e53931b9
commit
b4abbacbd8
|
@ -10,7 +10,7 @@
|
|||
#define MAGORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
|
||||
|
||||
|
||||
#define ACRO_RATE_TRIGGER 4200
|
||||
//#define ACRO_RATE_TRIGGER 4200
|
||||
// if you want full ACRO mode, set value to 0
|
||||
// if you want safe ACRO mode, set value to 100
|
||||
// if you want mostly stabilize with flips, set value to 4200
|
||||
|
|
|
@ -863,6 +863,7 @@ void update_current_flight_mode(void)
|
|||
case FBW:
|
||||
// we are currently using manual throttle during alpha testing.
|
||||
fbw_timer++;
|
||||
|
||||
//call at 5 hz
|
||||
if(fbw_timer > 20){
|
||||
fbw_timer = 0;
|
||||
|
@ -877,8 +878,8 @@ void update_current_flight_mode(void)
|
|||
dTnav = 200;
|
||||
}
|
||||
|
||||
next_WP.lat = home.lat + rc_1.control_in / 5; // 10 meteres
|
||||
next_WP.lng = home.lng - rc_2.control_in / 5; // 10 meteres
|
||||
next_WP.lat = home.lat + rc_1.control_in / 5; // 4500 / 5 = 900 = 10 meteres
|
||||
next_WP.lng = home.lng - rc_2.control_in / 5; // 4500 / 5 = 900 = 10 meteres
|
||||
}
|
||||
|
||||
// Yaw control
|
||||
|
|
|
@ -273,7 +273,7 @@
|
|||
#endif
|
||||
|
||||
#ifndef ACRO_RATE_TRIGGER
|
||||
# define ACRO_RATE_TRIGGER 4200
|
||||
# define ACRO_RATE_TRIGGER 100
|
||||
#endif
|
||||
|
||||
|
||||
|
|
|
@ -153,7 +153,7 @@ set_servos_4()
|
|||
num++;
|
||||
if (num > 10){
|
||||
num = 0;
|
||||
Serial.print("!");
|
||||
//Serial.print("!");
|
||||
//debugging with Channel 6
|
||||
|
||||
/*
|
||||
|
@ -211,7 +211,7 @@ set_servos_4()
|
|||
num++;
|
||||
if (num > 10){
|
||||
num = 0;
|
||||
Serial.print("-");
|
||||
//Serial.print("-");
|
||||
}
|
||||
|
||||
if(rc_3.control_in > 0){
|
||||
|
|
|
@ -69,11 +69,11 @@ void calc_nav()
|
|||
nav_lat = constrain(nav_lat, -DIST_ERROR_MAX, DIST_ERROR_MAX); // Limit max command
|
||||
|
||||
// rotate the vector
|
||||
nav_roll = (float)nav_lon * yawvector.x - (float)nav_lat * yawvector.y;
|
||||
nav_pitch = (float)nav_lon * yawvector.y + (float)nav_lat * yawvector.x;
|
||||
|
||||
nav_roll = constrain(nav_roll, -pitch_max, pitch_max);
|
||||
nav_pitch = constrain(nav_pitch, -pitch_max, pitch_max);
|
||||
nav_roll = (float)nav_lat * yawvector.x - (float)nav_lon * yawvector.y;
|
||||
nav_pitch = (float)nav_lon * yawvector.x + (float)nav_lat * yawvector.y;
|
||||
Serial.printf("vx %4.4f,vy %4.4f, nr %ld, np %ld ", yawvector.x, yawvector.y, nav_roll, nav_pitch);
|
||||
//nav_roll = constrain(nav_roll, -pitch_max, pitch_max);
|
||||
//nav_pitch = constrain(nav_pitch, -pitch_max, pitch_max);
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
|
@ -2,9 +2,9 @@ void init_rc_in()
|
|||
{
|
||||
read_EEPROM_radio(); // read Radio limits
|
||||
rc_1.set_angle(4500);
|
||||
rc_1.dead_zone = 60;
|
||||
rc_1.dead_zone = 60; // 60 = .6 degrees
|
||||
rc_2.set_angle(4500);
|
||||
rc_2.dead_zone = 60;
|
||||
rc_2.dead_zone = 60;
|
||||
rc_3.set_range(0,1000);
|
||||
rc_3.dead_zone = 20;
|
||||
rc_3.scale_output = .9;
|
||||
|
|
|
@ -164,24 +164,26 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
|
|||
oldSwitchPosition = readSwitch();
|
||||
|
||||
Serial.printf_P(PSTR("Unplug battery, throttle in neutral, turn off radio.\n"));
|
||||
while(rc_3.control_in > 0){
|
||||
delay(20);
|
||||
read_radio();
|
||||
}
|
||||
|
||||
while(1){
|
||||
delay(20);
|
||||
|
||||
// Filters radio input - adjust filters in the radio.pde file
|
||||
// ----------------------------------------------------------
|
||||
read_radio();
|
||||
|
||||
if(rc_3.control_in > 0){
|
||||
Serial.printf_P(PSTR("THROTTLE ERROR %d \n"), rc_3.control_in);
|
||||
Serial.printf_P(PSTR("THROTTLE CHANGED %d \n"), rc_3.control_in);
|
||||
fail_test++;
|
||||
}
|
||||
|
||||
if(oldSwitchPosition != readSwitch()){
|
||||
Serial.printf_P(PSTR("MODE CHANGE: "));
|
||||
Serial.printf_P(PSTR("CONTROL MODE CHANGED: "));
|
||||
Serial.println(flight_mode_strings[readSwitch()]);
|
||||
fail_test++;
|
||||
}
|
||||
|
||||
if(throttle_failsafe_enabled && rc_3.get_failsafe()){
|
||||
Serial.printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), rc_3.radio_in);
|
||||
Serial.println(flight_mode_strings[readSwitch()]);
|
||||
|
@ -339,49 +341,39 @@ test_fbw(uint8_t argc, const Menu::arg *argv)
|
|||
|
||||
// custom code/exceptions for flight modes
|
||||
// ---------------------------------------
|
||||
//update_current_flight_mode();
|
||||
update_current_flight_mode();
|
||||
|
||||
// write out the servo PWM values
|
||||
// ------------------------------
|
||||
//set_servos_4();
|
||||
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;
|
||||
if (ts_num == 5){
|
||||
// 10 hz
|
||||
ts_num = 0;
|
||||
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"),
|
||||
|
||||
Serial.printf_P(PSTR(" ys:%ld, next_WP.lat:%ld, next_WP.lng:%ld, n_lat:%ld, n_lon:%ld \n"),
|
||||
dcm.yaw_sensor,
|
||||
nav_yaw,
|
||||
yaw_error,
|
||||
next_WP.lat,
|
||||
next_WP.lng,
|
||||
nav_lat,
|
||||
nav_lon,
|
||||
nav_pitch,
|
||||
nav_roll);
|
||||
|
||||
//print_motor_out();
|
||||
}
|
||||
//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)delta_ms_fast_loop, ((int)dcm.roll_sensor/100), ((int)dcm.pitch_sensor/100), ((uint16_t)dcm.yaw_sensor/100));
|
||||
//Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)delta_ms_fast_loop, ((int)dcm.roll_sensor/100), ((int)dcm.pitch_sensor/100), ((uint16_t)dcm.yaw_sensor/100));
|
||||
|
||||
if(Serial.available() > 0){
|
||||
return (0);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static int8_t
|
||||
test_adc(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
|
|
Loading…
Reference in New Issue