diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 759e732671..30978d32b2 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduCopter V2.0.47 Beta" +#define THISFIRMWARE "ArduCopter V2.0.48 Beta" /* ArduCopter Version 2.0 Beta Authors: Jason Short @@ -774,9 +774,9 @@ static void slow_loop() slow_loopCounter++; superslow_loopCounter++; - if(superslow_loopCounter > 800){ // every 4 minutes + if(superslow_loopCounter > 1200){ #if HIL_MODE != HIL_MODE_ATTITUDE - if(g.rc_3.control_in == 0 && g.compass_enabled){ + if(g.rc_3.control_in == 0 && control_mode == STABILIZE && g.compass_enabled){ compass.save_offsets(); superslow_loopCounter = 0; } diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 894ec2f60d..34daa5dff4 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -46,6 +46,14 @@ #endif +////////////////////////////////////////////////////////////////////////////// +// PWM control +// +#ifndef INSTANT_PWM +# define INSTANT_PWM DISABLED +#endif + + ////////////////////////////////////////////////////////////////////////////// // Sonar // @@ -443,7 +451,7 @@ #endif #ifndef WAYPOINT_SPEED_MAX -# define WAYPOINT_SPEED_MAX 450 // for 6m/s error = 13mph +# define WAYPOINT_SPEED_MAX 300 // for 6m/s error = 13mph #endif diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 0e961c092c..3e1f5e2fea 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -39,7 +39,7 @@ static void arm_motors() // Tune down DCM // ------------------- #if HIL_MODE != HIL_MODE_ATTITUDE - dcm.kp_roll_pitch(0.120000); + dcm.kp_roll_pitch(0.030000); dcm.ki_roll_pitch(0.00001278), // 50 hz I term //dcm.ki_roll_pitch(0.000006); #endif diff --git a/ArduCopter/motors_hexa.pde b/ArduCopter/motors_hexa.pde index 066e7c2d17..1634f48f48 100644 --- a/ArduCopter/motors_hexa.pde +++ b/ArduCopter/motors_hexa.pde @@ -2,6 +2,15 @@ #if FRAME_CONFIG == HEXA_FRAME +static void init_motors_out() +{ + #if INSTANT_PWM == 0 + ICR5 = 5000; // 400 hz output CH 1, 2, 9 + ICR1 = 5000; // 400 hz output CH 3, 4, 10 + ICR3 = 5000; // 50 hz output CH 7, 8, 11 + #endif +} + static void output_motors_armed() { int roll_out, pitch_out; @@ -96,10 +105,13 @@ static void output_motors_armed() APM_RC.OutputCh(CH_7, motor_out[CH_7]); APM_RC.OutputCh(CH_8, motor_out[CH_8]); + #if INSTANT_PWM == 1 // InstantPWM APM_RC.Force_Out0_Out1(); - APM_RC.Force_Out6_Out7(); APM_RC.Force_Out2_Out3(); + APM_RC.Force_Out6_Out7(); + #endif + } static void output_motors_disarmed() diff --git a/ArduCopter/motors_octa.pde b/ArduCopter/motors_octa.pde index a89669ba06..dcabe7993d 100644 --- a/ArduCopter/motors_octa.pde +++ b/ArduCopter/motors_octa.pde @@ -2,6 +2,15 @@ #if FRAME_CONFIG == OCTA_FRAME +static void init_motors_out() +{ + #if INSTANT_PWM == 0 + ICR5 = 5000; // 400 hz output CH 1, 2, 9 + ICR1 = 5000; // 400 hz output CH 3, 4, 10 + ICR3 = 5000; // 50 hz output CH 7, 8, 11 + #endif +} + static void output_motors_armed() { int roll_out, pitch_out; @@ -149,10 +158,12 @@ static void output_motors_armed() APM_RC.OutputCh(CH_10, motor_out[CH_10]); APM_RC.OutputCh(CH_11, motor_out[CH_11]); + #if INSTANT_PWM == 1 // InstantPWM APM_RC.Force_Out0_Out1(); - APM_RC.Force_Out6_Out7(); APM_RC.Force_Out2_Out3(); + APM_RC.Force_Out6_Out7(); + #endif } static void output_motors_disarmed() @@ -183,7 +194,7 @@ static void output_motors_disarmed() static void output_motor_test() { if( g.frame_orientation == X_FRAME || g.frame_orientation == PLUS_FRAME ) - { + { APM_RC.OutputCh(CH_7, g.rc_3.radio_min); APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100); delay(1000); @@ -216,9 +227,9 @@ static void output_motor_test() APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 100); delay(1000); } - + if( g.frame_orientation == V_FRAME ) - { + { APM_RC.OutputCh(CH_7, g.rc_3.radio_min); APM_RC.OutputCh(CH_10, g.rc_3.radio_min + 100); delay(1000); diff --git a/ArduCopter/motors_octa_quad.pde b/ArduCopter/motors_octa_quad.pde index 7866c7d3ab..dfdd8c9bfd 100644 --- a/ArduCopter/motors_octa_quad.pde +++ b/ArduCopter/motors_octa_quad.pde @@ -2,6 +2,15 @@ #if FRAME_CONFIG == OCTA_QUAD_FRAME +static void init_motors_out() +{ + #if INSTANT_PWM == 0 + ICR5 = 5000; // 400 hz output CH 1, 2, 9 + ICR1 = 5000; // 400 hz output CH 3, 4, 10 + ICR3 = 5000; // 50 hz output CH 7, 8, 11 + #endif +} + static void output_motors_armed() { int roll_out, pitch_out; @@ -117,10 +126,12 @@ static void output_motors_armed() APM_RC.OutputCh(CH_10, motor_out[CH_10]); APM_RC.OutputCh(CH_11, motor_out[CH_11]); + #if INSTANT_PWM == 1 // InstantPWM APM_RC.Force_Out0_Out1(); - APM_RC.Force_Out6_Out7(); APM_RC.Force_Out2_Out3(); + APM_RC.Force_Out6_Out7(); + #endif } static void output_motors_disarmed() diff --git a/ArduCopter/motors_quad.pde b/ArduCopter/motors_quad.pde index d5af159ad4..1100cc3085 100644 --- a/ArduCopter/motors_quad.pde +++ b/ArduCopter/motors_quad.pde @@ -2,6 +2,15 @@ #if FRAME_CONFIG == QUAD_FRAME +static void init_motors_out() +{ + #if INSTANT_PWM == 0 + ICR5 = 5000; // 400 hz output CH 1, 2, 9 + ICR1 = 5000; // 400 hz output CH 3, 4, 10 + ICR3 = 40000; // 50 hz output CH 7, 8, 11 + #endif +} + static void output_motors_armed() { int roll_out, pitch_out; @@ -87,9 +96,11 @@ static void output_motors_armed() APM_RC.OutputCh(CH_3, motor_out[CH_3]); APM_RC.OutputCh(CH_4, motor_out[CH_4]); + #if INSTANT_PWM == 1 // InstantPWM APM_RC.Force_Out0_Out1(); APM_RC.Force_Out2_Out3(); + #endif } static void output_motors_disarmed() diff --git a/ArduCopter/motors_tri.pde b/ArduCopter/motors_tri.pde index ec9c747b41..2590eea1e6 100644 --- a/ArduCopter/motors_tri.pde +++ b/ArduCopter/motors_tri.pde @@ -1,6 +1,16 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #if FRAME_CONFIG == TRI_FRAME +static void init_motors_out() +{ + #if INSTANT_PWM == 0 + ICR5 = 5000; // 400 hz output CH 1, 2, 9 + ICR1 = 5000; // 400 hz output CH 3, 4, 10 + ICR3 = 40000; // 50 hz output CH 7, 8, 11 + #endif +} + + static void output_motors_armed() { int out_min = g.rc_3.radio_min; @@ -64,9 +74,12 @@ static void output_motors_armed() APM_RC.OutputCh(CH_1, motor_out[CH_1]); APM_RC.OutputCh(CH_2, motor_out[CH_2]); APM_RC.OutputCh(CH_4, motor_out[CH_4]); + + #if INSTANT_PWM == 1 // InstantPWM APM_RC.Force_Out0_Out1(); APM_RC.Force_Out2_Out3(); + #endif } static void output_motors_disarmed() diff --git a/ArduCopter/motors_y6.pde b/ArduCopter/motors_y6.pde index 23f51218a2..91cfc10190 100644 --- a/ArduCopter/motors_y6.pde +++ b/ArduCopter/motors_y6.pde @@ -4,6 +4,16 @@ #define YAW_DIRECTION 1 + +static void init_motors_out() +{ + #if INSTANT_PWM == 0 + ICR5 = 5000; // 400 hz output CH 1, 2, 9 + ICR1 = 5000; // 400 hz output CH 3, 4, 10 + ICR3 = 5000; // 50 hz output CH 7, 8, 11 + #endif +} + static void output_motors_armed() { int out_min = g.rc_3.radio_min; @@ -105,10 +115,12 @@ static void output_motors_armed() APM_RC.OutputCh(CH_7, motor_out[CH_7]); APM_RC.OutputCh(CH_8, motor_out[CH_8]); + #if INSTANT_PWM == 1 // InstantPWM APM_RC.Force_Out0_Out1(); - APM_RC.Force_Out6_Out7(); APM_RC.Force_Out2_Out3(); + APM_RC.Force_Out6_Out7(); + #endif } static void output_motors_disarmed() diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index 309ea91274..34f5f7ddc5 100644 --- a/ArduCopter/radio.pde +++ b/ArduCopter/radio.pde @@ -51,6 +51,7 @@ static void init_rc_out() APM_RC.Init(); // APM Radio initialization + init_motors_out(); // fix for crazy output OCR1B = 0xFFFF; // PB6, OUT3 diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 496fdceccf..460b3302b2 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -174,8 +174,8 @@ static void init_ardupilot() init_camera(); #if HIL_MODE != HIL_MODE_ATTITUDE - // begin filtering the ADC Gyros - adc.filter_result = true; + // begin filtering the ADC Gyros + adc.filter_result = true; adc.Init(); // APM ADC library initialization barometer.Init(); // APM Abs Pressure sensor initialization