From ff256d95e2e5090c4edeec25281f32f2053279b6 Mon Sep 17 00:00:00 2001 From: jasonshort Date: Tue, 4 Jan 2011 06:06:26 +0000 Subject: [PATCH] Added Tri-copter support for Max git-svn-id: https://arducopter.googlecode.com/svn/trunk@1412 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArduCopterMega/Attitude.pde | 41 +++++++++++-------------------- ArduCopterMega/EEPROM.pde | 1 + ArduCopterMega/defines.h | 8 +++--- ArduCopterMega/flight_control.pde | 3 --- ArduCopterMega/navigation.pde | 2 ++ ArduCopterMega/radio.pde | 38 +++++++++++++++++++--------- ArduCopterMega/setup.pde | 37 +++++++++++++++++++++++++--- ArduCopterMega/system.pde | 1 - 8 files changed, 83 insertions(+), 48 deletions(-) diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde index 122f47b3e3..fb3a0c999f 100644 --- a/ArduCopterMega/Attitude.pde +++ b/ArduCopterMega/Attitude.pde @@ -5,7 +5,7 @@ void init_pids() // this creates symmetry with the P gain value preventing oscillations max_stabilize_dampener = pid_stabilize_roll.kP() * 2500; // = 0.6 * 2500 = 1500 or 15° - max_yaw_dampener = pid_yaw.kP() * 6000; // .5 * 6000 = 3000 + max_yaw_dampener = pid_yaw.kP() * 6000; // .5 * 6000 = 3000 } @@ -16,8 +16,6 @@ void output_stabilize() long rate; int dampener; - //pitch_sensor = roll_sensor = 0; // testing only - // control +- 45° is mixed with the navigation request by the Autopilot // output is in degrees = target pitch and roll of copter rc_1.servo_out = rc_1.control_mix(nav_roll); @@ -30,14 +28,10 @@ void output_stabilize() roll_error = constrain(roll_error, -2500, 2500); pitch_error = constrain(pitch_error, -2500, 2500); - //Serial.printf("s: %d \t mix %d, err %d", (int)roll_sensor, (int)rc_1.servo_out, (int)roll_error); - // write out angles back to servo out - this will be converted to PWM by RC_Channel rc_1.servo_out = pid_stabilize_roll.get_pid(roll_error, deltaMiliSeconds, 1.0); rc_2.servo_out = pid_stabilize_pitch.get_pid(pitch_error, deltaMiliSeconds, 1.0); - //Serial.printf("\tpid: %d", (int)rc_1.servo_out); - // We adjust the output by the rate of rotation: // Rate control through bias corrected gyro rates // omega is the raw gyro reading @@ -45,16 +39,11 @@ void output_stabilize() // Limit dampening to be equal to propotional term for symmetry rate = degrees(omega.x) * 100; // 6rad = 34377 dampener = ((float)rate * stabilize_dampener); // 34377 * .175 = 6000 - rc_1.servo_out -= constrain(dampener, -max_stabilize_dampener, max_stabilize_dampener); // limit to 1500 + rc_1.servo_out -= constrain(dampener, -max_stabilize_dampener, max_stabilize_dampener); // limit to 1500 based on kP rate = degrees(omega.y) * 100; // 6rad = 34377 dampener = ((float)rate * stabilize_dampener); // 34377 * .175 = 6000 - rc_2.servo_out -= constrain(dampener, -max_stabilize_dampener, max_stabilize_dampener); // limit to 1500 - - //Serial.printf(" yaw out: %d, d: %d", (int)rc_4.angle_to_pwm(), yaw_dampener); - - //Serial.printf("\trd: %d", roll_dampener); - //Serial.printf("\tlimit: %d, PWM: %d", rc_1.servo_out, rc_1.angle_to_pwm()); + rc_2.servo_out -= constrain(dampener, -max_stabilize_dampener, max_stabilize_dampener); // limit to 1500 based on kP } void @@ -66,30 +55,32 @@ clear_yaw_control() yaw_error = 0; } + void output_yaw_with_hold(boolean hold) { Vector3f omega = dcm.get_gyro(); if(hold){ // yaw hold + if(rate_yaw_flag){ // we are still in motion from rate control if(fabs(omega.y) < .15){ - //Serial.print("trans "); clear_yaw_control(); hold = true; // just to be explicit }else{ - //Serial.print("coast "); // return to rate control until we slow down. hold = false; } }else{ - //Serial.print("hold "); + } - } else { - //Serial.print("rate "); + }else{ // rate control + + // this indicates we are under rate control, when we enter Yaw Hold and + // return to 0° per second, we exit rate control and hold the current Yaw rate_yaw_flag = true; yaw_error = 0; } @@ -100,6 +91,8 @@ void output_yaw_with_hold(boolean hold) // limit the error we're feeding to the PID yaw_error = constrain(yaw_error, -6000, 6000); // limit error to 60 degees + + // Apply PID and save the new angle back to RC_Channel rc_4.servo_out = pid_yaw.get_pid(yaw_error, deltaMiliSeconds, 1.0); // .5 * 6000 = 3000 // We adjust the output by the rate of rotation @@ -109,20 +102,16 @@ void output_yaw_with_hold(boolean hold) // Limit dampening to be equal to propotional term for symmetry rc_4.servo_out -= constrain(dampener, -max_yaw_dampener, max_yaw_dampener); // -3000 - }else{ - //yaw_error = 0; - + }else{ // rate control + long rate = degrees(omega.z) * 100; // 3rad = 17188 , 6rad = 34377 rate = constrain(rate, -36000, 36000); // limit to something fun! - long error = ((long)rc_4.control_in * 6) - rate; // control is += 6000 + long error = ((long)rc_4.control_in * 6) - rate; // control is += 6000 * 6 = 36000 // -error = CCW, +error = CW rc_4.servo_out = pid_acro_rate_yaw.get_pid(error, deltaMiliSeconds, 1.0); // .075 * 36000 = 2700 rc_4.servo_out = constrain(rc_4.servo_out, -2400, 2400); // limit to 2400 - // this indicates we are under rate control, when we enter Yaw Hold and - // return to 0° per second, we exit rate control and hold the current Yaw - //rate_yaw_flag = true; } } diff --git a/ArduCopterMega/EEPROM.pde b/ArduCopterMega/EEPROM.pde index eb0b94f3a0..9f0b95cd12 100644 --- a/ArduCopterMega/EEPROM.pde +++ b/ArduCopterMega/EEPROM.pde @@ -138,6 +138,7 @@ void read_EEPROM_PID(void) // yaw hold_yaw_dampener = read_EE_compressed_float(EE_HOLD_YAW_DAMPENER, 4); + init_pids(); } void save_EEPROM_PID(void) diff --git a/ArduCopterMega/defines.h b/ArduCopterMega/defines.h index ccba8ae3ce..4f4f448e14 100644 --- a/ArduCopterMega/defines.h +++ b/ArduCopterMega/defines.h @@ -46,10 +46,10 @@ #define CH_AUX2 CH_6 // motors -#define FRONT 0 -#define RIGHT 1 -#define BACK 2 -#define LEFT 3 +#define RIGHT 0 +#define LEFT 1 +#define FRONT 2 +#define BACK 3 #define MAX_SERVO_OUTPUT 2700 diff --git a/ArduCopterMega/flight_control.pde b/ArduCopterMega/flight_control.pde index c8d56a9dea..a697f6733f 100644 --- a/ArduCopterMega/flight_control.pde +++ b/ArduCopterMega/flight_control.pde @@ -37,9 +37,6 @@ float angle_boost() } - - - /************************************************************* yaw control ****************************************************************/ diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde index 12a9a80868..8cfb2f7ebf 100644 --- a/ArduCopterMega/navigation.pde +++ b/ArduCopterMega/navigation.pde @@ -50,6 +50,7 @@ void navigate() } } +/* void verify_missed_wp() { // check if we have missed the WP @@ -63,6 +64,7 @@ void verify_missed_wp() if (loiter_delta < -170) loiter_delta += 360; loiter_sum += abs(loiter_delta); } +*/ void calc_bearing_error() { diff --git a/ArduCopterMega/radio.pde b/ArduCopterMega/radio.pde index 48a1680ffa..558d7bea99 100644 --- a/ArduCopterMega/radio.pde +++ b/ArduCopterMega/radio.pde @@ -134,13 +134,14 @@ void set_servos_4(void) //Serial.printf("yaw: %d ", rc_4.radio_out); if(frame_type == PLUS_FRAME){ - //Serial.println("+"); + motor_out[RIGHT] = rc_3.radio_out - rc_1.pwm_out; motor_out[LEFT] = rc_3.radio_out + rc_1.pwm_out; motor_out[FRONT] = rc_3.radio_out + rc_2.pwm_out; motor_out[BACK] = rc_3.radio_out - rc_2.pwm_out; }else if(frame_type == X_FRAME){ + int roll_out = rc_1.pwm_out / 2; int pitch_out = rc_2.pwm_out / 2; @@ -149,28 +150,39 @@ void set_servos_4(void) motor_out[RIGHT] = rc_3.radio_out - roll_out + pitch_out; motor_out[BACK] = rc_3.radio_out - roll_out - pitch_out; - }else{ - /* - replace this with Tri-frame control law - int roll_out = rc_1.pwm_out / 2; + }else if(frame_type == TRI_FRAME){ + + // Tri-copter power distribution + + int roll_out = (float)rc_1.pwm_out * .866; int pitch_out = rc_2.pwm_out / 2; - - motor_out[FRONT] = rc_3.radio_out + roll_out + pitch_out; - motor_out[LEFT] = rc_3.radio_out + roll_out - pitch_out; - + + // front two motors + motor_out[LEFT] = rc_3.radio_out + roll_out + pitch_out; motor_out[RIGHT] = rc_3.radio_out - roll_out + pitch_out; - motor_out[BACK] = rc_3.radio_out - roll_out - pitch_out; - */ + + // rear motors + motor_out[BACK] = rc_3.radio_out - rc_2.pwm_out; + + // servo Yaw + motor_out[FRONT] = rc_4.radio_out; + + }else{ + + Serial.print("frame error"); + } //Serial.printf("\tb4: %d %d %d %d ", motor_out[RIGHT], motor_out[LEFT], motor_out[FRONT], motor_out[BACK]); if((frame_type == PLUS_FRAME) || (frame_type == X_FRAME)){ + motor_out[RIGHT] += rc_4.pwm_out; motor_out[LEFT] += rc_4.pwm_out; motor_out[FRONT] -= rc_4.pwm_out; motor_out[BACK] -= rc_4.pwm_out; + } //Serial.printf("\tl8r: %d %d %d %d\n", motor_out[RIGHT], motor_out[LEFT], motor_out[FRONT], motor_out[BACK]); @@ -229,15 +241,19 @@ void set_servos_4(void) // Send commands to motors if(rc_3.servo_out > 0){ + APM_RC.OutputCh(CH_1, motor_out[RIGHT]); APM_RC.OutputCh(CH_2, motor_out[LEFT]); APM_RC.OutputCh(CH_3, motor_out[FRONT]); APM_RC.OutputCh(CH_4, motor_out[BACK]); + }else{ + APM_RC.OutputCh(CH_1, rc_3.radio_min); APM_RC.OutputCh(CH_2, rc_3.radio_min); APM_RC.OutputCh(CH_3, rc_3.radio_min); APM_RC.OutputCh(CH_4, rc_3.radio_min); + } // InstantPWM diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde index 642e802138..b9ccca21ec 100644 --- a/ArduCopterMega/setup.pde +++ b/ArduCopterMega/setup.pde @@ -270,6 +270,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv) // stop here } } + rc_1.radio_min = rc_1.radio_in; rc_2.radio_min = rc_2.radio_in; rc_3.radio_min = rc_3.radio_in; @@ -346,9 +347,14 @@ setup_motors(uint8_t argc, const Menu::arg *argv) if(frame_type == PLUS_FRAME){ Serial.printf_P(PSTR("PLUS")); - }else{ + + }else if(frame_type == X_FRAME){ Serial.printf_P(PSTR("X")); + + }else if(frame_type == TRI_FRAME){ + Serial.printf_P(PSTR("TRI")); } + Serial.printf_P(PSTR(" Frame\n")); while(1){ @@ -380,7 +386,8 @@ setup_motors(uint8_t argc, const Menu::arg *argv) Serial.println("2"); } - }else{ + }else if(frame_type == PLUS_FRAME){ + // lower right if((rc_1.control_in > 0) && (rc_2.control_in > 0)){ motor_out[BACK] = out_min; @@ -400,6 +407,29 @@ setup_motors(uint8_t argc, const Menu::arg *argv) motor_out[RIGHT] = out_min; Serial.println("0"); } + + }else if(frame_type == TRI_FRAME){ + + if(rc_1.control_in > 0){ + motor_out[RIGHT] = out_min; + + }else if(rc_1.control_in < 0){ + motor_out[LEFT] = out_min; + } + + if(rc_2.control_in > 0){ + motor_out[BACK] = out_min; + } + + if(rc_4.control_in > 0){ + rc_4.servo_out = 2000; + + }else if(rc_4.control_in < 0){ + rc_4.servo_out = -2000; + } + + rc_4.calc_pwm(); + motor_out[FRONT] = rc_4.radio_out; } if(rc_3.control_in > 0){ @@ -419,6 +449,7 @@ setup_motors(uint8_t argc, const Menu::arg *argv) } } } + static int8_t setup_accel(uint8_t argc, const Menu::arg *argv) { @@ -620,7 +651,7 @@ setup_frame(uint8_t argc, const Menu::arg *argv) }else if(argv[1].i == 3){ Serial.printf_P(PSTR("Tri ")); - frame_type = X_FRAME; + frame_type = TRI_FRAME; } Serial.printf_P(PSTR("frame\n\n")); save_EEPROM_frame(); diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index 037c111680..5546f8adab 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -94,7 +94,6 @@ void init_ardupilot() read_EEPROM_startup(); // Read critical config information to start - init_pids(); init_rc_in(); // sets up rc channels from radio init_rc_out(); // sets up the timer libs