diff --git a/ArduCopterMega/APM_Config.h b/ArduCopterMega/APM_Config.h index 39f50f2a8d..57399259ba 100644 --- a/ArduCopterMega/APM_Config.h +++ b/ArduCopterMega/APM_Config.h @@ -5,7 +5,9 @@ #define GPS_PROTOCOL GPS_PROTOCOL_MTK #define GCS_PROTOCOL GCS_PROTOCOL_NONE -#define MAGORIENTATION AP_COMPASS_COMPONENTS_UP_PINS_BACK +//#define MAGORIENTATION AP_COMPASS_COMPONENTS_UP_PINS_BACK +#define MAGORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD + #define ARM_AT_STARTUP 0 diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index b5a5569dae..f557f77acd 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -141,19 +141,23 @@ byte frame_type = PLUS_FRAME; PID pid_acro_rate_roll (EE_GAIN_1); PID pid_acro_rate_pitch (EE_GAIN_2); PID pid_acro_rate_yaw (EE_GAIN_3); -float acro_rate_roll_pitch, acro_rate_yaw; //Stabilize PID pid_stabilize_roll (EE_GAIN_4); PID pid_stabilize_pitch (EE_GAIN_5); PID pid_yaw (EE_GAIN_6); -float stabilize_rate_roll_pitch; -float stabilize_rate_yaw; -float stabilze_dampener; + +// roll pitch +float stabilize_dampener; int max_stabilize_dampener; -float stabilze_yaw_dampener; + +// yaw +float hold_yaw_dampener; int max_yaw_dampener; +// used to transition yaw control from Rate control to Yaw hold +boolean rate_yaw_flag; + // Nav PID pid_nav (EE_GAIN_7); PID pid_throttle (EE_GAIN_8); @@ -740,7 +744,9 @@ void update_current_flight_mode(void) calc_nav_throttle(); // Output Pitch, Roll, Yaw and Throttle - // ------------------------------------ + // ------------------------------------ + auto_yaw(); + // perform stabilzation output_stabilize(); @@ -760,12 +766,12 @@ void update_current_flight_mode(void) nav_pitch = 0; nav_roll = 0; - // get desired yaw control from radio - input_yaw_hold(); - // Output Pitch, Roll, Yaw and Throttle // ------------------------------------ + // Yaw control + output_manual_yaw(); + // apply throttle control output_manual_throttle(); @@ -806,8 +812,9 @@ void update_current_flight_mode(void) calc_nav_roll(); calc_nav_pitch(); - // get desired yaw control from radio - input_yaw_hold(); + // Yaw control + // ----------- + output_manual_yaw(); // Output Pitch, Roll, Yaw and Throttle // ------------------------------------ @@ -830,8 +837,9 @@ void update_current_flight_mode(void) // get desired height from the throttle next_WP.alt = home.alt + (rc_3.control_in * 4) -100; // 0 - 1000 (40 meters) - // get desired yaw control from radio - input_yaw_hold(); + // Yaw control + // ----------- + output_manual_yaw(); // based on altitude error // ----------------------- @@ -860,6 +868,8 @@ void update_current_flight_mode(void) // Output Pitch, Roll, Yaw and Throttle // ------------------------------------ + auto_yaw(); + // apply throttle control output_auto_throttle(); @@ -874,8 +884,9 @@ void update_current_flight_mode(void) calc_nav_roll(); calc_nav_pitch(); - // get desired yaw control from radio - input_yaw_hold(); + // Yaw control + // ----------- + output_manual_yaw(); // based on altitude error // ----------------------- diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde index 6e5890dae0..122f47b3e3 100644 --- a/ArduCopterMega/Attitude.pde +++ b/ArduCopterMega/Attitude.pde @@ -1,19 +1,20 @@ -// desired angle in -// motor commands out (in degrees) void init_pids() { - max_stabilize_dampener = pid_stabilize_roll.kP() * 2500; - stabilze_dampener = 5729.57795 * stabilize_rate_roll_pitch; - - max_yaw_dampener = pid_yaw.kP() * 6000; // .3 * 6000 = 1800 - stabilze_yaw_dampener = 5729.57795 * stabilize_rate_yaw; // .3 + // create limits to how much dampening we'll allow + // 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 } + void output_stabilize() { float roll_error, pitch_error; Vector3f omega = dcm.get_gyro(); + long rate; + int dampener; //pitch_sensor = roll_sensor = 0; // testing only @@ -24,35 +25,31 @@ void output_stabilize() roll_error = rc_1.servo_out - roll_sensor; pitch_error = rc_2.servo_out - pitch_sensor; - yaw_error = nav_yaw - yaw_sensor; - yaw_error = wrap_180(yaw_error); // limit the error we're feeding to the PID roll_error = constrain(roll_error, -2500, 2500); pitch_error = constrain(pitch_error, -2500, 2500); - yaw_error = constrain(yaw_error, -6000, 6000); //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); - rc_4.servo_out = pid_yaw.get_pid(yaw_error, deltaMiliSeconds, 1.0); // .3 = 198pwm //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 - int roll_dampener = (omega.x * stabilze_dampener);// Omega is in radians - int pitch_dampener = (omega.y * stabilze_dampener); - int yaw_dampener = (omega.z * stabilze_yaw_dampener); - + // 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); - + 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 + + 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); @@ -60,22 +57,90 @@ void output_stabilize() //Serial.printf("\tlimit: %d, PWM: %d", rc_1.servo_out, rc_1.angle_to_pwm()); } -// err -2500 pid: -1100 rd: 1117 limit: -1650, PWM: -152 -//s: -1247 mix 0, err 1247 pid: 548 rd: -153 limit: 395, PWM: 35 +void +clear_yaw_control() +{ + //Serial.print("Clear "); + rate_yaw_flag = false; // exit rate_yaw_flag + nav_yaw = yaw_sensor; // save our Yaw + 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 "); + // rate control + rate_yaw_flag = true; + yaw_error = 0; + } + + if(hold){ + yaw_error = nav_yaw - yaw_sensor; // +- 60° + yaw_error = wrap_180(yaw_error); + + // limit the error we're feeding to the PID + yaw_error = constrain(yaw_error, -6000, 6000); // limit error to 60 degees + 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 + long rate = degrees(omega.z) * 100; // 3rad = 17188 , 6rad = 34377 + int dampener = ((float)rate * hold_yaw_dampener); // 18000 * .17 = 3000 + + // 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; + + // 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 + // -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; + } +} + + + void output_rate_control() { + /* Vector3f omega = dcm.get_gyro(); - rc_4.servo_out = rc_4.control_in; rc_1.servo_out = rc_2.control_in; rc_2.servo_out = rc_2.control_in; // Rate control through bias corrected gyro rates // omega is the raw gyro reading plus Omega_I, so it´s bias corrected - rc_1.servo_out -= (omega.x * 5729.57795 * acro_rate_roll_pitch); - rc_2.servo_out -= (omega.y * 5729.57795 * acro_rate_roll_pitch); - rc_4.servo_out -= (omega.z * 5729.57795 * acro_rate_yaw); + rc_1.servo_out -= (omega.x * 5729.57795 * acro_dampener); + rc_2.servo_out -= (omega.y * 5729.57795 * acro_dampener); //Serial.printf("\trated out %d, omega ", rc_1.servo_out); //Serial.print((Omega[0] * 5729.57795 * stabilize_rate_roll_pitch), 3); @@ -83,7 +148,7 @@ void output_rate_control() // Limit 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); + */ } diff --git a/ArduCopterMega/EEPROM.pde b/ArduCopterMega/EEPROM.pde index 5ac4627269..eb0b94f3a0 100644 --- a/ArduCopterMega/EEPROM.pde +++ b/ArduCopterMega/EEPROM.pde @@ -133,10 +133,11 @@ void read_EEPROM_PID(void) pid_nav.load_gains(); pid_throttle.load_gains(); - stabilize_rate_roll_pitch = read_EE_compressed_float(EE_STAB_RATE_RP, 4); - stabilize_rate_yaw = read_EE_compressed_float(EE_STAB_RATE_YAW, 4); - acro_rate_roll_pitch = read_EE_compressed_float(EE_ACRO_RATE_RP, 4); - acro_rate_yaw = read_EE_compressed_float(EE_ACRO_RATE_YAW, 4); + // roll pitch + stabilize_dampener = read_EE_compressed_float(EE_STAB_DAMPENER, 4); + + // yaw + hold_yaw_dampener = read_EE_compressed_float(EE_HOLD_YAW_DAMPENER, 4); } void save_EEPROM_PID(void) @@ -152,10 +153,11 @@ void save_EEPROM_PID(void) pid_nav.save_gains(); pid_throttle.save_gains(); - write_EE_compressed_float(stabilize_rate_roll_pitch, EE_STAB_RATE_RP, 4); - write_EE_compressed_float(stabilize_rate_yaw, EE_STAB_RATE_YAW, 4); - write_EE_compressed_float(acro_rate_roll_pitch, EE_ACRO_RATE_RP, 4); - write_EE_compressed_float(acro_rate_yaw, EE_ACRO_RATE_YAW, 4); + // roll pitch + write_EE_compressed_float(stabilize_dampener, EE_STAB_DAMPENER, 4); + + // yaw + write_EE_compressed_float(hold_yaw_dampener, EE_HOLD_YAW_DAMPENER, 4); } /********************************************************************************/ diff --git a/ArduCopterMega/commands_process.pde b/ArduCopterMega/commands_process.pde index bc2575dfe7..76050bbe93 100644 --- a/ArduCopterMega/commands_process.pde +++ b/ArduCopterMega/commands_process.pde @@ -134,10 +134,12 @@ void process_must() switch(command_must_ID){ case CMD_TAKEOFF: // TAKEOFF! - takeoff_altitude = (int)next_command.alt; + takeoff_altitude = (int)next_command.alt; next_WP.lat = current_loc.lat + 10; // so we don't have bad calcs next_WP.lng = current_loc.lng + 10; // so we don't have bad calcs + next_WP.alt = current_loc.alt + takeoff_altitude; takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction + //set_next_WP(&next_WP); break; case CMD_WAYPOINT: // Navigate to Waypoint @@ -395,7 +397,7 @@ void verify_must() break; case CMD_TAKEOFF: // Takeoff! - if (current_loc.alt > (home.alt + takeoff_altitude)){ + if (current_loc.alt > (next_WP.alt -100)){ command_must_index = 0; takeoff_complete = true; } diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h index b4e2544847..d61c4911c0 100644 --- a/ArduCopterMega/config.h +++ b/ArduCopterMega/config.h @@ -254,7 +254,7 @@ #endif #ifndef ACRO_RATE_YAW_P -# define ACRO_RATE_YAW_P .1 +# define ACRO_RATE_YAW_P .07 #endif #ifndef ACRO_RATE_YAW_I # define ACRO_RATE_YAW_I 0.0 @@ -263,19 +263,7 @@ # define ACRO_RATE_YAW_D 0.0 #endif #ifndef ACRO_RATE_YAW_IMAX -# define ACRO_RATE_YAW_IMAX 20 -#endif - -// STABILZE RATE Control -// -#ifndef ACRO_RATE_RP -# define ACRO_RATE_RP 0.1 -#endif - -// STABILZE RATE Control -// -#ifndef ACRO_RATE_YAW -# define ACRO_RATE_YAW 0.1 +# define ACRO_RATE_YAW_IMAX 0 #endif @@ -310,8 +298,8 @@ // STABILZE RATE Control // -#ifndef STABILIZE_RATE_RP -# define STABILIZE_RATE_RP 0.175 +#ifndef STABILIZE_DAMPENER +# define STABILIZE_DAMPENER 0.175 #endif @@ -319,7 +307,7 @@ // YAW Control // #ifndef YAW_P -# define YAW_P 0.8 +# define YAW_P 0.5 #endif #ifndef YAW_I # define YAW_I 0.0 @@ -333,8 +321,8 @@ // STABILZE YAW Control // -#ifndef STABILIZE_RATE_YAW -# define STABILIZE_RATE_YAW 0.000 +#ifndef HOLD_YAW_DAMPENER +# define HOLD_YAW_DAMPENER 0.175 #endif ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopterMega/defines.h b/ArduCopterMega/defines.h index a6654e07cb..ccba8ae3ce 100644 --- a/ArduCopterMega/defines.h +++ b/ArduCopterMega/defines.h @@ -68,6 +68,7 @@ #define PLUS_FRAME 0 #define X_FRAME 1 +#define TRI_FRAME 2 // Auto Pilot modes // ---------------- @@ -263,10 +264,9 @@ #define EE_GAIN_9 0x80 // all gains stored from here #define EE_GAIN_10 0x88 // all gains stored from here -#define EE_STAB_RATE_RP 0xA0 -#define EE_STAB_RATE_YAW 0xA2 -#define EE_ACRO_RATE_RP 0xA4 -#define EE_ACRO_RATE_YAW 0xA6 +#define EE_STAB_DAMPENER 0xA0 +#define EE_HOLD_YAW_DAMPENER 0xA2 + #define EE_MAG_DECLINATION 0xA8 #define EE_MAG_X 0xAA #define EE_MAG_Y 0xAC diff --git a/ArduCopterMega/flight_control.pde b/ArduCopterMega/flight_control.pde index 347fdb05fb..c8d56a9dea 100644 --- a/ArduCopterMega/flight_control.pde +++ b/ArduCopterMega/flight_control.pde @@ -44,47 +44,26 @@ float angle_boost() yaw control ****************************************************************/ -void input_yaw_hold_2() +void output_manual_yaw() { if(rc_3.control_in == 0){ - // Reset the yaw hold - nav_yaw = yaw_sensor; - - }else if(rc_4.control_in == 0){ - // do nothing - - }else{ - // create up to 60° of yaw error - nav_yaw = yaw_sensor + rc_4.control_in; - nav_yaw = wrap_360(nav_yaw); + clear_yaw_control(); + } else { + // Yaw control + if(rc_4.control_in == 0){ + //clear_yaw_control(); + output_yaw_with_hold(true); // hold yaw + }else{ + output_yaw_with_hold(false); // rate control yaw + } } } -void input_yaw_hold() +void auto_yaw() { - if(rc_3.control_in == 0){ - // Reset the yaw hold - nav_yaw = yaw_sensor; - - }else if(rc_4.control_in == 0){ - // do nothing - - }else{ - // create up to 60° of yaw error - nav_yaw += ((long)rc_4.control_in * (long)deltaMiliSeconds) / 500; // we'll get 60° * 2 or 120° per second - nav_yaw = wrap_360(nav_yaw); - } + output_yaw_with_hold(true); // hold yaw } -/*void output_yaw_stabilize() -{ - rc_4.servo_out = rc_4.control_in; - rc_4.servo_out = constrain(rc_4.servo_out, -MAX_SERVO_OUTPUT, MAX_SERVO_OUTPUT); -}*/ - - - - /************************************************************* picth and roll control ****************************************************************/ diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde index a6a7689326..12a9a80868 100644 --- a/ArduCopterMega/navigation.pde +++ b/ArduCopterMega/navigation.pde @@ -70,12 +70,6 @@ void calc_bearing_error() bearing_error = wrap_180(bearing_error); } -void calc_yaw_error() -{ - yaw_error = nav_yaw - yaw_sensor; - yaw_error = wrap_180(yaw_error); -} - void calc_distance_error() { wp_distance = GPS_wp_distance; diff --git a/ArduCopterMega/radio.pde b/ArduCopterMega/radio.pde index f8ca6a6c7a..48a1680ffa 100644 --- a/ArduCopterMega/radio.pde +++ b/ArduCopterMega/radio.pde @@ -7,14 +7,15 @@ void init_rc_in() rc_2.dead_zone = 60; rc_3.set_range(0,1000); rc_3.dead_zone = 20; - rc_3.scale_output = .8; + rc_3.scale_output = .9; rc_4.set_angle(6000); rc_4.dead_zone = 500; rc_5.set_range(0,1000); rc_5.set_filter(false); // for kP values - rc_6.set_range(200,800); + //rc_6.set_range(200,800); + rc_6.set_range(0,4000); // for camera angles //rc_6.set_angle(4500); @@ -133,26 +134,45 @@ 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{ + + }else if(frame_type == X_FRAME){ int roll_out = rc_1.pwm_out / 2; int pitch_out = rc_2.pwm_out / 2; - motor_out[RIGHT] = rc_3.radio_out - roll_out + pitch_out; - motor_out[LEFT] = rc_3.radio_out + roll_out - pitch_out; + motor_out[FRONT] = rc_3.radio_out + roll_out + pitch_out; + 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; + }else{ + /* + replace this with Tri-frame control law + + int roll_out = rc_1.pwm_out / 2; + 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; + + motor_out[RIGHT] = 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; - motor_out[LEFT] += rc_4.pwm_out; - motor_out[FRONT] -= rc_4.pwm_out; - motor_out[BACK] -= rc_4.pwm_out; - + 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]); motor_out[RIGHT] = constrain(motor_out[RIGHT], out_min, rc_3.radio_max); @@ -174,9 +194,10 @@ void set_servos_4(void) if (num > 50){ num = 0; - pid_stabilize_roll.kP((float)rc_6.control_in / 1000); - stabilize_rate_roll_pitch = pid_stabilize_roll.kP() *.25; - init_pids(); + hold_yaw_dampener = (float)rc_6.control_in; + //pid_stabilize_roll.kP((float)rc_6.control_in / 1000); + //stabilize_rate_roll_pitch = pid_stabilize_roll.kP() *.25; + //init_pids(); //Serial.print("nav_yaw: "); //Serial.println(nav_yaw,DEC); diff --git a/ArduCopterMega/read_commands.pde b/ArduCopterMega/read_commands.pde index 4686216879..4ccae0251b 100644 --- a/ArduCopterMega/read_commands.pde +++ b/ArduCopterMega/read_commands.pde @@ -101,7 +101,7 @@ void parseCommand(char *buffer) break; case 'R': - stabilize_rate_roll_pitch = (float)value / 1000; + //stabilize_rate_roll_pitch = (float)value / 1000; //save_EEPROM_PID(); break; } diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde index edf2c95a11..642e802138 100644 --- a/ArduCopterMega/setup.pde +++ b/ArduCopterMega/setup.pde @@ -2,7 +2,7 @@ // Functions called from the setup menu static int8_t setup_radio (uint8_t argc, const Menu::arg *argv); -static int8_t setup_esc (uint8_t argc, const Menu::arg *argv); +static int8_t setup_motors (uint8_t argc, const Menu::arg *argv); static int8_t setup_show (uint8_t argc, const Menu::arg *argv); static int8_t setup_accel (uint8_t argc, const Menu::arg *argv); static int8_t setup_accel_flat (uint8_t argc, const Menu::arg *argv); @@ -23,7 +23,7 @@ const struct Menu::command setup_menu_commands[] PROGMEM = { {"reset", setup_factory}, {"erase", setup_erase}, {"radio", setup_radio}, - {"esc", setup_esc}, + {"motors", setup_motors}, {"level", setup_accel}, {"flat", setup_accel_flat}, {"modes", setup_flightmodes}, @@ -61,27 +61,33 @@ static int8_t setup_show(uint8_t argc, const Menu::arg *argv) { uint8_t i; - - Serial.printf_P(PSTR("\nRadio:\n")); - + print_blanks(10); + Serial.printf_P(PSTR("Radio\n")); + print_divider(); // radio read_EEPROM_radio(); print_radio_values(); // frame + print_blanks(3); + Serial.printf_P(PSTR("Frame\n")); + print_divider(); + read_EEPROM_frame(); - Serial.printf_P(PSTR("\nFrame type:")); - Serial.printf_P(PSTR("%d \n"), (int)frame_type); if(frame_type == X_FRAME) - Serial.printf_P(PSTR(" X\n")); + Serial.printf_P(PSTR("X ")); else if(frame_type == PLUS_FRAME) - Serial.printf_P(PSTR("Plus\n")); + Serial.printf_P(PSTR("Plus ")); + Serial.printf_P(PSTR("(%d)\n"), (int)frame_type); + print_blanks(3); + Serial.printf_P(PSTR("Gains\n")); + print_divider(); + read_EEPROM_PID(); - Serial.printf_P(PSTR("\nGains:\n")); // Acro Serial.printf_P(PSTR("Acro:\nroll:\n")); print_PID(&pid_acro_rate_roll); @@ -89,21 +95,18 @@ setup_show(uint8_t argc, const Menu::arg *argv) print_PID(&pid_acro_rate_pitch); Serial.printf_P(PSTR("yaw:\n")); print_PID(&pid_acro_rate_yaw); - Serial.printf_P(PSTR("rates:\n")); - Serial.println(acro_rate_roll_pitch,3); - Serial.printf_P(PSTR("rates_yaw:\n")); - Serial.println(acro_rate_yaw,3); - Serial.println(" "); + // Stabilize - Serial.printf_P(PSTR("Stabilize:\nroll:\n")); + Serial.printf_P(PSTR("\nStabilize:\nroll:\n")); print_PID(&pid_stabilize_roll); Serial.printf_P(PSTR("pitch:\n")); print_PID(&pid_stabilize_pitch); Serial.printf_P(PSTR("yaw:\n")); print_PID(&pid_yaw); - Serial.printf_P(PSTR("Rate stabilize:\n")); - Serial.println(stabilize_rate_roll_pitch,3); - Serial.println(" "); + + Serial.printf_P(PSTR("Stabilize dampener: %4.3f\n"), stabilize_dampener); + Serial.printf_P(PSTR("Yaw Dampener: %4.3f\n\n"), hold_yaw_dampener); + // Nav Serial.printf_P(PSTR("Nav:\npitch:\n")); print_PID(&pid_nav); @@ -111,29 +114,33 @@ setup_show(uint8_t argc, const Menu::arg *argv) print_PID(&pid_throttle); Serial.println(" "); - + print_blanks(3); + Serial.printf_P(PSTR("User Configs\n")); + print_divider(); // Crosstrack read_EEPROM_nav(); - Serial.printf_P(PSTR("XTRACK:")); - Serial.println(x_track_gain,DEC); + Serial.printf_P(PSTR("XTRACK: %4.2f\n"), x_track_gain); Serial.printf_P(PSTR("XTRACK angle: %d\n"), x_track_angle); - Serial.printf_P(PSTR("PITCH_MAX: %d\n\n"), pitch_max); - + Serial.printf_P(PSTR("PITCH_MAX: %d\n"), pitch_max); // User Configs read_EEPROM_configs(); - Serial.printf_P(PSTR("\nUser config:\n")); Serial.printf_P(PSTR("throttle_min: %d\n"), throttle_min); Serial.printf_P(PSTR("throttle_max: %d\n"), throttle_max); Serial.printf_P(PSTR("throttle_cruise: %d\n"), throttle_cruise); Serial.printf_P(PSTR("throttle_failsafe_enabled: %d\n"), throttle_failsafe_enabled); Serial.printf_P(PSTR("throttle_failsafe_value: %d\n"), throttle_failsafe_value); - Serial.printf_P(PSTR("log_bitmask: %d\n\n"), log_bitmask); + Serial.printf_P(PSTR("log_bitmask: %d\n"), log_bitmask); + print_blanks(3); + Serial.printf_P(PSTR("IMU\n")); + print_divider(); imu.print_gyro_offsets(); imu.print_accel_offsets(); - // mag declination - Serial.printf_P(PSTR("\nCompass ")); + print_blanks(3); + Serial.printf_P(PSTR("Compass\n")); + print_divider(); + if(compass_enabled) Serial.printf_P(PSTR("en")); else @@ -324,41 +331,94 @@ setup_radio(uint8_t argc, const Menu::arg *argv) } static int8_t -setup_esc(uint8_t argc, const Menu::arg *argv) +setup_motors(uint8_t argc, const Menu::arg *argv) { - Serial.println("\n\nESC Setup - Motors armed"); + init_rc_in(); + // read the radio to set trims + // --------------------------- + trim_radio(); - rc_3.load_eeprom(); - rc_1.set_filter(false); - rc_2.set_filter(false); - rc_3.set_filter(false); - rc_4.set_filter(false); - motor_armed = true; + print_hit_enter(); + delay(1000); + + + int out_min = rc_3.radio_min + 70; + + if(frame_type == PLUS_FRAME){ + Serial.printf_P(PSTR("PLUS")); + }else{ + Serial.printf_P(PSTR("X")); + } + Serial.printf_P(PSTR(" Frame\n")); while(1){ - delay(20); read_radio(); - - APM_RC.OutputCh(CH_1, rc_3.radio_in); - APM_RC.OutputCh(CH_2, rc_3.radio_in); - APM_RC.OutputCh(CH_3, rc_3.radio_in); - APM_RC.OutputCh(CH_4, rc_3.radio_in); + motor_out[LEFT] = rc_3.radio_min; + motor_out[BACK] = rc_3.radio_min; + motor_out[FRONT] = rc_3.radio_min; + motor_out[RIGHT] = rc_3.radio_min; + + + if(frame_type == PLUS_FRAME){ + if(rc_1.control_in > 0){ + motor_out[RIGHT] = out_min; + Serial.println("0"); + + }else if(rc_1.control_in < 0){ + motor_out[LEFT] = out_min; + Serial.println("1"); + } + + if(rc_2.control_in > 0){ + motor_out[BACK] = out_min; + Serial.println("3"); + + }else if(rc_2.control_in < 0){ + motor_out[FRONT] = out_min; + Serial.println("2"); + } + + }else{ + // lower right + if((rc_1.control_in > 0) && (rc_2.control_in > 0)){ + motor_out[BACK] = out_min; + Serial.println("3"); + // lower left + }else if((rc_1.control_in < 0) && (rc_2.control_in > 0)){ + motor_out[LEFT] = out_min; + Serial.println("1"); + + // upper left + }else if((rc_1.control_in < 0) && (rc_2.control_in < 0)){ + motor_out[FRONT] = out_min; + Serial.println("2"); + + // upper right + }else if((rc_1.control_in > 0) && (rc_2.control_in < 0)){ + motor_out[RIGHT] = out_min; + Serial.println("0"); + } + } + + if(rc_3.control_in > 0){ + APM_RC.OutputCh(CH_1, rc_3.radio_in); + APM_RC.OutputCh(CH_2, rc_3.radio_in); + APM_RC.OutputCh(CH_3, rc_3.radio_in); + APM_RC.OutputCh(CH_4, rc_3.radio_in); + }else{ + 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]); + } + if(Serial.available() > 0){ - motor_armed = false; - rc_1.set_filter(true); - rc_2.set_filter(true); - rc_3.set_filter(true); - rc_4.set_filter(true); - read_radio(); - Serial.println("\n\nMotors disarmed)"); - break; + return (0); } } - return(0); } - static int8_t setup_accel(uint8_t argc, const Menu::arg *argv) { @@ -408,8 +468,6 @@ setup_pid(uint8_t argc, const Menu::arg *argv) pid_acro_rate_yaw.kD(ACRO_RATE_YAW_D); pid_acro_rate_yaw.imax(ACRO_RATE_YAW_IMAX * 100); - acro_rate_roll_pitch = ACRO_RATE_RP; - acro_rate_yaw = ACRO_RATE_YAW; // stabilize, angle error pid_stabilize_roll.kP(STABILIZE_ROLL_P); @@ -422,15 +480,21 @@ setup_pid(uint8_t argc, const Menu::arg *argv) pid_stabilize_pitch.kD(STABILIZE_PITCH_D); pid_stabilize_pitch.imax(STABILIZE_PITCH_IMAX * 100); - // rate control for angle error - stabilize_rate_roll_pitch = STABILIZE_RATE_RP; - stabilize_rate_yaw = STABILIZE_RATE_YAW; - + // YAW hold pid_yaw.kP(YAW_P); pid_yaw.kI(YAW_I); pid_yaw.kD(YAW_D); pid_yaw.imax(YAW_IMAX * 100); + + // custom dampeners + // roll pitch + stabilize_dampener = STABILIZE_DAMPENER; + + //yaw + hold_yaw_dampener = HOLD_YAW_DAMPENER; + + // navigation pid_nav.kP(NAV_P); pid_nav.kI(NAV_I); @@ -539,19 +603,26 @@ static int8_t setup_frame(uint8_t argc, const Menu::arg *argv) { if(argv[1].i < 1){ - Serial.printf_P(PSTR("\nUsage:\nPlus frame =>frame 1\nX frame =>frame 2\n\n")); + Serial.printf_P(PSTR("\nUsage:\nPlus frame =>frame 1\nX frame =>frame 2\nTRI frame =>frame 3\n\n")); return 0; } + Serial.printf_P(PSTR("\nSaving ")); if(argv[1].i == 1){ - Serial.printf_P(PSTR("\nSaving Plus frame\n\n")); + Serial.printf_P(PSTR("Plus ")); frame_type = PLUS_FRAME; }else if(argv[1].i == 2){ - Serial.printf_P(PSTR("\nSaving X frame\n\n")); + Serial.printf_P(PSTR("X ")); + frame_type = X_FRAME; + + }else if(argv[1].i == 3){ + + Serial.printf_P(PSTR("Tri ")); frame_type = X_FRAME; } + Serial.printf_P(PSTR("frame\n\n")); save_EEPROM_frame(); return 0; } @@ -640,14 +711,7 @@ setup_compass(uint8_t argc, const Menu::arg *argv) void print_PID(PID * pid) { - Serial.printf_P(PSTR("P: ")); - Serial.print((float)pid->kP(),3); - Serial.printf_P(PSTR(",I: ")); - Serial.print((float)pid->kI(),3); - Serial.printf_P(PSTR(",D: ")); - Serial.print((float)pid->kD(),3); - Serial.printf_P(PSTR(",IMAX: ")); - Serial.println(pid->imax()/100,DEC); // imax is stored as *100 degrees internally + Serial.printf_P(PSTR("P: %4.3f, I:%4.3f, D:%4.3f, IMAX:%d\n"), pid->kP(), pid->kI(), pid->kD(), (int)(pid->imax()/100)); } void @@ -685,6 +749,15 @@ print_blanks(int num) } } +void +print_divider(void) +{ + for (int i = 0; i < 40; i++) { + Serial.print("-"); + } + Serial.println(""); +} + // for reading in vales for mode switch boolean diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde index 4b9ba03a26..f75a3976fb 100644 --- a/ArduCopterMega/test.pde +++ b/ArduCopterMega/test.pde @@ -2,9 +2,9 @@ // are defined below. Order matters to the compiler. static int8_t test_radio_pwm(uint8_t argc, const Menu::arg *argv); static int8_t test_radio(uint8_t argc, const Menu::arg *argv); -static int8_t test_flaps(uint8_t argc, const Menu::arg *argv); static int8_t test_stabilize(uint8_t argc, const Menu::arg *argv); 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_gyro(uint8_t argc, const Menu::arg *argv); static int8_t test_dcm(uint8_t argc, const Menu::arg *argv); @@ -40,9 +40,9 @@ static int8_t test_eedump(uint8_t argc, const Menu::arg *argv); const struct Menu::command test_menu_commands[] PROGMEM = { {"pwm", test_radio_pwm}, {"radio", test_radio}, - {"flaps", test_flaps}, {"stabilize", test_stabilize}, {"gps", test_gps}, + {"adc", test_adc}, {"imu", test_imu}, {"gyro", test_gyro}, {"dcm", test_dcm}, @@ -111,7 +111,7 @@ test_radio(uint8_t argc, const Menu::arg *argv) // read the radio to set trims // --------------------------- - //trim_radio(); + trim_radio(); while(1){ delay(20); @@ -147,6 +147,8 @@ test_radio(uint8_t argc, const Menu::arg *argv) static int8_t test_stabilize(uint8_t argc, const Menu::arg *argv) { + static byte ts_num; + print_hit_enter(); delay(1000); @@ -155,15 +157,15 @@ test_stabilize(uint8_t argc, const Menu::arg *argv) init_rc_in(); control_mode = STABILIZE; - Serial.printf_P(PSTR("pid_stabilize_roll.kP: ")); - Serial.println(pid_stabilize_roll.kP(),3); + Serial.printf_P(PSTR("pid_stabilize_roll.kP: %4.4f\n"), pid_stabilize_roll.kP()); Serial.printf_P(PSTR("max_stabilize_dampener:%d\n\n "), max_stabilize_dampener); motor_armed = true; - + trim_radio(); + while(1){ // 50 hz - if (millis() - fast_loopTimer > 49) { + if (millis() - fast_loopTimer > 19) { deltaMiliSeconds = millis() - fast_loopTimer; fast_loopTimer = millis(); G_Dt = (float)deltaMiliSeconds / 1000.f; @@ -186,16 +188,40 @@ test_stabilize(uint8_t argc, const Menu::arg *argv) // IMU // --- read_AHRS(); - + + // allow us to zero out sensors with control switches + if(rc_5.control_in < 600){ + roll_sensor = pitch_sensor = 0; + } + // custom code/exceptions for flight modes // --------------------------------------- update_current_flight_mode(); - - //Serial.println(" "); // write out the servo PWM values // ------------------------------ set_servos_4(); + + ts_num++; + if (ts_num > 10){ + ts_num = 0; + Serial.printf_P(PSTR("r: %d, p:%d, rc1:%d, rc2:%d, rc4 %d, ny:%ld, ys:%ld, ye:%ld, R: %d, L: %d F: %d B: %d\n"), + (int)(roll_sensor/100), + (int)(pitch_sensor/100), + rc_1.pwm_out, + rc_2.pwm_out, + rc_4.pwm_out, + nav_yaw, + dcm.yaw_sensor, + yaw_error, + motor_out[RIGHT], + motor_out[LEFT], + motor_out[FRONT], + motor_out[BACK]); + } + + // R: 1417, L: 1453 F: 1453 B: 1417 + //Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)deltaMiliSeconds, ((int)roll_sensor/100), ((int)pitch_sensor/100), ((uint16_t)yaw_sensor/100)); //Serial.printf_P(PSTR("timer: %d, r: %d\tp: %d\t y: %d\n"), (int)deltaMiliSeconds, ((int)roll_sensor/100), ((int)pitch_sensor/100), ((uint16_t)yaw_sensor/100)); @@ -207,6 +233,26 @@ test_stabilize(uint8_t argc, const Menu::arg *argv) } } +static int8_t +test_adc(uint8_t argc, const Menu::arg *argv) +{ + print_hit_enter(); + adc.Init(); + delay(1000); + Serial.printf_P(PSTR("ADC\n")); + delay(1000); + + while(1){ + for(int i = 0; i < 9; i++){ + Serial.printf_P(PSTR("i:%d\t"),adc.Ch(i)); + } + Serial.println(); + delay(20); + if(Serial.available() > 0){ + return (0); + } + } +} static int8_t test_imu(uint8_t argc, const Menu::arg *argv) { @@ -402,20 +448,38 @@ test_dcm(uint8_t argc, const Menu::arg *argv) static int8_t test_omega(uint8_t argc, const Menu::arg *argv) { + static byte ts_num; + float old_yaw; + print_hit_enter(); delay(1000); Serial.printf_P(PSTR("Omega")); delay(1000); + + G_Dt = .02; while(1){ + delay(20); + // IMU + // --- + read_AHRS(); + float my_oz = (dcm.yaw - old_yaw) * 50; + + old_yaw = dcm.yaw; + Vector3f omega = dcm.get_gyro(); - Serial.printf_P(PSTR("R: %d\tP: %d\tY: %d\n"), (int)(ToDeg(omega.x)), (int)(ToDeg(omega.y)), (int)(ToDeg(omega.z))); - delay(100); + ts_num++; + if (ts_num > 2){ + ts_num = 0; + //Serial.printf_P(PSTR("R: %4.4f\tP: %4.4f\tY: %4.4f\tY: %4.4f\n"), omega.x, omega.y, omega.z, my_oz); + Serial.printf_P(PSTR(" Yaw: %ld\tY: %4.4f\tY: %4.4f\n"), dcm.yaw_sensor, omega.z, my_oz); + } if(Serial.available() > 0){ return (0); } } + return (0); } static int8_t @@ -465,27 +529,6 @@ test_relay(uint8_t argc, const Menu::arg *argv) } } -static int8_t -test_flaps(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - - while(1){ - delay(300); - read_radio(); - float temp = (float)rc_6.control_in / 1000; - - Serial.print("flaps: "); - Serial.println(temp, 3); - - if(Serial.available() > 0){ - return (0); - } - } -} - - static int8_t test_wp(uint8_t argc, const Menu::arg *argv) {