mirror of https://github.com/ArduPilot/ardupilot
173 lines
5.4 KiB
Plaintext
173 lines
5.4 KiB
Plaintext
|
/// -*- 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
|
||
|
*/
|
||
|
|
||
|
// ofloiter_init - initialise ofloiter controller
|
||
|
static bool ofloiter_init(bool ignore_checks)
|
||
|
{
|
||
|
#if OPTFLOW == ENABLED
|
||
|
if (g.optflow_enabled || ignore_checks) {
|
||
|
return true;
|
||
|
}else{
|
||
|
return false;
|
||
|
}
|
||
|
#else
|
||
|
return false;
|
||
|
#endif
|
||
|
}
|
||
|
|
||
|
// 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 target_yaw_rate = 0;
|
||
|
float target_climb_rate = 0;
|
||
|
|
||
|
// if not auto armed set throttle to zero and exit immediately
|
||
|
if(!ap.auto_armed || !inertial_nav.position_ok()) {
|
||
|
attitude_control.init_targets();
|
||
|
attitude_control.set_throttle_out(0, false);
|
||
|
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.init_targets();
|
||
|
attitude_control.set_throttle_out(0, false);
|
||
|
}else{
|
||
|
// mix in user control with optical flow
|
||
|
target_roll = get_of_roll(target_roll);
|
||
|
target_pitch = get_of_pitch(target_pitch);
|
||
|
|
||
|
// call attitude controller
|
||
|
attitude_control.angleef_rp_rateef_y(target_roll, target_pitch, target_yaw_rate);
|
||
|
|
||
|
// 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,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();
|
||
|
}
|
||
|
|
||
|
// re-fetch angle targets for reporting
|
||
|
const Vector3f angle_target = attitude_control.angle_ef_targets();
|
||
|
control_roll = angle_target.x;
|
||
|
control_pitch = angle_target.y;
|
||
|
control_yaw = angle_target.z;
|
||
|
}
|
||
|
|
||
|
|
||
|
// calculate modified roll/pitch depending upon optical flow calculated position
|
||
|
static int32_t get_of_roll(int32_t input_roll)
|
||
|
{
|
||
|
#if OPTFLOW == ENABLED
|
||
|
static float tot_x_cm = 0; // total distance from target
|
||
|
static uint32_t last_of_roll_update = 0;
|
||
|
int32_t new_roll = 0;
|
||
|
int32_t p,i,d;
|
||
|
|
||
|
// check if new optflow data available
|
||
|
if( optflow.last_update != last_of_roll_update) {
|
||
|
last_of_roll_update = optflow.last_update;
|
||
|
|
||
|
// add new distance moved
|
||
|
tot_x_cm += optflow.x_cm;
|
||
|
|
||
|
// only stop roll if caller isn't modifying roll
|
||
|
if( input_roll == 0 && current_loc.alt < 1500) {
|
||
|
p = g.pid_optflow_roll.get_p(-tot_x_cm);
|
||
|
i = g.pid_optflow_roll.get_i(-tot_x_cm,1.0f); // we could use the last update time to calculate the time change
|
||
|
d = g.pid_optflow_roll.get_d(-tot_x_cm,1.0f);
|
||
|
new_roll = p+i+d;
|
||
|
}else{
|
||
|
g.pid_optflow_roll.reset_I();
|
||
|
tot_x_cm = 0;
|
||
|
p = 0; // for logging
|
||
|
i = 0;
|
||
|
d = 0;
|
||
|
}
|
||
|
// limit amount of change and maximum angle
|
||
|
of_roll = constrain_int32(new_roll, (of_roll-20), (of_roll+20));
|
||
|
}
|
||
|
|
||
|
// limit max angle
|
||
|
of_roll = constrain_int32(of_roll, -1000, 1000);
|
||
|
|
||
|
return input_roll+of_roll;
|
||
|
#else
|
||
|
return input_roll;
|
||
|
#endif
|
||
|
}
|
||
|
|
||
|
static int32_t get_of_pitch(int32_t input_pitch)
|
||
|
{
|
||
|
#if OPTFLOW == ENABLED
|
||
|
static float tot_y_cm = 0; // total distance from target
|
||
|
static uint32_t last_of_pitch_update = 0;
|
||
|
int32_t new_pitch = 0;
|
||
|
int32_t p,i,d;
|
||
|
|
||
|
// check if new optflow data available
|
||
|
if( optflow.last_update != last_of_pitch_update ) {
|
||
|
last_of_pitch_update = optflow.last_update;
|
||
|
|
||
|
// add new distance moved
|
||
|
tot_y_cm += optflow.y_cm;
|
||
|
|
||
|
// only stop roll if caller isn't modifying pitch
|
||
|
if( input_pitch == 0 && current_loc.alt < 1500 ) {
|
||
|
p = g.pid_optflow_pitch.get_p(tot_y_cm);
|
||
|
i = g.pid_optflow_pitch.get_i(tot_y_cm, 1.0f); // we could use the last update time to calculate the time change
|
||
|
d = g.pid_optflow_pitch.get_d(tot_y_cm, 1.0f);
|
||
|
new_pitch = p + i + d;
|
||
|
}else{
|
||
|
tot_y_cm = 0;
|
||
|
g.pid_optflow_pitch.reset_I();
|
||
|
p = 0; // for logging
|
||
|
i = 0;
|
||
|
d = 0;
|
||
|
}
|
||
|
|
||
|
// limit amount of change
|
||
|
of_pitch = constrain_int32(new_pitch, (of_pitch-20), (of_pitch+20));
|
||
|
}
|
||
|
|
||
|
// limit max angle
|
||
|
of_pitch = constrain_int32(of_pitch, -1000, 1000);
|
||
|
|
||
|
return input_pitch+of_pitch;
|
||
|
#else
|
||
|
return input_pitch;
|
||
|
#endif
|
||
|
}
|