diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index 0c28c18760..f8ec4ab11b 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -21,6 +21,7 @@ version 2.1 of the License, or (at your option) any later version. // Libraries #include #include +#include #include // ArduPilot Mega RC Library #include // ArduPilot Mega RC Library #include // ArduPilot Mega Analog to Digital Converter Library @@ -886,4 +887,4 @@ void read_AHRS(void) roll_sensor = dcm.roll_sensor; pitch_sensor = dcm.pitch_sensor; yaw_sensor = dcm.yaw_sensor; -} +} \ No newline at end of file diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde index 39c28409c6..3270d1e3e5 100644 --- a/ArduCopterMega/Attitude.pde +++ b/ArduCopterMega/Attitude.pde @@ -13,13 +13,9 @@ void init_pids() void output_stabilize() { float roll_error, pitch_error; - int max_out; Vector3f omega = dcm.get_gyro(); - /*testing code:*/ //pitch_sensor = roll_sensor = 0; // testing only - //stabilize_rate_roll_pitch = (float)rc_6.control_in / 1000; - //init_pids(); // control +- 45° is mixed with the navigation request by the Autopilot // output is in degrees = target pitch and roll of copter @@ -55,7 +51,8 @@ void output_stabilize() // Limit dampening to be equal to propotional term for symmetry rc_1.servo_out -= constrain(roll_dampener, -max_stabilize_dampener, max_stabilize_dampener); // +- 15° rc_2.servo_out -= constrain(pitch_dampener, -max_stabilize_dampener, max_stabilize_dampener); // +- 15° - rc_4.servo_out -= constrain(yaw_dampener, -max_yaw_dampener, max_yaw_dampener); // +- 15° + rc_4.servo_out -= constrain(yaw_dampener, -max_yaw_dampener, max_yaw_dampener); + //Serial.printf(" yaw out: %d, d: %d", (int)rc_4.angle_to_pwm(), yaw_dampener); @@ -84,9 +81,9 @@ void output_rate_control() //Serial.print((Omega[0] * 5729.57795 * stabilize_rate_roll_pitch), 3); // Limit output - rc_1.servo_out = constrain(rc_1.servo_out, -MAX_SERVO_OUTPUT, MAX_SERVO_OUTPUT); + rc_1.servo_out = constrain(rc_1.servo_out, -MAX_SERVO_OUTPUT, MAX_SERVO_OUTPUT); rc_2.servo_out = constrain(rc_2.servo_out, -MAX_SERVO_OUTPUT, MAX_SERVO_OUTPUT); - rc_4.servo_out = constrain(rc_4.servo_out, -MAX_SERVO_OUTPUT, MAX_SERVO_OUTPUT); + rc_4.servo_out = constrain(rc_4.servo_out, -MAX_SERVO_OUTPUT, MAX_SERVO_OUTPUT); } @@ -105,7 +102,7 @@ void reset_I(void) void set_servos_4(void) { static byte num; - //motor_armed = false; + // Quadcopter mix if (motor_armed == true) { int out_min = rc_3.radio_min; @@ -118,7 +115,7 @@ void set_servos_4(void) //Serial.printf("out: %d %d %d %d\t\t", rc_1.servo_out, rc_2.servo_out, rc_3.servo_out, rc_4.servo_out); - // creates the radio_out anf pwm_out values + // creates the radio_out and pwm_out values rc_1.calc_pwm(); rc_2.calc_pwm(); rc_3.calc_pwm(); @@ -140,6 +137,7 @@ void set_servos_4(void) motor_out[FRONT] = rc_3.radio_out + roll_out + pitch_out; motor_out[BACK] = rc_3.radio_out - roll_out - pitch_out; } + //Serial.printf("\tb4: %d %d %d %d ", motor_out[RIGHT], motor_out[LEFT], motor_out[FRONT], motor_out[BACK]); motor_out[RIGHT] += rc_4.pwm_out; @@ -160,15 +158,23 @@ void set_servos_4(void) int f_out = ((long)(motor_out[FRONT] - rc_3.radio_min) * 100) / (long)(rc_3.radio_max - rc_3.radio_min); int b_out = ((long)(motor_out[BACK] - rc_3.radio_min) * 100) / (long)(rc_3.radio_max - rc_3.radio_min); //*/ - - //~#*set_servos_4: 398, -39 38 38 -36 - /* + + /* debugging and dynamic kP num++; if (num > 50){ num = 0; - 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(" pwm: %d %d %d %d\n", r_out, l_out, f_out, b_out); + //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]); + + pid_stabilize_roll.kP((float)rc_6.control_in / 1000); + stabilize_rate_roll_pitch = pid_stabilize_roll.kP() *.24; + init_pids(); + + Serial.print("kP: "); + Serial.println(pid_stabilize_roll.kP(),3); } //*/ @@ -222,9 +228,3 @@ void set_servos_4(void) rc_4.control_in = ToDeg(yaw); } } - - -void demo_servos(byte i) { - // nothing to do -} - diff --git a/ArduCopterMega/flight_control.pde b/ArduCopterMega/flight_control.pde index ab9f26e7da..f0f30c3a60 100644 --- a/ArduCopterMega/flight_control.pde +++ b/ArduCopterMega/flight_control.pde @@ -18,7 +18,6 @@ void output_auto_throttle() rc_3.servo_out = (float)nav_throttle * angle_boost(); rc_3.servo_out = max(rc_3.servo_out, 1); } -// Jason void calc_nav_throttle() { diff --git a/ArduCopterMega/radio.pde b/ArduCopterMega/radio.pde index 5a97611021..13a74f33b4 100644 --- a/ArduCopterMega/radio.pde +++ b/ArduCopterMega/radio.pde @@ -6,12 +6,13 @@ void init_radio() rc_2.dead_zone = 50; rc_3.set_range(0,1000); rc_3.dead_zone = 20; - rc_3.radio_max += 300; // hack for better throttle control + //rc_3.radio_max += 300; // hack for better throttle control + rc_3.scale_output = .8; rc_4.set_angle(6000); rc_4.dead_zone = 500; rc_5.set_range(0,1000); rc_5.set_filter(false); - rc_6.set_range(50,200); + rc_6.set_range(200,800); rc_7.set_range(0,1000); rc_8.set_range(0,1000); @@ -33,7 +34,7 @@ void init_radio() } void read_radio() -{ +{ rc_1.set_pwm(APM_RC.InputCh(CH_1)); rc_2.set_pwm(APM_RC.InputCh(CH_2)); rc_3.set_pwm(APM_RC.InputCh(CH_3)); @@ -47,7 +48,9 @@ void read_radio() void trim_radio() { - read_radio(); + for (byte i = 0; i < 50; i++){ + read_radio(); + } rc_1.trim(); // roll rc_2.trim(); // pitch rc_4.trim(); // yaw diff --git a/ArduCopterMega/read_commands.pde b/ArduCopterMega/read_commands.pde index 728ec4c9f1..4686216879 100644 --- a/ArduCopterMega/read_commands.pde +++ b/ArduCopterMega/read_commands.pde @@ -79,32 +79,33 @@ void parseCommand(char *buffer) case 'P': pid_stabilize_roll.kP((float)value / 1000); pid_stabilize_pitch.kP((float)value / 1000); - save_EEPROM_PID(); + //save_EEPROM_PID(); break; case 'I': pid_stabilize_roll.kI((float)value / 1000); pid_stabilize_pitch.kI((float)value / 1000); - save_EEPROM_PID(); + //save_EEPROM_PID(); break; case 'D': pid_stabilize_roll.kD((float)value / 1000); pid_stabilize_pitch.kD((float)value / 1000); - save_EEPROM_PID(); + //save_EEPROM_PID(); break; case 'X': pid_stabilize_roll.imax((int)(value * 100)); pid_stabilize_pitch.imax((int)(value * 100)); - save_EEPROM_PID(); + //save_EEPROM_PID(); break; case 'R': stabilize_rate_roll_pitch = (float)value / 1000; - save_EEPROM_PID(); + //save_EEPROM_PID(); break; } + init_pids(); //*/ } }