From 9c446dbd351a6f9622e774f1a7f00be1c3e2aeb0 Mon Sep 17 00:00:00 2001 From: jasonshort Date: Mon, 21 Feb 2011 04:30:56 +0000 Subject: [PATCH] continued APvar int git-svn-id: https://arducopter.googlecode.com/svn/trunk@1703 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArduCopterMega/ArduCopterMega.pde | 46 +++++++++++++++++++------------ ArduCopterMega/Attitude.pde | 14 +++++----- ArduCopterMega/EEPROM.pde | 1 - ArduCopterMega/commands.pde | 4 ++- ArduCopterMega/config.h | 10 +++---- ArduCopterMega/control_modes.pde | 22 ++++++++------- ArduCopterMega/motors.pde | 22 +++++++++++---- ArduCopterMega/sensors.pde | 10 ++++--- ArduCopterMega/test.pde | 10 ++++--- 9 files changed, 83 insertions(+), 56 deletions(-) diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index 2632b85b75..5818870536 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -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; } } \ No newline at end of file diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde index 03caebc8c7..5f59252208 100644 --- a/ArduCopterMega/Attitude.pde +++ b/ArduCopterMega/Attitude.pde @@ -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; } diff --git a/ArduCopterMega/EEPROM.pde b/ArduCopterMega/EEPROM.pde index 6ec07d3587..90c328a0a6 100644 --- a/ArduCopterMega/EEPROM.pde +++ b/ArduCopterMega/EEPROM.pde @@ -205,7 +205,6 @@ void save_EEPROM_pressure(void) { g.ground_pressure.save(); g.ground_temperature.save(); - } void read_EEPROM_pressure(void) diff --git a/ArduCopterMega/commands.pde b/ArduCopterMega/commands.pde index ed5a8e95f8..bf29dadf2e 100644 --- a/ArduCopterMega/commands.pde +++ b/ArduCopterMega/commands.pde @@ -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(); diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h index cc687ebc24..3f1acca704 100644 --- a/ArduCopterMega/config.h +++ b/ArduCopterMega/config.h @@ -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 ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopterMega/control_modes.pde b/ArduCopterMega/control_modes.pde index 7b1b20eb23..34cadb5e79 100644 --- a/ArduCopterMega/control_modes.pde +++ b/ArduCopterMega/control_modes.pde @@ -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; diff --git a/ArduCopterMega/motors.pde b/ArduCopterMega/motors.pde index 682d585adf..16725c345c 100644 --- a/ArduCopterMega/motors.pde +++ b/ArduCopterMega/motors.pde @@ -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]); diff --git a/ArduCopterMega/sensors.pde b/ArduCopterMega/sensors.pde index 8a9199e9b6..c9190f7afa 100644 --- a/ArduCopterMega/sensors.pde +++ b/ArduCopterMega/sensors.pde @@ -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 diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde index 5fdaa565bf..03a4a73f9b 100644 --- a/ArduCopterMega/test.pde +++ b/ArduCopterMega/test.pde @@ -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,