LCOV - code coverage report
Current view: top level - home/pbarker/rc/ardupilot/ArduCopter - mode_flowhold.cpp (source / functions) Hit Total Coverage
Test: lcov.info Lines: 185 200 92.5 %
Date: 2026-09-09 11:02:08 Functions: 6 6 100.0 %

          Line data    Source code
       1             : #include "Copter.h"
       2             : #include <utility>
       3             : 
       4             : #if MODE_FLOWHOLD_ENABLED
       5             : 
       6             : /*
       7             :   implement FLOWHOLD mode, for position hold using optical flow
       8             :   without rangefinder
       9             :  */
      10             : 
      11             : const AP_Param::GroupInfo ModeFlowHold::var_info[] = {
      12             :     // @Param: _XY_P
      13             :     // @DisplayName: FlowHold P gain
      14             :     // @Description: FlowHold (horizontal) P gain.
      15             :     // @Range: 0.1 6.0
      16             :     // @Increment: 0.1
      17             :     // @User: Advanced
      18             : 
      19             :     // @Param: _XY_I
      20             :     // @DisplayName: FlowHold I gain
      21             :     // @Description: FlowHold (horizontal) I gain
      22             :     // @Range: 0.02 1.00
      23             :     // @Increment: 0.01
      24             :     // @User: Advanced
      25             : 
      26             :     // @Param: _XY_IMAX
      27             :     // @DisplayName: FlowHold Integrator Max
      28             :     // @Description: FlowHold (horizontal) integrator maximum
      29             :     // @Range: 0 4500
      30             :     // @Increment: 10
      31             :     // @Units: cdeg
      32             :     // @User: Advanced
      33             : 
      34             :     // @Param: _XY_FILT_HZ
      35             :     // @DisplayName: FlowHold filter on input to control
      36             :     // @Description: FlowHold (horizontal) filter on input to control
      37             :     // @Range: 0 100
      38             :     // @Units: Hz
      39             :     // @User: Advanced
      40             :     AP_SUBGROUPINFO(flow_pi_xy, "_XY_",  1, ModeFlowHold, AC_PI_2D),
      41             : 
      42             :     // @Param: _FLOW_MAX
      43             :     // @DisplayName: FlowHold Flow Rate Max
      44             :     // @Description: Controls maximum apparent flow rate in flowhold
      45             :     // @Range: 0.1 2.5
      46             :     // @User: Standard
      47             :     AP_GROUPINFO("_FLOW_MAX", 2, ModeFlowHold, flow_max, 0.6),
      48             : 
      49             :     // @Param: _FILT_HZ
      50             :     // @DisplayName: FlowHold Filter Frequency
      51             :     // @Description: Filter frequency for flow data
      52             :     // @Range: 1 100
      53             :     // @Units: Hz
      54             :     // @User: Standard
      55             :     AP_GROUPINFO("_FILT_HZ", 3, ModeFlowHold, flow_filter_hz, 5),
      56             : 
      57             :     // @Param: _QUAL_MIN
      58             :     // @DisplayName: FlowHold Flow quality minimum
      59             :     // @Description: Minimum flow quality to use flow position hold
      60             :     // @Range: 0 255
      61             :     // @User: Standard
      62             :     AP_GROUPINFO("_QUAL_MIN", 4, ModeFlowHold, flow_min_quality, 10),
      63             : 
      64             :     // 5 was FLOW_SPEED
      65             : 
      66             :     // @Param: _BRAKE_RATE
      67             :     // @DisplayName: FlowHold Braking rate
      68             :     // @Description: Controls deceleration rate on stick release
      69             :     // @Range: 1 30
      70             :     // @User: Standard
      71             :     // @Units: deg/s
      72             :     AP_GROUPINFO("_BRAKE_RATE", 6, ModeFlowHold, brake_rate_dps, 8),
      73             : 
      74             :     AP_GROUPEND
      75             : };
      76             : 
      77        1001 : ModeFlowHold::ModeFlowHold(void) : Mode()
      78             : {
      79        1001 :     AP_Param::setup_object_defaults(this, var_info);
      80        1001 : }
      81             : 
      82             : #define CONTROL_FLOWHOLD_EARTH_FRAME 0
      83             : 
      84             : // Return true if this mode is enabled, used by MAVLink available modes
      85       75224 : bool ModeFlowHold::enabled() const
      86             : {
      87       75224 :     return copter.optflow.enabled();
      88             : }
      89             : 
      90             : 
      91             : // flowhold_init - initialise flowhold controller
      92           1 : bool ModeFlowHold::init(bool ignore_checks)
      93             : {
      94           1 :     if (!enabled() || !copter.optflow.healthy()) {
      95           0 :         return false;
      96             :     }
      97             : 
      98             :     // set vertical speed and acceleration limits
      99           1 :     pos_control->D_set_max_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss());
     100           1 :     pos_control->D_set_correction_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss());
     101             : 
     102             :     // initialise the vertical position controller
     103           1 :     if (!copter.pos_control->D_is_active()) {
     104           1 :         pos_control->D_init_controller();
     105             :     }
     106             : 
     107           1 :     flow_filter.set_cutoff_frequency(copter.scheduler.get_loop_rate_hz(), flow_filter_hz.get());
     108             : 
     109           1 :     quality_filtered = 0;
     110           1 :     flow_pi_xy.reset_I();
     111           1 :     limited = false;
     112             : 
     113           1 :     flow_pi_xy.set_dt(1.0/copter.scheduler.get_loop_rate_hz());
     114             : 
     115             :     // start with INS height
     116           1 :     last_ins_height_m = pos_control->get_pos_estimate_U_m();
     117           1 :     height_offset_m = 0;
     118             : 
     119           1 :     return true;
     120             : }
     121             : 
     122             : /*
     123             :   calculate desired attitude from flow sensor. Called when flow sensor is healthy
     124             :  */
     125       49116 : void ModeFlowHold::flowhold_flow_to_angle(Vector2f &bf_angles_rad, bool stick_input)
     126             : {
     127       49116 :     uint32_t now = AP_HAL::millis();
     128       49116 :     const float angle_max_rad = copter.attitude_control->lean_angle_max_rad();
     129             : 
     130             :     // get corrected raw flow rate
     131       49116 :     Vector2f raw_flow_rads = copter.optflow.flowRate() - copter.optflow.bodyRate();
     132             : 
     133             :     // limit sensor flow, this prevents oscillation at low altitudes
     134       49116 :     raw_flow_rads.x = constrain_float(raw_flow_rads.x, -flow_max, flow_max);
     135       49116 :     raw_flow_rads.y = constrain_float(raw_flow_rads.y, -flow_max, flow_max);
     136             : 
     137             :     // filter the flow rate
     138       49116 :     Vector2f sensor_flow_rads = flow_filter.apply(raw_flow_rads);
     139             : 
     140             :     // scale by height estimate, limiting it to height_min_m to height_max
     141       49116 :     float ins_height_m = pos_control->get_pos_estimate_U_m();
     142       49116 :     float height_estimate_m = ins_height_m + height_offset_m;
     143             : 
     144             :     // compensate for height, this converts to (approx) m/s
     145       49116 :     Vector2f sensor_flow_ms = sensor_flow_rads * constrain_float(height_estimate_m, height_min_m, height_max);
     146             : 
     147             :     // rotate controller input to earth frame
     148       49116 :     Vector2f input_ne_ms = copter.ahrs.body_to_earth2D(sensor_flow_ms);
     149             : 
     150             :     // run PI controller
     151       49116 :     flow_pi_xy.set_input(input_ne_ms);
     152             : 
     153             :     // get the normalised earth frame controller attitude
     154       49116 :     Vector2f ef_output;
     155             : 
     156             :     // get P term
     157       49116 :     ef_output = flow_pi_xy.get_p();
     158             : 
     159       49116 :     if (stick_input) {
     160       26960 :         last_stick_input_ms = now;
     161       26960 :         braking = true;
     162             :     }
     163       49116 :     if (!stick_input && braking) {
     164             :         // stop braking if either 3s has passed, or we have slowed below 0.3m/s
     165         677 :         if (now - last_stick_input_ms > 3000 || sensor_flow_ms.length() < 0.3) {
     166           1 :             braking = false;
     167             : #if 0
     168             :             printf("braking done at %u vel=%f\n", now - last_stick_input_ms,
     169             :                    (double)sensor_flow_ms.length());
     170             : #endif
     171             :         }
     172             :     }
     173             : 
     174       49116 :     if (!stick_input && !braking) {
     175             :         // get I term
     176       21480 :         if (limited) {
     177             :             // only allow I term to shrink in length
     178           0 :             xy_I = flow_pi_xy.get_i_shrink();
     179             :         } else {
     180             :             // normal I term operation
     181       21480 :             xy_I = flow_pi_xy.get_pi();
     182             :         }
     183             :     }
     184             : 
     185       49116 :     if (!stick_input && braking) {
     186             :         // calculate brake angle for each axis separately
     187        2028 :         for (uint8_t i=0; i<2; i++) {
     188        1352 :             float &velocity = sensor_flow_ms[i];
     189        1352 :             float abs_vel_ms = fabsf(velocity);
     190        1352 :             const float brake_gain = (15.0f * brake_rate_dps.get() + 95.0f) * 0.01f;
     191        1352 :             float lean_angle_rad = radians(brake_gain * abs_vel_ms * (1.0f + 5.0f/(abs_vel_ms + 0.60f)));
     192        1352 :             if (velocity < 0) {
     193         135 :                 lean_angle_rad = -lean_angle_rad;
     194             :             }
     195        1352 :             bf_angles_rad[i] = lean_angle_rad;
     196             :         }
     197         676 :         ef_output.zero();
     198             :     }
     199             : 
     200       49116 :     ef_output += xy_I;
     201       49116 :     Vector2f ef_output_rad = ef_output * angle_max_rad;
     202             : 
     203             :     // convert to body frame
     204       49116 :     bf_angles_rad += copter.ahrs.earth_to_body2D(ef_output_rad);
     205             : 
     206             :     // set limited flag to prevent integrator windup
     207       49116 :     limited = fabsf(bf_angles_rad.x) > angle_max_rad || fabsf(bf_angles_rad.y) > angle_max_rad;
     208             : 
     209             :     // constrain to angle limit
     210       49116 :     bf_angles_rad.x = constrain_float(bf_angles_rad.x, -angle_max_rad, angle_max_rad);
     211       49116 :     bf_angles_rad.y = constrain_float(bf_angles_rad.y, -angle_max_rad, angle_max_rad);
     212             : 
     213             : #if HAL_LOGGING_ENABLED
     214             : // @LoggerMessage: FHLD
     215             : // @Description: FlowHold mode messages
     216             : // @URL: https://ardupilot.club/copter/docs/flowhold-mode.html
     217             : // @Field: TimeUS: Time since system startup
     218             : // @Field: SFx: Filtered flow rate, X-Axis
     219             : // @Field: SFy: Filtered flow rate, Y-Axis
     220             : // @Field: Ax: Target lean angle, X-Axis
     221             : // @Field: Ay: Target lean angle, Y-Axis
     222             : // @Field: Qual: Flow sensor quality. If this value falls below FHLD_QUAL_MIN parameter, FlowHold will act just like AltHold.
     223             : // @Field: Ix: Integral part of PI controller, X-Axis
     224             : // @Field: Iy: Integral part of PI controller, Y-Axis
     225             : 
     226       49116 :     if (log_counter++ % 20 == 0) {
     227        4988 :         AP::logger().WriteStreaming("FHLD", "TimeUS,SFx,SFy,Ax,Ay,Qual,Ix,Iy", "Qfffffff",
     228             :                                                AP_HAL::micros64(),
     229        2494 :                                                (double)sensor_flow_ms.x, (double)sensor_flow_ms.y,
     230        2494 :                                                (double)rad_to_cd(bf_angles_rad.x), (double)rad_to_cd(bf_angles_rad.y),
     231        2494 :                                                (double)quality_filtered,
     232        2494 :                                                (double)xy_I.x, (double)xy_I.y);
     233             :     }
     234             : #endif  // HAL_LOGGING_ENABLED
     235       49116 : }
     236             : 
     237             : // flowhold_run - runs the flowhold controller
     238             : // should be called at 100hz or more
     239       50319 : void ModeFlowHold::run()
     240             : {
     241       50319 :     update_height_estimate();
     242             : 
     243             :     // set vertical speed and acceleration limits
     244       50319 :     pos_control->D_set_max_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss());
     245             : 
     246             :     // check for filter change
     247       50319 :     if (!is_equal(flow_filter.get_cutoff_freq(), flow_filter_hz.get())) {
     248           0 :         flow_filter.set_cutoff_frequency(copter.scheduler.get_loop_rate_hz(), flow_filter_hz.get());
     249             :     }
     250             : 
     251             :     // get pilot desired climb rate
     252       50319 :     float target_climb_rate_ms = copter.get_pilot_desired_climb_rate_ms();
     253             : 
     254             :     // get pilot's desired yaw rate
     255       50319 :     float target_yaw_rate_rads = get_pilot_desired_yaw_rate_rads();
     256             : 
     257             :     // Flow Hold State Machine Determination
     258       50319 :     AltHoldModeState flowhold_state = get_alt_hold_state_D_ms(target_climb_rate_ms);
     259             : 
     260       50319 :     if (copter.optflow.healthy()) {
     261       50319 :         const float filter_constant = 0.95;
     262       50319 :         quality_filtered = filter_constant * quality_filtered + (1-filter_constant) * copter.optflow.quality();
     263             :     } else {
     264           0 :         quality_filtered = 0;
     265             :     }
     266             : 
     267             :     // Flow Hold State Machine
     268       50319 :     switch (flowhold_state) {
     269             : 
     270        1600 :     case AltHoldModeState::MotorStopped:
     271        1600 :         copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
     272        1600 :         copter.attitude_control->reset_rate_controller_I_terms();
     273        1600 :         copter.attitude_control->reset_yaw_target_and_rate();
     274        1600 :         copter.pos_control->D_relax_controller(0.0f);   // forces throttle output to decay to zero
     275        1600 :         flow_pi_xy.reset_I();
     276        1600 :         break;
     277             : 
     278         381 :     case AltHoldModeState::Takeoff:
     279             :         // set motors to full range
     280         381 :         copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
     281             : 
     282             :         // initiate take-off
     283         381 :         if (!takeoff.running()) {
     284           1 :             takeoff.start_m(constrain_float(g2.pilot_takeoff_alt_m, 0.0, 10.0));
     285             :         }
     286             : 
     287             :         // get avoidance adjusted climb rate
     288         381 :         target_climb_rate_ms = get_avoidance_adjusted_climbrate_ms(target_climb_rate_ms);
     289             : 
     290             :         // set position controller targets adjusted for pilot input
     291         381 :         takeoff.do_pilot_takeoff_ms(target_climb_rate_ms);
     292         381 :         break;
     293             : 
     294         204 :     case AltHoldModeState::Landed_Ground_Idle:
     295         204 :         attitude_control->reset_yaw_target_and_rate();
     296             :         FALLTHROUGH;
     297             : 
     298        1006 :     case AltHoldModeState::Landed_Pre_Takeoff:
     299        1006 :         attitude_control->reset_rate_controller_I_terms_smoothly();
     300        1006 :         pos_control->D_relax_controller(0.0f);   // forces throttle output to decay to zero
     301        1006 :         break;
     302             : 
     303       47332 :     case AltHoldModeState::Flying:
     304       47332 :         copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
     305             : 
     306             :         // get avoidance adjusted climb rate
     307       47332 :         target_climb_rate_ms = get_avoidance_adjusted_climbrate_ms(target_climb_rate_ms);
     308             : 
     309             : #if AP_RANGEFINDER_ENABLED
     310             :         // update the vertical offset based on the surface measurement
     311       47332 :         copter.surface_tracking.update_surface_offset();
     312             : #endif
     313             : 
     314             :         // Send the commanded climb rate to the position controller
     315       47332 :         pos_control->D_set_pos_target_from_climb_rate_ms(target_climb_rate_ms);
     316       47332 :         break;
     317             :     }
     318             : 
     319             :     // flowhold attitude target calculations
     320       50319 :     Vector2f bf_angles_rad;
     321             : 
     322             :     // calculate alt-hold angles
     323       50319 :     int16_t roll_in = copter.channel_roll->get_control_in();
     324       50319 :     int16_t pitch_in = copter.channel_pitch->get_control_in();
     325       50319 :     const float angle_max_rad = copter.attitude_control->lean_angle_max_rad();
     326             : 
     327             :     float target_roll_rad, target_pitch_rad;
     328       50319 :     get_pilot_desired_lean_angles_rad(target_roll_rad, target_pitch_rad, attitude_control->lean_angle_max_rad(), attitude_control->get_althold_lean_angle_max_rad());
     329       50319 :     bf_angles_rad.x = target_roll_rad;
     330       50319 :     bf_angles_rad.y = target_pitch_rad;
     331             : 
     332      100634 :     if (quality_filtered >= flow_min_quality &&
     333       50315 :         AP_HAL::millis() - copter.arm_time_ms > 3000) {
     334             :         // don't use for first 3s when we are just taking off
     335       49116 :         Vector2f flow_angles;
     336             : 
     337       49116 :         flowhold_flow_to_angle(flow_angles, (roll_in != 0) || (pitch_in != 0));
     338       49116 :         flow_angles.x = constrain_float(flow_angles.x, -angle_max_rad/2, angle_max_rad/2);
     339       49116 :         flow_angles.y = constrain_float(flow_angles.y, -angle_max_rad/2, angle_max_rad/2);
     340       49116 :         bf_angles_rad += flow_angles;
     341             :     }
     342       50319 :     bf_angles_rad.x = constrain_float(bf_angles_rad.x, -angle_max_rad, angle_max_rad);
     343       50319 :     bf_angles_rad.y = constrain_float(bf_angles_rad.y, -angle_max_rad, angle_max_rad);
     344             : 
     345             : #if AP_AVOIDANCE_ALTHOLD_ENABLED
     346             :     // apply avoidance
     347             :     copter.avoid.adjust_roll_pitch_rad(bf_angles_rad.x, bf_angles_rad.y, attitude_control->lean_angle_max_rad());
     348             : #endif
     349             : 
     350             :     // call attitude controller
     351       50319 :     copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_rad(bf_angles_rad.x, bf_angles_rad.y, target_yaw_rate_rads);
     352             : 
     353             :     // run the vertical position controller and set output throttle
     354       50319 :     pos_control->D_update_controller();
     355       50319 : }
     356             : 
     357             : /*
     358             :   update height estimate using integrated accelerometer ratio with optical flow
     359             :  */
     360       50319 : void ModeFlowHold::update_height_estimate(void)
     361             : {
     362       50319 :     float ins_height_m = copter.pos_control->get_pos_estimate_U_m();
     363             : 
     364             : #if 1
     365             :     // assume on ground when disarmed, or if we have only just started spooling the motors up
     366       99038 :     if (!hal.util->get_soft_armed() ||
     367       99038 :         copter.motors->get_desired_spool_state() != AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED ||
     368       47918 :         AP_HAL::millis() - copter.arm_time_ms < 1500) {
     369        2401 :         height_offset_m = -ins_height_m;
     370        2401 :         last_ins_height_m = ins_height_m;
     371       49121 :         return;
     372             :     }
     373             : #endif
     374             : 
     375             :     // get delta velocity in body frame
     376       47918 :     Vector3f delta_vel_ms;
     377             :     float delta_vel_dt;
     378       47918 :     if (!copter.ins.get_delta_velocity(delta_vel_ms, delta_vel_dt)) {
     379           0 :         return;
     380             :     }
     381             : 
     382             :     // integrate delta velocity in earth frame
     383       47918 :     const Matrix3f &rotMat = copter.ahrs.get_rotation_body_to_ned();
     384       47918 :     delta_vel_ms = rotMat * delta_vel_ms;
     385       47918 :     delta_velocity_ne_ms.x += delta_vel_ms.x;
     386       47918 :     delta_velocity_ne_ms.y += delta_vel_ms.y;
     387             : 
     388       47918 :     if (!copter.optflow.healthy()) {
     389             :         // can't update height model with no flow sensor
     390           0 :         last_flow_ms = AP_HAL::millis();
     391           0 :         delta_velocity_ne_ms.zero();
     392           0 :         return;
     393             :     }
     394             : 
     395       47918 :     if (last_flow_ms == 0) {
     396             :         // just starting up
     397           1 :         last_flow_ms = copter.optflow.last_update();
     398           1 :         delta_velocity_ne_ms.zero();
     399           1 :         height_offset_m = 0;
     400           1 :         return;
     401             :     }
     402             : 
     403       47917 :     if (copter.optflow.last_update() == last_flow_ms) {
     404             :         // no new flow data
     405       46719 :         return;
     406             :     }
     407             : 
     408             :     // convert delta velocity back to body frame to match the flow sensor
     409        1198 :     Vector2f delta_vel_bf_ms = copter.ahrs.earth_to_body2D(delta_velocity_ne_ms);
     410             : 
     411             :     // and convert to an rate equivalent, to be comparable to flow
     412        1198 :     Vector2f delta_vel_rate_ms(-delta_vel_bf_ms.y, delta_vel_bf_ms.x);
     413             : 
     414             :     // get body flow rate in radians per second
     415        1198 :     Vector2f flow_rate_rads = copter.optflow.flowRate() - copter.optflow.bodyRate();
     416             : 
     417        1198 :     uint32_t dt_ms = copter.optflow.last_update() - last_flow_ms;
     418        1198 :     if (dt_ms > 500) {
     419             :         // too long between updates, ignore
     420           0 :         last_flow_ms = copter.optflow.last_update();
     421           0 :         delta_velocity_ne_ms.zero();
     422           0 :         last_flow_rate_rads = flow_rate_rads;
     423           0 :         last_ins_height_m = ins_height_m;
     424           0 :         height_offset_m = 0;
     425           0 :         return;        
     426             :     }
     427             : 
     428             :     /*
     429             :       basic equation is:
     430             :       height_m = delta_velocity_ms / delta_flowrate_rads;
     431             :      */
     432             : 
     433             :     // get delta_flowrate_rps
     434        1198 :     Vector2f delta_flowrate_rads = flow_rate_rads - last_flow_rate_rads;
     435        1198 :     last_flow_rate_rads = flow_rate_rads;
     436        1198 :     last_flow_ms = copter.optflow.last_update();
     437             : 
     438             :     /*
     439             :       update height estimate
     440             :      */
     441        1198 :     const float min_velocity_change_ms = 0.04;
     442        1198 :     const float min_flow_change = 0.04;
     443        1198 :     const float height_delta_max_m = 0.25;
     444             : 
     445             :     /*
     446             :       for each axis update the height estimate
     447             :      */
     448        1198 :     float delta_height_m = 0;
     449        1198 :     float total_weight = 0;
     450        1198 :     float height_estimate_m = ins_height_m + height_offset_m;
     451             : 
     452        3594 :     for (uint8_t i=0; i<2; i++) {
     453             :         // only use height estimates when we have significant delta-velocity and significant delta-flow
     454        2396 :         float abs_flow = fabsf(delta_flowrate_rads[i]);
     455        2483 :         if (abs_flow < min_flow_change ||
     456          87 :             fabsf(delta_vel_rate_ms[i]) < min_velocity_change_ms) {
     457        2309 :             continue;
     458             :         }
     459             :         // get instantaneous height estimate
     460          87 :         float height_m = delta_vel_rate_ms[i] / delta_flowrate_rads[i];
     461          87 :         if (height_m <= 0) {
     462             :             // discard negative heights
     463           0 :             continue;
     464             :         }
     465          87 :         delta_height_m += (height_m - height_estimate_m) * abs_flow;
     466          87 :         total_weight += abs_flow;
     467             :     }
     468        1198 :     if (total_weight > 0) {
     469          87 :         delta_height_m /= total_weight;
     470             :     }
     471             : 
     472        1198 :     if (delta_height_m < 0) {
     473             :         // bias towards lower heights, as we'd rather have too low
     474             :         // gain than have oscillation. This also compensates a bit for
     475             :         // the discard of negative heights above
     476          11 :         delta_height_m *= 2;
     477             :     }
     478             : 
     479             :     // don't update height by more than height_delta_max_m, this is a simple way of rejecting noise
     480        1198 :     float new_offset_m = height_offset_m + constrain_float(delta_height_m, -height_delta_max_m, height_delta_max_m);
     481             : 
     482             :     // apply a simple filter
     483        1198 :     height_offset_m = 0.8 * height_offset_m + 0.2 * new_offset_m;
     484             : 
     485        1198 :     if (ins_height_m + height_offset_m < height_min_m) {
     486             :         // height estimate is never allowed below the minimum
     487           3 :         height_offset_m = height_min_m - ins_height_m;
     488             :     }
     489             : 
     490             :     // new height estimate for logging
     491        1198 :     height_estimate_m = ins_height_m + height_offset_m;
     492             : 
     493             : #if HAL_LOGGING_ENABLED
     494             : // @LoggerMessage: FHXY
     495             : // @Description: Height estimation using optical flow sensor 
     496             : // @Field: TimeUS: Time since system startup
     497             : // @Field: DFx: Delta flow rate, X-Axis
     498             : // @Field: DFy: Delta flow rate, Y-Axis
     499             : // @Field: DVx: Integrated delta velocity rate, X-Axis
     500             : // @Field: DVy: Integrated delta velocity rate, Y-Axis
     501             : // @Field: Hest: Estimated Height
     502             : // @Field: DH: Delta Height
     503             : // @Field: Hofs: Height offset
     504             : // @Field: InsH: Height estimate from inertial navigation library
     505             : // @Field: LastInsH: Last used INS height in optical flow sensor height estimation calculations 
     506             : // @Field: DTms: Time between optical flow sensor updates. This should be less than 500ms for performing the height estimation calculations
     507             : 
     508        2396 :     AP::logger().WriteStreaming("FHXY", "TimeUS,DFx,DFy,DVx,DVy,Hest,DH,Hofs,InsH,LastInsH,DTms", "QfffffffffI",
     509             :                                            AP_HAL::micros64(),
     510        1198 :                                            (double)delta_flowrate_rads.x,
     511        1198 :                                            (double)delta_flowrate_rads.y,
     512        1198 :                                            (double)delta_vel_rate_ms.x,
     513        1198 :                                            (double)delta_vel_rate_ms.y,
     514             :                                            (double)height_estimate_m,
     515             :                                            (double)delta_height_m,
     516        1198 :                                            (double)height_offset_m,
     517             :                                            (double)ins_height_m,
     518        1198 :                                            (double)last_ins_height_m,
     519             :                                            dt_ms);
     520             : #endif
     521             : 
     522        1198 :     gcs().send_named_float("HEST", height_estimate_m);
     523        1198 :     delta_velocity_ne_ms.zero();
     524        1198 :     last_ins_height_m = ins_height_m;
     525             : }
     526             : 
     527             : #endif // MODE_FLOWHOLD_ENABLED

Generated by: LCOV version 1.14