Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 09:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Yaw fix,

Browse Source 
Logs updated to do better Yaw Debugging
Landing is now rate limited
updated command desctiption.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1911 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-04-20 05:37:05 +00:00
parent 757a25553d
commit fe6e0d2f44
9 changed files with 45 additions and 75 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduCopterMega/ArduCopterMega.pde
View File

@ -375,6 +375,11 @@ long condition_value; // used in condition commands (eg delay, change alt,
long condition_start;
int condition_rate;
// land command
// ------------
long land_start; // when we intiated command in millis()
long original_alt; // altitide reference for start of command
// 3D Location vectors
// -------------------
struct Location home; // home location
@ -385,7 +390,6 @@ struct Location target_WP; // where do we want to you towards?
struct Location tell_command; // command for telemetry
struct Location next_command; // command preloaded
long target_altitude; // used for
//long offset_altitude; // used for
boolean home_is_set; // Flag for if we have g_gps lock and have set the home location

18
ArduCopterMega/Attitude.pde
View File

@ -108,9 +108,9 @@ void
output_yaw_with_hold(boolean hold)
{
// rate control
int dampener;
long rate = degrees(omega.z) * 100; // 3rad = 17188 , 6rad = 34377
rate = constrain(rate, -36000, 36000); // limit to something fun!
int dampener = rate * g.hold_yaw_dampener; // 34377 * .175 = 6000
if(hold){
// look to see if we have exited rate control properly - ie stopped turning
@ -147,7 +147,9 @@ output_yaw_with_hold(boolean hold)
// Apply PID and save the new angle back to RC_Channel
g.rc_4.servo_out = g.pid_yaw.get_pid(yaw_error, delta_ms_fast_loop, 1.0); // .5 * 6000 = 3000
yaw_debug = YAW_HOLD;
// add in yaw dampener
g.rc_4.servo_out -= constrain(dampener, -1800, 1800);
yaw_debug = YAW_HOLD; //0
}else{
// -error = CCW, +error = CW
@ -155,21 +157,19 @@ output_yaw_with_hold(boolean hold)
// we are breaking;
//g.rc_4.servo_out = (omega.z > 0) ? -600 : 600;
// adaptive braking
g.rc_4.servo_out = (int)((1800.0 * omega.z) / 6.0);
g.rc_4.servo_out = (int)((-1800.0 * omega.z) / 1.5);
// -1800 * 0.925 / 6 = -277
yaw_debug = YAW_BRAKE;
yaw_debug = YAW_BRAKE; // 1
}else{
// RATE control
long error = ((long)g.rc_4.control_in * 6) - rate; // control is += 6000 * 6 = 36000
g.rc_4.servo_out = g.pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 1.0); // kP .07 * 36000 = 2520
yaw_debug = YAW_RATE;
yaw_debug = YAW_RATE; // 2
}
}
// Limit dampening to be equal to propotional term for symmetry
dampener = rate * g.hold_yaw_dampener; // 34377 * .175 = 6000
g.rc_4.servo_out -= constrain(dampener, -1800, 1800);
// Limit Output
g.rc_4.servo_out = constrain(g.rc_4.servo_out, -1800, 1800); // limit to 24°

11
ArduCopterMega/command description.txt
View File

@ -22,13 +22,12 @@ Command Structure in bytes
Commands below MAV_CMD_NAV_LAST are commands that have a end criteria, eg "reached waypoint" or "reached altitude"
***********************************
Command ID Name Parameter 1 Altitude Latitude Longitude
0x10 / 16 MAV_CMD_NAV_WAYPOINT delay (seconds) altitude lat lon
0x10 / 16 MAV_CMD_NAV_WAYPOINT delay (seconds) altitude lat lon
0x11 / 17 MAV_CMD_NAV_LOITER_UNLIM (forever) altitude lat lon
0x12 / 18 MAV_CMD_NAV_LOITER_TURNS turns altitude lat lon
0x13 / 19 MAV_CMD_NAV_LOITER_TIME - time (seconds) - -
0x14 / 20 MAV_CMD_NAV_RETURN_TO_LAUNCH - altitude lat lon
0x15 / 21 MAV_CMD_NAV_LAND - altitude lat lon
0x16 / 22 MAV_CMD_NAV_TAKEOFF takeoff pitch altitude - -
0x14 / 20 MAV_CMD_NAV_RETURN_TO_LAUNCH - - - -
0x15 / 21 MAV_CMD_NAV_LAND - - - -
0x16 / 22 MAV_CMD_NAV_TAKEOFF - altitude - -
0x17 / 23 MAV_CMD_NAV_TARGET - altitude lat lon
NOTE: for command 0x16 the value takeoff pitch specifies the minimum pitch for the case with airspeed sensor and the target pitch for the case without.

6
ArduCopterMega/commands.pde
View File

@ -49,13 +49,13 @@ struct Location get_wp_with_index(int i)
temp.p1 = eeprom_read_byte((uint8_t*)mem);
mem++;
temp.alt = (long)eeprom_read_dword((uint32_t*)mem); // alt is stored in CM!
temp.alt = (long)eeprom_read_dword((uint32_t*)mem); // alt is stored in CM! Alt is stored relative!
mem += 4;
temp.lat = (long)eeprom_read_dword((uint32_t*)mem);
temp.lat = (long)eeprom_read_dword((uint32_t*)mem); // lat is stored in decimal * 10,000,000
mem += 4;
temp.lng = (long)eeprom_read_dword((uint32_t*)mem);
temp.lng = (long)eeprom_read_dword((uint32_t*)mem); // lon is stored in decimal * 10,000,000
}
// Add on home altitude if we are a nav command

16
ArduCopterMega/commands_logic.pde
View File

@ -246,18 +246,20 @@ void do_land()
Serial.println("dlnd ");
land_complete = false; // set flag to use g_gps ground course during TO. IMU will be doing yaw drift correction
velocity_land = 2000;
land_start = millis();
original_alt = current_loc.alt;
Location temp = current_loc;
//Location temp = current_loc;
//temp.alt = home.alt;
// just go down far
temp.alt = -100000;
//temp.alt = -100000;
set_next_WP(&temp);
set_next_WP(&current_loc);
}
void do_loiter_unlimited()
{
Serial.println("dloi ");
//Serial.println("dloi ");
if(next_command.lat == 0)
set_next_WP(&current_loc);
else
@ -310,12 +312,14 @@ bool verify_takeoff()
bool verify_land()
{
//Serial.print("vlnd ");
// land at 1 meter per second
next_WP.alt = original_alt - ((millis() - land_start) / 10); // condition_value = our initial
velocity_land = ((old_alt - current_loc.alt) *.2) + (velocity_land * .8);
old_alt = current_loc.alt;
if(g.sonar_enabled){
// decide which sensor we're usings
// decide which sensor we're using
if(sonar_alt < 40){
land_complete = true;
Serial.println("Y");

1
ArduCopterMega/config.h
View File

@ -515,7 +515,6 @@
#ifndef ALT_HOLD_HOME
# define ALT_HOLD_HOME 8
#endif
#define ALT_HOLD_HOME_CM ALT_HOLD_HOME * 100
#ifndef USE_CURRENT_ALT
# define USE_CURRENT_ALT FALSE

3
ArduCopterMega/setup.pde
View File

@ -786,9 +786,10 @@ void report_wp(byte index = 255)
void print_wp(struct Location *cmd, byte index)
{
Serial.printf_P(PSTR("command #: %d id:%d p1:%d p2:%ld p3:%ld p4:%ld \n"),
Serial.printf_P(PSTR("command #: %d id:%d op:%d p1:%d p2:%ld p3:%ld p4:%ld \n"),
(int)index,
(int)cmd->id,
(int)cmd->options,
(int)cmd->p1,
cmd->alt,
cmd->lat,

2
ArduCopterMega/system.pde
View File

@ -105,7 +105,7 @@ void init_ardupilot()
Serial.printf_P(PSTR("Please Run Setup...\n"));
while (true) {
delay(100);
delay(1000);
if(motor_light){
digitalWrite(A_LED_PIN, HIGH);
digitalWrite(B_LED_PIN, HIGH);

57
ArduCopterMega/test.pde
View File

@ -735,59 +735,22 @@ test_xbee(uint8_t argc, const Menu::arg *argv)
static int8_t
test_altitude(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR("Uncalibrated relative airpressure\n"));
print_hit_enter();
home.alt = 0;
wp_distance = 0;
init_barometer();
reset_I();
// to prevent boost from skewing results
cos_pitch_x = cos_roll_x = 1;
while(1){
if (millis() - fast_loopTimer > 100) {
delta_ms_fast_loop = millis() - fast_loopTimer;
G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator
fast_loopTimer = millis();
}
if (millis() - medium_loopTimer > 100) {
medium_loopTimer = millis();
read_radio(); // read the radio first
next_WP.alt = home.alt + g.rc_6.control_in; // 0 - 2000 (20 meters)
next_WP.alt = max(next_WP.alt, 30);
read_trim_switch();
update_alt();
output_auto_throttle();
Serial.printf_P(PSTR("B_alt: %ld, S_alt: %ld, \tnext_alt: %ld \terror: %ld, \tcruise: %d, \tint: %6.2f \tout:%d\n"),
baro_alt,
sonar_alt,
next_WP.alt,
altitude_error,
(int)g.throttle_cruise,
g.pid_baro_throttle.get_integrator(),
g.rc_3.servo_out);
/*
Serial.print("Altitude: ");
Serial.print((int)current_loc.alt,DEC);
Serial.print("\tnext_alt: ");
Serial.print((int)next_WP.alt,DEC);
Serial.print("\talt_err: ");
Serial.print((int)altitude_error,DEC);
Serial.print("\ttNom: ");
Serial.print(g.,DEC);
Serial.print("\ttOut: ");
Serial.println(g.rc_3.servo_out,DEC);
*/
//Serial.print(" Raw pressure value: ");
//Serial.println(abs_pressure);
delay(100);
baro_alt = read_barometer();
if(g.sonar_enabled){
// decide which sensor we're usings
sonar_alt = sonar.read();
}
Serial.printf_P(PSTR("B_alt: %ld, S_alt: %ld\n"),
baro_alt,
sonar_alt);
if(Serial.available() > 0){
return (0);