Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues 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();
} else {
update_crosstrack();
// calc a rate dampened pitch to the target
calc_rate_nav();

4
ArduCopterMega/Attitude.pde
View File

@ -175,10 +175,10 @@ void calc_nav_throttle()
if(altitude_sensor == BARO){
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{
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;

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, %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(), //distance
DataFlash.ReadByte(), //bitmask

11
ArduCopterMega/commands.pde
View File

@ -2,6 +2,7 @@
void init_commands()
{
// zero is home, but we always load the next command (1), in the code.
g.waypoint_index.set_and_save(0);
// This are registers for the current may and must commands
@ -15,10 +16,10 @@ void init_commands()
void init_auto()
{
if (g.waypoint_index == g.waypoint_total) {
Serial.println("ia_f");
do_RTL();
}
//if (g.waypoint_index == g.waypoint_total) {
// Serial.println("ia_f");
// do_RTL();
//}
// initialize commands
// -------------------
@ -28,7 +29,7 @@ void init_auto()
// forces the loading of a new command
// queue is emptied after a new command is processed
void clear_command_queue(){
next_command.id = CMD_BLANK;
next_command.id = NO_COMMAND;
}
// 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_alt, %ld\n", next_WP.alt);
//update_crosstrack();
return false;
}
bool verify_nav_wp()
{
update_crosstrack();
// Altitude checking
if(next_WP.options & WP_OPTION_ALT_REQUIRED){
// 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
// when a new command is loaded, it is processed with process_XXX()
// If we have a command in the queue,
// nothing to do.
// If we have a command in the queue already
if(next_command.id != NO_COMMAND){
return;
}
@ -55,7 +54,11 @@ void update_commands(void)
increment_WP_index();
// 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
// ---------------------------------
@ -95,12 +99,13 @@ void process_next_command()
// Act on the new command
process_must();
return true;
}
}
// 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 ){
// we remember the index of our mission here
@ -115,6 +120,7 @@ void process_next_command()
Log_Write_Cmd(g.waypoint_index, &next_command);
process_may();
return true;
}
// these are Do/Now commands
@ -126,8 +132,11 @@ void process_next_command()
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(g.waypoint_index, &next_command);
process_now();
return true;
}
}
// we did not need any new commands
return false;
}
/**************************************************/
@ -150,9 +159,6 @@ void process_must()
// implements the Flight Logic
handle_process_must();
// invalidate command queue so a new one is loaded
// -----------------------------------------------
clear_command_queue();
}
void process_may()
@ -163,19 +169,10 @@ void process_may()
command_may_ID = next_command.id;
handle_process_may();
// invalidate command queue so a new one is loaded
// -----------------------------------------------
clear_command_queue();
}
void process_now()
{
Serial.print("pnow");
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
//
#ifndef THROTTLE_BARO_P
# define THROTTLE_BARO_P 0.4
# define THROTTLE_BARO_P 0.3
#endif
#ifndef THROTTLE_BARO_I
# define THROTTLE_BARO_I 0.1
#endif
#ifndef THROTTLE_BARO_D
# define THROTTLE_BARO_D 0.1
# define THROTTLE_BARO_D 0.01
#endif
#ifndef THROTTLE_BARO_IMAX
# define THROTTLE_BARO_IMAX 50
@ -420,13 +420,13 @@
#ifndef THROTTLE_SONAR_P
# define THROTTLE_SONAR_P .8 // upped from .5
# define THROTTLE_SONAR_P 0.8 // upped from .5
#endif
#ifndef THROTTLE_SONAR_I
# define THROTTLE_SONAR_I 0.1
#endif
#ifndef THROTTLE_SONAR_D
# define THROTTLE_SONAR_D 0.1
# define THROTTLE_SONAR_D 0.05
#endif
#ifndef THROTTLE_SONAR_IMAX
# 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 pitch_out = g.rc_2.pwm_out * .707;
motor_out[CH_3] = g.rc_3.radio_out + roll_out + pitch_out;
motor_out[CH_2] = g.rc_3.radio_out + roll_out - pitch_out;
// left
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;
motor_out[CH_4] = g.rc_3.radio_out - roll_out - pitch_out;
// right
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]);
@ -163,14 +165,14 @@ set_servos_4()
int pitch_out = (float)g.rc_2.pwm_out * .5;
//Back side
motor_out[CH_8] = g.rc_3.radio_out - g.rc_2.pwm_out; // CCW
motor_out[CH_1] = g.rc_3.radio_out + roll_out - pitch_out; // CW
motor_out[CH_3] = g.rc_3.radio_out - roll_out - pitch_out; // CW
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, BACK LEFT
motor_out[CH_3] = g.rc_3.radio_out - roll_out - pitch_out; // CW BACK RIGHT
//Front side
motor_out[CH_7] = g.rc_3.radio_out + g.rc_2.pwm_out; // CW
motor_out[CH_2] = g.rc_3.radio_out + roll_out + pitch_out; // CCW
motor_out[CH_4] = g.rc_3.radio_out - roll_out + pitch_out; // CCW
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 FRONT LEFT
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_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_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) {
//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);
#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
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.
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()
{