Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-08 17:03:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: pilot override used immediately in OF_Loiter

Browse Source 
Replace some hard coded numbers with definitions
This commit is contained in:
Randy Mackay 2014-10-15 15:15:38 +09:00
parent a9cfbb71b8
commit 7b07b575cf
1 changed files with 32 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
ArduCopter/control_ofloiter.pde
View File

@ -6,6 +6,10 @@
#if OPTFLOW == ENABLED #if OPTFLOW == ENABLED
#define OPTFLOW_ALT_MAX_CM 1500 // maximum altitude above home that optical flow sensor will be used
#define OPTFLOW_TIMEOUT_MS 200 // timeout in milliseconds after which we will give up on optical flow readings and return control to the pilot
#define OPTFLOW_RP_RATE_LIM (2000/MAIN_LOOP_RATE) // limit in centi-degrees/sec on rate of change of roll-pitch target. Equal to 20deg/sec
// ofloiter_init - initialise ofloiter controller // ofloiter_init - initialise ofloiter controller
static bool ofloiter_init(bool ignore_checks) static bool ofloiter_init(bool ignore_checks)
{ {
@ -82,7 +86,6 @@ static void ofloiter_run()
pos_control.set_alt_target_to_current_alt(); pos_control.set_alt_target_to_current_alt();
of_roll = ahrs.roll_sensor; of_roll = ahrs.roll_sensor;
of_pitch = ahrs.pitch_sensor; of_pitch = ahrs.pitch_sensor;
reset_optflow_I();
}else{ }else{
// mix in user control with optical flow // mix in user control with optical flow
get_of_roll_pitch(target_roll, target_pitch, final_roll, final_pitch); get_of_roll_pitch(target_roll, target_pitch, final_roll, final_pitch);
@ -107,6 +110,7 @@ static void get_of_roll_pitch(int16_t input_roll, int16_t input_pitch, float &ro
{ {
static uint32_t last_of_update = 0; static uint32_t last_of_update = 0;
float dt; float dt;
Vector2f vel;
// To-Do: pass input_roll, input_pitch through to roll_out, pitch_out if input is non-zero or previous iteration was non-zero // To-Do: pass input_roll, input_pitch through to roll_out, pitch_out if input is non-zero or previous iteration was non-zero
@ -115,36 +119,42 @@ static void get_of_roll_pitch(int16_t input_roll, int16_t input_pitch, float &ro
// calculate dt and sanity check // calculate dt and sanity check
dt = (optflow.last_update() - last_of_update) / 1000.0f; dt = (optflow.last_update() - last_of_update) / 1000.0f;
if (dt > 0.2) { if (dt > 0.2f) {
dt = 0; dt = 0.0f;
g.pid_optflow_roll.reset_I(); g.pid_optflow_roll.reset_I();
g.pid_optflow_pitch.reset_I(); g.pid_optflow_pitch.reset_I();
} }
last_of_update = optflow.last_update(); last_of_update = optflow.last_update();
// get latest velocity from sensor // get latest velocity from sensor
const Vector2f &vel = optflow.velocity(); vel = optflow.velocity();
}
// allow pilot override of roll // calculate time since last update
if (input_roll == 0 && current_loc.alt < 1500) { uint32_t time_since_update_ms = millis() - last_of_update;
roll_out = g.pid_optflow_roll.get_pid(-vel.x, dt);
// limit amount of change and maximum angle // use pilot roll input if input is non-zero, altitude above 15m or optical flow sensor has timed out
roll_out = constrain_float(roll_out, (of_roll-20), (of_roll+20)); if (input_roll != 0 || current_loc.alt > OPTFLOW_ALT_MAX_CM || time_since_update_ms > OPTFLOW_TIMEOUT_MS) {
} else {
roll_out = input_roll; roll_out = input_roll;
} } else {
of_roll = roll_out; // run velocity through pid controller
roll_out = g.pid_optflow_roll.get_pid(-vel.x, dt);
if (input_pitch == 0 && current_loc.alt < 1500) { // limit amount of change and maximum angle
pitch_out = g.pid_optflow_pitch.get_pid(vel.y, dt); // To-Do: replace reliance on of_roll, of_pitch within this function
pitch_out = constrain_float(pitch_out, (of_pitch-20), (of_pitch+20)); roll_out = constrain_float(roll_out, (of_roll-OPTFLOW_RP_RATE_LIM), (of_roll+OPTFLOW_RP_RATE_LIM));
} else {
pitch_out = input_pitch;
} }
of_pitch = pitch_out;
// use pilot pitch input if input is non-zero, altitude above 15m or optical flow sensor has timed out
if (input_pitch != 0 || current_loc.alt > OPTFLOW_ALT_MAX_CM || time_since_update_ms > OPTFLOW_TIMEOUT_MS) {
pitch_out = input_pitch;
} else { } else {
roll_out = of_roll; // run velocity through pid controller
pitch_out = of_pitch; pitch_out = g.pid_optflow_pitch.get_pid(vel.y, dt);
// limit amount of change and maximum angle
// To-Do: replace reliance on of_roll, of_pitch within this function
pitch_out = constrain_float(pitch_out, (of_pitch-OPTFLOW_RP_RATE_LIM), (of_pitch+OPTFLOW_RP_RATE_LIM));
} }
} }