/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- static void get_stabilize_roll(int32_t target_angle) { // angle error target_angle = wrap_180(target_angle - ahrs.roll_sensor); // limit the error we're feeding to the PID target_angle = constrain(target_angle, -4500, 4500); // convert to desired Rate: int32_t target_rate = g.pi_stabilize_roll.get_p(target_angle); int16_t i_stab; if(labs(ahrs.roll_sensor) < 500) { target_angle = constrain(target_angle, -500, 500); i_stab = g.pi_stabilize_roll.get_i(target_angle, G_Dt); }else{ i_stab = g.pi_stabilize_roll.get_integrator(); } // set targets for rate controller set_roll_rate_target(target_rate+i_stab, EARTH_FRAME); } static void get_stabilize_pitch(int32_t target_angle) { // angle error target_angle = wrap_180(target_angle - ahrs.pitch_sensor); // limit the error we're feeding to the PID target_angle = constrain(target_angle, -4500, 4500); // convert to desired Rate: int32_t target_rate = g.pi_stabilize_pitch.get_p(target_angle); int16_t i_stab; if(labs(ahrs.pitch_sensor) < 500) { target_angle = constrain(target_angle, -500, 500); i_stab = g.pi_stabilize_pitch.get_i(target_angle, G_Dt); }else{ i_stab = g.pi_stabilize_pitch.get_integrator(); } // set targets for rate controller set_pitch_rate_target(target_rate + i_stab, EARTH_FRAME); } static void get_stabilize_yaw(int32_t target_angle) { int32_t target_rate,i_term; int32_t angle_error; int32_t output = 0; // angle error angle_error = wrap_180(target_angle - ahrs.yaw_sensor); // limit the error we're feeding to the PID angle_error = constrain(angle_error, -4500, 4500); // convert angle error to desired Rate: target_rate = g.pi_stabilize_yaw.get_p(angle_error); i_term = g.pi_stabilize_yaw.get_i(angle_error, G_Dt); // do not use rate controllers for helicotpers with external gyros #if FRAME_CONFIG == HELI_FRAME if(motors.ext_gyro_enabled) { g.rc_4.servo_out = constrain((target_rate + i_term), -4500, 4500); } #endif #if LOGGING_ENABLED == ENABLED static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash // log output if PID logging is on and we are tuning the yaw if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_YAW_KP || g.radio_tuning == CH6_YAW_RATE_KP) ) { log_counter++; if( log_counter >= 10 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10 log_counter = 0; Log_Write_PID(CH6_YAW_KP, angle_error, target_rate, i_term, 0, output, tuning_value); } } #endif // set targets for rate controller set_yaw_rate_target(target_rate+i_term, EARTH_FRAME); } static void get_acro_roll(int32_t target_rate) { target_rate = target_rate * g.acro_p; // set targets for rate controller set_roll_rate_target(target_rate, BODY_FRAME); } static void get_acro_pitch(int32_t target_rate) { target_rate = target_rate * g.acro_p; // set targets for rate controller set_pitch_rate_target(target_rate, BODY_FRAME); } static void get_acro_yaw(int32_t target_rate) { target_rate = target_rate * g.acro_p; // set targets for rate controller set_yaw_rate_target(target_rate, BODY_FRAME); } // Roll with rate input and stabilized in the earth frame static void get_roll_rate_stabilized_ef(int32_t stick_angle) { int32_t angle_error = 0; // convert the input to the desired roll rate int32_t target_rate = stick_angle * g.acro_p - (roll_axis * g.acro_balance_roll)/100; // convert the input to the desired roll rate roll_axis += target_rate * G_Dt; roll_axis = wrap_180(roll_axis); // ensure that we don't reach gimbal lock if (roll_axis > 4500 || roll_axis < -4500) { roll_axis = constrain(roll_axis, -4500, 4500); angle_error = wrap_180(roll_axis - ahrs.roll_sensor); } else { // angle error with maximum of +- max_angle_overshoot angle_error = wrap_180(roll_axis - ahrs.roll_sensor); angle_error = constrain(angle_error, -MAX_ROLL_OVERSHOOT, MAX_ROLL_OVERSHOOT); } if (motors.armed() == false || ((g.rc_3.control_in == 0) && !ap.failsafe)) { angle_error = 0; } // update roll_axis to be within max_angle_overshoot of our current heading roll_axis = wrap_180(angle_error + ahrs.roll_sensor); // set earth frame targets for rate controller // set earth frame targets for rate controller set_roll_rate_target(g.pi_stabilize_roll.get_p(angle_error) + target_rate, EARTH_FRAME); } // Pitch with rate input and stabilized in the earth frame static void get_pitch_rate_stabilized_ef(int32_t stick_angle) { int32_t angle_error = 0; // convert the input to the desired pitch rate int32_t target_rate = stick_angle * g.acro_p - (pitch_axis * g.acro_balance_pitch)/100; // convert the input to the desired pitch rate pitch_axis += target_rate * G_Dt; pitch_axis = wrap_180(pitch_axis); // ensure that we don't reach gimbal lock if (pitch_axis > 4500 || pitch_axis < -4500) { pitch_axis = constrain(pitch_axis, -4500, 4500); angle_error = wrap_180(pitch_axis - ahrs.pitch_sensor); } else { // angle error with maximum of +- max_angle_overshoot angle_error = wrap_180(pitch_axis - ahrs.pitch_sensor); angle_error = constrain(angle_error, -MAX_PITCH_OVERSHOOT, MAX_PITCH_OVERSHOOT); } if (motors.armed() == false || ((g.rc_3.control_in == 0) && !ap.failsafe)) { angle_error = 0; } // update pitch_axis to be within max_angle_overshoot of our current heading pitch_axis = wrap_180(angle_error + ahrs.pitch_sensor); // set earth frame targets for rate controller set_pitch_rate_target(g.pi_stabilize_pitch.get_p(angle_error) + target_rate, EARTH_FRAME); } // Yaw with rate input and stabilized in the earth frame static void get_yaw_rate_stabilized_ef(int32_t stick_angle) { int32_t angle_error = 0; // convert the input to the desired yaw rate int32_t target_rate = stick_angle * g.acro_p; // convert the input to the desired yaw rate nav_yaw += target_rate * G_Dt; nav_yaw = wrap_360(nav_yaw); // calculate difference between desired heading and current heading angle_error = wrap_180(nav_yaw - ahrs.yaw_sensor); // limit the maximum overshoot angle_error = constrain(angle_error, -MAX_YAW_OVERSHOOT, MAX_YAW_OVERSHOOT); if (motors.armed() == false || ((g.rc_3.control_in == 0) && !ap.failsafe)) { angle_error = 0; } // update nav_yaw to be within max_angle_overshoot of our current heading nav_yaw = wrap_360(angle_error + ahrs.yaw_sensor); // set earth frame targets for rate controller set_yaw_rate_target(g.pi_stabilize_yaw.get_p(angle_error)+target_rate, EARTH_FRAME); } // set_roll_rate_target - to be called by upper controllers to set roll rate targets in the earth frame void set_roll_rate_target( int32_t desired_rate, uint8_t earth_or_body_frame ) { rate_targets_frame = earth_or_body_frame; if( earth_or_body_frame == BODY_FRAME ) { roll_rate_target_bf = desired_rate; }else{ roll_rate_target_ef = desired_rate; } } // set_pitch_rate_target - to be called by upper controllers to set pitch rate targets in the earth frame void set_pitch_rate_target( int32_t desired_rate, uint8_t earth_or_body_frame ) { rate_targets_frame = earth_or_body_frame; if( earth_or_body_frame == BODY_FRAME ) { pitch_rate_target_bf = desired_rate; }else{ pitch_rate_target_ef = desired_rate; } } // set_yaw_rate_target - to be called by upper controllers to set yaw rate targets in the earth frame void set_yaw_rate_target( int32_t desired_rate, uint8_t earth_or_body_frame ) { rate_targets_frame = earth_or_body_frame; if( earth_or_body_frame == BODY_FRAME ) { yaw_rate_target_bf = desired_rate; }else{ yaw_rate_target_ef = desired_rate; } } // update_rate_contoller_targets - converts earth frame rates to body frame rates for rate controllers void update_rate_contoller_targets() { if( rate_targets_frame == EARTH_FRAME ) { // convert earth frame rates to body frame rates roll_rate_target_bf = roll_rate_target_ef - sin_pitch * yaw_rate_target_ef; pitch_rate_target_bf = cos_roll_x * pitch_rate_target_ef + sin_roll * cos_pitch_x * yaw_rate_target_ef; yaw_rate_target_bf = cos_pitch_x * cos_roll_x * yaw_rate_target_ef - sin_roll * pitch_rate_target_ef; } } #if FRAME_CONFIG == HELI_FRAME // init_rate_controllers - set-up filters for rate controller inputs void init_rate_controllers() { // initalise low pass filters on rate controller inputs // 1st parameter is time_step, 2nd parameter is time_constant rate_roll_filter.set_cutoff_frequency(0.01, 2.0); rate_pitch_filter.set_cutoff_frequency(0.01, 2.0); // rate_yaw_filter.set_cutoff_frequency(0.01, 2.0); // other option for initialisation is rate_roll_filter.set_cutoff_frequency(,); } #endif // HELI_FRAME // run roll, pitch and yaw rate controllers and send output to motors // targets for these controllers comes from stabilize controllers void run_rate_controllers() { #if FRAME_CONFIG == HELI_FRAME // helicopters only use rate controllers for yaw and only when not using an external gyro if(!motors.ext_gyro_enabled) { g.rc_1.servo_out = get_heli_rate_roll(roll_rate_target_bf); g.rc_2.servo_out = get_heli_rate_pitch(pitch_rate_target_bf); g.rc_4.servo_out = get_heli_rate_yaw(yaw_rate_target_bf); } #else // call rate controllers g.rc_1.servo_out = get_rate_roll(roll_rate_target_bf); g.rc_2.servo_out = get_rate_pitch(pitch_rate_target_bf); g.rc_4.servo_out = get_rate_yaw(yaw_rate_target_bf); #endif // run throttle controller if an accel based target has been provided if( throttle_accel_controller_active ) { g.rc_3.servo_out = get_throttle_accel(throttle_accel_target_ef); } } #if FRAME_CONFIG == HELI_FRAME static int16_t get_heli_rate_roll(int32_t target_rate) { int32_t p,i,d,ff; // used to capture pid values for logging int32_t current_rate; // this iteration's rate int32_t rate_error; // simply target_rate - current_rate int32_t output; // output from pid controller // get current rate current_rate = (omega.x * DEGX100); // filter input current_rate = rate_roll_filter.apply(current_rate); // call pid controller rate_error = target_rate - current_rate; p = g.pid_rate_roll.get_p(rate_error); if (motors.flybar_mode == 1) { // Mechanical Flybars get regular integral for rate auto trim if (target_rate > -50 && target_rate < 50){ // Frozen at high rates i = g.pid_rate_roll.get_i(rate_error, G_Dt); } else { i = g.pid_rate_roll.get_integrator(); } } else { i = g.pid_rate_roll.get_leaky_i(rate_error, G_Dt, RATE_INTEGRATOR_LEAK_RATE); // Flybarless Helis get huge I-terms. I-term controls much of the rate } d = g.pid_rate_roll.get_d(rate_error, G_Dt); ff = g.heli_roll_ff * target_rate; output = p + i + d + ff; // constrain output output = constrain(output, -4500, 4500); #if LOGGING_ENABLED == ENABLED static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash // log output if PID logging is on and we are tuning the rate P, I or D gains if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_RATE_KP || g.radio_tuning == CH6_RATE_KI || g.radio_tuning == CH6_RATE_KD) ) { log_counter++; if( log_counter >= 10 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10 log_counter = 0; Log_Write_PID(CH6_RATE_KP, rate_error, p, i, d, output, tuning_value); } } #endif // output control return output; } static int16_t get_heli_rate_pitch(int32_t target_rate) { int32_t p,i,d,ff; // used to capture pid values for logging int32_t current_rate; // this iteration's rate int32_t rate_error; // simply target_rate - current_rate int32_t output; // output from pid controller // get current rate current_rate = (omega.y * DEGX100); // filter input current_rate = rate_pitch_filter.apply(current_rate); // call pid controller rate_error = target_rate - current_rate; p = g.pid_rate_pitch.get_p(rate_error); // Helicopters get huge feed-forward if (motors.flybar_mode == 1) { // Mechanical Flybars get regular integral for rate auto trim if (target_rate > -50 && target_rate < 50){ // Frozen at high rates i = g.pid_rate_pitch.get_i(rate_error, G_Dt); } else { i = g.pid_rate_pitch.get_integrator(); } } else { i = g.pid_rate_pitch.get_leaky_i(rate_error, G_Dt, RATE_INTEGRATOR_LEAK_RATE); // Flybarless Helis get huge I-terms. I-term controls much of the rate } d = g.pid_rate_pitch.get_d(rate_error, G_Dt); ff = g.heli_pitch_ff*target_rate; output = p + i + d + ff; // constrain output output = constrain(output, -4500, 4500); #if LOGGING_ENABLED == ENABLED static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash // log output if PID logging is on and we are tuning the rate P, I or D gains if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_RATE_KP || g.radio_tuning == CH6_RATE_KI || g.radio_tuning == CH6_RATE_KD) ) { log_counter++; if( log_counter >= 10 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10 log_counter = 0; Log_Write_PID(CH6_RATE_KP+100, rate_error, p, i, 0, output, tuning_value); } } #endif // output control return output; } static int16_t get_heli_rate_yaw(int32_t target_rate) { int32_t p,i,d,ff; // used to capture pid values for logging int32_t current_rate; // this iteration's rate int32_t rate_error; int32_t output; // get current rate current_rate = (omega.z * DEGX100); // filter input // current_rate = rate_yaw_filter.apply(current_rate); // rate control rate_error = target_rate - current_rate; // separately calculate p, i, d values for logging p = g.pid_rate_yaw.get_p(rate_error); i = g.pid_rate_yaw.get_i(rate_error, G_Dt); d = g.pid_rate_yaw.get_d(rate_error, G_Dt); ff = g.heli_yaw_ff*target_rate; output = p + i + d + ff; output = constrain(output, -4500, 4500); #if LOGGING_ENABLED == ENABLED static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash // log output if PID loggins is on and we are tuning the yaw if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_YAW_KP || g.radio_tuning == CH6_YAW_RATE_KP) ) { log_counter++; if( log_counter >= 10 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10 log_counter = 0; Log_Write_PID(CH6_YAW_RATE_KP, rate_error, p, i, d, output, tuning_value); } } #endif // output control return output; } #endif // HELI_FRAME #if FRAME_CONFIG != HELI_FRAME static int16_t get_rate_roll(int32_t target_rate) { int32_t p,i,d; // used to capture pid values for logging int32_t current_rate; // this iteration's rate int32_t rate_error; // simply target_rate - current_rate int32_t output; // output from pid controller // get current rate current_rate = (omega.x * DEGX100); // call pid controller rate_error = target_rate - current_rate; p = g.pid_rate_roll.get_p(rate_error); // freeze I term if we've breached roll-pitch limits if( motors.reached_limit(AP_MOTOR_ROLLPITCH_LIMIT) ) { i = g.pid_rate_roll.get_integrator(); }else{ i = g.pid_rate_roll.get_i(rate_error, G_Dt); } d = g.pid_rate_roll.get_d(rate_error, G_Dt); output = p + i + d; // constrain output output = constrain(output, -5000, 5000); #if LOGGING_ENABLED == ENABLED static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash // log output if PID logging is on and we are tuning the rate P, I or D gains if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_RATE_KP || g.radio_tuning == CH6_RATE_KI || g.radio_tuning == CH6_RATE_KD) ) { log_counter++; if( log_counter >= 10 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10 log_counter = 0; Log_Write_PID(CH6_RATE_KP, rate_error, p, i, d /*-rate_d_dampener*/, output, tuning_value); } } #endif // output control return output; } static int16_t get_rate_pitch(int32_t target_rate) { int32_t p,i,d; // used to capture pid values for logging int32_t current_rate; // this iteration's rate int32_t rate_error; // simply target_rate - current_rate int32_t output; // output from pid controller // get current rate current_rate = (omega.y * DEGX100); // call pid controller rate_error = target_rate - current_rate; p = g.pid_rate_pitch.get_p(rate_error); // freeze I term if we've breached roll-pitch limits if( motors.reached_limit(AP_MOTOR_ROLLPITCH_LIMIT) ) { i = g.pid_rate_pitch.get_integrator(); }else{ i = g.pid_rate_pitch.get_i(rate_error, G_Dt); } d = g.pid_rate_pitch.get_d(rate_error, G_Dt); output = p + i + d; // constrain output output = constrain(output, -5000, 5000); #if LOGGING_ENABLED == ENABLED static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash // log output if PID logging is on and we are tuning the rate P, I or D gains if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_RATE_KP || g.radio_tuning == CH6_RATE_KI || g.radio_tuning == CH6_RATE_KD) ) { log_counter++; if( log_counter >= 10 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10 log_counter = 0; Log_Write_PID(CH6_RATE_KP+100, rate_error, p, i, d/*-rate_d_dampener*/, output, tuning_value); } } #endif // output control return output; } static int16_t get_rate_yaw(int32_t target_rate) { int32_t p,i,d; // used to capture pid values for logging int32_t rate_error; int32_t output; // rate control rate_error = target_rate - (omega.z * DEGX100); // separately calculate p, i, d values for logging p = g.pid_rate_yaw.get_p(rate_error); // freeze I term if we've breached yaw limits if( motors.reached_limit(AP_MOTOR_YAW_LIMIT) ) { i = g.pid_rate_yaw.get_integrator(); }else{ i = g.pid_rate_yaw.get_i(rate_error, G_Dt); } d = g.pid_rate_yaw.get_d(rate_error, G_Dt); output = p+i+d; output = constrain(output, -4500, 4500); #if LOGGING_ENABLED == ENABLED static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash // log output if PID loggins is on and we are tuning the yaw if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_YAW_KP || g.radio_tuning == CH6_YAW_RATE_KP) ) { log_counter++; if( log_counter >= 10 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10 log_counter = 0; Log_Write_PID(CH6_YAW_RATE_KP, rate_error, p, i, d, output, tuning_value); } } #endif #if FRAME_CONFIG == TRI_FRAME // constrain output return output; #else // !TRI_FRAME // output control: int16_t yaw_limit = 2200 + abs(g.rc_4.control_in); // smoother Yaw control: return constrain(output, -yaw_limit, yaw_limit); #endif // TRI_FRAME } #endif // !HELI_FRAME // calculate modified roll/pitch depending upon optical flow calculated position static int32_t get_of_roll(int32_t input_roll) { #ifdef 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.0); // we could use the last update time to calculate the time change d = g.pid_optflow_roll.get_d(-tot_x_cm,1.0); 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(new_roll, (of_roll-20), (of_roll+20)); #if LOGGING_ENABLED == ENABLED static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash // log output if PID logging is on and we are tuning the rate P, I or D gains if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_OPTFLOW_KP || g.radio_tuning == CH6_OPTFLOW_KI || g.radio_tuning == CH6_OPTFLOW_KD) ) { log_counter++; if( log_counter >= 5 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10 log_counter = 0; Log_Write_PID(CH6_OPTFLOW_KP, tot_x_cm, p, i, d, of_roll, tuning_value); } } #endif // LOGGING_ENABLED == ENABLED } // limit max angle of_roll = constrain(of_roll, -1000, 1000); return input_roll+of_roll; #else return input_roll; #endif } static int32_t get_of_pitch(int32_t input_pitch) { #ifdef 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.0); // we could use the last update time to calculate the time change d = g.pid_optflow_pitch.get_d(tot_y_cm, 1.0); 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(new_pitch, (of_pitch-20), (of_pitch+20)); #if LOGGING_ENABLED == ENABLED static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash // log output if PID logging is on and we are tuning the rate P, I or D gains if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_OPTFLOW_KP || g.radio_tuning == CH6_OPTFLOW_KI || g.radio_tuning == CH6_OPTFLOW_KD) ) { log_counter++; if( log_counter >= 5 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10 log_counter = 0; Log_Write_PID(CH6_OPTFLOW_KP+100, tot_y_cm, p, i, d, of_pitch, tuning_value); } } #endif // LOGGING_ENABLED == ENABLED } // limit max angle of_pitch = constrain(of_pitch, -1000, 1000); return input_pitch+of_pitch; #else return input_pitch; #endif } /************************************************************* * throttle control ****************************************************************/ // update_throttle_cruise - update throttle cruise if necessary static void update_throttle_cruise(int16_t throttle) { // ensure throttle_avg has been initialised if( throttle_avg == 0 ) { throttle_avg = g.throttle_cruise; } // calc average throttle if ((throttle > g.throttle_min) && (abs(climb_rate) < 60) && (g_gps->ground_speed < 200)) { throttle_avg = throttle_avg * .99 + (float)throttle * .01; g.throttle_cruise = throttle_avg; } } static int16_t get_angle_boost(int16_t value) { float temp = cos_pitch_x * cos_roll_x; temp = constrain(temp, .75, 1.0); return ((float)(value + 80) / temp) - (value + 80); } #if FRAME_CONFIG == HELI_FRAME // heli_angle_boost - adds a boost depending on roll/pitch values // equivalent of quad's angle_boost function // throttle value should be 0 ~ 1000 static int16_t heli_get_angle_boost(int16_t throttle) { float angle_boost_factor = cos_pitch_x * cos_roll_x; angle_boost_factor = 1.0 - constrain(angle_boost_factor, .5, 1.0); int16_t throttle_above_mid = max(throttle - motors.throttle_mid,0); return throttle + throttle_above_mid*angle_boost_factor; } #endif // HELI_FRAME // set_throttle_out - to be called by upper throttle controllers when they wish to provide throttle output directly to motors (instead of using accel based throttle controller) // provide 0 to cut motors void set_throttle_out( int16_t throttle_out ) { g.rc_3.servo_out = throttle_out; throttle_accel_controller_active = false; } // set_throttle_accel_target - to be called by upper throttle controllers to set desired vertical acceleration in earth frame void set_throttle_accel_target( int16_t desired_acceleration ) { throttle_accel_target_ef = desired_acceleration; throttle_accel_controller_active = true; } // get_throttle_accel - accelerometer based throttle controller // returns an actual throttle output (0 ~ 1000) to be sent to the motors static int16_t get_throttle_accel(int16_t z_target_accel) { static float accel_one_g = -980; // filtered estimate of 1G in cm/s/s int32_t p,i,d; // used to capture pid values for logging int16_t z_accel_error, output; float z_accel_meas_temp; Vector3f accel = ins.get_accel(); Matrix3f dcm_matrix = ahrs.get_dcm_matrix(); // Calculate Earth Frame Z acceleration z_accel_meas_temp = (dcm_matrix.c.x * accel.x + dcm_matrix.c.y * accel.y + dcm_matrix.c.z * accel.z)* 100.0; // Filter Earth Frame Z acceleration with fc = 0.01 Hz to find 1 g accel_one_g = accel_one_g + 0.00062792 * (z_accel_meas_temp - accel_one_g); z_accel_meas_temp = z_accel_meas_temp - accel_one_g; // Filter Earth Frame Z acceleration with fc = 1 Hz z_accel_meas = z_accel_meas + 0.059117 * (z_accel_meas_temp - z_accel_meas); // calculate accel error z_accel_error = constrain(z_target_accel + z_accel_meas, -32000, 32000); // separately calculate p, i, d values for logging p = g.pid_throttle_accel.get_p(z_accel_error); // freeze I term if we've breached throttle limits if( motors.reached_limit(AP_MOTOR_THROTTLE_LIMIT) ) { i = g.pid_throttle_accel.get_integrator(); }else{ i = g.pid_throttle_accel.get_i(z_accel_error, .02); } d = g.pid_throttle_accel.get_d(z_accel_error, .02); // // limit the rate output = constrain(p+i+d+g.throttle_cruise, g.throttle_min, g.throttle_max); //Serial.printf("ThAccel 1 z_target_accel:%4.2f z_accel_meas:%4.2f z_accel_error:%4.2f output:%4.2f p:%4.2f i:%4.2f d:%4.2f \n", 1.0*z_target_accel, 1.0*z_accel_meas, 1.0*z_accel_error, 1.0*output, 1.0*p, 1.0*i, 1.0*d); //Serial.printf("motors.reached_limit:%4.2f reset_accel_throttle_counter:%4.2f output:%4.2f p:%4.2f i:%4.2f d:%4.2f \n", 1.0*motors.reached_limit(0xff), 1.0*reset_accel_throttle_counter, 1.0*output, 1.0*p, 1.0*i, 1.0*d); //Serial.printf("One G: z_target_accel:%4.2f z_accel_meas:%4.2f accel_one_g:%4.2f \n", 1.0*z_target_accel, 1.0*z_accel_meas, 1.0*accel_one_g); #if LOGGING_ENABLED == ENABLED static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash // log output if PID loggins is on and we are tuning the yaw if( g.log_bitmask & MASK_LOG_PID && g.radio_tuning == CH6_THROTTLE_KP ) { log_counter++; if( log_counter >= 10 ) { // (update rate / desired output rate) = (50hz / 10hz) = 5hz log_counter = 0; Log_Write_PID(CH6_THROTTLE_KP, z_accel_error, p, i, d, output, tuning_value); } } #endif return output; } // get_pilot_desired_climb_rate - transform pilot's throttle input to // climb rate in cm/s. we use radio_in instead of control_in to get the full range // without any deadzone at the bottom #define THROTTLE_IN_MIDDLE 500 // the throttle mid point #define THROTTLE_IN_DEADBAND 100 // the throttle input channel's deadband in PWM #define THROTTLE_IN_DEADBAND_TOP (THROTTLE_IN_MIDDLE+THROTTLE_IN_DEADBAND) // top of the deadband #define THROTTLE_IN_DEADBAND_BOTTOM (THROTTLE_IN_MIDDLE-THROTTLE_IN_DEADBAND) // bottom of the deadband static int16_t get_pilot_desired_climb_rate(int16_t throttle_control) { int16_t desired_rate = 0; // throttle failsafe check if( ap.failsafe ) { return 0; } // ensure a reasonable throttle value throttle_control = constrain(throttle_control,0,1000); // check throttle is above, below or in the deadband if (throttle_control < THROTTLE_IN_DEADBAND_BOTTOM) { // below the deadband desired_rate = (int32_t)THROTTLE_IN_MIDDLE * (throttle_control-THROTTLE_IN_DEADBAND_BOTTOM) / (THROTTLE_IN_MIDDLE - THROTTLE_IN_DEADBAND); }else if (throttle_control > THROTTLE_IN_DEADBAND_TOP) { // above the deadband desired_rate = (int32_t)THROTTLE_IN_MIDDLE * (throttle_control-THROTTLE_IN_DEADBAND_TOP) / (THROTTLE_IN_MIDDLE - THROTTLE_IN_DEADBAND); }else{ // must be in the deadband desired_rate = 0; } return desired_rate; } // get_pilot_desired_acceleration - transform pilot's throttle input to a desired acceleration // default upper and lower bounds are 500 cm/s/s (roughly 1/2 a G) // returns acceleration in cm/s/s static int16_t get_pilot_desired_acceleration(int16_t throttle_control) { int32_t desired_accel = 0; // throttle failsafe check if( ap.failsafe ) { return 0; } // ensure a reasonable throttle value throttle_control = constrain(throttle_control,0,1000); // check throttle is above, below or in the deadband if (throttle_control < THROTTLE_IN_DEADBAND_BOTTOM) { // below the deadband desired_accel = (int32_t)THROTTLE_IN_MIDDLE * (throttle_control-THROTTLE_IN_DEADBAND_BOTTOM) / (THROTTLE_IN_MIDDLE - THROTTLE_IN_DEADBAND); }else if(throttle_control > THROTTLE_IN_DEADBAND_TOP) { // above the deadband desired_accel = (int32_t)THROTTLE_IN_MIDDLE * (throttle_control-THROTTLE_IN_DEADBAND_TOP) / (THROTTLE_IN_MIDDLE - THROTTLE_IN_DEADBAND); }else{ // must be in the deadband desired_accel = 0; } return desired_accel; } // get_pilot_desired_direct_alt - transform pilot's throttle input to a desired altitude // return altitude in cm between 0 to 10m static int32_t get_pilot_desired_direct_alt(int16_t throttle_control) { int32_t desired_alt = 0; // throttle failsafe check if( ap.failsafe ) { return 0; } // ensure a reasonable throttle value throttle_control = constrain(throttle_control,0,1000); desired_alt = throttle_control; return desired_alt; } // get_throttle_rate - calculates desired accel required to achieve desired z_target_speed // sets accel based throttle controller target static void get_throttle_rate(int16_t z_target_speed) { int32_t p,i,d; // used to capture pid values for logging int16_t z_rate_error; int16_t target_accel; // calculate rate error #if INERTIAL_NAV == ENABLED z_rate_error = z_target_speed - inertial_nav._velocity.z; // calc the speed error #else z_rate_error = z_target_speed - climb_rate; // calc the speed error #endif // separately calculate p, i, d values for logging p = g.pid_throttle.get_p(z_rate_error); // freeze I term if we've breached throttle limits if(motors.reached_limit(AP_MOTOR_THROTTLE_LIMIT)) { i = g.pid_throttle.get_integrator(); }else{ i = g.pid_throttle.get_i(z_rate_error, .02); } d = g.pid_throttle.get_d(z_rate_error, .02); // consolidate target acceleration target_accel = p+i+d; #if LOGGING_ENABLED == ENABLED static uint8_t log_counter = 0; // used to slow down logging of PID values to dataflash // log output if PID loggins is on and we are tuning the yaw if( g.log_bitmask & MASK_LOG_PID && g.radio_tuning == CH6_THROTTLE_KP ) { log_counter++; if( log_counter >= 10 ) { // (update rate / desired output rate) = (50hz / 10hz) = 5hz log_counter = 0; Log_Write_PID(CH6_THROTTLE_KP, z_rate_error, p, i, d, target_accel, tuning_value); } } #endif // set target for accel based throttle controller set_throttle_accel_target(target_accel); } // get_throttle_rate_stabilized - rate controller with additional 'stabilizer' // 'stabilizer' ensure desired rate is being met // calls normal throttle rate controller which updates accel based throttle controller targets static void get_throttle_rate_stabilized(int16_t target_rate) { static float alt_desired = 0; // The desired altitude in cm. static float alt_rate = 0; // the desired climb rate in cm/s. static uint32_t last_call_ms = 0; float altitude_error; int16_t desired_rate; int16_t alt_error_max; uint32_t now = millis(); // reset target altitude if this controller has just been engaged if( now - last_call_ms > 1000 ) { alt_desired = current_loc.alt; } last_call_ms = millis(); // Limit acceleration of the desired altitude to +-5 m/s^2 alt_rate += constrain(target_rate-alt_rate, -10, 10); alt_desired += alt_rate * 0.02; alt_error_max = constrain(600-abs(target_rate),100,600); altitude_error = constrain(alt_desired - current_loc.alt, -alt_error_max, alt_error_max); alt_desired = current_loc.alt + altitude_error; if(altitude_error > 0){ desired_rate = g.pi_alt_hold.get_p(10*sqrt(altitude_error)); }else if(altitude_error < 0){ desired_rate = g.pi_alt_hold.get_p(-10*sqrt(abs(altitude_error))); }else{ desired_rate = g.pi_alt_hold.get_p(0); } desired_rate = constrain(desired_rate, -200, 200) + target_rate; // call rate based throttle controller which will update accel based throttle controller targets get_throttle_rate(desired_rate); } // get_throttle_althold - hold at the desired altitude in cm // updates accel based throttle controller targets // Note: max_climb_rate is an optional parameter to allow reuse of this function by landing controller static void get_throttle_althold(int32_t target_alt, int16_t max_climb_rate) { int32_t altitude_error; int16_t desired_rate; altitude_error = target_alt - current_loc.alt; if(altitude_error > 0){ desired_rate = g.pi_alt_hold.get_p(10*sqrt(altitude_error)); }else if(altitude_error < 0){ desired_rate = g.pi_alt_hold.get_p(-10*sqrt(abs(altitude_error))); }else{ desired_rate = g.pi_alt_hold.get_p(0); } desired_rate = constrain(desired_rate, ALTHOLD_MIN_CLIMB_RATE, max_climb_rate); // call rate based throttle controller which will update accel based throttle controller targets get_throttle_rate(desired_rate); // TO-DO: enabled PID logging for this controller //Log_Write_PID(1, (int32_t)target_alt, (int32_t)current_loc.alt, (int32_t)climb_rate, (int32_t)altitude_error, (int32_t)desired_rate, (float)desired_accel); //Log_Write_PID(1, target_alt, current_loc.alt, climb_rate, altitude_error, desired_rate, desired_accel); } // get_throttle_land - high level landing logic // sends the desired acceleration in the accel based throttle controller // called at 50hz #define LAND_START_ALT 1000 // altitude in cm where land controller switches to slow rate of descent #define LAND_DESCENT_SPEED -50 // minimum descent speed in cm/s static void get_throttle_land() { // if we are above 10m perform regular alt hold descent if (current_loc.alt >= LAND_START_ALT) { get_throttle_althold(LAND_START_ALT, LAND_DESCENT_SPEED); }else{ get_throttle_rate_stabilized(LAND_DESCENT_SPEED); } // TO-DO: add detection of whether we've landed or not and set ap.land_complete } /* * reset all I integrators */ static void reset_I_all(void) { reset_rate_I(); reset_stability_I(); reset_wind_I(); reset_throttle_I(); reset_optflow_I(); // This is the only place we reset Yaw g.pi_stabilize_yaw.reset_I(); } static void reset_rate_I() { g.pid_rate_roll.reset_I(); g.pid_rate_pitch.reset_I(); g.pid_rate_yaw.reset_I(); } static void reset_optflow_I(void) { g.pid_optflow_roll.reset_I(); g.pid_optflow_pitch.reset_I(); of_roll = 0; of_pitch = 0; } static void reset_wind_I(void) { // Wind Compensation // this i is not currently being used, but we reset it anyway // because someone may modify it and not realize it, causing a bug g.pi_loiter_lat.reset_I(); g.pi_loiter_lon.reset_I(); g.pid_loiter_rate_lat.reset_I(); g.pid_loiter_rate_lon.reset_I(); g.pid_nav_lat.reset_I(); g.pid_nav_lon.reset_I(); } static void reset_throttle_I(void) { // For Altitude Hold g.pi_alt_hold.reset_I(); g.pid_throttle.reset_I(); } static void reset_stability_I(void) { // Used to balance a quad // This only needs to be reset during Auto-leveling in flight g.pi_stabilize_roll.reset_I(); g.pi_stabilize_pitch.reset_I(); }