/// calc_leash_length - calculates the vertical leash lengths from maximum speed, acceleration /// called by update_z_controller if z-axis speed or accelerations are changed void AC_PosControl::calc_leash_length_z() { if (_flags.recalc_leash_z) { _leash_up_z = calc_leash_length(_speed_up_cms, _accel_z_cms, _p_pos_z.kP()); _leash_down_z = calc_leash_length(-_speed_down_cms, _accel_z_cms, _p_pos_z.kP()); _flags.recalc_leash_z = false; } }
/// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration /// should be called whenever the speed, acceleration or position kP is modified void AC_PosControl::calc_leash_length_xy() { if (_flags.recalc_leash_xy) { _leash = calc_leash_length(_speed_cms, _accel_cms, _p_pos_xy.kP()); _flags.recalc_leash_xy = false; } }
/// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration /// should be called whenever the speed, acceleration or position kP is modified void AC_PosControl::calc_leash_length_xy() { // todo: remove _flags.recalc_leash_xy or don't call this function after each variable change. if (_flags.recalc_leash_xy) { _leash = calc_leash_length(_speed_cms, _accel_cms, _p_pos_xy.kP()); _flags.recalc_leash_xy = false; } }
// update the controller // dt: time dt // user_rate: user desired climb rate, usually from stick. int altitude_controller::update(float dt, float user_rate) { // sonar switching if (m_sonar_ticker < 0.5f) { m_sonar_ticker += dt; if (m_sonar_ticker > 0.5f) { // sonar state changed more than 0.5 second. if (isnan(m_sonar)) { m_sonar_target = NAN; } else { m_sonar_target = m_sonar + target_altitude - m_states[0]; } LOGE("sonar changed: %f/%f,%f,%f\n", m_sonar_target, m_sonar, target_altitude, m_states[0]); } } if (!isnan(target_altitude)) { float leash_up = calc_leash_length(quadcopter_max_climb_rate, quadcopter_max_acceleration, pid_quad_altitude[0]); float leash_down = calc_leash_length(quadcopter_max_descend_rate, quadcopter_max_acceleration, pid_quad_altitude[0]); // only move altitude target if throttle and target climb rate didn't hit limits if ((!(m_motor_state & MOTOR_LIMIT_MAX) && user_rate > 0 && (target_climb_rate < quadcopter_max_climb_rate)) || (!(m_motor_state & MOTOR_LIMIT_MIN) && user_rate < 0 && (target_climb_rate > -quadcopter_max_descend_rate)) ) { target_altitude += user_rate * dt; } target_altitude = limit(target_altitude, m_states[0]-leash_down, m_states[0]+leash_up); // new target rate, directly use linear approach since we use very tight limit // TODO: use sqrt approach on large errors (see get_throttle_althold() in Attitude.pde) altitude_error_pid[0] = target_altitude - m_states[0]; altitude_error_pid[0] = limit(altitude_error_pid[0], -2.5f, 2.5f); target_climb_rate = pid_quad_altitude[0] * altitude_error_pid[0]; } else { target_climb_rate = 0; } // feed forward // feed_forward_factor += m_airborne ? -dt : dt; feed_forward_factor = limit(feed_forward_factor, 0.35f, 0.8f); target_climb_rate += user_rate * feed_forward_factor; TRACE("\rtarget_climb_rate=%.2f climb from alt =%.2f, user=%.2f, out=%2f. ", target_climb_rate, target_climb_rate-user_rate * feed_forward_factor, user_rate, throttle_result); // new climb rate error float climb_rate_error = target_climb_rate - m_states[1]; climb_rate_error = limit(climb_rate_error, -quadcopter_max_descend_rate, quadcopter_max_climb_rate); // apply a 2Hz LPF to rate error const float RC = 1.0f/(2*3.1415926 * 5.0f); float alpha = dt / (dt + RC); // 5Hz LPF filter const float RC5 = 1.0f/(2*3.1415926 * 2.0f); float alpha5 = dt / (dt + RC5); // 30Hz LPF for derivative factor const float RC30 = 1.0f/(2*3.1415926 * 30.0f); float alpha30 = dt / (dt + RC30); // TODO: add feed forward // reference: get_throttle_rate() climb_rate_error_pid[0] = isnan(climb_rate_error_pid[0]) ? (climb_rate_error) : (climb_rate_error_pid[0] * (1-alpha) + alpha * climb_rate_error); target_accel = climb_rate_error_pid[0] * pid_quad_alt_rate[0]; target_accel = limit(target_accel, m_airborne ? -quadcopter_max_acceleration : -2 * quadcopter_max_acceleration, quadcopter_max_acceleration); // new accel error, +2Hz LPF float accel_error = target_accel - m_states[2]; if (isnan(accel_error_pid[0])) { accel_error_pid[0] = accel_error; } else { accel_error = accel_error_pid[0] * (1-alpha) + alpha * accel_error; } // core pid // only integrate if throttle didn't hit limits or I term will reduce if (m_airborne) if ((!(m_motor_state & MOTOR_LIMIT_MAX) && accel_error_pid[0] > 0) || (!(m_motor_state & MOTOR_LIMIT_MIN) && accel_error_pid[0] < 0)) { accel_error_pid[1] += accel_error_pid[0] * dt; accel_error_pid[1] = limit(accel_error_pid[1], -pid_quad_accel[3], pid_quad_accel[3]); } float D = (accel_error - accel_error_pid[0])/dt; accel_error_pid[2] = isnan(accel_error_pid[2])? D : (accel_error_pid[2] * (1-alpha30) + alpha30 * D); accel_error_pid[0] = accel_error; float output = 0; output += accel_error_pid[0] * pid_quad_accel[0]; output += accel_error_pid[1] * pid_quad_accel[1]; output += accel_error_pid[2] * pid_quad_accel[2]; output *= throttle_hover / default_throttle_hover; // normalize throttle output PID, from throttle percentage to acceleration throttle_result = output + throttle_hover; float angle_boost_factor = limit(1/ cos(m_attitude[0]) / cos(m_attitude[1]), 1.0f, 1.5f); throttle_result = throttle_result * angle_boost_factor; if (throttle_result > 1 - QUADCOPTER_THROTTLE_RESERVE) { throttle_result = 1 - QUADCOPTER_THROTTLE_RESERVE; m_motor_state = MOTOR_LIMIT_MAX; } else if (throttle_result < (m_airborne ? QUADCOPTER_THROTTLE_RESERVE : 0)) { throttle_result = m_airborne ? QUADCOPTER_THROTTLE_RESERVE : 0; m_motor_state = MOTOR_LIMIT_MIN; } else { m_motor_state = 0; } LOGE("\rthrottle=%f, altitude = %.2f/%.2f, pid=%.2f,%.2f,%.2f, limit=%d", throttle_result, m_states[0], target_altitude, accel_error_pid[0], accel_error_pid[1], accel_error_pid[2], m_motor_state); // update throttle_real_crusing if we're in near level state and no violent climbing/descending action if (m_airborne && m_throttle_realized>0 && fabs(m_states[1]) < 0.5f && fabs(m_states[2])<0.5f && fabs(m_attitude[0])<5*PI/180 && fabs(m_attitude[1])<5*PI/180 && fabs(user_rate) < 0.001f) { // 0.2Hz low pass filter const float RC02 = 1.0f/(2*3.1415926 * 0.2f); float alpha02 = dt / (dt + RC02); throttle_hover = throttle_hover * (1-alpha02) + alpha02 * m_throttle_realized; // TODO: estimate throttle cursing correctly TRACE("\rthrottle_hover=%f", throttle_hover); } return 0; }
// update the controller // dt: time dt // user_rate: user desired climb rate, usually from stick. int altitude_controller::update(float dt, float user_rate) { if (systimer->gettime() - last_update > 1000000) reset(); if (throttle_hover <= 0.01f || isnan(throttle_hover)) throttle_hover = default_throttle_hover; // sonar switching if (isnan(m_sonar) == isnan(m_sonar_target)) // reset ticker if sonar state didn't changed m_sonar_ticker = 0; if (m_sonar_ticker < 0.3f) { m_sonar_ticker += dt; if (m_sonar_ticker > 0.3f) { // sonar state changed more than 0.5 second. if (isnan(m_sonar)) { baro_target = m_baro_states[0] + m_sonar_target - m_last_valid_sonar; m_sonar_target = NAN; LOGE("changed to baro: %f/%f,%f,%f\n", m_sonar_target, m_sonar, baro_target, m_baro_states[0]); } else { m_sonar_target = m_sonar + baro_target - m_baro_states[0]; LOGE("changed to sonar: %f/%f,%f,%f\n", m_sonar_target, m_sonar, baro_target, m_baro_states[0]); } } } float leash_up = calc_leash_length(quadcopter_max_climb_rate, quadcopter_max_acceleration, pid_quad_altitude[0]); float leash_down = calc_leash_length(quadcopter_max_descend_rate, quadcopter_max_acceleration, pid_quad_altitude[0]); float &alt_target = isnan(m_sonar_target) ? baro_target : m_sonar_target; float &alt_state = isnan(m_sonar_target) ? m_baro_states[0]: m_last_valid_sonar; // only move altitude target if throttle and target climb rate didn't hit limits, and no climb target override exists if (isnan(climb_rate_override) && ( (!(m_motor_state & LIMIT_POSITIVE_HARD) && user_rate > 0 && (target_climb_rate < quadcopter_max_climb_rate)) || (!(m_motor_state & LIMIT_NEGATIVE_HARD) && user_rate < 0 && (target_climb_rate > -quadcopter_max_descend_rate)) ) ) { alt_target += user_rate * dt; } alt_target = limit(alt_target, alt_state-leash_down, alt_state+leash_up); // new target rate, directly use linear approach since we use very tight limit // TODO: use sqrt approach on large errors (see get_throttle_althold() in Attitude.pde) altitude_error_pid[0] = alt_target - alt_state; altitude_error_pid[0] = limit(altitude_error_pid[0], -leash_down, leash_up); target_climb_rate = pid_quad_altitude[0] * altitude_error_pid[0]; if (!isnan(climb_rate_override)) target_climb_rate = climb_rate_override; // feed forward // feed_forward_factor += m_airborne ? -dt : dt; feed_forward_factor = limit(feed_forward_factor, 0.35f, 0.8f); target_climb_rate += user_rate * feed_forward_factor; TRACE("\rtarget_climb_rate=%.2f climb from alt =%.2f, user=%.2f, out=%2f. ", target_climb_rate, target_climb_rate-user_rate * feed_forward_factor, user_rate, throttle_result); // a better outter loop for centered throttle stick remote controller. if (int(alt_braking_enabled) && m_airborne) { bool near_ground = !isnan(m_sonar_target) && m_last_valid_sonar < 0.15f; static bool last_near_ground; if (!last_near_ground && near_ground) m_sonar_target = m_last_valid_sonar + m_baro_states[1] / 2; last_near_ground = near_ground; if (!near_ground) { bool stick_centered = fabs(user_rate) < 0.1f; if (!stick_centered) { m_controller_state = alt_override; m_braking_timer = 0; target_climb_rate = user_rate; } else { if (m_controller_state == alt_override) { m_controller_state = alt_braking; m_braking_timer = 0; target_climb_rate = 0; } else if (m_controller_state == alt_braking) { if (fabs(m_baro_states[1]) < 0.5f) m_braking_timer += dt; else m_braking_timer = 0; if (m_braking_timer > 0.5f) { float &target = isnan(m_sonar_target) ? baro_target : m_sonar_target; target = (isnan(m_sonar_target) ? m_baro_states[0] : m_last_valid_sonar) + m_baro_states[1] /2; LOGE("alt braking done, target set to %.2f(%s)", target, isnan(m_sonar_target) ? "baro" : "sonar"); m_controller_state = alt_hold; } target_climb_rate = 0; } else if (m_controller_state == alt_hold) { // do nothing } } } } target_climb_rate = limit(target_climb_rate, -quadcopter_max_descend_rate, quadcopter_max_climb_rate); // new climb rate error float climb_rate_error = target_climb_rate - m_baro_states[1]; //climb_rate_error = limit(climb_rate_error, -quadcopter_max_descend_rate, quadcopter_max_climb_rate); // apply a 2Hz LPF to rate error const float RC2 = 1.0f/(2*3.1415926 * 3.5f); float alpha2 = dt / (dt + RC2); // 5Hz LPF filter const float RC5 = 1.0f/(2*3.1415926 * 5.0f); float alpha5 = dt / (dt + RC5); // 30Hz LPF for derivative factor const float RC30 = 1.0f/(2*3.1415926 * 30.0f); float alpha30 = dt / (dt + RC30); // TODO: add feed forward // reference: get_throttle_rate() climb_rate_error_pid[0] = isnan(climb_rate_error_pid[0]) ? (climb_rate_error) : (climb_rate_error_pid[0] * (1-alpha2) + alpha2 * climb_rate_error); target_accel = climb_rate_error_pid[0] * pid_quad_alt_rate[0]; target_accel = limit(target_accel, m_airborne ? -quadcopter_max_acceleration : -2 * quadcopter_max_acceleration, quadcopter_max_acceleration); // new accel error, +2Hz LPF float accel_error = target_accel - m_baro_states[2]; if (isnan(accel_error_pid[0])) { accel_error_pid[0] = accel_error; } else { accel_error = accel_error_pid[0] * (1-alpha2) + alpha2 * accel_error; } // core pid // only integrate if throttle didn't hit limits or I term will reduce if (m_airborne) if ((!(m_motor_state & LIMIT_POSITIVE_HARD) && accel_error_pid[0] > 0) || (!(m_motor_state & LIMIT_NEGATIVE_HARD) && accel_error_pid[0] < 0)) { accel_error_pid[1] += accel_error_pid[0] * dt; accel_error_pid[1] = limit(accel_error_pid[1], -pid_quad_accel[3], pid_quad_accel[3]); } float D = (accel_error - accel_error_pid[0])/dt; accel_error_pid[2] = isnan(accel_error_pid[2])? D : (accel_error_pid[2] * (1-alpha30) + alpha30 * D); accel_error_pid[0] = accel_error; float output = 0; output += accel_error_pid[0] * pid_quad_accel[0]; output += accel_error_pid[1] * pid_quad_accel[1]; output += accel_error_pid[2] * pid_quad_accel[2]; output *= throttle_hover / default_throttle_hover; // normalize throttle output PID, from throttle percentage to acceleration throttle_result = output + throttle_hover; float angle_boost_factor = 1/ cos(m_attitude[0]) / cos(m_attitude[1]); if (angle_boost_factor < 0) angle_boost_factor = 0; // if we got reverted, kill thrust else if (angle_boost_factor > 1.5f) angle_boost_factor = 1.5f; else if (angle_boost_factor < 1.0f) angle_boost_factor = 1.0f; throttle_result = throttle_result * angle_boost_factor; if (throttle_result > 1 - QUADCOPTER_THROTTLE_RESERVE) { m_motor_state = LIMIT_POSITIVE_HARD; } else if (throttle_result < 0) { m_motor_state = LIMIT_NEGATIVE_HARD; } else { m_motor_state = 0; } throttle_result = limit(throttle_result, 0.0f, 1.0f); TRACE("\rthrottle=%f, altitude = %.2f/%.2f, pid=%.2f,%.2f,%.2f, limit=%d", throttle_result, m_baro_states[0], baro_target, accel_error_pid[0], accel_error_pid[1], accel_error_pid[2], m_motor_state); // update throttle_real_crusing if we're in near level state and no violent climbing/descending action if (m_airborne && m_throttle_realized>0 && fabs(m_baro_states[1]) < 0.5f && fabs(m_baro_states[2])<0.5f && fabs(m_attitude[0])<5*PI/180 && fabs(m_attitude[1])<5*PI/180 && fabs(user_rate) < 0.001f) { // 0.2Hz low pass filter const float RC02 = 1.0f/(2*3.1415926 * 0.02f); float alpha02 = dt / (dt + RC02); throttle_hover = throttle_hover * (1-alpha02) + alpha02 * m_throttle_realized; // TODO: estimate throttle cursing correctly TRACE("\rthrottle_hover=%f", throttle_hover); } last_update = systimer->gettime(); return 0; }