diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index ce6a222165..fefbf13730 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -43,7 +43,7 @@ version 2.1 of the License, or (at your option) any later version. #define MAVLINK_COMM_NUM_BUFFERS 2 #include // MAVLink GCS definitions -//#include + // Configuration @@ -178,6 +178,7 @@ GPS *g_gps; GCS_Class gcs; #endif +//#include //GCS_SIMPLE gcs_simple(&Serial); AP_RangeFinder_MaxsonarXL sonar; diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde index 40f46142bf..674f906eef 100644 --- a/ArduCopterMega/Attitude.pde +++ b/ArduCopterMega/Attitude.pde @@ -139,13 +139,6 @@ 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 - // We adjust the output by the rate of rotation - long rate = degrees(omega.z) * 100.0; // 3rad = 17188 , 6rad = 34377 - int dampener = (float)rate * g.hold_yaw_dampener; // 18000 * .17 = 3000 - - // Limit dampening to be equal to propotional term for symmetry - g.rc_4.servo_out -= constrain(dampener, -max_yaw_dampener, max_yaw_dampener); // -3000 - }else{ // rate control @@ -155,33 +148,29 @@ output_yaw_with_hold(boolean hold) // -error = CCW, +error = CW if(g.rc_4.control_in == 0){ - // we are breaking; - g.rc_4.servo_out = (omega.z > 0) ? -1800 : 1800; - - //g.rc_4.servo_out = (int)((float)g.rc_4.servo_out * (omega.z / 6.0)); - - //switch comments to get old behavior. - //g.rc_4.servo_out = g.pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 1.0);// kP .07 * 36000 = 2520 + //g.rc_4.servo_out = (omega.z > 0) ? -600 : 600; + g.rc_4.servo_out = (int)((float)g.rc_4.servo_out * (omega.z / 6.0)); }else{ + g.rc_4.servo_out = g.pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 1.0); // kP .07 * 36000 = 2520 - - // We adjust the output by the rate of rotation - //long rate = degrees(omega.z) * 100.0; // 3rad = 17188 , 6rad = 34377 - //int dampener = (float)rate * g.hold_yaw_dampener; // 18000 * .17 = 3000 - - // Limit dampening to be equal to propotional term for symmetry - //g.rc_4.servo_out -= constrain(dampener, -max_yaw_dampener, max_yaw_dampener); // -3000 - g.rc_4.servo_out = constrain(g.rc_4.servo_out, -1800, 1800); // limit to 24° } - } + // We adjust the output by the rate of rotation + long rate = degrees(omega.z) * 100.0; // 3rad = 17188 , 6rad = 34377 + int dampener = (float)rate * g.hold_yaw_dampener; // 18000 * .17 = 3000 + + // Limit dampening to be equal to propotional term for symmetry + g.rc_4.servo_out -= constrain(dampener, -max_yaw_dampener, max_yaw_dampener); // -3000 + + // Limit Output + g.rc_4.servo_out = constrain(g.rc_4.servo_out, -1800, 1800); // limit to 24° + //Serial.printf("%d\n",g.rc_4.servo_out); } -// slight left rudder gives right roll. void output_rate_roll() { diff --git a/ArduCopterMega/motors.pde b/ArduCopterMega/motors.pde index 90e82a9e88..8b5bff50f8 100644 --- a/ArduCopterMega/motors.pde +++ b/ArduCopterMega/motors.pde @@ -29,7 +29,6 @@ void arm_motors() }else{ arming_counter = 0; } - }else{ arming_counter = 0; } @@ -55,7 +54,7 @@ set_servos_4() g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, 1000); if(g.rc_3.servo_out > 0) - out_min = g.rc_3.radio_min + 50; + out_min = g.rc_3.radio_min + 60; //Serial.printf("out: %d %d %d %d\t\t", g.rc_1.servo_out, g.rc_2.servo_out, g.rc_3.servo_out, g.rc_4.servo_out); @@ -65,6 +64,10 @@ set_servos_4() g.rc_3.calc_pwm(); g.rc_4.calc_pwm(); + // limit Yaw control so we don't clip and loose altitude + // this is only a partial solution. + g.rc_4.pwm_out = min(g.rc_4.pwm_out, (g.rc_3.radio_out - out_min)); + //Serial.printf("out: %d %d %d %d\n", g.rc_1.radio_out, g.rc_2.radio_out, g.rc_3.radio_out, g.rc_4.radio_out); //Serial.printf("yaw: %d ", g.rc_4.radio_out); @@ -241,6 +244,7 @@ set_servos_4() gcs_simple.write_int((int)nav_roll); gcs_simple.write_int((int)nav_pitch); + gcs_simple.write_long(current_loc.lat); //28 gcs_simple.write_long(current_loc.lng); //32 gcs_simple.write_int((int)current_loc.alt); //34 diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde index 819a7b452b..746e489edb 100644 --- a/ArduCopterMega/navigation.pde +++ b/ArduCopterMega/navigation.pde @@ -54,6 +54,7 @@ void calc_loiter_nav() Becuase we are using lat and lon to do our distance errors here's a quick chart: 100 = 1m 1000 = 11m + 1800 = 1980m = 60 feet 3000 = 33m 10000 = 111m pitch_max = 22° (2200) @@ -89,26 +90,21 @@ void calc_waypoint_nav() //nav_lat = max(wp_distance, -DIST_ERROR_MAX); //nav_lat = min(wp_distance, DIST_ERROR_MAX); - //Serial.printf("nav_lat %ld, ", nav_lat); // Scale response by kP nav_lat *= g.pid_nav_lat.kP(); // 1800 * 2 = 3600 or 36° - //Serial.printf("%ld, ",nav_lat); // get the sin and cos of the bearing error - rotated 90° sin_nav_y = sin(radians((float)(9000 - bearing_error) / 100)); cos_nav_x = cos(radians((float)(bearing_error - 9000) / 100)); - //Serial.printf("X%2.4f, Y%2.4f ", cos_nav_x, sin_nav_y); // rotate the vector nav_roll = (float)nav_lat * cos_nav_x; nav_pitch = -(float)nav_lat * sin_nav_y; - //Serial.printf("R%ld, P%ld ", nav_roll, nav_pitch); long pmax = g.pitch_max.get(); nav_roll = constrain(nav_roll, -pmax, pmax); nav_pitch = constrain(nav_pitch, -pmax, pmax); - //Serial.printf("R%ld, P%ld \n", nav_roll, nav_pitch); } void calc_bearing_error() diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde index fdc5bdee0c..f982149b30 100644 --- a/ArduCopterMega/setup.pde +++ b/ArduCopterMega/setup.pde @@ -202,10 +202,6 @@ setup_motors(uint8_t argc, const Menu::arg *argv) init_rc_in(); - // read the radio to set trims - // --------------------------- - trim_radio(); - print_hit_enter(); delay(1000); @@ -382,7 +378,6 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) Serial.printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes.")); print_hit_enter(); - trim_radio(); while(1){ delay(20); diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index 3ac5447dae..1772be3ab8 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -107,7 +107,21 @@ void init_ardupilot() // save the new format version g.format_version.set_and_save(Parameters::k_format_version); - //Serial.println_P(PSTR("done.")); + Serial.printf_P(PSTR("Please Run Setup...\n")); + while (true) { + delay(100); + if(motor_light){ + digitalWrite(A_LED_PIN, HIGH); + digitalWrite(B_LED_PIN, HIGH); + digitalWrite(C_LED_PIN, HIGH); + }else{ + digitalWrite(A_LED_PIN, LOW); + digitalWrite(B_LED_PIN, LOW); + digitalWrite(C_LED_PIN, LOW); + } + motor_light = !motor_light; + } + }else{ unsigned long before = micros(); // Load all auto-loaded EEPROM variables @@ -245,7 +259,7 @@ void startup_ground(void) // read the radio to set trims // --------------------------- - trim_radio(); + //trim_radio(); if (g.log_bitmask & MASK_LOG_CMD) Log_Write_Startup(TYPE_GROUNDSTART_MSG); diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde index d8a0aebf11..6e28b2ddc5 100644 --- a/ArduCopterMega/test.pde +++ b/ArduCopterMega/test.pde @@ -129,10 +129,6 @@ test_radio(uint8_t argc, const Menu::arg *argv) print_hit_enter(); delay(1000); - // read the radio to set trims - // --------------------------- - trim_radio(); - while(1){ delay(20); read_radio(); @@ -183,10 +179,6 @@ test_failsafe(uint8_t argc, const Menu::arg *argv) read_radio(); } - // read the radio to set trims - // --------------------------- - trim_radio(); - oldSwitchPosition = readSwitch(); Serial.printf_P(PSTR("Unplug battery, throttle in neutral, turn off radio.\n")); @@ -243,8 +235,6 @@ test_stabilize(uint8_t argc, const Menu::arg *argv) Serial.printf_P(PSTR("g.pid_stabilize_roll.kP: %4.4f\n"), g.pid_stabilize_roll.kP()); Serial.printf_P(PSTR("max_stabilize_dampener:%d\n\n "), max_stabilize_dampener); - trim_radio(); - motor_auto_safe = false; motor_armed = true;