diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index 05f1e1e2a1..158d30278e 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -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 diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde index 4193b4ed99..81a0a095a6 100644 --- a/ArduCopterMega/Attitude.pde +++ b/ArduCopterMega/Attitude.pde @@ -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° diff --git a/ArduCopterMega/command description.txt b/ArduCopterMega/command description.txt index 09dfef520c..a40745cfc6 100644 --- a/ArduCopterMega/command description.txt +++ b/ArduCopterMega/command description.txt @@ -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. diff --git a/ArduCopterMega/commands.pde b/ArduCopterMega/commands.pde index 3bee843977..b263102303 100644 --- a/ArduCopterMega/commands.pde +++ b/ArduCopterMega/commands.pde @@ -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 diff --git a/ArduCopterMega/commands_logic.pde b/ArduCopterMega/commands_logic.pde index 925b9c4d61..c02692f7e4 100644 --- a/ArduCopterMega/commands_logic.pde +++ b/ArduCopterMega/commands_logic.pde @@ -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(¤t_loc); } void do_loiter_unlimited() { - Serial.println("dloi "); + //Serial.println("dloi "); if(next_command.lat == 0) set_next_WP(¤t_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"); diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h index 0b45003432..fc1c237b8e 100644 --- a/ArduCopterMega/config.h +++ b/ArduCopterMega/config.h @@ -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 diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde index f982149b30..69f9016650 100644 --- a/ArduCopterMega/setup.pde +++ b/ArduCopterMega/setup.pde @@ -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, diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index c938c7c50f..eaead3e77c 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -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); diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde index 69fb595885..7b588997b9 100644 --- a/ArduCopterMega/test.pde +++ b/ArduCopterMega/test.pde @@ -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);