ArduCopter/mode_flowhold.cpp

   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 ModeFlowHold::ModeFlowHold(void) : Mode()
  78 {
  79     AP_Param::setup_object_defaults(this, var_info);
  80 }
  81
  82 #define CONTROL_FLOWHOLD_EARTH_FRAME 0
  83
  84 // flowhold_init - initialise flowhold controller
  85 bool ModeFlowHold::init(bool ignore_checks)
  86 {
  87     if (!copter.optflow.enabled() || !copter.optflow.healthy()) {
  88         return false;
  89     }
  90
  91     // set vertical speed and acceleration limits
  92     pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
  93     pos_control->set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
  94
  95     // initialise the vertical position controller
  96     if (!copter.pos_control->is_active_z()) {
  97         pos_control->init_z_controller();
  98     }
  99
 100     flow_filter.set_cutoff_frequency(copter.scheduler.get_loop_rate_hz(), flow_filter_hz.get());
 101
 102     quality_filtered = 0;
 103     flow_pi_xy.reset_I();
 104     limited = false;
 105
 106     flow_pi_xy.set_dt(1.0/copter.scheduler.get_loop_rate_hz());
 107
 108     // start with INS height
 109     last_ins_height = copter.inertial_nav.get_position_z_up_cm() * 0.01;
 110     height_offset = 0;
 111
 112     return true;
 113 }
 114
 115 /*
 116   calculate desired attitude from flow sensor. Called when flow sensor is healthy
 117  */
 118 void ModeFlowHold::flowhold_flow_to_angle(Vector2f &bf_angles, bool stick_input)
 119 {
 120     uint32_t now = AP_HAL::millis();
 121
 122     // get corrected raw flow rate
 123     Vector2f raw_flow = copter.optflow.flowRate() - copter.optflow.bodyRate();
 124
 125     // limit sensor flow, this prevents oscillation at low altitudes
 126     raw_flow.x = constrain_float(raw_flow.x, -flow_max, flow_max);
 127     raw_flow.y = constrain_float(raw_flow.y, -flow_max, flow_max);
 128
 129     // filter the flow rate
 130     Vector2f sensor_flow = flow_filter.apply(raw_flow);
 131
 132     // scale by height estimate, limiting it to height_min to height_max
 133     float ins_height = copter.inertial_nav.get_position_z_up_cm() * 0.01;
 134     float height_estimate = ins_height + height_offset;
 135
 136     // compensate for height, this converts to (approx) m/s
 137     sensor_flow *= constrain_float(height_estimate, height_min, height_max);
 138
 139     // rotate controller input to earth frame
 140     Vector2f input_ef = copter.ahrs.body_to_earth2D(sensor_flow);
 141
 142     // run PI controller
 143     flow_pi_xy.set_input(input_ef);
 144
 145     // get earth frame controller attitude in centi-degrees
 146     Vector2f ef_output;
 147
 148     // get P term
 149     ef_output = flow_pi_xy.get_p();
 150
 151     if (stick_input) {
 152         last_stick_input_ms = now;
 153         braking = true;
 154     }
 155     if (!stick_input && braking) {
 156         // stop braking if either 3s has passed, or we have slowed below 0.3m/s
 157         if (now - last_stick_input_ms > 3000 || sensor_flow.length() < 0.3) {
 158             braking = false;
 159 #if 0
 160             printf("braking done at %u vel=%f\n", now - last_stick_input_ms,
 161                    (double)sensor_flow.length());
 162 #endif
 163         }
 164     }
 165
 166     if (!stick_input && !braking) {
 167         // get I term
 168         if (limited) {
 169             // only allow I term to shrink in length
 170             xy_I = flow_pi_xy.get_i_shrink();
 171         } else {
 172             // normal I term operation
 173             xy_I = flow_pi_xy.get_pi();
 174         }
 175     }
 176
 177     if (!stick_input && braking) {
 178         // calculate brake angle for each axis separately
 179         for (uint8_t i=0; i<2; i++) {
 180             float &velocity = sensor_flow[i];
 181             float abs_vel_cms = fabsf(velocity)*100;
 182             const float brake_gain = (15.0f * brake_rate_dps.get() + 95.0f) * 0.01f;
 183             float lean_angle_cd = brake_gain * abs_vel_cms * (1.0f+500.0f/(abs_vel_cms+60.0f));
 184             if (velocity < 0) {
 185                 lean_angle_cd = -lean_angle_cd;
 186             }
 187             bf_angles[i] = lean_angle_cd;
 188         }
 189         ef_output.zero();
 190     }
 191
 192     ef_output += xy_I;
 193     ef_output *= copter.aparm.angle_max;
 194
 195     // convert to body frame
 196     bf_angles += copter.ahrs.earth_to_body2D(ef_output);
 197
 198     // set limited flag to prevent integrator windup
 199     limited = fabsf(bf_angles.x) > copter.aparm.angle_max || fabsf(bf_angles.y) > copter.aparm.angle_max;
 200
 201     // constrain to angle limit
 202     bf_angles.x = constrain_float(bf_angles.x, -copter.aparm.angle_max, copter.aparm.angle_max);
 203     bf_angles.y = constrain_float(bf_angles.y, -copter.aparm.angle_max, copter.aparm.angle_max);
 204
 205 #if HAL_LOGGING_ENABLED
 206 // @LoggerMessage: FHLD
 207 // @Description: FlowHold mode messages
 208 // @URL: https://ardupilot.org/copter/docs/flowhold-mode.html
 209 // @Field: TimeUS: Time since system startup
 210 // @Field: SFx: Filtered flow rate, X-Axis
 211 // @Field: SFy: Filtered flow rate, Y-Axis
 212 // @Field: Ax: Target lean angle, X-Axis
 213 // @Field: Ay: Target lean angle, Y-Axis
 214 // @Field: Qual: Flow sensor quality. If this value falls below FHLD_QUAL_MIN parameter, FlowHold will act just like AltHold.
 215 // @Field: Ix: Integral part of PI controller, X-Axis
 216 // @Field: Iy: Integral part of PI controller, Y-Axis
 217
 218     if (log_counter++ % 20 == 0) {
 219         AP::logger().WriteStreaming("FHLD", "TimeUS,SFx,SFy,Ax,Ay,Qual,Ix,Iy", "Qfffffff",
 220                                                AP_HAL::micros64(),
 221                                                (double)sensor_flow.x, (double)sensor_flow.y,
 222                                                (double)bf_angles.x, (double)bf_angles.y,
 223                                                (double)quality_filtered,
 224                                                (double)xy_I.x, (double)xy_I.y);
 225     }
 226 #endif  // HAL_LOGGING_ENABLED
 227 }
 228
 229 // flowhold_run - runs the flowhold controller
 230 // should be called at 100hz or more
 231 void ModeFlowHold::run()
 232 {
 233     update_height_estimate();
 234
 235     // set vertical speed and acceleration limits
 236     pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
 237
 238     // apply SIMPLE mode transform to pilot inputs
 239     update_simple_mode();
 240
 241     // check for filter change
 242     if (!is_equal(flow_filter.get_cutoff_freq(), flow_filter_hz.get())) {
 243         flow_filter.set_cutoff_frequency(copter.scheduler.get_loop_rate_hz(), flow_filter_hz.get());
 244     }
 245
 246     // get pilot desired climb rate
 247     float target_climb_rate = copter.get_pilot_desired_climb_rate(copter.channel_throttle->get_control_in());
 248     target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), copter.g.pilot_speed_up);
 249
 250     // get pilot's desired yaw rate
 251     float target_yaw_rate = get_pilot_desired_yaw_rate();
 252
 253     // Flow Hold State Machine Determination
 254     AltHoldModeState flowhold_state = get_alt_hold_state(target_climb_rate);
 255
 256     if (copter.optflow.healthy()) {
 257         const float filter_constant = 0.95;
 258         quality_filtered = filter_constant * quality_filtered + (1-filter_constant) * copter.optflow.quality();
 259     } else {
 260         quality_filtered = 0;
 261     }
 262
 263     // Flow Hold State Machine
 264     switch (flowhold_state) {
 265
 266     case AltHoldModeState::MotorStopped:
 267         copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
 268         copter.attitude_control->reset_rate_controller_I_terms();
 269         copter.attitude_control->reset_yaw_target_and_rate();
 270         copter.pos_control->relax_z_controller(0.0f);   // forces throttle output to decay to zero
 271         flow_pi_xy.reset_I();
 272         break;
 273
 274     case AltHoldModeState::Takeoff:
 275         // set motors to full range
 276         copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 277
 278         // initiate take-off
 279         if (!takeoff.running()) {
 280             takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
 281         }
 282
 283         // get avoidance adjusted climb rate
 284         target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
 285
 286         // set position controller targets adjusted for pilot input
 287         takeoff.do_pilot_takeoff(target_climb_rate);
 288         break;
 289
 290     case AltHoldModeState::Landed_Ground_Idle:
 291         attitude_control->reset_yaw_target_and_rate();
 292         FALLTHROUGH;
 293
 294     case AltHoldModeState::Landed_Pre_Takeoff:
 295         attitude_control->reset_rate_controller_I_terms_smoothly();
 296         pos_control->relax_z_controller(0.0f);   // forces throttle output to decay to zero
 297         break;
 298
 299     case AltHoldModeState::Flying:
 300         copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 301
 302         // get avoidance adjusted climb rate
 303         target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
 304
 305 #if AP_RANGEFINDER_ENABLED
 306         // update the vertical offset based on the surface measurement
 307         copter.surface_tracking.update_surface_offset();
 308 #endif
 309
 310         // Send the commanded climb rate to the position controller
 311         pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate);
 312         break;
 313     }
 314
 315     // flowhold attitude target calculations
 316     Vector2f bf_angles;
 317
 318     // calculate alt-hold angles
 319     int16_t roll_in = copter.channel_roll->get_control_in();
 320     int16_t pitch_in = copter.channel_pitch->get_control_in();
 321     float angle_max = copter.aparm.angle_max;
 322     get_pilot_desired_lean_angles(bf_angles.x, bf_angles.y, angle_max, attitude_control->get_althold_lean_angle_max_cd());
 323
 324     if (quality_filtered >= flow_min_quality &&
 325         AP_HAL::millis() - copter.arm_time_ms > 3000) {
 326         // don't use for first 3s when we are just taking off
 327         Vector2f flow_angles;
 328
 329         flowhold_flow_to_angle(flow_angles, (roll_in != 0) || (pitch_in != 0));
 330         flow_angles.x = constrain_float(flow_angles.x, -angle_max/2, angle_max/2);
 331         flow_angles.y = constrain_float(flow_angles.y, -angle_max/2, angle_max/2);
 332         bf_angles += flow_angles;
 333     }
 334     bf_angles.x = constrain_float(bf_angles.x, -angle_max, angle_max);
 335     bf_angles.y = constrain_float(bf_angles.y, -angle_max, angle_max);
 336
 337 #if AP_AVOIDANCE_ENABLED
 338     // apply avoidance
 339     copter.avoid.adjust_roll_pitch(bf_angles.x, bf_angles.y, copter.aparm.angle_max);
 340 #endif
 341
 342     // call attitude controller
 343     copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.x, bf_angles.y, target_yaw_rate);
 344
 345     // run the vertical position controller and set output throttle
 346     pos_control->update_z_controller();
 347 }
 348
 349 /*
 350   update height estimate using integrated accelerometer ratio with optical flow
 351  */
 352 void ModeFlowHold::update_height_estimate(void)
 353 {
 354     float ins_height = copter.inertial_nav.get_position_z_up_cm() * 0.01;
 355
 356 #if 1
 357     // assume on ground when disarmed, or if we have only just started spooling the motors up
 358     if (!hal.util->get_soft_armed() ||
 359         copter.motors->get_desired_spool_state() != AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED ||
 360         AP_HAL::millis() - copter.arm_time_ms < 1500) {
 361         height_offset = -ins_height;
 362         last_ins_height = ins_height;
 363         return;
 364     }
 365 #endif
 366
 367     // get delta velocity in body frame
 368     Vector3f delta_vel;
 369     float delta_vel_dt;
 370     if (!copter.ins.get_delta_velocity(delta_vel, delta_vel_dt)) {
 371         return;
 372     }
 373
 374     // integrate delta velocity in earth frame
 375     const Matrix3f &rotMat = copter.ahrs.get_rotation_body_to_ned();
 376     delta_vel = rotMat * delta_vel;
 377     delta_velocity_ne.x += delta_vel.x;
 378     delta_velocity_ne.y += delta_vel.y;
 379
 380     if (!copter.optflow.healthy()) {
 381         // can't update height model with no flow sensor
 382         last_flow_ms = AP_HAL::millis();
 383         delta_velocity_ne.zero();
 384         return;
 385     }
 386
 387     if (last_flow_ms == 0) {
 388         // just starting up
 389         last_flow_ms = copter.optflow.last_update();
 390         delta_velocity_ne.zero();
 391         height_offset = 0;
 392         return;
 393     }
 394
 395     if (copter.optflow.last_update() == last_flow_ms) {
 396         // no new flow data
 397         return;
 398     }
 399
 400     // convert delta velocity back to body frame to match the flow sensor
 401     Vector2f delta_vel_bf = copter.ahrs.earth_to_body2D(delta_velocity_ne);
 402
 403     // and convert to an rate equivalent, to be comparable to flow
 404     Vector2f delta_vel_rate(-delta_vel_bf.y, delta_vel_bf.x);
 405
 406     // get body flow rate in radians per second
 407     Vector2f flow_rate_rps = copter.optflow.flowRate() - copter.optflow.bodyRate();
 408
 409     uint32_t dt_ms = copter.optflow.last_update() - last_flow_ms;
 410     if (dt_ms > 500) {
 411         // too long between updates, ignore
 412         last_flow_ms = copter.optflow.last_update();
 413         delta_velocity_ne.zero();
 414         last_flow_rate_rps = flow_rate_rps;
 415         last_ins_height = ins_height;
 416         height_offset = 0;
 417         return;
 418     }
 419
 420     /*
 421       basic equation is:
 422       height_m = delta_velocity_mps / delta_flowrate_rps;
 423      */
 424
 425     // get delta_flowrate_rps
 426     Vector2f delta_flowrate = flow_rate_rps - last_flow_rate_rps;
 427     last_flow_rate_rps = flow_rate_rps;
 428     last_flow_ms = copter.optflow.last_update();
 429
 430     /*
 431       update height estimate
 432      */
 433     const float min_velocity_change = 0.04;
 434     const float min_flow_change = 0.04;
 435     const float height_delta_max = 0.25;
 436
 437     /*
 438       for each axis update the height estimate
 439      */
 440     float delta_height = 0;
 441     uint8_t total_weight = 0;
 442     float height_estimate = ins_height + height_offset;
 443
 444     for (uint8_t i=0; i<2; i++) {
 445         // only use height estimates when we have significant delta-velocity and significant delta-flow
 446         float abs_flow = fabsf(delta_flowrate[i]);
 447         if (abs_flow < min_flow_change ||
 448             fabsf(delta_vel_rate[i]) < min_velocity_change) {
 449             continue;
 450         }
 451         // get instantaneous height estimate
 452         float height = delta_vel_rate[i] / delta_flowrate[i];
 453         if (height <= 0) {
 454             // discard negative heights
 455             continue;
 456         }
 457         delta_height += (height - height_estimate) * abs_flow;
 458         total_weight += abs_flow;
 459     }
 460     if (total_weight > 0) {
 461         delta_height /= total_weight;
 462     }
 463
 464     if (delta_height < 0) {
 465         // bias towards lower heights, as we'd rather have too low
 466         // gain than have oscillation. This also compensates a bit for
 467         // the discard of negative heights above
 468         delta_height *= 2;
 469     }
 470
 471     // don't update height by more than height_delta_max, this is a simple way of rejecting noise
 472     float new_offset = height_offset + constrain_float(delta_height, -height_delta_max, height_delta_max);
 473
 474     // apply a simple filter
 475     height_offset = 0.8 * height_offset + 0.2 * new_offset;
 476
 477     if (ins_height + height_offset < height_min) {
 478         // height estimate is never allowed below the minimum
 479         height_offset = height_min - ins_height;
 480     }
 481
 482     // new height estimate for logging
 483     height_estimate = ins_height + height_offset;
 484
 485 #if HAL_LOGGING_ENABLED
 486 // @LoggerMessage: FHXY
 487 // @Description: Height estimation using optical flow sensor
 488 // @Field: TimeUS: Time since system startup
 489 // @Field: DFx: Delta flow rate, X-Axis
 490 // @Field: DFy: Delta flow rate, Y-Axis
 491 // @Field: DVx: Integrated delta velocity rate, X-Axis
 492 // @Field: DVy: Integrated delta velocity rate, Y-Axis
 493 // @Field: Hest: Estimated Height
 494 // @Field: DH: Delta Height
 495 // @Field: Hofs: Height offset
 496 // @Field: InsH: Height estimate from inertial navigation library
 497 // @Field: LastInsH: Last used INS height in optical flow sensor height estimation calculations
 498 // @Field: DTms: Time between optical flow sensor updates. This should be less than 500ms for performing the height estimation calculations
 499
 500     AP::logger().WriteStreaming("FHXY", "TimeUS,DFx,DFy,DVx,DVy,Hest,DH,Hofs,InsH,LastInsH,DTms", "QfffffffffI",
 501                                            AP_HAL::micros64(),
 502                                            (double)delta_flowrate.x,
 503                                            (double)delta_flowrate.y,
 504                                            (double)delta_vel_rate.x,
 505                                            (double)delta_vel_rate.y,
 506                                            (double)height_estimate,
 507                                            (double)delta_height,
 508                                            (double)height_offset,
 509                                            (double)ins_height,
 510                                            (double)last_ins_height,
 511                                            dt_ms);
 512 #endif
 513
 514     gcs().send_named_float("HEST", height_estimate);
 515     delta_velocity_ne.zero();
 516     last_ins_height = ins_height;
 517 }
 518
 519 #endif // MODE_FLOWHOLD_ENABLED