diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 5d12c30de3..d88fa06061 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -77,6 +77,15 @@ http://code.google.com/p/ardupilot-mega/downloads/list #include // PI library #include // PID library #include // RC Channel Library +#include // AP Motors library +#include // AP Motors library for Quad +#include // AP Motors library for Tri +#include // AP Motors library for Hexa +#include // AP Motors library for Y6 +#include // AP Motors library for Octa +#include // AP Motors library for OctaQuad +#include // AP Motors library for Heli +#include // AP Motors library for Heli #include // Range finder library #include // Optical Flow library #include // Filter library @@ -362,11 +371,46 @@ static byte old_control_mode = STABILIZE; // Motor Output //////////////////////////////////////////////////////////////////////////////// // This is the array of PWM values being sent to the motors -static int16_t motor_out[11]; +//static int16_t motor_out[11]; // This is the array of PWM values being sent to the motors that has been lightly filtered with a simple LPF // This was added to try and deal with biger motors -static int16_t motor_filtered[11]; +//static int16_t motor_filtered[11]; +#if FRAME_CONFIG == QUAD_FRAME + #define MOTOR_CLASS AP_MotorsQuad +#endif +#if FRAME_CONFIG == TRI_FRAME + #define MOTOR_CLASS AP_MotorsTri +#endif +#if FRAME_CONFIG == HEXA_FRAME + #define MOTOR_CLASS AP_MotorsHexa +#endif +#if FRAME_CONFIG == Y6_FRAME + #define MOTOR_CLASS AP_MotorsY6 +#endif +#if FRAME_CONFIG == OCTA_FRAME + #define MOTOR_CLASS AP_MotorsOcta +#endif +#if FRAME_CONFIG == OCTA_QUAD_FRAME + #define MOTOR_CLASS AP_MotorsOctaQuad +#endif +#if FRAME_CONFIG == HELI_FRAME + #define MOTOR_CLASS AP_MotorsHeli +#endif + +#if FRAME_CONFIG == HELI_FRAME // helicopter constructor requires more arguments + #if INSTANT_PWM == 1 + MOTOR_CLASS motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.heli_servo_1, &g.heli_servo_2, &g.heli_servo_3, &g.heli_servo_4, AP_MOTORS_SPEED_INSTANT_PWM); // this hardware definition is slightly bad because it assumes APM_HARDWARE_APM2 == AP_MOTORS_APM2 + #else + MOTOR_CLASS motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.heli_servo_1, &g.heli_servo_2, &g.heli_servo_3, &g.heli_servo_4); + #endif +#else + #if INSTANT_PWM == 1 + MOTOR_CLASS motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, AP_MOTORS_SPEED_INSTANT_PWM); // this hardware definition is slightly bad because it assumes APM_HARDWARE_APM2 == AP_MOTORS_APM2 + #else + MOTOR_CLASS motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4); + #endif +#endif //////////////////////////////////////////////////////////////////////////////// // Mavlink/HIL control @@ -379,30 +423,12 @@ static bool rc_override_active = false; // Status flag that tracks whether we are under GCS control static uint32_t rc_override_fs_timer = 0; -//////////////////////////////////////////////////////////////////////////////// -// Heli -//////////////////////////////////////////////////////////////////////////////// -#if FRAME_CONFIG == HELI_FRAME -static float heli_rollFactor[3], heli_pitchFactor[3], heli_collectiveFactor[3]; // only required for 3 swashplate servos -static int16_t heli_servo_min[3], heli_servo_max[3]; // same here. for yaw servo we use heli_servo4_min/max parameter directly -static int32_t heli_servo_out[4]; // used for servo averaging for analog servos -static int16_t heli_servo_out_count; // use for servo averaging -#endif - //////////////////////////////////////////////////////////////////////////////// // Failsafe //////////////////////////////////////////////////////////////////////////////// // A status flag for the failsafe state // did our throttle dip below the failsafe value? static boolean failsafe; -// A status flag for arming the motors. This is the arming that is performed when the -// Yaw control is held right or left while throttle is low. -static boolean motor_armed; -// A status flag for whether or not we should allow AP to take over copter -// This is tied to the throttle. If the throttle = 0 or low/nuetral, then we do not allow -// the APM to take control of the copter. -static boolean motor_auto_armed; - //////////////////////////////////////////////////////////////////////////////// // PIDs @@ -948,9 +974,6 @@ static void fast_loop() // ------------------------------ set_servos_4(); - //if(motor_armed) - //Log_Write_Attitude(); - // agmatthews - USERHOOKS #ifdef USERHOOK_FASTLOOP USERHOOK_FASTLOOP @@ -1026,7 +1049,7 @@ static void medium_loop() // ----------------------------- update_navigation(); - if (g.log_bitmask & MASK_LOG_NTUN && motor_armed){ + if (g.log_bitmask & MASK_LOG_NTUN && motors.armed()){ Log_Write_Nav_Tuning(); } } @@ -1060,7 +1083,7 @@ static void medium_loop() } } - if(motor_armed){ + if(motors.armed()){ if (g.log_bitmask & MASK_LOG_ATTITUDE_MED) Log_Write_Attitude(); @@ -1149,22 +1172,16 @@ static void fifty_hz_loop() camera_stabilization(); # if HIL_MODE == HIL_MODE_DISABLED - if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST && motor_armed) + if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST && motors.armed()) Log_Write_Attitude(); - if (g.log_bitmask & MASK_LOG_RAW && motor_armed) + if (g.log_bitmask & MASK_LOG_RAW && motors.armed()) Log_Write_Raw(); #endif // kick the GCS to process uplink data gcs_update(); gcs_data_stream_send(); - - #if FRAME_CONFIG == TRI_FRAME - // servo Yaw - g.rc_4.calc_pwm(); - APM_RC.OutputCh(CH_TRI_YAW, g.rc_4.radio_out); - #endif } @@ -1190,6 +1207,12 @@ static void slow_loop() } #endif } + + // check the user hasn't updated the frame orientation + if( !motors.armed() ) { + motors.set_frame_orientation(g.frame_orientation); + } + break; case 1: @@ -1235,7 +1258,7 @@ static void slow_loop() // 1Hz loop static void super_slow_loop() { - if (g.log_bitmask & MASK_LOG_CUR && motor_armed) + if (g.log_bitmask & MASK_LOG_CUR && motors.armed()) Log_Write_Current(); // this function disarms the copter if it has been sitting on the ground for any moment of time greater than 30s @@ -1381,7 +1404,7 @@ static void update_GPS(void) current_loc.lng = g_gps->longitude; // Lon * 10 * *7 current_loc.lat = g_gps->latitude; // Lat * 10 * *7 - if (g.log_bitmask & MASK_LOG_GPS && motor_armed){ + if (g.log_bitmask & MASK_LOG_GPS && motors.armed()){ Log_Write_GPS(); } @@ -1664,7 +1687,8 @@ void update_throttle_mode(void) update_throttle_cruise(); } - if(motor_auto_armed == true){ + // 10hz, don't run up i term + if( motors.auto_armed() == true ){ // how far off are we altitude_error = get_altitude_error(); @@ -1972,7 +1996,7 @@ static void update_altitude_est() update_altitude(); alt_sensor_flag = false; - if(g.log_bitmask & MASK_LOG_CTUN && motor_armed){ + if(g.log_bitmask & MASK_LOG_CTUN && motors.armed()){ Log_Write_Control_Tuning(); } @@ -2067,7 +2091,7 @@ static void tuning(){ break; case CH6_TOP_BOTTOM_RATIO: - g.top_bottom_ratio = tuning_value; + motors.top_bottom_ratio = tuning_value; break; case CH6_RELAY: @@ -2116,7 +2140,7 @@ static void tuning(){ #if FRAME_CONFIG == HELI_FRAME case CH6_HELI_EXTERNAL_GYRO: - g.heli_ext_gyro_gain = tuning_value; + motors.ext_gyro_gain = tuning_value; break; #endif