aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

TradHeli: dynamic_flight flag based on inertial nav speed

Browse Source 
Created new heli.pde for heli_integrated_swash and check_dynamic_flight
This commit is contained in:
Randy Mackay 2013-11-03 14:26:19 +09:00
parent 6f54d63f6f
commit 8e5b398b79
3 changed files with 204 additions and 181 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
ArduCopter/ArduCopter.pde
View File

@ -670,11 +670,6 @@ static uint8_t throttle_mode;
static int16_t angle_boost;
// counter to verify landings
static uint16_t land_detector;
// counter to control dynamic flight profile
#if FRAME_CONFIG == HELI_FRAME
static uint8_t dynamic_flight_counter;
static bool dynamic_flight;
#endif // HELI_FRAME
////////////////////////////////////////////////////////////////////////////////
@ -2016,37 +2011,6 @@ void update_throttle_mode(void)
}
}
#if FRAME_CONFIG == HELI_FRAME
static void check_dynamic_flight(void){
if (!motors.armed() || throttle_mode == THROTTLE_LAND || motors.motor_runup_complete == false){
dynamic_flight_counter=0;
dynamic_flight=false;
return;
}
if (dynamic_flight_counter < 100){ // check if we're in dynamic flight mode
if (g.rc_3.servo_out > 800 || (ahrs.pitch_sensor < -1500)) { // Nose down for forward flight. Remember, nose down is negative pitch.
if (!ap.takeoff_complete){
set_takeoff_complete(true);
}
dynamic_flight_counter++;
}
if (dynamic_flight_counter > 99){ // we must be in the air by now
dynamic_flight = true;
}
}
if (dynamic_flight_counter > 0){
if ((labs(ahrs.roll_sensor) < 1500) && (labs(ahrs.pitch_sensor) < 1200)) {
dynamic_flight_counter--;
}
if (dynamic_flight_counter < 1){
dynamic_flight = false;
}
}
}
#endif // HELI_FRAME
// set_target_alt_for_reporting - set target altitude in cm for reporting purposes (logs and gcs)
static void set_target_alt_for_reporting(float alt_cm)
{

145
ArduCopter/Attitude.pde
View File

@ -460,151 +460,6 @@ run_rate_controllers()
}
}
#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.01f, 0.1f);
// rate_pitch_filter.set_cutoff_frequency(0.01f, 0.1f);
}
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.flybar_mode == 1) { // 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 (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.flybar_mode == 1) { // 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 (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 {
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;
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
}
#endif // HELI_FRAME
#if FRAME_CONFIG != HELI_FRAME
static int16_t
get_rate_roll(int32_t target_rate)

204
ArduCopter/heli.pde Normal file
View File

@ -0,0 +1,204 @@
/// -*- 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);
}
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.flybar_mode == 1) { // 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.flybar_mode == 1) { // 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 {
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;
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
}
#endif // FRAME_CONFIG == TRAD_HELI