diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 7b49ac2056..43bdbd76c3 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -472,91 +472,6 @@ void set_yaw_rate_target( int32_t desired_rate, uint8_t earth_or_body_frame ) { } } -// calculate modified roll/pitch depending upon optical flow calculated position -static int32_t -get_of_roll(int32_t input_roll) -{ -#if OPTFLOW == ENABLED - static float tot_x_cm = 0; // total distance from target - static uint32_t last_of_roll_update = 0; - int32_t new_roll = 0; - int32_t p,i,d; - - // check if new optflow data available - if( optflow.last_update != last_of_roll_update) { - last_of_roll_update = optflow.last_update; - - // add new distance moved - tot_x_cm += optflow.x_cm; - - // only stop roll if caller isn't modifying roll - if( input_roll == 0 && current_loc.alt < 1500) { - p = g.pid_optflow_roll.get_p(-tot_x_cm); - i = g.pid_optflow_roll.get_i(-tot_x_cm,1.0f); // we could use the last update time to calculate the time change - d = g.pid_optflow_roll.get_d(-tot_x_cm,1.0f); - new_roll = p+i+d; - }else{ - g.pid_optflow_roll.reset_I(); - tot_x_cm = 0; - p = 0; // for logging - i = 0; - d = 0; - } - // limit amount of change and maximum angle - of_roll = constrain_int32(new_roll, (of_roll-20), (of_roll+20)); - } - - // limit max angle - of_roll = constrain_int32(of_roll, -1000, 1000); - - return input_roll+of_roll; -#else - return input_roll; -#endif -} - -static int32_t -get_of_pitch(int32_t input_pitch) -{ -#if OPTFLOW == ENABLED - static float tot_y_cm = 0; // total distance from target - static uint32_t last_of_pitch_update = 0; - int32_t new_pitch = 0; - int32_t p,i,d; - - // check if new optflow data available - if( optflow.last_update != last_of_pitch_update ) { - last_of_pitch_update = optflow.last_update; - - // add new distance moved - tot_y_cm += optflow.y_cm; - - // only stop roll if caller isn't modifying pitch - if( input_pitch == 0 && current_loc.alt < 1500 ) { - p = g.pid_optflow_pitch.get_p(tot_y_cm); - i = g.pid_optflow_pitch.get_i(tot_y_cm, 1.0f); // we could use the last update time to calculate the time change - d = g.pid_optflow_pitch.get_d(tot_y_cm, 1.0f); - new_pitch = p + i + d; - }else{ - tot_y_cm = 0; - g.pid_optflow_pitch.reset_I(); - p = 0; // for logging - i = 0; - d = 0; - } - - // limit amount of change - of_pitch = constrain_int32(new_pitch, (of_pitch-20), (of_pitch+20)); - } - - // limit max angle - of_pitch = constrain_int32(of_pitch, -1000, 1000); - - return input_pitch+of_pitch; -#else - return input_pitch; -#endif -} /************************************************************* * yaw controllers