diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index 065bd47a99..081aa88af8 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -773,11 +773,14 @@ void update_current_flight_mode(void) //call at 5 hz if(fbw_timer > 20){ fbw_timer = 0; - current_loc.lat = 0; - current_loc.lng = 0; + + if(home_is_set == false){ + current_loc.lat = home.lat = 0; + current_loc.lng = home.lng = 0; + } - next_WP.lat = rc_1.control_in /5; // 10 meteres - next_WP.lng = -rc_2.control_in /5; // 10 meteres + next_WP.lat = home.lat + rc_1.control_in /5; // 10 meteres + next_WP.lng = home.lng -rc_2.control_in /5; // 10 meteres // waypoint distance from plane // ---------------------------- diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde index 3270d1e3e5..61a4afdcc1 100644 --- a/ArduCopterMega/Attitude.pde +++ b/ArduCopterMega/Attitude.pde @@ -159,6 +159,7 @@ void set_servos_4(void) int b_out = ((long)(motor_out[BACK] - rc_3.radio_min) * 100) / (long)(rc_3.radio_max - rc_3.radio_min); //*/ + // /* debugging and dynamic kP num++; if (num > 50){ @@ -166,16 +167,18 @@ void set_servos_4(void) //Serial.printf("control_in: %d ", rc_3.control_in); //Serial.printf(" servo: %d %d %d %d\t", rc_1.servo_out, rc_2.servo_out, rc_3.servo_out, rc_4.servo_out); //Serial.printf(" cwm: %d %d %d %d, %d\t", rc_1.pwm_out, rc_2.pwm_out, rc_3.pwm_out, rc_4.pwm_out, rc_3.radio_out); - //Serial.printf(" pwm: %d %d %d %d\n", r_out, l_out, f_out, b_out); - //Serial.printf(" pwm: %d %d %d %d\n", motor_out[RIGHT], motor_out[LEFT], motor_out[FRONT], motor_out[BACK]); + //Serial.printf(" out: %d %d %d %d\n", r_out, l_out, f_out, b_out); + //Serial.printf(" pwm: %d, %d %d %d %d\n",rc_3.pwm_out, motor_out[RIGHT], motor_out[LEFT], motor_out[FRONT], motor_out[BACK]); pid_stabilize_roll.kP((float)rc_6.control_in / 1000); - stabilize_rate_roll_pitch = pid_stabilize_roll.kP() *.24; + stabilize_rate_roll_pitch = pid_stabilize_roll.kP() *.25; init_pids(); - Serial.print("kP: "); - Serial.println(pid_stabilize_roll.kP(),3); + //Serial.print("kP: "); + //Serial.println(pid_stabilize_roll.kP(),3); } + // out: 41 38 39 39 + // pwm: 358, 1412 1380 1395 1389 //*/ //Serial.printf("set: %d %d %d %d\n", motor_out[RIGHT], motor_out[LEFT], motor_out[FRONT], motor_out[BACK]); diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h index 123951c64c..42c1033617 100644 --- a/ArduCopterMega/config.h +++ b/ArduCopterMega/config.h @@ -283,7 +283,7 @@ // STABILZE Angle Control // #ifndef STABILIZE_ROLL_P -# define STABILIZE_ROLL_P 0.44 +# define STABILIZE_ROLL_P 0.7 #endif #ifndef STABILIZE_ROLL_I # define STABILIZE_ROLL_I 0.00 @@ -296,7 +296,7 @@ #endif #ifndef STABILIZE_PITCH_P -# define STABILIZE_PITCH_P 0.44 +# define STABILIZE_PITCH_P 0.7 #endif #ifndef STABILIZE_PITCH_I # define STABILIZE_PITCH_I 0.0 @@ -311,7 +311,7 @@ // STABILZE RATE Control // #ifndef STABILIZE_RATE_RP -# define STABILIZE_RATE_RP 0.1 +# define STABILIZE_RATE_RP 0.175 #endif @@ -334,7 +334,7 @@ // STABILZE YAW Control // #ifndef STABILIZE_RATE_YAW -# define STABILIZE_RATE_YAW 0.008 +# define STABILIZE_RATE_YAW 0.000 #endif ////////////////////////////////////////////////////////////////////////////// @@ -356,7 +356,7 @@ // // how much to we pitch towards the target #ifndef PITCH_MAX -# define PITCH_MAX 12 +# define PITCH_MAX 8 #endif @@ -381,16 +381,16 @@ // Throttle control gains // #ifndef THROTTLE_P -# define THROTTLE_P 0.5 +# define THROTTLE_P 1.0 #endif #ifndef THROTTLE_I # define THROTTLE_I 0.01 #endif #ifndef THROTTLE_D -# define THROTTLE_D 0.3 +# define THROTTLE_D 0.4 #endif #ifndef THROTTLE_IMAX -# define THROTTLE_IMAX 50 +# define THROTTLE_IMAX 20 #endif diff --git a/ArduCopterMega/control_modes.pde b/ArduCopterMega/control_modes.pde index 5688885075..9474f32a53 100644 --- a/ArduCopterMega/control_modes.pde +++ b/ArduCopterMega/control_modes.pde @@ -81,11 +81,16 @@ void read_trim_switch() imu.init_accel(); imu.print_accel_offsets(); }*/ - Serial.printf("r: %d, p: %d\n", rc_1.control_in, rc_2.control_in); - // set new accel offset values - imu.ax(((long)rc_1.control_in * -15) / 100); - imu.ay(((long)rc_2.control_in * -15) / 100); - imu.print_accel_offsets(); + if(rc_3.control_in == 0){ + imu.zero_accel(); + }else{ + Serial.printf("r: %d, p: %d\n", rc_1.control_in, rc_2.control_in); + // set new accel offset values + imu.ax(((long)rc_1.control_in * -15) / 100); + imu.ay(((long)rc_2.control_in * -15) / 100); + imu.print_accel_offsets(); + } + } else { // set the throttle nominal if(rc_3.control_in > 50){ diff --git a/ArduCopterMega/flight_control.pde b/ArduCopterMega/flight_control.pde index 2ce0025ccb..347fdb05fb 100644 --- a/ArduCopterMega/flight_control.pde +++ b/ArduCopterMega/flight_control.pde @@ -21,7 +21,7 @@ void output_auto_throttle() void calc_nav_throttle() { - long err = constrain (altitude_error, -300, 300); //+-3 meters + long err = constrain (altitude_error, -150, 150); //+-1.5 meters long temp = pid_throttle.get_pid(err, deltaMiliSeconds, 1.0); nav_throttle = (float)(throttle_cruise + temp) * angle_boost(); } diff --git a/ArduCopterMega/read_me.text b/ArduCopterMega/read_me.text index 89dbfbcf5e..8f9308d730 100644 --- a/ArduCopterMega/read_me.text +++ b/ArduCopterMega/read_me.text @@ -7,38 +7,40 @@ ch2 = pitch ch3 = throttle ch4 = yaw ch5 = mode switch - use your 3 position switch -ch6 = used for tuning - not currently active +ch6 = used for tuning - not currently active, search for "rc_6" to enable ch7 = use to set throttle hold value while hovering (quick toggle), hold > 5 seconds on ground to reset the accelerometer offsets. ch8 = not used setup: -erase = run this first, just in case -reset = run this second -radio = run this third -esc = just ignore this for now -level = optional - sets accelerometer offsets -flat = optional - sets accelerometer offsets to 0 -modes = interactive setup for flight modes -pid = optional - writes default PID values to eeprom -frame = optional - default is "+" -enable_mag = enables the compass -disable_mag = disables the compass -compass = interactive setup for compass offsets -declination = usage: "declination 14.25" -show = shows all values +erase - run this first, just in case +reset - run this second +radio - run this third +esc - just ignore this for now +level - optional - sets accelerometer offsets +flat - optional - sets accelerometer offsets to 0 +modes - interactive setup for flight modes +pid - optional - writes default PID values to eeprom +frame - optional - default is "+" +enable_mag - enables the compass +disable_mag - disables the compass +compass - interactive setup for compass offsets +declination - usage: "declination 14.25" +show - shows all values Flight modes to try: -stabilize - I'm having a little trouble inputting Yaw commands, would love some feedback on how to best handle it. -alt_hold - You need to set your throttle_cruise value by toggling ch_7 for a second. Mine is 330 -- altitude is controlled by the throttle lever. +stabilize - yay +Alt_hold - You need to set your throttle_cruise value by toggling ch_7 for less than 1 second. (Mine is 330), altitude is controlled by the throttle lever. +FBW - Simulates GPS Hold with the sticks being the position offset. Manual Throttle. position_hold - When selected, it will hold the current altitude, position and yaw. Yaw is user controllable. roll and pitch can be overridden with the radio. -RTL - will try and fly back to home at the current altitude. -Auto - not tested yet, i'm finishing a waypoint writing sketch and will be ready to test soon +RTL - not tested yet, will try and fly back to home at the current altitude. +Auto - not tested yet, i'm finishing a waypoint writing sketch and will be ready to test soon + - what's new - CMD_ANGLE - will set the desired yaw with control of angle/second and direction. - CMD_ALTITUDE - will send the copter up from current position to desired altitude - CMD_R_WAYPOINT - is just like a waypoint but relative to home - CMD_TRACK_WAYPOINT - coming soon, will point the copter at the waypoint no matter the position + Special note: Any mode other than stabilize will cause the props to spin once the control switch is engaged. The props will NOT spin in stabilize when throttle is in the off position, even when armed. diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde index adb4880a7e..34e21ea5b7 100644 --- a/ArduCopterMega/test.pde +++ b/ArduCopterMega/test.pde @@ -114,9 +114,26 @@ test_radio(uint8_t argc, const Menu::arg *argv) while(1){ delay(20); read_radio(); + output_manual_throttle(); - Serial.printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\n"), (rc_1.control_in), (rc_2.control_in), (rc_3.control_in), (rc_4.control_in), rc_5.control_in, rc_6.control_in, rc_7.control_in); + rc_1.calc_pwm(); + rc_2.calc_pwm(); + rc_3.calc_pwm(); + rc_4.calc_pwm(); + + //Serial.printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\n"), (rc_1.control_in), (rc_2.control_in), (rc_3.control_in), (rc_4.control_in), rc_5.control_in, rc_6.control_in, rc_7.control_in); //Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d\n"), (rc_1.servo_out / 100), (rc_2.servo_out / 100), rc_3.servo_out, (rc_4.servo_out / 100)); + Serial.printf_P(PSTR( "min: %d" + "\t in: %d" + "\t pwm_in: %d" + "\t sout: %d" + "\t pwm_out %d\n"), + rc_3.radio_min, + rc_3.control_in, + rc_3.radio_in, + rc_3.servo_out, + rc_3.pwm_out + ); if(Serial.available() > 0){ return (0);