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

alt hold updates 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1633 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-02-13 05:21:32 +00:00
parent 1e63c2f368
commit 1f46609eca
8 changed files with 34 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
ArduCopterMega/ArduCopterMega.pde
View File

@ -298,6 +298,7 @@ long nav_yaw; // deg * 100 : target yaw angle
long nav_lat; // for error calcs
long nav_lon; // for error calcs
int nav_throttle; // 0-1000 for throttle control
int nav_throttle_old; // 0-1000 for throttle control
long command_yaw_start; // what angle were we to begin with
long command_yaw_start_time; // when did we start turning
@ -708,6 +709,7 @@ void update_GPS(void)
GPS.update();
update_GPS_light();
// !!! comment out after testing
//fake_out_gps();
if (GPS.new_data && GPS.fix) {
@ -768,11 +770,6 @@ void update_current_flight_mode(void)
// break;
default:
// Intput Pitch, Roll, Yaw and Throttle
// ------------------------------------
//calc_nav_pid();
//calc_nav_roll();
//calc_nav_pitch();
// based on altitude error
// -----------------------
@ -798,8 +795,6 @@ void update_current_flight_mode(void)
switch(control_mode){
case ACRO:
// Intput Pitch, Roll, Yaw and Throttle
// ------------------------------------
// clear any AP naviagtion values
nav_pitch = 0;
nav_roll = 0;
@ -835,8 +830,6 @@ void update_current_flight_mode(void)
break;
case STABILIZE:
// Intput Pitch, Roll, Yaw and Throttle
// ------------------------------------
// clear any AP naviagtion values
nav_pitch = 0;
nav_roll = 0;
@ -903,8 +896,6 @@ void update_current_flight_mode(void)
break;
case ALT_HOLD:
// Intput Pitch, Roll, Yaw and Throttle
// ------------------------------------
// clear any AP naviagtion values
nav_pitch = 0;
nav_roll = 0;
@ -913,6 +904,9 @@ void update_current_flight_mode(void)
// get desired height from the throttle
next_WP.alt = home.alt + (rc_3.control_in * 4); // 0 - 1000 (40 meters)
// !!! testing
//next_WP.alt -= 500;
// Yaw control
// -----------
output_manual_yaw();
@ -934,13 +928,7 @@ void update_current_flight_mode(void)
output_stabilize_pitch();
break;
case RTL:
// Intput Pitch, Roll, Yaw and Throttle
// ------------------------------------
//calc_nav_pid();
//calc_nav_roll();
//calc_nav_pitch();
case RTL:
// based on altitude error
// -----------------------
calc_nav_throttle();
@ -961,12 +949,7 @@ void update_current_flight_mode(void)
break;
case POSITION_HOLD:
// Intput Pitch, Roll, Yaw and Throttle
// ------------------------------------
//calc_nav_pid();
//calc_nav_roll();
//calc_nav_pitch();
// Yaw control
// -----------
output_manual_yaw();

8
ArduCopterMega/commands.pde
View File

@ -133,7 +133,9 @@ void
set_current_loc_here()
{
//struct Location temp;
set_next_WP(&current_loc);
Location l = current_loc;
l.alt = get_altitude_above_home();
set_next_WP(&l);
}
/*
@ -160,8 +162,8 @@ set_next_WP(struct Location *wp)
// used to control FBW and limit the rate of climb
// -----------------------------------------------
target_altitude = current_loc.alt;
offset_altitude = next_WP.alt - current_loc.alt;
target_altitude = current_loc.alt; // PH: target_altitude = 200
offset_altitude = next_WP.alt - current_loc.alt; // PH: offset_altitude = 0
// zero out our loiter vals to watch for missed waypoints
loiter_delta = 0;

4
ArduCopterMega/config.h
View File

@ -386,10 +386,10 @@
# define THROTTLE_BARO_I 0.05
#endif
#ifndef THROTTLE_BARO_D
# define THROTTLE_BARO_D 0.06
# define THROTTLE_BARO_D 0.2
#endif
#ifndef THROTTLE_BARO_IMAX
# define THROTTLE_BARO_IMAX 100
# define THROTTLE_BARO_IMAX 50
#endif
#ifndef THROTTLE_SONAR_P

9
ArduCopterMega/flight_control.pde
View File

@ -28,13 +28,15 @@ void calc_nav_throttle()
float t = pid_baro_throttle.kP();
if(error > 0){
//pid_baro_throttle.kP(.25);
pid_baro_throttle.kP(t);
}else{
pid_baro_throttle.kP(t/4.0);
}
// limit output of throttle control
nav_throttle = pid_baro_throttle.get_pid(error, delta_ms_fast_loop, 1.0);
nav_throttle = throttle_cruise + constrain(nav_throttle, -20, 70);
nav_throttle = throttle_cruise + constrain(nav_throttle, -30, 80);
pid_baro_throttle.kP(t);
} else {
@ -44,6 +46,9 @@ void calc_nav_throttle()
// limit output of throttle control
nav_throttle = throttle_cruise + constrain(nav_throttle, -60, 100);
}
nav_throttle = (nav_throttle + nav_throttle_old) >> 1;
nav_throttle_old = nav_throttle;
}
float angle_boost()

10
ArduCopterMega/motors.pde
View File

@ -182,6 +182,7 @@ set_servos_4()
write_int(motor_out[CH_3]);
write_int(motor_out[CH_4]);
write_int(rc_3.servo_out);
write_int((int)(cos_yaw_x * 100));
write_int((int)(sin_yaw_y * 100));
write_int((int)(dcm.yaw_sensor / 100));
@ -192,11 +193,12 @@ set_servos_4()
write_int((int)nav_roll);
write_int((int)nav_pitch);
//24
write_long(home.lat); //28
write_long(home.lng); //32
write_int((int)home.alt); //34
write_long(current_loc.lat); //28
write_long(current_loc.lng); //32
write_int((int)current_loc.alt); //34
write_long(next_WP.lat);
write_long(next_WP.lng);
write_int((int)next_WP.alt); //44

4
ArduCopterMega/navigation.pde
View File

@ -149,6 +149,10 @@ void calc_altitude_error()
//Serial.printf("s: %d %d t_alt %d\n", (int)current_loc.alt, (int)altitude_error, (int)target_altitude);
}
// target_altitude = current_loc.alt; // PH: target_altitude = -200
// offset_altitude = next_WP.alt - current_loc.alt; // PH: offset_altitude = 0
long wrap_360(long error)
{
if (error > 36000) error -= 36000;

4
ArduCopterMega/system.pde
View File

@ -258,9 +258,7 @@ void set_mode(byte mode)
control_mode = mode;
control_mode = constrain(control_mode, 0, NUM_MODES - 1);
save_EEPROM_PID();
// used to stop fly_aways
if(rc_1.control_in == 0){
// we are on the ground is this is true

3
ArduCopterMega/test.pde
View File

@ -868,7 +868,8 @@ void fake_out_gps()
GPS.latitude = 377696000; // Y
GPS.longitude = -1224319000; // X
GPS.altitude = 9000; // meters * 100
//next_WP.lng = home.lng - length * sin(rads); // X
//next_WP.lat = home.lat + length * cos(rads); // Y
}