diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index ee6dc1a740..870a07cc40 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -55,8 +55,6 @@ // See the config.h and defines.h files for how to set this up! // -// lets use SIMPLE mode for Roll and Pitch during Alt Hold -//#define ALT_HOLD_RP ROLL_PITCH_SIMPLE // lets use Manual throttle during Loiter //#define LOITER_THR THROTTLE_MANUAL diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 6f38cd40ab..69e92c4b7b 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -61,7 +61,7 @@ And much more so PLEASE PM me on DIYDRONES to add your contribution to the List #include // PI library #include // RC Channel Library #include // Range finder library -#include // Optical Flow library +#include // Optical Flow library #include #include // MAVLink GCS definitions #include @@ -127,9 +127,6 @@ static AP_Int8 *flight_modes = &g.flight_mode1; APM_BMP085_Class barometer; AP_Compass_HMC5843 compass(Parameters::k_param_compass); - #ifdef OPTFLOW_ENABLED - AP_OpticalFlow_ADNS3080 optflow; - #endif // real GPS selection #if GPS_PROTOCOL == GPS_PROTOCOL_AUTO @@ -171,7 +168,7 @@ static AP_Int8 *flight_modes = &g.flight_mode1; AP_Compass_HIL compass; // never used AP_IMU_Shim imu; // never used #ifdef OPTFLOW_ENABLED - AP_OpticalFlow_ADNS3080 optflow; + AP_OpticalFlow_ADNS3080 optflow(&dcm); #endif static int32_t gps_base_alt; #else @@ -201,6 +198,16 @@ static AP_Int8 *flight_modes = &g.flight_mode1; #endif // normal dcm AP_DCM dcm(&imu, g_gps); + + #ifdef OPTFLOW_ENABLED + AP_OpticalFlow_ADNS3080 optflow(&dcm); + #endif +#endif + +#elif HIL_MODE == HIL_MODE_ATTITUDE + #ifdef OPTFLOW_ENABLED + AP_OpticalFlow_ADNS3080 optflow(&dcm); + #endif #endif //////////////////////////////////////////////////////////////////////////////// @@ -467,15 +474,12 @@ static struct Location next_command; // command preloaded static struct Location guided_WP; // guided mode waypoint static long target_altitude; // used for static boolean home_is_set; // Flag for if we have g_gps lock and have set the home location -static struct Location optflow_offset; // optical flow base position static boolean new_location; // flag to tell us if location has been updated - // IMU variables // ------------- static float G_Dt = 0.02; // Integration time for the gyros (DCM algorithm) - // Performance monitoring // ---------------------- static long perf_mon_timer; @@ -483,17 +487,10 @@ static long perf_mon_timer; static int G_Dt_max; // Max main loop cycle time in milliseconds static int gps_fix_count; -// GCS -// --- -//static char GCS_buffer[53]; -//static char display_PID = -1; // Flag used by DebugTerminal to indicate that the next PID calculation with this index should be displayed - // System Timers // -------------- static unsigned long fast_loopTimer; // Time in miliseconds of main control loop -static unsigned long fast_loopTimeStamp; // Time Stamp when fast loop was complete static uint8_t delta_ms_fast_loop; // Delta Time in miliseconds -//static int mainLoop_count; static unsigned long medium_loopTimer; // Time in miliseconds of navigation control loop static byte medium_loopCounter; // Counters for branching from main control loop to slower loops @@ -514,6 +511,7 @@ static unsigned long nav_loopTimer; // used to track the elapsed ime for GPS static byte counter_one_herz; static bool GPS_enabled = false; static byte loop_step; +static bool new_radio_frame; //////////////////////////////////////////////////////////////////////////////// // Top-level logic @@ -537,7 +535,6 @@ void loop() // Execute the fast loop // --------------------- fast_loop(); - fast_loopTimeStamp = millis(); } //PORTK &= B11101111; @@ -562,15 +559,12 @@ void loop() } if (millis() - perf_mon_timer > 20000) { - //if (mainLoop_count != 0) { + gcs.send_message(MSG_PERF_REPORT); - gcs.send_message(MSG_PERF_REPORT); + if (g.log_bitmask & MASK_LOG_PM) + Log_Write_Performance(); - if (g.log_bitmask & MASK_LOG_PM) - Log_Write_Performance(); - - resetPerfData(); - //} + resetPerfData(); } //PORTK &= B10111111; } @@ -623,11 +617,15 @@ static void medium_loop() medium_loopCounter++; - /* if(g.optflow_enabled){ - optflow.update() + optflow.read(); + optflow.update_position(cos_yaw_x, sin_yaw_y, current_loc.alt); // updates internal lon and lat with estimation based on optical flow + + // write to log + if (g.log_bitmask & MASK_LOG_OPTFLOW){ + Log_Write_Optflow(); + } } - */ if(GPS_enabled){ update_GPS(); @@ -795,19 +793,8 @@ static void fifty_hz_loop() hil.send_message(MSG_RADIO_OUT); #endif - // use Yaw to find our bearing error - //calc_bearing_error(); - - //if (throttle_slew < 0) - // throttle_slew++; - //else if (throttle_slew > 0) - // throttle_slew--; - camera_stabilization(); - - //throttle_slew = min((800 - g.rc_3.control_in), throttle_slew); - # if HIL_MODE == HIL_MODE_DISABLED if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) Log_Write_Attitude(); @@ -882,10 +869,10 @@ static void slow_loop() #if AUTO_RESET_LOITER == 1 if(control_mode == LOITER){ - if((abs(g.rc_2.control_in) + abs(g.rc_1.control_in)) > 1500){ + //if((abs(g.rc_2.control_in) + abs(g.rc_1.control_in)) > 1500){ // reset LOITER to current position //next_WP = current_loc; - } + //} } #endif @@ -915,11 +902,6 @@ static void slow_loop() if(g.radio_tuning > 0) tuning(); - // filter out the baro offset. - //if(baro_alt_offset > 0) baro_alt_offset--; - //if(baro_alt_offset < 0) baro_alt_offset++; - - #if MOTOR_LEDS == 1 update_motor_leds(); #endif @@ -1005,6 +987,7 @@ static void update_GPS(void) } } + void update_yaw_mode(void) { switch(yaw_mode){ @@ -1054,28 +1037,19 @@ void update_roll_pitch_mode(void) int control_roll = 0, control_pitch = 0; - if(do_simple){ - simple_timer++; - if(simple_timer > 5){ - simple_timer = 0; + //read_radio(); - int delta = wrap_360(dcm.yaw_sensor - initial_simple_bearing)/100; + if(do_simple && new_radio_frame){ + new_radio_frame = false; + //Serial.printf("1: %d, 2: %d ",g.rc_1.control_in, g.rc_2.control_in); - // pre-calc rotation (stored in real degrees) - // roll - float cos_x = sin(radians(90 - delta)); - // pitch - float sin_y = cos(radians(90 - delta)); + // Rotate input by the initial bearing + control_roll = g.rc_1.control_in * sin_yaw_y + g.rc_2.control_in * cos_yaw_x; + control_pitch = g.rc_2.control_in * sin_yaw_y - g.rc_1.control_in * cos_yaw_x; - // Rotate input by the initial bearing - // roll - control_roll = g.rc_1.control_in * cos_x + g.rc_2.control_in * sin_y; - // pitch - control_pitch = -(g.rc_1.control_in * sin_y - g.rc_2.control_in * cos_x); - - g.rc_1.control_in = control_roll; - g.rc_2.control_in = control_pitch; - } + g.rc_1.control_in = control_roll; + g.rc_2.control_in = control_pitch; + //Serial.printf("\t1: %d, 2: %d\n",g.rc_1.control_in, g.rc_2.control_in); } switch(roll_pitch_mode){ @@ -1085,29 +1059,26 @@ void update_roll_pitch_mode(void) // Pitch control g.rc_2.servo_out = get_rate_pitch(g.rc_2.control_in); - return; break; case ROLL_PITCH_STABLE: - control_roll = g.rc_1.control_in; - control_pitch = g.rc_2.control_in; + g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in); + g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in); break; case ROLL_PITCH_AUTO: // mix in user control with Nav control - control_roll = g.rc_1.control_mix(nav_roll); - control_pitch = g.rc_2.control_mix(nav_pitch); + control_roll = g.rc_1.control_mix(nav_roll); + control_pitch = g.rc_2.control_mix(nav_pitch); + g.rc_1.servo_out = get_stabilize_roll(control_roll); + g.rc_2.servo_out = get_stabilize_pitch(control_pitch); break; } - // Roll control - g.rc_1.servo_out = get_stabilize_roll(control_roll); - - // Pitch control - g.rc_2.servo_out = get_stabilize_pitch(control_pitch); } +// 50 hz update rate, not 250 void update_throttle_mode(void) { switch(throttle_mode){ @@ -1238,8 +1209,6 @@ static void update_trig(void){ yawvector.y = temp.b.x; // cos yawvector.normalize(); - cos_yaw_x = yawvector.y; // 0 x = north - sin_yaw_y = yawvector.x; // 1 y sin_pitch_y = -temp.c.x; cos_pitch_x = sqrt(1 - (temp.c.x * temp.c.x)); @@ -1247,6 +1216,9 @@ static void update_trig(void){ cos_roll_x = temp.c.z / cos_pitch_x; sin_roll_y = temp.c.y / cos_pitch_x; + cos_yaw_x = yawvector.y; // 0 x = north + sin_yaw_y = yawvector.x; // 1 y + //flat: // 0 ° = cp: 1.00, sp: 0.00, cr: 1.00, sr: 0.00, cy: 0.00, sy: 1.00, // 90° = cp: 1.00, sp: 0.00, cr: 1.00, sr: 0.00, cy: 1.00, sy: 0.00, @@ -1366,8 +1338,8 @@ static void tuning(){ case CH6_RELAY: g.rc_6.set_range(0,1000); - if (g.rc_6.control_in <= 600) relay_on(); - if (g.rc_6.control_in >= 400) relay_off(); + if (g.rc_6.control_in > 525) relay_on(); + if (g.rc_6.control_in < 475) relay_off(); break; case CH6_TRAVERSE_SPEED: diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index b6672e0752..ee7b041b4b 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -91,7 +91,6 @@ static int get_nav_throttle(long z_error, int target_speed) { int rate_error; - //int throttle; float scaler = (float)target_speed/(float)ALT_ERROR_MAX; // limit error to prevent I term run up @@ -101,14 +100,9 @@ get_nav_throttle(long z_error, int target_speed) rate_error = target_speed - altitude_rate; rate_error = constrain(rate_error, -110, 110); - //throttle = g.pi_throttle.get_pi(rate_error, delta_ms_medium_loop); - //return g.throttle_cruise + throttle; - - return g.pi_throttle.get_pi(rate_error, delta_ms_medium_loop); } - static int get_rate_roll(long target_rate) { diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index f96b3ffe2c..d5a71cc5a9 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -98,9 +98,6 @@ dump_log(uint8_t argc, const Menu::arg *argv) int dump_log_start; int dump_log_end; - //byte last_log_num = get_num_logs(); - //Serial.printf_P(PSTR("\n%d logs\n"), last_log_num); - // check that the requested log number can be read dump_log = argv[1].i; @@ -506,8 +503,8 @@ static void Log_Write_Optflow() DataFlash.WriteInt((int)optflow.dx); DataFlash.WriteInt((int)optflow.dy); DataFlash.WriteInt((int)optflow.surface_quality); - DataFlash.WriteLong(optflow.lat);//optflow_offset.lat + optflow.lat); - DataFlash.WriteLong(optflow.lng);//optflow_offset.lng + optflow.lng); + DataFlash.WriteLong(optflow.vlat);//optflow_offset.lat + optflow.lat); + DataFlash.WriteLong(optflow.vlon);//optflow_offset.lng + optflow.lng); DataFlash.WriteByte(END_BYTE); } #endif diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index b9581a7705..5b3d88fe2e 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -462,12 +462,7 @@ static void do_change_alt() Location temp = next_WP; condition_start = current_loc.alt; condition_value = next_command.alt; - //if (next_command.options & WP_OPTION_ALT_RELATIVE) { - // condition_value = next_command.alt + home.alt; - //} else { - - //} - temp.alt = condition_value; + temp.alt = next_command.alt; set_next_WP(&temp); } diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 2f2fe769c4..3f6937a374 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -207,9 +207,11 @@ ////////////////////////////////////////////////////////////////////////////// // OPTICAL_FLOW #if defined( __AVR_ATmega2560__ ) // determines if optical flow code is included - //#define OPTFLOW_ENABLED + #define OPTFLOW_ENABLED #endif +#define OPTFLOW_ENABLED + #ifndef OPTFLOW // sets global enabled/disabled flag for optflow (as seen in CLI) # define OPTFLOW DISABLED #endif @@ -310,7 +312,7 @@ #endif #ifndef SIMPLE_RP -# define SIMPLE_RP ROLL_PITCH_SIMPLE +# define SIMPLE_RP ROLL_PITCH_STABLE #endif #ifndef SIMPLE_THR @@ -413,7 +415,7 @@ // Rate Control // #ifndef RATE_ROLL_P -# define RATE_ROLL_P .13 +# define RATE_ROLL_P 0.13 #endif #ifndef RATE_ROLL_I # define RATE_ROLL_I 0.0 @@ -488,22 +490,6 @@ # define NAV_IMAX 20 // degrees #endif -/* -#ifndef NAV_LOITER_P -# define NAV_LOITER_P .4 //1.4 // -#endif -#ifndef NAV_LOITER_I -# define NAV_LOITER_I 0.05 // -#endif -#ifndef NAV_LOITER_D -# define NAV_LOITER_D 2 // -#endif -#ifndef NAV_LOITER_IMAX -# define NAV_LOITER_IMAX 8 // degrees° -#endif -*/ - - #ifndef WAYPOINT_SPEED_MAX # define WAYPOINT_SPEED_MAX 450 // for 6m/s error = 13mph #endif @@ -518,9 +504,6 @@ #ifndef THROTTLE_I # define THROTTLE_I 0.10 // with 4m error, 12.5s windup #endif -//#ifndef THROTTLE_D -//# define THROTTLE_D 0.6 // upped with filter -//#endif #ifndef THROTTLE_IMAX # define THROTTLE_IMAX 300 #endif diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index a12957ac97..63c3918534 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -14,10 +14,13 @@ static void read_control_switch() set_mode(flight_modes[switchPosition]); - // setup Simple mode - // do we enable simple mode? - do_simple = (g.simple_modes & 1 << switchPosition); - Serial.printf("do simple: %d \n", (int)do_simple); + #if CH7_OPTION != SIMPLE_MODE_CONTROL + // setup Simple mode + // do we enable simple mode? + do_simple = (g.simple_modes & 1 << switchPosition); + //Serial.printf("do simple: %d \n", (int)do_simple); + #endif + }else{ switch_debouncer = true; @@ -56,6 +59,7 @@ static void read_trim_switch() #elif CH7_OPTION == SIMPLE_MODE_CONTROL do_simple = (g.rc_7.control_in > 800); + //Serial.println(g.rc_7.control_in, DEC); #elif CH7_OPTION == DO_SET_HOVER // switch is engaged diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 1bbd29cbcf..498fc91bcf 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -14,8 +14,7 @@ #define ROLL_PITCH_STABLE 0 #define ROLL_PITCH_ACRO 1 -#define ROLL_PITCH_SIMPLE 2 -#define ROLL_PITCH_AUTO 3 +#define ROLL_PITCH_AUTO 2 #define THROTTLE_MANUAL 0 #define THROTTLE_HOLD 1 diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index a40ef79f27..a11b96e894 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -30,16 +30,6 @@ static void navigate() // target_bearing is where we should be heading // -------------------------------------------- target_bearing = get_bearing(¤t_loc, &next_WP); - - // nav_bearing will include xtrac correction - // ----------------------------------------- - //xtrack_enabled = false; - - //if(xtrack_enabled){ - // crosstrack_correction = get_crosstrack_correction(); - //}else { - // crosstrack_correction = 0; - //} } static bool check_missed_wp() @@ -71,6 +61,9 @@ static void calc_location_error(struct Location *next_loc) lat_error = next_loc->lat - current_loc.lat; // 0 - 500 = -500 pitch NORTH } + +// nav_roll = g.pid_of_roll.get_pid(-optflow.x_cm * 10, dTnav, 1.0); + #define NAV_ERR_MAX 400 static void calc_nav_rate(int x_error, int y_error, int max_speed, int min_speed) { @@ -105,15 +98,20 @@ static void calc_nav_rate(int x_error, int y_error, int max_speed, int min_speed float temp = radians((float)g_gps->ground_course/100.0); // calc the cos of the error to tell how fast we are moving towards the target in cm - y_actual_speed = (float)g_gps->ground_speed * cos(temp); + if(g.optflow_enabled && current_loc.alt < 500 && g_gps->ground_speed < 150){ + x_actual_speed = optflow.vlon * 10; + y_actual_speed = optflow.vlat * 10; + }else{ + x_actual_speed = (float)g_gps->ground_speed * sin(temp); + y_actual_speed = (float)g_gps->ground_speed * cos(temp); + } + y_rate_error = y_target_speed - y_actual_speed; // 413 y_rate_error = constrain(y_rate_error, -600, 600); // added a rate error limit to keep pitching down to a minimum nav_lat = constrain(g.pi_nav_lat.get_pi(y_rate_error, dTnav), -3500, 3500); //Serial.printf("yr: %d, nav_lat: %d, int:%d \n",y_rate_error, nav_lat, g.pi_nav_lat.get_integrator()); - // calc the sin of the error to tell how fast we are moving laterally to the target in cm - x_actual_speed = (float)g_gps->ground_speed * sin(temp); x_rate_error = x_target_speed - x_actual_speed; x_rate_error = constrain(x_rate_error, -600, 600); nav_lon = constrain(g.pi_nav_lon.get_pi(x_rate_error, dTnav), -3500, 3500); diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index 4025730e8c..86b30c9de7 100644 --- a/ArduCopter/radio.pde +++ b/ArduCopter/radio.pde @@ -101,7 +101,7 @@ void output_min() static void read_radio() { if (APM_RC.GetState() == 1){ - + new_radio_frame = true; g.rc_1.set_pwm(APM_RC.InputCh(CH_1)); g.rc_2.set_pwm(APM_RC.InputCh(CH_2)); g.rc_3.set_pwm(APM_RC.InputCh(CH_3)); diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde index 552fbee763..e9c25cf4c2 100644 --- a/ArduCopter/setup.pde +++ b/ArduCopter/setup.pde @@ -304,7 +304,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) byte _oldSwitchPosition = 0; byte mode = 0; - Serial.printf_P(PSTR("\nMove mode switch to edit, aileron: select modes, rudder: Simple on/off")); + Serial.printf_P(PSTR("\nMove mode switch to edit, aileron: select modes, rudder: Simple on/off\n")); print_hit_enter(); while(1){ @@ -341,7 +341,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) } // look for stick input - if (abs(g.rc_4.control_in) > 3000){ + if (g.rc_4.control_in > 3000){ g.simple_modes |= (1<<_switchPosition); // print new mode print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition))); @@ -349,7 +349,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) } // look for stick input - if (abs(g.rc_4.control_in) < 3000){ + if (g.rc_4.control_in < -3000){ g.simple_modes &= ~(1<<_switchPosition); // print new mode print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition))); @@ -361,6 +361,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) for (mode = 0; mode < 6; mode++) flight_modes[mode].save(); + g.simple_modes.save(); print_done(); report_flight_modes(); return (0); @@ -760,10 +761,6 @@ setup_optflow(uint8_t argc, const Menu::arg *argv) } else if (!strcmp_P(argv[1].str, PSTR("off"))) { g.optflow_enabled = false; - //} else if(argv[1].i > 10){ - // g.optflow_fov.set_and_save(argv[1].i); - // optflow.set_field_of_view(g.optflow_fov.get()); - }else{ Serial.printf_P(PSTR("\nOptions:[on, off]\n")); report_optflow(); @@ -978,13 +975,13 @@ print_radio_values() static void print_switch(byte p, byte m, bool b) { - Serial.printf_P(PSTR("Pos %d: "),p); + Serial.printf_P(PSTR("Pos %d:\t"),p); Serial.print(flight_mode_strings[m]); - Serial.printf_P(PSTR(", Simple: ")); + Serial.printf_P(PSTR(",\t\tSimple: ")); if(b) - Serial.printf_P(PSTR("T\n")); + Serial.printf_P(PSTR("ON\n")); else - Serial.printf_P(PSTR("F\n")); + Serial.printf_P(PSTR("OFF\n")); } static void diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 55f8cec2be..34d9acd1c4 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -204,12 +204,12 @@ static void init_ardupilot() if(g.compass_enabled) init_compass(); -#ifdef OPTFLOW_ENABLED + #ifdef OPTFLOW_ENABLED // init the optical flow sensor if(g.optflow_enabled) { init_optflow(); } -#endif + #endif // Logging: // -------- @@ -321,8 +321,7 @@ static void startup_ground(void) #define ROLL_PITCH_STABLE 0 #define ROLL_PITCH_ACRO 1 -#define ROLL_PITCH_SIMPLE 2 -#define ROLL_PITCH_AUTO 3 +#define ROLL_PITCH_AUTO 2 #define THROTTLE_MANUAL 0 #define THROTTLE_HOLD 1 @@ -489,7 +488,10 @@ init_compass() static void init_optflow() { - optflow.init(); + if( optflow.init() == false ) { + g.optflow_enabled = false; + //SendDebug("\nFailed to Init OptFlow "); + } optflow.set_orientation(OPTFLOW_ORIENTATION); // set optical flow sensor's orientation on aircraft optflow.set_field_of_view(OPTFLOW_FOV); // set optical flow sensor's field of view } diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index 895b41d55b..ab6a17085f 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -871,6 +871,7 @@ test_sonar(uint8_t argc, const Menu::arg *argv) static int8_t test_optflow(uint8_t argc, const Menu::arg *argv) { + ///* if(g.optflow_enabled) { Serial.printf_P(PSTR("man id: %d\t"),optflow.read_register(ADNS3080_PRODUCT_ID)); print_hit_enter(); diff --git a/libraries/APM_BMP085/APM_BMP085.h b/libraries/APM_BMP085/APM_BMP085.h index 1263f3aa5d..e8a5085d0f 100644 --- a/libraries/APM_BMP085/APM_BMP085.h +++ b/libraries/APM_BMP085/APM_BMP085.h @@ -2,7 +2,7 @@ #define APM_BMP085_h #define TEMP_FILTER_SIZE 16 -#define PRESS_FILTER_SIZE 8 +#define PRESS_FILTER_SIZE 10 class APM_BMP085_Class { diff --git a/libraries/AP_RC_Channel/AP_RC_Channel.cpp b/libraries/AP_RC_Channel/AP_RC_Channel.cpp index b23879e0fd..9c8c8a6ca4 100644 --- a/libraries/AP_RC_Channel/AP_RC_Channel.cpp +++ b/libraries/AP_RC_Channel/AP_RC_Channel.cpp @@ -79,15 +79,15 @@ AP_RC_Channel::set_pwm(int pwm) //Serial.print("range "); control_in = pwm_to_range(); control_in = (control_in < dead_zone) ? 0 : control_in; - if (fabs(scale_output) > 0){ - control_in *= scale_output; - } }else{ control_in = pwm_to_angle(); control_in = (abs(control_in) < dead_zone) ? 0 : control_in; - if (fabs(scale_output) > 0){ - control_in *= scale_output; + + if(expo) { + long temp = control_in; + temp = (temp * temp) / (long)_high; + control_in = (int)((control_in >= 0) ? temp : -temp); } } } diff --git a/libraries/AP_RC_Channel/AP_RC_Channel.h b/libraries/AP_RC_Channel/AP_RC_Channel.h index d39806d904..db5dc68cd3 100644 --- a/libraries/AP_RC_Channel/AP_RC_Channel.h +++ b/libraries/AP_RC_Channel/AP_RC_Channel.h @@ -23,17 +23,13 @@ class AP_RC_Channel{ _high(1), _filter(true), _reverse(1), - dead_zone(0), - scale_output(1.0) - {} + dead_zone(0){} AP_RC_Channel() : _high(1), _filter(true), _reverse(1), - dead_zone(0), - scale_output(1.0) - {} + dead_zone(0){} // setup min and max radio values in CLI void update_min_max(); @@ -94,8 +90,6 @@ class AP_RC_Channel{ int16_t pwm_to_range(); int16_t range_to_pwm(); - float scale_output; - private: bool _filter; int8_t _reverse;