mirror of https://github.com/ArduPilot/ardupilot
237 lines
9.5 KiB
Plaintext
237 lines
9.5 KiB
Plaintext
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
// Traditional helicopter variables and functions
|
|
|
|
#if FRAME_CONFIG == HELI_FRAME
|
|
|
|
#ifndef HELI_DYNAMIC_FLIGHT_SPEED_MIN
|
|
#define HELI_DYNAMIC_FLIGHT_SPEED_MIN 100 // we are in "dynamic flight" when the speed is over 1m/s for 2 seconds
|
|
#endif
|
|
|
|
// counter to control dynamic flight profile
|
|
static int8_t heli_dynamic_flight_counter;
|
|
|
|
// Tradheli flags
|
|
static struct {
|
|
uint8_t dynamic_flight : 1; // 0 // true if we are moving at a significant speed (used to turn on/off leaky I terms)
|
|
} heli_flags;
|
|
|
|
// heli_check_dynamic_flight - updates the dynamic_flight flag based on our horizontal velocity
|
|
// should be called at 50hz
|
|
static void check_dynamic_flight(void)
|
|
{
|
|
if (!motors.armed() || throttle_mode == THROTTLE_LAND || !motors.motor_runup_complete()) {
|
|
heli_dynamic_flight_counter = 0;
|
|
heli_flags.dynamic_flight = false;
|
|
return;
|
|
}
|
|
|
|
bool moving = false;
|
|
|
|
// with GPS lock use inertial nav to determine if we are moving
|
|
if (GPS_ok()) {
|
|
// get horizontal velocity
|
|
float velocity = inertial_nav.get_velocity_xy();
|
|
moving = (velocity >= HELI_DYNAMIC_FLIGHT_SPEED_MIN);
|
|
}else{
|
|
// with no GPS lock base it on throttle and forward lean angle
|
|
moving = (g.rc_3.servo_out > 800 || ahrs.pitch_sensor < -1500);
|
|
}
|
|
|
|
if (moving) {
|
|
// if moving for 2 seconds, set the dynamic flight flag
|
|
if (!heli_flags.dynamic_flight) {
|
|
heli_dynamic_flight_counter++;
|
|
if (heli_dynamic_flight_counter >= 100) {
|
|
heli_flags.dynamic_flight = true;
|
|
heli_dynamic_flight_counter = 100;
|
|
}
|
|
}
|
|
}else{
|
|
// if not moving for 2 seconds, clear the dynamic flight flag
|
|
if (heli_flags.dynamic_flight) {
|
|
if (heli_dynamic_flight_counter > 0) {
|
|
heli_dynamic_flight_counter--;
|
|
}else{
|
|
heli_flags.dynamic_flight = false;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// 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.01f, 0.1f);
|
|
// rate_pitch_filter.set_cutoff_frequency(0.01f, 0.1f);
|
|
}
|
|
|
|
// heli_integrated_swash_controller - convert desired roll and pitch rate to roll and pitch swash angles
|
|
// should be called at 100hz
|
|
// output placed directly into g.rc_1.servo_out and g.rc_2.servo_out
|
|
static void heli_integrated_swash_controller(int32_t target_roll_rate, int32_t target_pitch_rate)
|
|
{
|
|
int32_t roll_p, roll_i, roll_d, roll_ff; // used to capture pid values for logging
|
|
int32_t pitch_p, pitch_i, pitch_d, pitch_ff;
|
|
int32_t current_roll_rate, current_pitch_rate; // this iteration's rate
|
|
int32_t roll_rate_error, pitch_rate_error; // simply target_rate - current_rate
|
|
int32_t roll_output, pitch_output; // output from pid controller
|
|
static bool roll_pid_saturated, pitch_pid_saturated; // tracker from last loop if the PID was saturated
|
|
|
|
current_roll_rate = (omega.x * DEGX100); // get current roll rate
|
|
current_pitch_rate = (omega.y * DEGX100); // get current pitch rate
|
|
|
|
roll_rate_error = target_roll_rate - current_roll_rate;
|
|
pitch_rate_error = target_pitch_rate - current_pitch_rate;
|
|
|
|
roll_p = g.pid_rate_roll.get_p(roll_rate_error);
|
|
pitch_p = g.pid_rate_pitch.get_p(pitch_rate_error);
|
|
|
|
if (roll_pid_saturated){
|
|
roll_i = g.pid_rate_roll.get_integrator(); // Locked Integrator due to PID saturation on previous cycle
|
|
} else {
|
|
if (motors.has_flybar()) { // Mechanical Flybars get regular integral for rate auto trim
|
|
if (target_roll_rate > -50 && target_roll_rate < 50){ // Frozen at high rates
|
|
roll_i = g.pid_rate_roll.get_i(roll_rate_error, G_Dt);
|
|
} else {
|
|
roll_i = g.pid_rate_roll.get_integrator();
|
|
}
|
|
} else {
|
|
if (heli_flags.dynamic_flight){
|
|
roll_i = g.pid_rate_roll.get_i(roll_rate_error, G_Dt);
|
|
} else {
|
|
roll_i = g.pid_rate_roll.get_leaky_i(roll_rate_error, G_Dt, RATE_INTEGRATOR_LEAK_RATE);
|
|
}
|
|
}
|
|
}
|
|
|
|
if (pitch_pid_saturated){
|
|
pitch_i = g.pid_rate_pitch.get_integrator(); // Locked Integrator due to PID saturation on previous cycle
|
|
} else {
|
|
if (motors.has_flybar()) { // Mechanical Flybars get regular integral for rate auto trim
|
|
if (target_pitch_rate > -50 && target_pitch_rate < 50){ // Frozen at high rates
|
|
pitch_i = g.pid_rate_pitch.get_i(pitch_rate_error, G_Dt);
|
|
} else {
|
|
pitch_i = g.pid_rate_pitch.get_integrator();
|
|
}
|
|
} else {
|
|
if (heli_flags.dynamic_flight){
|
|
pitch_i = g.pid_rate_pitch.get_i(pitch_rate_error, G_Dt);
|
|
} else {
|
|
pitch_i = g.pid_rate_pitch.get_leaky_i(pitch_rate_error, G_Dt, RATE_INTEGRATOR_LEAK_RATE);
|
|
}
|
|
}
|
|
}
|
|
|
|
roll_d = g.pid_rate_roll.get_d(target_roll_rate, G_Dt);
|
|
pitch_d = g.pid_rate_pitch.get_d(target_pitch_rate, G_Dt);
|
|
|
|
roll_ff = g.heli_roll_ff * target_roll_rate;
|
|
pitch_ff = g.heli_pitch_ff * target_pitch_rate;
|
|
|
|
// Joly, I think your PC and CC code goes here
|
|
|
|
roll_output = roll_p + roll_i + roll_d + roll_ff;
|
|
pitch_output = pitch_p + pitch_i + pitch_d + pitch_ff;
|
|
|
|
if (labs(roll_output) > 4500){
|
|
roll_output = constrain_int32(roll_output, -4500, 4500); // constrain output
|
|
roll_pid_saturated = true; // freeze integrator next cycle
|
|
} else {
|
|
roll_pid_saturated = false; // unfreeze integrator
|
|
}
|
|
|
|
if (labs(pitch_output) > 4500){
|
|
pitch_output = constrain_int32(pitch_output, -4500, 4500); // constrain output
|
|
pitch_pid_saturated = true; // freeze integrator next cycle
|
|
} else {
|
|
pitch_pid_saturated = false; // unfreeze integrator
|
|
}
|
|
|
|
g.rc_1.servo_out = roll_output;
|
|
g.rc_2.servo_out = pitch_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;
|
|
static bool pid_saturated; // tracker from last loop if the PID was saturated
|
|
|
|
current_rate = (omega.z * DEGX100); // get 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);
|
|
|
|
if (pid_saturated){
|
|
i = g.pid_rate_yaw.get_integrator(); // Locked Integrator due to PID saturation on previous cycle
|
|
} else {
|
|
if (motors.motor_runup_complete()){
|
|
i = g.pid_rate_yaw.get_i(rate_error, G_Dt);
|
|
} else {
|
|
i = g.pid_rate_yaw.get_leaky_i(rate_error, G_Dt, RATE_INTEGRATOR_LEAK_RATE); // If motor is not running use leaky I-term to avoid excessive build-up
|
|
}
|
|
}
|
|
|
|
d = g.pid_rate_yaw.get_d(rate_error, G_Dt);
|
|
|
|
ff = g.heli_yaw_ff*target_rate;
|
|
|
|
output = p + i + d + ff;
|
|
|
|
if (labs(output) > 4500){
|
|
output = constrain_int32(output, -4500, 4500); // constrain output
|
|
pid_saturated = true; // freeze integrator next cycle
|
|
} else {
|
|
pid_saturated = false; // unfreeze integrator
|
|
}
|
|
|
|
#if LOGGING_ENABLED == ENABLED
|
|
// 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_RATE_KP || g.radio_tuning == CH6_YAW_RATE_KD) ) {
|
|
pid_log_counter++;
|
|
if( pid_log_counter >= 10 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10
|
|
pid_log_counter = 0;
|
|
Log_Write_PID(CH6_YAW_RATE_KP, rate_error, p, i, d, output, tuning_value);
|
|
}
|
|
}
|
|
#endif
|
|
|
|
return output; // output control
|
|
}
|
|
|
|
// heli_update_landing_swash - sets swash plate flag so higher minimum is used when landed or landing
|
|
// should be called soon after update_land_detector in main code
|
|
static void heli_update_landing_swash()
|
|
{
|
|
switch(throttle_mode) {
|
|
case THROTTLE_MANUAL:
|
|
case THROTTLE_MANUAL_TILT_COMPENSATED:
|
|
case THROTTLE_MANUAL_HELI:
|
|
// manual modes always uses full swash range
|
|
motors.set_collective_for_landing(false);
|
|
break;
|
|
|
|
case THROTTLE_LAND:
|
|
// landing always uses limit swash range
|
|
motors.set_collective_for_landing(true);
|
|
break;
|
|
|
|
case THROTTLE_HOLD:
|
|
case THROTTLE_AUTO:
|
|
default:
|
|
// auto and hold use limited swash when landed
|
|
motors.set_collective_for_landing(ap.land_complete || !ap.auto_armed);
|
|
break;
|
|
}
|
|
}
|
|
|
|
#endif // FRAME_CONFIG == HELI_FRAME |