4 #if !HAL_MINIMIZE_FEATURES && OPTFLOW == ENABLED 75 #define CONTROL_FLOWHOLD_EARTH_FRAME 0 80 #if FRAME_CONFIG == HELI_FRAME 82 if (!ignore_checks && !
motors->rotor_runup_complete()){
87 if (!
copter.optflow.enabled() || !
copter.optflow.healthy()) {
96 if (!
copter.pos_control->is_active_z()) {
97 copter.pos_control->set_alt_target_to_current_alt();
98 copter.pos_control->set_desired_velocity_z(
copter.inertial_nav.get_velocity_z());
134 float ins_height =
copter.inertial_nav.get_altitude() * 0.01;
141 Vector2f input_ef =
copter.ahrs.rotate_body_to_earth2D(sensor_flow);
162 (
double)sensor_flow.
length());
167 if (!stick_input && !
braking) {
180 for (uint8_t i=0; i<2; i++) {
181 float &velocity = sensor_flow[i];
182 float abs_vel_cms = fabsf(velocity)*100;
184 float lean_angle_cd = brake_gain * abs_vel_cms * (1.0f+500.0f/(abs_vel_cms+60.0f));
186 lean_angle_cd = -lean_angle_cd;
188 bf_angles[i] = lean_angle_cd;
195 ef_output *=
copter.aparm.angle_max;
198 bf_angles +=
copter.ahrs.rotate_earth_to_body2D(ef_output);
201 limited = fabsf(bf_angles.
x) >
copter.aparm.angle_max || fabsf(bf_angles.
y) >
copter.aparm.angle_max;
208 DataFlash_Class::instance()->
Log_Write(
"FHLD",
"TimeUS,SFx,SFy,Ax,Ay,Qual,Ix,Iy",
"Qfffffffff",
210 (
double)sensor_flow.
x, (
double)sensor_flow.
y,
211 (
double)bf_angles.
x, (
double)bf_angles.
y,
222 float takeoff_climb_rate = 0.0f;
228 copter.pos_control->set_accel_z(
copter.g.pilot_accel_z);
231 copter.update_simple_mode();
239 float target_climb_rate =
copter.get_pilot_desired_climb_rate(
copter.channel_throttle->get_control_in());
243 float target_yaw_rate =
copter.get_pilot_desired_yaw_rate(
copter.channel_yaw->get_control_in());
245 if (!
copter.motors->armed() || !
copter.motors->get_interlock()) {
249 }
else if (!
copter.ap.auto_armed ||
copter.ap.land_complete) {
255 if (
copter.optflow.healthy()) {
256 const float filter_constant = 0.95;
265 int16_t roll_in =
copter.channel_roll->get_control_in();
266 int16_t pitch_in =
copter.channel_pitch->get_control_in();
267 float angle_max =
copter.attitude_control->get_althold_lean_angle_max();
278 bf_angles += flow_angles;
284 switch (flowhold_state) {
289 copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.
x, bf_angles.
y, target_yaw_rate);
290 #if FRAME_CONFIG == HELI_FRAME 292 copter.pos_control->set_alt_target_from_climb_rate(-abs(
copter.g.land_speed),
copter.G_Dt,
false);
294 copter.attitude_control->reset_rate_controller_I_terms();
295 copter.attitude_control->set_yaw_target_to_current_heading();
296 copter.pos_control->relax_alt_hold_controllers(0.0
f);
299 copter.pos_control->update_z_controller();
307 if (!
copter.takeoff_state.running) {
310 copter.set_land_complete(
false);
312 copter.set_throttle_takeoff();
316 copter.takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);
319 target_climb_rate =
copter.get_avoidance_adjusted_climbrate(target_climb_rate);
322 copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.
x, bf_angles.
y, target_yaw_rate);
325 copter.pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate,
copter.G_Dt,
false);
326 copter.pos_control->add_takeoff_climb_rate(takeoff_climb_rate,
copter.G_Dt);
327 copter.pos_control->update_z_controller();
332 if (target_climb_rate < 0.0
f) {
338 copter.attitude_control->reset_rate_controller_I_terms();
339 copter.attitude_control->set_yaw_target_to_current_heading();
340 copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.
x, bf_angles.
y, target_yaw_rate);
341 copter.pos_control->relax_alt_hold_controllers(0.0
f);
342 copter.pos_control->update_z_controller();
348 #if AC_AVOID_ENABLED == ENABLED 350 copter.avoid.adjust_roll_pitch(bf_angles.
x, bf_angles.
y,
copter.aparm.angle_max);
354 copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.
x, bf_angles.
y, target_yaw_rate);
357 if (
copter.rangefinder_alt_ok()) {
359 target_climb_rate =
copter.get_surface_tracking_climb_rate(target_climb_rate,
copter.pos_control->get_alt_target(),
copter.G_Dt);
363 target_climb_rate =
copter.get_avoidance_adjusted_climbrate(target_climb_rate);
366 copter.pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate,
copter.G_Dt,
false);
367 copter.pos_control->update_z_controller();
378 float ins_height =
copter.inertial_nav.get_altitude() * 0.01;
393 if (!
copter.ins.get_delta_velocity(delta_vel)) {
399 delta_vel = rotMat * delta_vel;
403 if (!
copter.optflow.healthy()) {
427 Vector2f delta_vel_rate(-delta_vel_bf.
y, delta_vel_bf.
x);
450 last_flow_rate_rps = flow_rate_rps;
456 const float min_velocity_change = 0.04;
457 const float min_flow_change = 0.04;
458 const float height_delta_max = 0.25;
463 float delta_height = 0;
464 uint8_t total_weight=0;
467 for (uint8_t i=0; i<2; i++) {
469 float abs_flow = fabsf(delta_flowrate[i]);
470 if (abs_flow < min_flow_change ||
471 fabsf(delta_vel_rate[i]) < min_velocity_change) {
475 float height = delta_vel_rate[i] / delta_flowrate[i];
480 delta_height += (height - height_estimate) * abs_flow;
481 total_weight += abs_flow;
483 if (total_weight > 0) {
484 delta_height /= total_weight;
487 if (delta_height < 0) {
495 float new_offset = height_offset +
constrain_float(delta_height, -height_delta_max, height_delta_max);
498 height_offset = 0.8 * height_offset + 0.2 * new_offset;
500 if (ins_height + height_offset <
height_min) {
508 DataFlash_Class::instance()->
Log_Write(
"FXY",
"TimeUS,DFx,DFy,DVx,DVy,Hest,DH,Hofs,InsH,LastInsH,DTms",
"QfffffffffI",
510 (
double)delta_flowrate.
x,
511 (
double)delta_flowrate.
y,
512 (
double)delta_vel_rate.
x,
513 (
double)delta_vel_rate.
y,
514 (
double)height_estimate,
515 (
double)delta_height,
516 (
double)height_offset,
522 last_ins_height = ins_height;
525 #endif // OPTFLOW == ENABLED int printf(const char *fmt,...)
bool get_soft_armed() const
float get_cutoff_freq(void) const
bool takeoff_triggered(float target_climb_rate) const
#define AP_GROUPINFO(name, idx, clazz, element, def)
AC_AttitudeControl_t *& attitude_control
void set_cutoff_frequency(float cutoff_freq)
uint32_t last_stick_input_ms
bool init(bool ignore_checks) override
void Log_Write(const char *name, const char *labels, const char *fmt,...)
static DataFlash_Class * instance(void)
void send_named_float(const char *name, float value) const
void get_pilot_desired_lean_angles(float &roll_out, float &pitch_out, float angle_max, float angle_limit) const
DESIRED_THROTTLE_UNLIMITED
Vector2f delta_velocity_ne
void update_height_estimate(void)
float constrain_float(const float amt, const float low, const float high)
void set_input(const Vector2f &input)
static const struct AP_Param::GroupInfo var_info[]
std::enable_if< std::is_integral< typename std::common_type< Arithmetic1, Arithmetic2 >::type >::value, bool >::type is_equal(const Arithmetic1 v_1, const Arithmetic2 v_2)
AP_HAL_MAIN_CALLBACKS & copter
LowPassFilterVector2f flow_filter
void flowhold_flow_to_angle(Vector2f &angle, bool stick_input)
#define AP_SUBGROUPINFO(element, name, idx, thisclazz, elclazz)
Vector2f last_flow_rate_rps
Vector2f apply(Vector2f sample, float dt)
static void setup_object_defaults(const void *object_pointer, const struct GroupInfo *group_info)