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

continued APvar int

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1703 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-02-21 04:30:56 +00:00
parent bd9f0f4647
commit 5282c72d1f
9 changed files with 83 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
ArduCopterMega/ArduCopterMega.pde
View File

@ -510,17 +510,10 @@ void medium_loop()
case 2:
medium_loopCounter++;
// Read Baro pressure
// ------------------
read_barometer();
if(g.sonar_enabled){
sonar.read();
update_alt();
}else{
// no sonar altitude
current_loc.alt = baro_alt;
}
// Read altitude from sensors
// ------------------
update_alt();
// altitude smoothing
// ------------------
@ -867,7 +860,7 @@ void update_current_flight_mode(void)
//if(g.rc_3.control_in)
// get desired height from the throttle
next_WP.alt = home.alt + (g.rc_3.control_in * 4); // 0 - 1000 (40 meters)
next_WP.alt = home.alt + (g.rc_3.control_in); // 0 - 1000 (40 meters)
next_WP.alt = max(next_WP.alt, 30);
// !!! testing
@ -998,14 +991,33 @@ void update_trig(void){
}
void
update_alt(){
void update_alt()
{
altitude_sensor = BARO;
baro_alt = read_barometer();
altitude_sensor = (target_altitude > 500) ? BARO : SONAR;
if(g.sonar_enabled){
// decide which sensor we're usings
sonar_alt = sonar.read();
if(altitude_sensor == BARO){
current_loc.alt = baro_alt;
if(baro_alt < 550){
altitude_sensor = SONAR;
}
if(sonar_alt > 600){
altitude_sensor = BARO;
}
//altitude_sensor = (target_altitude > (home.alt + 500)) ? BARO : SONAR;
if(altitude_sensor == BARO){
current_loc.alt = baro_alt + home.alt;
}else{
sonar_alt = min(sonar_alt, 600);
current_loc.alt = sonar_alt + home.alt;
}
}else{
current_loc.alt = sonar_alt;
// no sonar altitude
current_loc.alt = baro_alt + home.alt;
}
}

14
ArduCopterMega/Attitude.pde
View File

@ -202,30 +202,30 @@ void calc_nav_throttle()
{
// limit error
long error = constrain(altitude_error, -400, 400);
if(altitude_sensor == BARO) {
float t = g.pid_baro_throttle.kP();
if(error > 0){ // go up
g.pid_baro_throttle.kP(t);
}else{ // go down
g.pid_baro_throttle.kP(t/4.0);
}
// limit output of throttle control
nav_throttle = g.pid_baro_throttle.get_pid(error, delta_ms_fast_loop, 1.0);
nav_throttle = g.throttle_cruise + constrain(nav_throttle, -30, 80);
g.pid_baro_throttle.kP(t);
} else {
// SONAR
nav_throttle = g.pid_sonar_throttle.get_pid(error, delta_ms_fast_loop, 1.0);
// limit output of throttle control
nav_throttle = g.throttle_cruise + constrain(nav_throttle, -60, 100);
}
nav_throttle = (nav_throttle + nav_throttle_old) >> 1;
nav_throttle_old = nav_throttle;
}

1
ArduCopterMega/EEPROM.pde
View File

@ -205,7 +205,6 @@ void save_EEPROM_pressure(void)
{
g.ground_pressure.save();
g.ground_temperature.save();
}
void read_EEPROM_pressure(void)

4
ArduCopterMega/commands.pde
View File

@ -216,9 +216,11 @@ void init_home()
home.id = MAV_CMD_NAV_WAYPOINT;
home.lng = g_gps->longitude; // Lon * 10**7
home.lat = g_gps->latitude; // Lat * 10**7
home.alt = g_gps->altitude;
home.alt = max(g_gps->altitude, 0);
home_is_set = true;
Serial.printf("gps alt: %ld", home.alt);
// ground altitude in centimeters for pressure alt calculations
// ------------------------------------------------------------
save_EEPROM_pressure();

10
ArduCopterMega/config.h
View File

@ -409,22 +409,20 @@
#ifndef THROTTLE_BARO_IMAX
# define THROTTLE_BARO_IMAX 50
#endif
# define THROTTLE_BARO_IMAX_CENTIDEGREE THROTTLE_BARO_IMAX * 100
#ifndef THROTTLE_SONAR_P
# define THROTTLE_SONAR_P .8
# define THROTTLE_SONAR_P .3
#endif
#ifndef THROTTLE_SONAR_I
# define THROTTLE_SONAR_I 0.3
# define THROTTLE_SONAR_I 0.01
#endif
#ifndef THROTTLE_SONAR_D
# define THROTTLE_SONAR_D 0.7
# define THROTTLE_SONAR_D 0.2
#endif
#ifndef THROTTLE_SONAR_IMAX
# define THROTTLE_SONAR_IMAX 300
# define THROTTLE_SONAR_IMAX 50
#endif
# define THROTTLE_SONAR_IMAX_CENTIDEGREE THROTTLE_SONAR_IMAX * 100
//////////////////////////////////////////////////////////////////////////////

22
ArduCopterMega/control_modes.pde
View File

@ -2,11 +2,11 @@ void read_control_switch()
{
byte switchPosition = readSwitch();
//motor_armed = (switchPosition < 5);
if (oldSwitchPosition != switchPosition){
set_mode(g.flight_modes[switchPosition]);
oldSwitchPosition = switchPosition;
// reset navigation integrators
@ -23,7 +23,7 @@ byte readSwitch(void){
#elif FLIGHT_MODE_CHANNEL == CH_7
int pulsewidth = g.rc_7.radio_in; //
#elif FLIGHT_MODE_CHANNEL == CH_8
int pulsewidth = g.rc_8.radio_in; // default for Ardupilot. Don't use for Arducopter! it has a hardware failsafe mux!
int pulsewidth = g.rc_8.radio_in; // default for Ardupilot. Don't use for Arducopter! it has a hardware failsafe mux!
#else
# error Must define FLIGHT_MODE_CHANNEL as CH_5 - CH_8
#endif
@ -62,24 +62,26 @@ void read_trim_switch()
// called once
trim_timer = millis();
}
trim_flag = true;
trim_accel();
}else{ // switch is disengaged
if(trim_flag){
// switch was just released
if((millis() - trim_timer) > 2000){
imu.save();
} else {
// set the throttle nominal
if(g.rc_3.control_in > 50){
g.throttle_cruise.set(g.rc_3.control_in);
Serial.printf("tnom %d\n", g.throttle_cruise.get());
save_EEPROM_throttle_cruise();
//save_EEPROM_throttle_cruise();
g.throttle_cruise.save();
}
}
trim_flag = false;

22
ArduCopterMega/motors.pde
View File

@ -60,7 +60,7 @@ set_servos_4()
//Serial.printf("yaw: %d ", g.rc_4.radio_out);
if(g.frame_type == PLUS_FRAME){
Serial.println("P_FRAME");
//Serial.println("P_FRAME");
motor_out[CH_1] = g.rc_3.radio_out - g.rc_1.pwm_out;
motor_out[CH_2] = g.rc_3.radio_out + g.rc_1.pwm_out;
motor_out[CH_3] = g.rc_3.radio_out + g.rc_2.pwm_out;
@ -73,7 +73,7 @@ set_servos_4()
}else if(g.frame_type == X_FRAME){
Serial.println("X_FRAME");
//Serial.println("X_FRAME");
int roll_out = g.rc_1.pwm_out / 2;
int pitch_out = g.rc_2.pwm_out / 2;
@ -94,7 +94,7 @@ set_servos_4()
}else if(g.frame_type == TRI_FRAME){
Serial.println("TRI_FRAME");
//Serial.println("TRI_FRAME");
// Tri-copter power distribution
int roll_out = (float)g.rc_1.pwm_out * .866;
@ -112,7 +112,7 @@ set_servos_4()
}else if (g.frame_type == HEXA_FRAME) {
Serial.println("6_FRAME");
//Serial.println("6_FRAME");
int roll_out = (float)g.rc_1.pwm_out * .866;
int pitch_out = g.rc_2.pwm_out / 2;
@ -154,8 +154,18 @@ set_servos_4()
}
num++;
if (num > 10){
if (num > 50){
num = 0;
Serial.printf("t_alt:%ld, alt:%ld, thr: %d sen: ",
target_altitude,
current_loc.alt,
g.rc_3.servo_out);
if(altitude_sensor == BARO){
Serial.println("Baro");
}else{
Serial.println("Sonar");
}
//Serial.print("!");
//debugging with Channel 6
@ -178,7 +188,7 @@ set_servos_4()
init_pids();
//*/
///*
/*
write_int(motor_out[CH_1]);
write_int(motor_out[CH_2]);
write_int(motor_out[CH_3]);

10
ArduCopterMega/sensors.pde
View File

@ -6,13 +6,15 @@ void init_pressure_ground(void)
for(int i = 0; i < 300; i++){ // We take some readings...
delay(20);
barometer.Read(); // Get initial data from absolute pressure sensor
ground_pressure = (ground_pressure * 9l + barometer.Press) / 10l;
ground_pressure = (ground_pressure * 9l + barometer.Press) / 10l;
ground_temperature = (ground_temperature * 9 + barometer.Temp) / 10;
}
abs_pressure = barometer.Press;
}
void read_barometer(void){
long
read_barometer(void)
{
float x, scaling, temp;
barometer.Read(); // Get new data from absolute pressure sensor
@ -20,9 +22,9 @@ void read_barometer(void){
//abs_pressure = (abs_pressure + barometer.Press) >> 1; // Small filtering
abs_pressure = ((float)abs_pressure * .7) + ((float)barometer.Press * .3); // large filtering
scaling = (float)ground_pressure / (float)abs_pressure;
temp = ((float)ground_temperature / 10.f) + 273.15;
temp = ((float)ground_temperature / 10.0f) + 273.15f;
x = log(scaling) * temp * 29271.267f;
baro_alt = (long)(x / 10) + home.alt;// + baro_offset; // Pressure altitude in centimeters
return (x / 10);
}
// in M/S * 100

10
ArduCopterMega/test.pde
View File

@ -762,7 +762,7 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
Serial.printf_P(PSTR("\nCalibrating....\n"));
/*
for (int i = 1; i < 301; i++) {
read_barometer();
baro_alt = read_barometer();
if(i > 200)
sum += abs_pressure;
delay(10);
@ -790,12 +790,14 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
read_radio(); // read the radio first
next_WP.alt = home.alt + g.rc_6.control_in; // 0 - 2000 (20 meters)
read_trim_switch();
read_barometer();
next_WP.alt = max(next_WP.alt, 30);
read_trim_switch();
baro_alt = read_barometer();
current_loc.alt = baro_alt + home.alt;
Serial.printf_P(PSTR("AP: %ld,\tAlt: %ld, \tnext_alt: %ld \terror: %ld, \tcruise: %d, \t out:%d\n"),
abs_pressure,
current_loc.alt,
baro_alt,
next_WP.alt,
altitude_error,
(int)g.throttle_cruise,