mirror of https://github.com/ArduPilot/ardupilot
Copter : Remove legacy optical flow control mode
This mode is not compatible with the new PX4Flow interface
This commit is contained in:
priseborough
Andrew Tridgell
parent
900519b35b
commit
0d7555b259
|
|
@ -1,168 +0,0 @@
|
||||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
||||||
|
|
||||||
/*
|
|
||||||
* control_ofloiter.pde - init and run calls for of_loiter (optical flow loiter) flight mode
|
|
||||||
*/
|
|
||||||
|
|
||||||
#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
|
|
||||||
static bool ofloiter_init(bool ignore_checks)
|
|
||||||
{
|
|
||||||
if (optflow.enabled() || ignore_checks) {
|
|
||||||
|
|
||||||
// initialize vertical speed and acceleration
|
|
||||||
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
|
|
||||||
pos_control.set_accel_z(g.pilot_accel_z);
|
|
||||||
|
|
||||||
// initialise altitude target to stopping point
|
|
||||||
pos_control.set_target_to_stopping_point_z();
|
|
||||||
|
|
||||||
// initialise of_roll, pitch to current attitude
|
|
||||||
of_roll = ahrs.roll_sensor;
|
|
||||||
of_pitch = ahrs.pitch_sensor;
|
|
||||||
reset_optflow_I();
|
|
||||||
|
|
||||||
return true;
|
|
||||||
}else{
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// ofloiter_run - runs the optical flow loiter controller
|
|
||||||
// should be called at 100hz or more
|
|
||||||
static void ofloiter_run()
|
|
||||||
{
|
|
||||||
int16_t target_roll, target_pitch;
|
|
||||||
float final_roll, final_pitch;
|
|
||||||
float target_yaw_rate = 0;
|
|
||||||
float target_climb_rate = 0;
|
|
||||||
|
|
||||||
// if not auto armed set throttle to zero and exit immediately
|
|
||||||
if(!ap.auto_armed) {
|
|
||||||
attitude_control.relax_bf_rate_controller();
|
|
||||||
attitude_control.set_yaw_target_to_current_heading();
|
|
||||||
attitude_control.set_throttle_out(0, false);
|
|
||||||
pos_control.set_alt_target_to_current_alt();
|
|
||||||
of_roll = ahrs.roll_sensor;
|
|
||||||
of_pitch = ahrs.pitch_sensor;
|
|
||||||
reset_optflow_I();
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// process pilot inputs
|
|
||||||
if (!failsafe.radio) {
|
|
||||||
// apply SIMPLE mode transform to pilot inputs
|
|
||||||
update_simple_mode();
|
|
||||||
|
|
||||||
// convert pilot input to lean angles
|
|
||||||
get_pilot_desired_lean_angles(g.rc_1.control_in, g.rc_2.control_in, target_roll, target_pitch);
|
|
||||||
|
|
||||||
// get pilot's desired yaw rate
|
|
||||||
target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in);
|
|
||||||
|
|
||||||
// get pilot desired climb rate
|
|
||||||
target_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in);
|
|
||||||
|
|
||||||
// check for pilot requested take-off
|
|
||||||
if (ap.land_complete && target_climb_rate > 0) {
|
|
||||||
// indicate we are taking off
|
|
||||||
set_land_complete(false);
|
|
||||||
// clear i term when we're taking off
|
|
||||||
set_throttle_takeoff();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// when landed reset targets and output zero throttle
|
|
||||||
if (ap.land_complete) {
|
|
||||||
attitude_control.relax_bf_rate_controller();
|
|
||||||
attitude_control.set_yaw_target_to_current_heading();
|
|
||||||
// move throttle to between minimum and non-takeoff-throttle to keep us on the ground
|
|
||||||
attitude_control.set_throttle_out(get_throttle_pre_takeoff(g.rc_3.control_in), false);
|
|
||||||
pos_control.set_alt_target_to_current_alt();
|
|
||||||
of_roll = ahrs.roll_sensor;
|
|
||||||
of_pitch = ahrs.pitch_sensor;
|
|
||||||
}else{
|
|
||||||
// mix in user control with optical flow
|
|
||||||
get_of_roll_pitch(target_roll, target_pitch, final_roll, final_pitch);
|
|
||||||
|
|
||||||
// call attitude controller
|
|
||||||
attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(final_roll, final_pitch, target_yaw_rate, get_smoothing_gain());
|
|
||||||
|
|
||||||
// run altitude controller
|
|
||||||
if (sonar_alt_health >= SONAR_ALT_HEALTH_MAX) {
|
|
||||||
// if sonar is ok, use surface tracking
|
|
||||||
target_climb_rate = get_throttle_surface_tracking(target_climb_rate, pos_control.get_alt_target(), G_Dt);
|
|
||||||
}
|
|
||||||
|
|
||||||
// update altitude target and call position controller
|
|
||||||
pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt);
|
|
||||||
pos_control.update_z_controller();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// calculate modified roll/pitch depending upon optical flow calculated position
|
|
||||||
static void get_of_roll_pitch(int16_t input_roll, int16_t input_pitch, float &roll_out, float &pitch_out)
|
|
||||||
{
|
|
||||||
static uint32_t last_of_update = 0;
|
|
||||||
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
|
|
||||||
|
|
||||||
// check if new optflow data available
|
|
||||||
if (optflow.last_update() != last_of_update) {
|
|
||||||
|
|
||||||
// calculate dt and sanity check
|
|
||||||
dt = (optflow.last_update() - last_of_update) / 1000.0f;
|
|
||||||
if (dt > 0.2f) {
|
|
||||||
dt = 0.0f;
|
|
||||||
g.pid_optflow_roll.reset_I();
|
|
||||||
g.pid_optflow_pitch.reset_I();
|
|
||||||
}
|
|
||||||
last_of_update = optflow.last_update();
|
|
||||||
|
|
||||||
// get latest velocity from sensor
|
|
||||||
vel = optflow.velocity();
|
|
||||||
}
|
|
||||||
|
|
||||||
// calculate time since last update
|
|
||||||
uint32_t time_since_update_ms = millis() - last_of_update;
|
|
||||||
|
|
||||||
// use pilot roll input if input is non-zero, altitude above 15m or optical flow sensor has timed out
|
|
||||||
if (input_roll != 0 || current_loc.alt > OPTFLOW_ALT_MAX_CM || time_since_update_ms > OPTFLOW_TIMEOUT_MS) {
|
|
||||||
roll_out = input_roll;
|
|
||||||
} else {
|
|
||||||
// run velocity through pid controller
|
|
||||||
roll_out = g.pid_optflow_roll.get_pid(-vel.x, dt);
|
|
||||||
|
|
||||||
// limit amount of change and maximum angle
|
|
||||||
// To-Do: replace reliance on of_roll, of_pitch within this function
|
|
||||||
roll_out = constrain_float(roll_out, (of_roll-OPTFLOW_RP_RATE_LIM), (of_roll+OPTFLOW_RP_RATE_LIM));
|
|
||||||
}
|
|
||||||
|
|
||||||
// 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 {
|
|
||||||
// run velocity through pid controller
|
|
||||||
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));
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// reset_optflow_I - reset optflow position hold I terms
|
|
||||||
static void reset_optflow_I(void)
|
|
||||||
{
|
|
||||||
g.pid_optflow_roll.reset_I();
|
|
||||||
g.pid_optflow_pitch.reset_I();
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif // OPTFLOW == ENABLED
|
|
||||||
Loading…
Reference in New Issue