diff --git a/ArduCopterMega/APM_Config.h b/ArduCopterMega/APM_Config.h index 826cca5c8b..55d0633597 100644 --- a/ArduCopterMega/APM_Config.h +++ b/ArduCopterMega/APM_Config.h @@ -8,7 +8,7 @@ #define NAV_TEST 1 // 0 = traditional, 1 = rate controlled #define YAW_OPTION 0 // 0 = hybrid rate approach, 1 = offset Yaw approach -#define AUTO_RESET_LOITER 1 // enables Loiter to reset it's current location based on stick input. +#define AUTO_RESET_LOITER 0 // enables Loiter to reset it's current location based on stick input. #define FRAME_CONFIG QUAD_FRAME /* diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index 3ec3f3bf61..1d40eb7c91 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -393,7 +393,7 @@ struct Location prev_WP; // last waypoint struct Location current_loc; // current location struct Location next_WP; // next waypoint struct Location target_WP; // where do we want to you towards? -struct Location simple_WP; // command for telemetry +struct Location simple_WP; // struct Location next_command; // command preloaded long target_altitude; // used for boolean home_is_set; // Flag for if we have g_gps lock and have set the home location @@ -1107,8 +1107,8 @@ void update_current_flight_mode(void) #if AUTO_RESET_LOITER == 1 if((g.rc_2.control_in + g.rc_1.control_in) != 0){ // reset LOITER to current position - long temp = next_WP.alt; - next_WP = current_loc; + long temp = next_WP.alt; + next_WP = current_loc; next_WP.alt = temp; } #endif @@ -1172,7 +1172,7 @@ void update_navigation() update_nav_wp(); break; - #if YAW_HACK == 1 + /*#if YAW_HACK == 1 case SIMPLE: // calculates desired Yaw // exprimental_hack @@ -1183,7 +1183,7 @@ void update_navigation() } break; #endif - + */ } } diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde index ed03bda315..25a8a2dfc3 100644 --- a/ArduCopterMega/Attitude.pde +++ b/ArduCopterMega/Attitude.pde @@ -209,7 +209,6 @@ output_yaw_with_hold(boolean hold) // rate control long rate = degrees(omega.z) * 100; // 3rad = 17188 , 6rad = 34377 rate = constrain(rate, -36000, 36000); // limit to something fun! - int dampener = rate * g.pid_yaw.kD(); // 34377 * .175 = 6000 if(hold){ // look to see if we have exited rate control properly - ie stopped turning @@ -218,11 +217,8 @@ output_yaw_with_hold(boolean hold) if(fabs(omega.z) < .2){ clear_yaw_control(); hold = true; // just to be explicit - //Serial.print("C"); }else{ - hold = false; // return to rate control until we slow down. - //Serial.print("D"); } } @@ -247,7 +243,7 @@ output_yaw_with_hold(boolean hold) g.rc_4.servo_out = g.pid_yaw.get_pi(yaw_error, delta_ms_fast_loop, 1.0); // .4 * 4000 = 1600 // add in yaw dampener - g.rc_4.servo_out -= dampener; + g.rc_4.servo_out -= rate * g.pid_yaw.kD(); yaw_debug = YAW_HOLD; //0 @@ -256,7 +252,7 @@ output_yaw_with_hold(boolean hold) if(g.rc_4.control_in == 0){ // adaptive braking - g.rc_4.servo_out = (int)(-800.0 * omega.z); + g.rc_4.servo_out = (int)(-1000.0 * omega.z); yaw_debug = YAW_BRAKE; // 1 @@ -266,12 +262,13 @@ output_yaw_with_hold(boolean hold) long error = ((long)g.rc_4.control_in * 6) - (rate * 2); // 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; // 2 - nav_yaw = dcm.yaw_sensor; // + + //nav_yaw = dcm.yaw_sensor; // I think this caused the free rotation, dont know why. } } // Limit Output - g.rc_4.servo_out = constrain(g.rc_4.servo_out, -2400, 2400); // limit to 24° + g.rc_4.servo_out = constrain(g.rc_4.servo_out, -2500, 2500); // limit to 24° //Serial.printf("%d\n",g.rc_4.servo_out); } @@ -303,6 +300,6 @@ output_yaw_with_hold(boolean hold) // add in yaw dampener g.rc_4.servo_out -= degrees(omega.z) * 100 * g.pid_yaw.kD(); - yaw_error = constrain(yaw_error, -2500, 2500); // limit error to 60 degees + g.rc_4.servo_out = constrain(g.rc_4.servo_out, -2500, 2500); // limit error to 60 degees } #endif diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h index 26fa457abd..a3ab05800d 100644 --- a/ArduCopterMega/config.h +++ b/ArduCopterMega/config.h @@ -354,13 +354,13 @@ // YAW Control // #ifndef YAW_P -# define YAW_P 0.6 // increase for more aggressive Yaw Hold, decrease if it's bouncy +# define YAW_P 0.4 // increase for more aggressive Yaw Hold, decrease if it's bouncy #endif #ifndef YAW_I # define YAW_I 0.01 // set to .0001 to try and get over user's steady state error caused by poor balance #endif #ifndef YAW_D -# define YAW_D 0.1 // Trying a lower value to prevent odd behavior +# define YAW_D 0.05 // Trying a lower value to prevent odd behavior #endif #ifndef YAW_IMAX # define YAW_IMAX 1 // degrees * 100 diff --git a/ArduCopterMega/motors.pde b/ArduCopterMega/motors.pde index 206fd082cc..beb74f27e5 100644 --- a/ArduCopterMega/motors.pde +++ b/ArduCopterMega/motors.pde @@ -22,11 +22,14 @@ void arm_motors() } else{ arming_counter++; } + // full left }else if (g.rc_4.control_in < -4000) { if (arming_counter >= DISARM_DELAY){ motor_armed = false; arming_counter = DISARM_DELAY; + compass.save_offsets(); + }else{ arming_counter++; } diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde index a02729ea4b..17b925a0d3 100644 --- a/ArduCopterMega/setup.pde +++ b/ArduCopterMega/setup.pde @@ -405,56 +405,61 @@ setup_sonar(uint8_t argc, const Menu::arg *argv) static int8_t setup_mag_offset(uint8_t argc, const Menu::arg *argv) { + Vector3f _offsets; + + if (!strcmp_P(argv[1].str, PSTR("c"))) { + compass.set_offsets(_offsets); + report_compass(); + return (0); + } + print_hit_enter(); init_compass(); - float _min[3], _max[3]; - Vector3f _offsets; - Vector3f compass_mag; + int _min[3] = {0,0,0}; + int _max[3] = {0,0,0}; + + compass.read(); + compass.calculate(0,0); // roll = 0, pitch = 0 while(1){ - static float min[3], _max[3], offset[3]; - if (millis() - fast_loopTimer > 100) { - delta_ms_fast_loop = millis() - fast_loopTimer; - fast_loopTimer = millis(); - G_Dt = (float)delta_ms_fast_loop / 1000.f; + delay(50); + compass.read(); + compass.calculate(0,0); // roll = 0, pitch = 0 - compass.read(); - compass.calculate(0, 0); // roll = 0, pitch = 0 for this example - compass_mag = compass.get_offsets(); + if(compass.mag_x < _min[0]) _min[0] = compass.mag_x; + if(compass.mag_y < _min[1]) _min[1] = compass.mag_y; + if(compass.mag_z < _min[2]) _min[2] = compass.mag_z; - // capture min - _min[0] = min(_min[0], compass_mag.x); - _min[1] = min(_min[1], compass_mag.y); - _min[2] = min(_min[2], compass_mag.z); + // capture max + if(compass.mag_x > _max[0]) _max[0] = compass.mag_x; + if(compass.mag_y > _max[1]) _max[1] = compass.mag_y; + if(compass.mag_z > _max[2]) _max[2] = compass.mag_z; - // capture max - _max[0] = max(_max[0], compass_mag.x); - _max[1] = max(_max[1], compass_mag.y); - _max[2] = max(_max[2], compass_mag.z); + // calculate offsets + _offsets.x = (float)(_max[0] + _min[0]) / -2; + _offsets.y = (float)(_max[1] + _min[1]) / -2; + _offsets.z = (float)(_max[2] + _min[2]) / -2; - // calculate offsets - _offsets.x = -(_max[0] + _min[0]) / 2; - _offsets.y = -(_max[1] + _min[1]) / 2; - _offsets.z = -(_max[2] + _min[2]) / 2; + // display all to user + Serial.printf_P(PSTR("Heading: %u, \t (%d, %d, %d), (%4.4f, %4.4f, %4.4f)\n"), - // display all to user - Serial.printf_P(PSTR("Heading: %u, \t (%4.4f, %4.4f, %4.4f) (%4.4f, %4.4f, %4.4f)\n"), - (uint16_t)wrap_360(ToDeg(compass.heading)), - compass_mag.x, - compass_mag.y, - compass_mag.z, - _offsets.x, - _offsets.y, - _offsets.z); + (uint16_t)(wrap_360(ToDeg(compass.heading) * 100)) /100, - if(Serial.available() > 0){ - compass.set_offsets(_offsets); - //compass.set_offsets(mag_offset_x, mag_offset_y, mag_offset_z); - report_compass(); - break; - } + compass.mag_x, + compass.mag_y, + compass.mag_z, + + _offsets.x, + _offsets.y, + _offsets.z); + + if(Serial.available() > 1){ + compass.set_offsets(_offsets); + //compass.set_offsets(mag_offset_x, mag_offset_y, mag_offset_z); + report_compass(); + break; } } } diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index b6316ac311..1d322905f6 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -37,7 +37,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = { }; // Create the top-level menu object. -MENU(main_menu, "AC 2.0.17 Beta", main_menu_commands); +MENU(main_menu, "AC 2.0.18 Beta", main_menu_commands); void init_ardupilot() {