From 4319e37f0c0c4924dbc25539f25f1be800d72470 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 8 Feb 2018 21:21:10 +0900 Subject: [PATCH] Copter: non-functional FlowHold changes class declaration moved to alphabetical position in mode.h parameter descriptions include FlowHow at beginning to help distinguished from optical flow parameters resolved compiler warnings --- ArduCopter/mode.h | 169 +++++++++++++++++------------------ ArduCopter/mode_flowhold.cpp | 50 +++++------ 2 files changed, 109 insertions(+), 110 deletions(-) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 3e4ae835c8..73238572cf 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -581,6 +581,90 @@ private: }; +#if OPTFLOW == ENABLED +/* + class to support FLOWHOLD mode, which is a position hold mode using + optical flow directly, avoiding the need for a rangefinder + */ + +class ModeFlowHold : public Mode { +public: + // need a constructor for parameters + ModeFlowHold(void); + + bool init(bool ignore_checks) override; + void run(void) override; + + bool requires_GPS() const override { return false; } + bool has_manual_throttle() const override { return false; } + bool allows_arming(bool from_gcs) const override { return true; }; + bool is_autopilot() const override { return false; } + + static const struct AP_Param::GroupInfo var_info[]; + +protected: + const char *name() const override { return "FLOWHOLD"; } + const char *name4() const override { return "FHLD"; } + +private: + + // FlowHold states + enum FlowHoldModeState { + FlowHold_MotorStopped, + FlowHold_Takeoff, + FlowHold_Flying, + FlowHold_Landed + }; + + // calculate attitude from flow data + void flow_to_angle(Vector2f &bf_angle); + + LowPassFilterVector2f flow_filter; + + bool flowhold_init(bool ignore_checks); + void flowhold_run(); + void flowhold_flow_to_angle(Vector2f &angle, bool stick_input); + void update_height_estimate(void); + + // minimum assumed height + const float height_min = 0.1; + + // maximum scaling height + const float height_max = 3.0; + + AP_Float flow_max; + AC_PI_2D flow_pi_xy{0.2, 0.3, 3000, 5, 0.0025}; + AP_Float flow_filter_hz; + AP_Int8 flow_min_quality; + AP_Int8 brake_rate_dps; + + float quality_filtered; + + uint8_t log_counter; + bool limited; + Vector2f xy_I; + + // accumulated INS delta velocity in north-east form since last flow update + Vector2f delta_velocity_ne; + + // last flow rate in radians/sec in north-east axis + Vector2f last_flow_rate_rps; + + // timestamp of last flow data + uint32_t last_flow_ms; + + float last_ins_height; + float height_offset; + + // are we braking after pilot input? + bool braking; + + // last time there was significant stick input + uint32_t last_stick_input_ms; +}; +#endif // OPTFLOW + + class ModeGuided : public Mode { public: @@ -1000,88 +1084,3 @@ protected: private: }; - - -#if OPTFLOW == ENABLED -/* - class to support FLOWHOLD mode, which is a position hold mode using - optical flow directly, avoiding the need for a rangefinder - */ - -class ModeFlowHold : public Mode { -public: - // need a constructor for parameters - ModeFlowHold(void); - - bool init(bool ignore_checks) override; - void run(void) override; - - bool requires_GPS() const override { return false; } - bool has_manual_throttle() const override { return false; } - bool allows_arming(bool from_gcs) const override { return true; }; - bool is_autopilot() const override { return false; } - - static const struct AP_Param::GroupInfo var_info[]; - -protected: - const char *name() const override { return "FLOWHOLD"; } - const char *name4() const override { return "FHLD"; } - -private: - - // FlowHold states - enum FlowHoldModeState { - FlowHold_MotorStopped, - FlowHold_Takeoff, - FlowHold_Flying, - FlowHold_Landed - }; - - // calculate attitude from flow data - void flow_to_angle(Vector2f &bf_angle); - - LowPassFilterVector2f flow_filter; - - bool flowhold_init(bool ignore_checks); - void flowhold_run(); - void flowhold_flow_to_angle(Vector2f &angle, bool stick_input); - void update_height_estimate(void); - - // minimum assumed height - const float height_min = 0.1; - - // maximum scaling height - const float height_max = 3.0; - - AP_Float flow_max; - AC_PI_2D flow_pi_xy{0.2, 0.3, 3000, 5, 0.0025}; - AP_Float flow_filter_hz; - AP_Int8 flow_min_quality; - AP_Int8 brake_rate_dps; - - float quality_filtered; - - uint8_t log_counter; - bool limited; - Vector2f xy_I; - - // accumulated INS delta velocity in north-east form since last flow update - Vector2f delta_velocity_ne; - - // last flow rate in radians/sec in north-east axis - Vector2f last_flow_rate_rps; - - // timestamp of last flow data - uint32_t last_flow_ms; - - float last_ins_height; - float height_offset; - - // are we braking after pilot input? - bool braking; - - // last time there was significant stick input - uint32_t last_stick_input_ms; -}; -#endif // OPTFLOW - diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp index 1bf1ae86c3..95b7888cf4 100644 --- a/ArduCopter/mode_flowhold.cpp +++ b/ArduCopter/mode_flowhold.cpp @@ -10,22 +10,22 @@ const AP_Param::GroupInfo Copter::ModeFlowHold::var_info[] = { // @Param: _XY_P - // @DisplayName: Flow (horizontal) P gain - // @Description: Flow (horizontal) P gain. + // @DisplayName: FlowHold P gain + // @Description: FlowHold (horizontal) P gain. // @Range: 0.1 6.0 // @Increment: 0.1 // @User: Advanced // @Param: _XY_I - // @DisplayName: Flow I gain - // @Description: Flow I gain + // @DisplayName: FlowHold I gain + // @Description: FlowHold (horizontal) I gain // @Range: 0.02 1.00 // @Increment: 0.01 // @User: Advanced // @Param: _XY_IMAX - // @DisplayName: Flow integrator maximum - // @Description: Flow integrator maximum + // @DisplayName: FlowHold Integrator Max + // @Description: FlowHold (horizontal) integrator maximum // @Range: 0 4500 // @Increment: 10 // @Units: cdeg @@ -33,30 +33,30 @@ const AP_Param::GroupInfo Copter::ModeFlowHold::var_info[] = { AP_SUBGROUPINFO(flow_pi_xy, "_XY_", 1, Copter::ModeFlowHold, AC_PI_2D), // @Param: _FLOW_MAX - // @DisplayName: Flow Max + // @DisplayName: FlowHold Flow Rate Max // @Description: Controls maximum apparent flow rate in flowhold // @Range: 0.1 2.5 // @User: Standard AP_GROUPINFO("_FLOW_MAX", 2, Copter::ModeFlowHold, flow_max, 0.6), // @Param: _FILT_HZ - // @DisplayName: Flow Filter Frequency + // @DisplayName: FlowHold Filter Frequency // @Description: Filter frequency for flow data // @Range: 1 100 // @User: Standard AP_GROUPINFO("_FILT_HZ", 3, Copter::ModeFlowHold, flow_filter_hz, 5), // @Param: _QUAL_MIN - // @DisplayName: Minimum flow quality + // @DisplayName: FlowHold Flow quality minimum // @Description: Minimum flow quality to use flow position hold // @Range: 0 255 // @User: Standard AP_GROUPINFO("_QUAL_MIN", 4, Copter::ModeFlowHold, flow_min_quality, 10), // 5 was FLOW_SPEED - + // @Param: _BRAKE_RATE - // @DisplayName: Braking rate + // @DisplayName: FlowHold Braking rate // @Description: Controls deceleration rate on stick release // @Range: 1 30 // @User: Standard @@ -161,7 +161,7 @@ void Copter::ModeFlowHold::flowhold_flow_to_angle(Vector2f &bf_angles, bool stic braking = false; #if 0 printf("braking done at %u vel=%f\n", now - last_stick_input_ms, - sensor_flow.length()); + (double)sensor_flow.length()); #endif } } @@ -209,10 +209,10 @@ void Copter::ModeFlowHold::flowhold_flow_to_angle(Vector2f &bf_angles, bool stic if (log_counter++ % 20 == 0) { DataFlash_Class::instance()->Log_Write("FHLD", "TimeUS,SFx,SFy,Ax,Ay,Qual,Ix,Iy", "Qfffffffff", AP_HAL::micros64(), - sensor_flow.x, sensor_flow.y, - bf_angles.x, bf_angles.y, - quality_filtered, - xy_I.x, xy_I.y); + (double)sensor_flow.x, (double)sensor_flow.y, + (double)bf_angles.x, (double)bf_angles.y, + (double)quality_filtered, + (double)xy_I.x, (double)xy_I.y); } } @@ -511,15 +511,15 @@ void Copter::ModeFlowHold::update_height_estimate(void) DataFlash_Class::instance()->Log_Write("FXY", "TimeUS,DFx,DFy,DVx,DVy,Hest,DH,Hofs,InsH,LastInsH,DTms", "QfffffffffI", AP_HAL::micros64(), - delta_flowrate.x, - delta_flowrate.y, - delta_vel_rate.x, - delta_vel_rate.y, - height_estimate, - delta_height, - height_offset, - ins_height, - last_ins_height, + (double)delta_flowrate.x, + (double)delta_flowrate.y, + (double)delta_vel_rate.x, + (double)delta_vel_rate.y, + (double)height_estimate, + (double)delta_height, + (double)height_offset, + (double)ins_height, + (double)last_ins_height, dt_ms); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, AP_HAL::millis(), "HEST", height_estimate); mavlink_msg_named_value_float_send(MAVLINK_COMM_1, AP_HAL::millis(), "HEST", height_estimate);