diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index 60fe54572e..750d065b2c 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -206,6 +206,8 @@ const char* flight_mode_strings[] = { 8 TBD */ +long error_a; + // Radio // ----- int motor_out[4]; @@ -678,7 +680,8 @@ void slow_loop() // between 1 and 5 Hz #else gcs.send_message(MSG_LOCATION); -// XXX gcs.send_message(MSG_CPU_LOAD, load*100); + // XXX + // gcs.send_message(MSG_CPU_LOAD, load*100); #endif gcs.send_message(MSG_HEARTBEAT); // XXX This is running at 3 1/3 Hz instead of 1 Hz diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde index 06c7f6dd4f..60d462ff59 100644 --- a/ArduCopterMega/Attitude.pde +++ b/ArduCopterMega/Attitude.pde @@ -88,16 +88,23 @@ clear_yaw_control() void output_yaw_with_hold(boolean hold) { + //digitalWrite(B_LED_PIN, LOW); if(hold){ // look to see if we have exited rate control properly - ie stopped turning if(rate_yaw_flag){ // we are still in motion from rate control - if(fabs(omega.y) < .08){ + if(fabs(omega.z) < .5){ clear_yaw_control(); - hold = true; // just to be explicit + hold = true; // just to be explicit + //Serial.print("C"); + }else{ + + //digitalWrite(B_LED_PIN, HIGH); + // return to rate control until we slow down. hold = false; + //Serial.print("D"); } } @@ -111,6 +118,7 @@ output_yaw_with_hold(boolean hold) } if(hold){ + //Serial.println("H"); // try and hold the current nav_yaw setting yaw_error = nav_yaw - dcm.yaw_sensor; // +- 60° yaw_error = wrap_180(yaw_error); @@ -130,19 +138,26 @@ output_yaw_with_hold(boolean hold) }else{ + //Serial.println("R"); // rate control long rate = degrees(omega.z) * 100; // 3rad = 17188 , 6rad = 34377 rate = constrain(rate, -36000, 36000); // limit to something fun! - if(abs(rate) < 1000 ) //experiment to limit yaw reversing - rate = 0; + //if(abs(rate) < 1000 ) //experiment to limit yaw reversing + // rate = 0; - long error = ((long)g.rc_4.control_in * 6) - rate; // control is += 6000 * 6 = 36000 + long error = ((long)g.rc_4.control_in * 6) - rate; // control is += 6000 * 6 = 36000 // -error = CCW, +error = CW - g.rc_4.servo_out = g.pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 1.0); // kP .07 * 36000 = 2520 + + if(g.rc_4.control_in == 0) + g.rc_4.servo_out = g.pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 3.0);// kP .07 * 36000 = 2520 + else + 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 = constrain(g.rc_4.servo_out, -2400, 2400); // limit to 24° } } + // slight left rudder give right roll. void diff --git a/ArduCopterMega/commands_logic.pde b/ArduCopterMega/commands_logic.pde index 9d30f3e314..f59e2d76e5 100644 --- a/ArduCopterMega/commands_logic.pde +++ b/ArduCopterMega/commands_logic.pde @@ -41,7 +41,7 @@ handle_process_may() do_change_alt(); break; - case MAV_CMD_CONDITION_YAW: + case MAV_CMD_CONDITION_ANGLE: do_yaw(); break; diff --git a/ArduCopterMega/defines.h b/ArduCopterMega/defines.h index 3a9f97de0c..1df632d579 100644 --- a/ArduCopterMega/defines.h +++ b/ArduCopterMega/defines.h @@ -117,7 +117,7 @@ #define RELAY_TOGGLE 5 #define STOP_REPEAT 10 -#define MAV_CMD_CONDITION_YAW 23 +//#define MAV_CMD_CONDITION_YAW 23 // GCS Message ID's #define MSG_ACKNOWLEDGE 0x00 diff --git a/ArduCopterMega/motors.pde b/ArduCopterMega/motors.pde index b1072ed83f..11b1d558b7 100644 --- a/ArduCopterMega/motors.pde +++ b/ArduCopterMega/motors.pde @@ -159,6 +159,7 @@ set_servos_4() num++; if (num > 50){ num = 0; + /* Serial.printf("t_alt:%ld, alt:%ld, thr: %d sen: ", target_altitude, current_loc.alt, @@ -169,6 +170,10 @@ set_servos_4() }else{ Serial.println("Sonar"); } + */ + + + //Serial.print("!"); //debugging with Channel 6 diff --git a/ArduCopterMega/radio.pde b/ArduCopterMega/radio.pde index 32997c6a29..7e186d571c 100644 --- a/ArduCopterMega/radio.pde +++ b/ArduCopterMega/radio.pde @@ -28,6 +28,8 @@ void init_rc_in() g.rc_6.set_range(0,1000); g.rc_7.set_range(0,1000); g.rc_8.set_range(0,1000); + + //catch bad RC_3 min values } void init_rc_out() @@ -46,15 +48,23 @@ void init_rc_out() APM_RC.Init(); // APM Radio initialization + for(byte i = 0; i < 10; i++){ + delay(20); + read_radio(); + } + + // sanity check on the EEPROM values for radio_min + if(abs(g.rc_3.radio_min - g.rc_3.radio_in) > 40){ + g.rc_3.radio_min = g.rc_3.radio_in; + } + APM_RC.OutputCh(CH_1, g.rc_3.radio_min); // Initialization of servo outputs APM_RC.OutputCh(CH_2, g.rc_3.radio_min); APM_RC.OutputCh(CH_3, g.rc_3.radio_min); APM_RC.OutputCh(CH_4, g.rc_3.radio_min); - APM_RC.OutputCh(CH_7, g.rc_3.radio_min); APM_RC.OutputCh(CH_8, g.rc_3.radio_min); - } void read_radio() diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde index fcd2c7b1a6..f9ef6dc785 100644 --- a/ArduCopterMega/setup.pde +++ b/ArduCopterMega/setup.pde @@ -558,11 +558,12 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv) if(Serial.available() > 0){ + //mag_offset_x = offset[0]; //mag_offset_y = offset[1]; //mag_offset_z = offset[2]; - //save_EEPROM_mag_offset(); + //setup_mag_offset(); // set offsets to account for surrounding interference //compass.set_offsets(mag_offset_x, mag_offset_y, mag_offset_z); diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde index c657cba172..60157177cf 100644 --- a/ArduCopterMega/test.pde +++ b/ArduCopterMega/test.pde @@ -11,7 +11,7 @@ static int8_t test_gps(uint8_t argc, const Menu::arg *argv); static int8_t test_adc(uint8_t argc, const Menu::arg *argv); static int8_t test_imu(uint8_t argc, const Menu::arg *argv); //static int8_t test_dcm(uint8_t argc, const Menu::arg *argv); -//static int8_t test_omega(uint8_t argc, const Menu::arg *argv); +static int8_t test_omega(uint8_t argc, const Menu::arg *argv); static int8_t test_battery(uint8_t argc, const Menu::arg *argv); static int8_t test_current(uint8_t argc, const Menu::arg *argv); static int8_t test_relay(uint8_t argc, const Menu::arg *argv); @@ -52,7 +52,7 @@ const struct Menu::command test_menu_commands[] PROGMEM = { #endif {"imu", test_imu}, //{"dcm", test_dcm}, - //{"omega", test_omega}, + {"omega", test_omega}, {"battery", test_battery}, {"current", test_current}, {"relay", test_relay}, @@ -238,6 +238,7 @@ test_stabilize(uint8_t argc, const Menu::arg *argv) control_mode = STABILIZE; 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); + Serial.printf_P(PSTR("max_yaw_dampener:%d\n\n "), max_yaw_dampener); trim_radio(); @@ -285,14 +286,21 @@ test_stabilize(uint8_t argc, const Menu::arg *argv) set_servos_4(); ts_num++; - if (ts_num > 10){ + if (ts_num > 0){ ts_num = 0; + /* Serial.printf_P(PSTR("r: %d, p:%d, rc1:%d, "), (int)(dcm.roll_sensor/100), (int)(dcm.pitch_sensor/100), g.rc_1.pwm_out); + */ - print_motor_out(); + Serial.printf_P(PSTR("e:%ld, 4out:%d, O%4.4f\n"), + error_a, + g.rc_4.servo_out, + omega.z); + + //print_motor_out(); } // R: 1417, L: 1453 F: 1453 B: 1417 @@ -611,7 +619,8 @@ test_dcm(uint8_t argc, const Menu::arg *argv) } */ -/*static int8_t +///* +static int8_t test_omega(uint8_t argc, const Menu::arg *argv) { static byte ts_num; @@ -629,6 +638,7 @@ test_omega(uint8_t argc, const Menu::arg *argv) // IMU // --- read_AHRS(); + float my_oz = (dcm.yaw - old_yaw) * 50; old_yaw = dcm.yaw; @@ -646,7 +656,7 @@ test_omega(uint8_t argc, const Menu::arg *argv) } return (0); } -*/ +//*/ static int8_t test_battery(uint8_t argc, const Menu::arg *argv)