aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

better Baro hold - Baro_KD was way too high.

Browse Source 
Clean up with WP processing
moved crosstrack call to more logical location

git-svn-id: https://arducopter.googlecode.com/svn/trunk@2259 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-05-14 04:30:42 +00:00
parent 7fa6e35c49
commit 12357746a9
10 changed files with 45 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduCopterMega/ArduCopterMega.pde
View File

@ -1139,6 +1139,9 @@ void update_navigation()
calc_loiter_nav(); calc_loiter_nav();
} else { } else {
update_crosstrack();
// calc a rate dampened pitch to the target // calc a rate dampened pitch to the target
calc_rate_nav(); calc_rate_nav();

4
ArduCopterMega/Attitude.pde
View File

@ -175,10 +175,10 @@ void calc_nav_throttle()
if(altitude_sensor == BARO){ if(altitude_sensor == BARO){
nav_throttle = g.pid_baro_throttle.get_pid(error, delta_ms_fast_loop, scaler); // .25 nav_throttle = g.pid_baro_throttle.get_pid(error, delta_ms_fast_loop, scaler); // .25
nav_throttle = g.throttle_cruise + constrain(nav_throttle, -50, 120); nav_throttle = g.throttle_cruise + constrain(nav_throttle, -70, 140);
}else{ }else{
nav_throttle = g.pid_sonar_throttle.get_pid(error, delta_ms_fast_loop, scaler); // .5 nav_throttle = g.pid_sonar_throttle.get_pid(error, delta_ms_fast_loop, scaler); // .5
nav_throttle = g.throttle_cruise + constrain(nav_throttle, -50, 150); nav_throttle = g.throttle_cruise + constrain(nav_throttle, -70, 150);
} }
nav_throttle = (nav_throttle + nav_throttle_old) >> 1; nav_throttle = (nav_throttle + nav_throttle_old) >> 1;

4
ArduCopterMega/Log.pde
View File

@ -361,9 +361,7 @@ void Log_Read_Nav_Tuning()
"%d, %d, " "%d, %d, "
"%d, %d, %d, %d, " "%d, %d, %d, %d, "
"%d, %d, %d, " "%d, %d, %d, "
"%d, %ld, %4.4f, %4.4f, %4.4f, %4.4f\n" "%d, %ld, %4.4f, %4.4f, %4.4f, %4.4f\n"),
),
DataFlash.ReadInt(), //yaw sensor DataFlash.ReadInt(), //yaw sensor
DataFlash.ReadInt(), //distance DataFlash.ReadInt(), //distance
DataFlash.ReadByte(), //bitmask DataFlash.ReadByte(), //bitmask

11
ArduCopterMega/commands.pde
View File

@ -2,6 +2,7 @@
void init_commands() void init_commands()
{ {
// zero is home, but we always load the next command (1), in the code.
g.waypoint_index.set_and_save(0); g.waypoint_index.set_and_save(0);
// This are registers for the current may and must commands // This are registers for the current may and must commands
@ -15,10 +16,10 @@ void init_commands()
void init_auto() void init_auto()
{ {
if (g.waypoint_index == g.waypoint_total) { //if (g.waypoint_index == g.waypoint_total) {
Serial.println("ia_f"); // Serial.println("ia_f");
do_RTL(); // do_RTL();
} //}
// initialize commands // initialize commands
// ------------------- // -------------------
@ -28,7 +29,7 @@ void init_auto()
// forces the loading of a new command // forces the loading of a new command
// queue is emptied after a new command is processed // queue is emptied after a new command is processed
void clear_command_queue(){ void clear_command_queue(){
next_command.id = CMD_BLANK; next_command.id = NO_COMMAND;
} }
// Getters // Getters

3
ArduCopterMega/commands_logic.pde
View File

@ -374,14 +374,11 @@ bool verify_land()
//Serial.printf("N, %d\n", velocity_land); //Serial.printf("N, %d\n", velocity_land);
//Serial.printf("N_alt, %ld\n", next_WP.alt); //Serial.printf("N_alt, %ld\n", next_WP.alt);
//update_crosstrack();
return false; return false;
} }
bool verify_nav_wp() bool verify_nav_wp()
{ {
update_crosstrack();
// Altitude checking // Altitude checking
if(next_WP.options & WP_OPTION_ALT_REQUIRED){ if(next_WP.options & WP_OPTION_ALT_REQUIRED){
// we desire a certain minimum altitude // we desire a certain minimum altitude

31
ArduCopterMega/commands_process.pde
View File

@ -24,8 +24,7 @@ void update_commands(void)
// This function loads commands into three buffers // This function loads commands into three buffers
// when a new command is loaded, it is processed with process_XXX() // when a new command is loaded, it is processed with process_XXX()
// If we have a command in the queue, // If we have a command in the queue already
// nothing to do.
if(next_command.id != NO_COMMAND){ if(next_command.id != NO_COMMAND){
return; return;
} }
@ -55,7 +54,11 @@ void update_commands(void)
increment_WP_index(); increment_WP_index();
// act on our new command // act on our new command
process_next_command(); if (process_next_command()){
// invalidate command queue so a new one is loaded
// -----------------------------------------------
clear_command_queue();
}
} }
} }
@ -73,7 +76,8 @@ void verify_commands(void)
} }
} }
void process_next_command() bool
process_next_command()
{ {
// these are Navigation/Must commands // these are Navigation/Must commands
// --------------------------------- // ---------------------------------
@ -95,12 +99,13 @@ void process_next_command()
// Act on the new command // Act on the new command
process_must(); process_must();
return true;
} }
} }
// these are Condition/May commands // these are Condition/May commands
// ---------------------- // ----------------------
if (command_may_index == 0){ if (command_may_index == NO_COMMAND){
if (next_command.id > MAV_CMD_NAV_LAST && next_command.id < MAV_CMD_CONDITION_LAST ){ if (next_command.id > MAV_CMD_NAV_LAST && next_command.id < MAV_CMD_CONDITION_LAST ){
// we remember the index of our mission here // we remember the index of our mission here
@ -115,6 +120,7 @@ void process_next_command()
Log_Write_Cmd(g.waypoint_index, &next_command); Log_Write_Cmd(g.waypoint_index, &next_command);
process_may(); process_may();
return true;
} }
// these are Do/Now commands // these are Do/Now commands
@ -126,8 +132,11 @@ void process_next_command()
if (g.log_bitmask & MASK_LOG_CMD) if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(g.waypoint_index, &next_command); Log_Write_Cmd(g.waypoint_index, &next_command);
process_now(); process_now();
return true;
} }
} }
// we did not need any new commands
return false;
} }
/**************************************************/ /**************************************************/
@ -150,9 +159,6 @@ void process_must()
// implements the Flight Logic // implements the Flight Logic
handle_process_must(); handle_process_must();
// invalidate command queue so a new one is loaded
// -----------------------------------------------
clear_command_queue();
} }
void process_may() void process_may()
@ -163,19 +169,10 @@ void process_may()
command_may_ID = next_command.id; command_may_ID = next_command.id;
handle_process_may(); handle_process_may();
// invalidate command queue so a new one is loaded
// -----------------------------------------------
clear_command_queue();
} }
void process_now() void process_now()
{ {
Serial.print("pnow"); Serial.print("pnow");
handle_process_now(); handle_process_now();
// invalidate command queue so a new one is loaded
// -----------------------------------------------
clear_command_queue();
} }

8
ArduCopterMega/config.h
View File

@ -406,13 +406,13 @@
// Throttle control gains // Throttle control gains
// //
#ifndef THROTTLE_BARO_P #ifndef THROTTLE_BARO_P
# define THROTTLE_BARO_P 0.4 # define THROTTLE_BARO_P 0.3
#endif #endif
#ifndef THROTTLE_BARO_I #ifndef THROTTLE_BARO_I
# define THROTTLE_BARO_I 0.1 # define THROTTLE_BARO_I 0.1
#endif #endif
#ifndef THROTTLE_BARO_D #ifndef THROTTLE_BARO_D
# define THROTTLE_BARO_D 0.1 # define THROTTLE_BARO_D 0.01
#endif #endif
#ifndef THROTTLE_BARO_IMAX #ifndef THROTTLE_BARO_IMAX
# define THROTTLE_BARO_IMAX 50 # define THROTTLE_BARO_IMAX 50
@ -420,13 +420,13 @@
#ifndef THROTTLE_SONAR_P #ifndef THROTTLE_SONAR_P
# define THROTTLE_SONAR_P .8 // upped from .5 # define THROTTLE_SONAR_P 0.8 // upped from .5
#endif #endif
#ifndef THROTTLE_SONAR_I #ifndef THROTTLE_SONAR_I
# define THROTTLE_SONAR_I 0.1 # define THROTTLE_SONAR_I 0.1
#endif #endif
#ifndef THROTTLE_SONAR_D #ifndef THROTTLE_SONAR_D
# define THROTTLE_SONAR_D 0.1 # define THROTTLE_SONAR_D 0.05
#endif #endif
#ifndef THROTTLE_SONAR_IMAX #ifndef THROTTLE_SONAR_IMAX
# define THROTTLE_SONAR_IMAX 60 # define THROTTLE_SONAR_IMAX 60

24
ArduCopterMega/motors.pde
View File

@ -94,11 +94,13 @@ set_servos_4()
int roll_out = g.rc_1.pwm_out * .707; int roll_out = g.rc_1.pwm_out * .707;
int pitch_out = g.rc_2.pwm_out * .707; int pitch_out = g.rc_2.pwm_out * .707;
motor_out[CH_3] = g.rc_3.radio_out + roll_out + pitch_out; // left
motor_out[CH_2] = g.rc_3.radio_out + roll_out - pitch_out; motor_out[CH_3] = g.rc_3.radio_out + roll_out + pitch_out; // FRONT
motor_out[CH_2] = g.rc_3.radio_out + roll_out - pitch_out; // BACK
motor_out[CH_1] = g.rc_3.radio_out - roll_out + pitch_out; // right
motor_out[CH_4] = g.rc_3.radio_out - roll_out - pitch_out; motor_out[CH_1] = g.rc_3.radio_out - roll_out + pitch_out; // FRONT
motor_out[CH_4] = g.rc_3.radio_out - roll_out - pitch_out; // BACK
//Serial.printf("\tb4: %d %d %d %d ", motor_out[CH_1], motor_out[CH_2], motor_out[CH_3], motor_out[CH_4]); //Serial.printf("\tb4: %d %d %d %d ", motor_out[CH_1], motor_out[CH_2], motor_out[CH_3], motor_out[CH_4]);
@ -163,14 +165,14 @@ set_servos_4()
int pitch_out = (float)g.rc_2.pwm_out * .5; int pitch_out = (float)g.rc_2.pwm_out * .5;
//Back side //Back side
motor_out[CH_8] = g.rc_3.radio_out - g.rc_2.pwm_out; // CCW motor_out[CH_8] = g.rc_3.radio_out - g.rc_2.pwm_out; // CCW BACK
motor_out[CH_1] = g.rc_3.radio_out + roll_out - pitch_out; // CW motor_out[CH_1] = g.rc_3.radio_out + roll_out - pitch_out; // CW, BACK LEFT
motor_out[CH_3] = g.rc_3.radio_out - roll_out - pitch_out; // CW motor_out[CH_3] = g.rc_3.radio_out - roll_out - pitch_out; // CW BACK RIGHT
//Front side //Front side
motor_out[CH_7] = g.rc_3.radio_out + g.rc_2.pwm_out; // CW motor_out[CH_7] = g.rc_3.radio_out + g.rc_2.pwm_out; // CW FRONT
motor_out[CH_2] = g.rc_3.radio_out + roll_out + pitch_out; // CCW motor_out[CH_2] = g.rc_3.radio_out + roll_out + pitch_out; // CCW FRONT LEFT
motor_out[CH_4] = g.rc_3.radio_out - roll_out + pitch_out; // CCW motor_out[CH_4] = g.rc_3.radio_out - roll_out + pitch_out; // CCW FRONT RIGHT
motor_out[CH_8] += g.rc_4.pwm_out; // CCW motor_out[CH_8] += g.rc_4.pwm_out; // CCW
motor_out[CH_2] += g.rc_4.pwm_out; // CCW motor_out[CH_2] += g.rc_4.pwm_out; // CCW
@ -178,7 +180,7 @@ set_servos_4()
motor_out[CH_1] -= g.rc_4.pwm_out; // CW motor_out[CH_1] -= g.rc_4.pwm_out; // CW
motor_out[CH_7] -= g.rc_4.pwm_out; // CW motor_out[CH_7] -= g.rc_4.pwm_out; // CW
motor_out[CH_3] -= g.rc_4.pwm_out; // CW motor_out[CH_3] -= g.rc_4.pwm_out; // CW
}else if (g.frame_type == Y6_FRAME) { }else if (g.frame_type == Y6_FRAME) {
//Serial.println("Y6_FRAME"); //Serial.println("Y6_FRAME");

2
ArduCopterMega/radio.pde
View File

@ -35,7 +35,7 @@ void init_rc_in()
g.rc_6.set_range(0,300); g.rc_6.set_range(0,300);
#elif CHANNEL_6_TUNING == CH6_BARO_KP #elif CHANNEL_6_TUNING == CH6_BARO_KP
g.rc_6.set_range(0,500); g.rc_6.set_range(0,800);
#elif CHANNEL_6_TUNING == CH6_BARO_KD #elif CHANNEL_6_TUNING == CH6_BARO_KD
g.rc_6.set_range(0,500); g.rc_6.set_range(0,500);

2
ArduCopterMega/system.pde
View File

@ -37,7 +37,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = {
}; };
// Create the top-level menu object. // Create the top-level menu object.
MENU(main_menu, "AC 2.0.3 Beta", main_menu_commands); MENU(main_menu, "AC 2.0.4 Beta", main_menu_commands);
void init_ardupilot() void init_ardupilot()
{ {