diff --git a/ArduCopter/Camera.pde b/ArduCopter/Camera.pde index e204d747fb..3a359d617b 100644 --- a/ArduCopter/Camera.pde +++ b/ArduCopter/Camera.pde @@ -20,8 +20,11 @@ camera_stabilization() // allow control mixing g.rc_camera_pitch.set_pwm(APM_RC.InputCh(CH_6)); // I'm using CH 6 input here. g.rc_camera_pitch.servo_out = g.rc_camera_pitch.control_mix(dcm.pitch_sensor); + + g.rc_camera_pitch.servo_out = (float)g.rc_camera_pitch.servo_out * g.camera_pitch_gain; + // limit - g.rc_camera_pitch.servo_out = constrain(g.rc_camera_pitch.servo_out, -4500, 4500); + //g.rc_camera_pitch.servo_out = constrain(g.rc_camera_pitch.servo_out, -4500, 4500); // dont allow control mixing /* @@ -38,10 +41,10 @@ camera_stabilization() */ // dont allow control mixing + g.rc_camera_roll.servo_out = (float)g.rc_camera_roll.servo_out * g.camera_roll_gain; + // limit - g.rc_camera_roll.servo_out = constrain(-dcm.roll_sensor, -4500, 4500); - - + //g.rc_camera_roll.servo_out = constrain(-dcm.roll_sensor, -4500, 4500); // Output // ------ diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 8b1ce7fff3..85ee0bda43 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -17,7 +17,7 @@ public: // The increment will prevent old parameters from being used incorrectly // by newer code. // - static const uint16_t k_format_version = 110; + static const uint16_t k_format_version = 111; // The parameter software_type is set up solely for ground station use // and identifies the software type (eg ArduPilotMega versus ArduCopterMega) @@ -128,6 +128,8 @@ public: k_param_throttle_cruise, k_param_esc_calibrate, k_param_radio_tuning, + k_param_camera_pitch_gain, + k_param_camera_roll_gain, // // 210: flight modes @@ -262,6 +264,8 @@ public: RC_Channel rc_8; RC_Channel rc_camera_pitch; RC_Channel rc_camera_roll; + AP_Float camera_pitch_gain; + AP_Float camera_roll_gain; // PI/D controllers APM_PI pi_rate_roll; @@ -364,8 +368,13 @@ public: rc_6 (k_param_rc_6, PSTR("RC6_")), rc_7 (k_param_rc_7, PSTR("RC7_")), rc_8 (k_param_rc_8, PSTR("RC8_")), - rc_camera_pitch (k_param_rc_9, PSTR("RC_CP_")), - rc_camera_roll (k_param_rc_10, PSTR("RC_CR_")), + rc_camera_pitch (k_param_rc_9, PSTR("CAM_P_")), + rc_camera_roll (k_param_rc_10, PSTR("CAM_R_")), + + // variable default key name + //------------------------------------------------------------------------------------------------------------------- + camera_pitch_gain (CAM_PITCH_GAIN, k_param_frame_orientation, PSTR("CAM_P_G")), + camera_roll_gain (CAM_ROLL_GAIN, k_param_camera_roll_gain, PSTR("CAM_R_G")), // PI controller group key name initial P initial I initial imax //-------------------------------------------------------------------------------------------------------------------------------------------------------------------- diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 505a70aa2d..692644cf5f 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -179,6 +179,16 @@ #endif +////////////////////////////////////////////////////////////////////////////// +// CAMERA GAINS +#ifndef CAM_ROLL_GAIN +# define CAM_ROLL_GAIN 1.0 +#endif +#ifndef CAM_PITCH_GAIN +# define CAM_PITCH_GAIN 1.0 +#endif + + ////////////////////////////////////////////////////////////////////////////// // OPTICAL_FLOW #if defined( __AVR_ATmega2560__ ) // determines if optical flow code is included @@ -458,8 +468,6 @@ ////////////////////////////////////////////////////////////////////////////// // Throttle control gains // - - #ifndef THROTTLE_CRUISE # define THROTTLE_CRUISE 350 // #endif diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index d7d11ffa54..852178c43f 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -50,17 +50,16 @@ static boolean trim_flag; // set this to your trainer switch static void read_trim_switch() { -#if CH7_OPTION == CH7_FLIP + #if CH7_OPTION == CH7_FLIP if (g.rc_7.control_in > 800 && g.rc_3.control_in != 0){ do_flip = true; } -#elif CH7_OPTION == CH7_SIMPLE_MODE - + #elif CH7_OPTION == CH7_SIMPLE_MODE do_simple = (g.rc_7.control_in > 800); //Serial.println(g.rc_7.control_in, DEC); -#elif CH7_OPTION == CH7_RTL + #elif CH7_OPTION == CH7_RTL static bool ch7_rtl_flag = false; if (ch7_rtl_flag == false && g.rc_7.control_in > 800){ @@ -75,7 +74,7 @@ static void read_trim_switch() } } -#elif CH7_OPTION == CH7_SET_HOVER + #elif CH7_OPTION == CH7_SET_HOVER // switch is engaged if (g.rc_7.control_in > 800){ trim_flag = true; @@ -92,17 +91,19 @@ static void read_trim_switch() trim_flag = false; } } -#elif CH7_OPTION == CH7_ADC_FILTER + + #elif CH7_OPTION == CH7_ADC_FILTER if (g.rc_7.control_in > 800){ adc.filter_result = true; }else{ adc.filter_result = false; } -#elif CH7_OPTION == CH7_AUTO_TRIM + + #elif CH7_OPTION == CH7_AUTO_TRIM if (g.rc_7.control_in > 800){ auto_level_counter = 10; } -#endif + #endif } diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde index c425ba7f08..db5c615eb1 100644 --- a/ArduCopter/setup.pde +++ b/ArduCopter/setup.pde @@ -132,7 +132,7 @@ setup_factory(uint8_t argc, const Menu::arg *argv) return(-1); AP_Var::erase_all(); - Serial.printf_P(PSTR("\nFACTORY RESET complete - reboot APM")); + Serial.printf_P(PSTR("\nReboot APM")); delay(1000); //default_gains(); @@ -936,7 +936,7 @@ void report_optflow() static void report_heli() { int servo_rate; - + Serial.printf_P(PSTR("Heli\n")); print_divider(); @@ -950,7 +950,7 @@ static void report_heli() Serial.printf_P(PSTR("roll max: \t%d\n"), (int)g.heli_roll_max); Serial.printf_P(PSTR("pitch max: \t%d\n"), (int)g.heli_pitch_max); Serial.printf_P(PSTR("coll min:\t%d\t mid:%d\t max:%d\n"),(int)g.heli_coll_min, (int)g.heli_coll_mid, (int)g.heli_coll_max); - + // calculate and print servo rate if( g.heli_servo_averaging <= 1 ) { servo_rate = 250;