zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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
master
Randy Mackay 7 years ago committed by Andrew Tridgell
parent
10f940c8d0
commit
4319e37f0c
2 changed files with 109 additions and 110 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 169
      ArduCopter/mode.h
  2. 50
      ArduCopter/mode_flowhold.cpp

169
ArduCopter/mode.h
Unescape View File

@ -581,6 +581,90 @@ private: @@ -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: @@ -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

50
ArduCopter/mode_flowhold.cpp
Unescape View File

@ -10,22 +10,22 @@ @@ -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[] = { @@ -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 @@ -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 @@ -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) @@ -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);

Write Preview
Loading…
Cancel
Save