Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 08:13:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: flowhold formatting fixes

Browse Source
This commit is contained in:
Randy Mackay 2018-10-26 15:58:42 +09:00
parent 4eee1fc06a
commit 01c48f6c76
1 changed files with 19 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
ArduCopter/mode_flowhold.cpp
View File

@ -4,7 +4,7 @@
#if !HAL_MINIMIZE_FEATURES && OPTFLOW == ENABLED
/*
implement FLOWHOLD mode, for position hold using opttical flow
implement FLOWHOLD mode, for position hold using optical flow
without rangefinder
*/
@ -63,7 +63,7 @@ const AP_Param::GroupInfo Copter::ModeFlowHold::var_info[] = {
// @User: Standard
// @Units: deg/s
AP_GROUPINFO("_BRAKE_RATE", 6, Copter::ModeFlowHold, brake_rate_dps, 8),
AP_GROUPEND
};
@ -109,7 +109,7 @@ bool Copter::ModeFlowHold::init(bool ignore_checks)
// start with INS height
last_ins_height = copter.inertial_nav.get_altitude() * 0.01;
height_offset = 0;
return true;
}
@ -119,7 +119,7 @@ bool Copter::ModeFlowHold::init(bool ignore_checks)
void Copter::ModeFlowHold::flowhold_flow_to_angle(Vector2f &bf_angles, bool stick_input)
{
uint32_t now = AP_HAL::millis();
// get corrected raw flow rate
Vector2f raw_flow = copter.optflow.flowRate() - copter.optflow.bodyRate();
@ -163,7 +163,7 @@ void Copter::ModeFlowHold::flowhold_flow_to_angle(Vector2f &bf_angles, bool stic
#endif
}
}
if (!stick_input && !braking) {
// get I term
if (limited) {
@ -190,7 +190,6 @@ void Copter::ModeFlowHold::flowhold_flow_to_angle(Vector2f &bf_angles, bool stic
ef_output.zero();
}
ef_output += xy_I;
ef_output *= copter.aparm.angle_max;
@ -222,7 +221,7 @@ void Copter::ModeFlowHold::run()
float takeoff_climb_rate = 0.0f;
update_height_estimate();
// initialize vertical speeds and acceleration
copter.pos_control->set_speed_z(-copter.g2.pilot_speed_dn, copter.g.pilot_speed_up);
copter.pos_control->set_accel_z(copter.g.pilot_accel_z);
@ -241,7 +240,7 @@ void Copter::ModeFlowHold::run()
// get pilot's desired yaw rate
float target_yaw_rate = copter.get_pilot_desired_yaw_rate(copter.channel_yaw->get_control_in());
if (!copter.motors->armed() || !copter.motors->get_interlock()) {
flowhold_state = FlowHold_MotorStopped;
} else if (takeoff.running() || takeoff.triggered(target_climb_rate)) {
@ -258,7 +257,7 @@ void Copter::ModeFlowHold::run()
} else {
quality_filtered = 0;
}
Vector2f bf_angles;
// calculate alt-hold angles
@ -266,7 +265,7 @@ void Copter::ModeFlowHold::run()
int16_t pitch_in = copter.channel_pitch->get_control_in();
float angle_max = copter.attitude_control->get_althold_lean_angle_max();
get_pilot_desired_lean_angles(bf_angles.x, bf_angles.y,angle_max, attitude_control->get_althold_lean_angle_max());
if (quality_filtered >= flow_min_quality &&
AP_HAL::millis() - copter.arm_time_ms > 3000) {
// don't use for first 3s when we are just taking off
@ -279,7 +278,7 @@ void Copter::ModeFlowHold::run()
}
bf_angles.x = constrain_float(bf_angles.x, -angle_max, angle_max);
bf_angles.y = constrain_float(bf_angles.y, -angle_max, angle_max);
// Flow Hold State Machine
switch (flowhold_state) {
@ -366,7 +365,6 @@ void Copter::ModeFlowHold::run()
}
}
/*
update height estimate using integrated accelerometer ratio with optical flow
*/
@ -384,7 +382,7 @@ void Copter::ModeFlowHold::update_height_estimate(void)
return;
}
#endif
// get delta velocity in body frame
Vector3f delta_vel;
if (!copter.ins.get_delta_velocity(delta_vel)) {
@ -396,7 +394,7 @@ void Copter::ModeFlowHold::update_height_estimate(void)
delta_vel = rotMat * delta_vel;
delta_velocity_ne.x += delta_vel.x;
delta_velocity_ne.y += delta_vel.y;
if (!copter.optflow.healthy()) {
// can't update height model with no flow sensor
last_flow_ms = AP_HAL::millis();
@ -411,7 +409,7 @@ void Copter::ModeFlowHold::update_height_estimate(void)
height_offset = 0;
return;
}
if (copter.optflow.last_update() == last_flow_ms) {
// no new flow data
return;
@ -422,7 +420,7 @@ void Copter::ModeFlowHold::update_height_estimate(void)
// and convert to an rate equivalent, to be comparable to flow
Vector2f delta_vel_rate(-delta_vel_bf.y, delta_vel_bf.x);
// get body flow rate in radians per second
Vector2f flow_rate_rps = copter.optflow.flowRate() - copter.optflow.bodyRate();
@ -441,7 +439,7 @@ void Copter::ModeFlowHold::update_height_estimate(void)
basic equation is:
height_m = delta_velocity_mps / delta_flowrate_rps;
*/
// get delta_flowrate_rps
Vector2f delta_flowrate = flow_rate_rps - last_flow_rate_rps;
last_flow_rate_rps = flow_rate_rps;
@ -458,7 +456,7 @@ void Copter::ModeFlowHold::update_height_estimate(void)
for each axis update the height estimate
*/
float delta_height = 0;
uint8_t total_weight=0;
uint8_t total_weight = 0;
float height_estimate = ins_height + height_offset;
for (uint8_t i=0; i<2; i++) {
@ -480,7 +478,7 @@ void Copter::ModeFlowHold::update_height_estimate(void)
if (total_weight > 0) {
delta_height /= total_weight;
}
if (delta_height < 0) {
// bias towards lower heights, as we'd rather have too low
// gain than have oscillation. This also compensates a bit for
@ -493,7 +491,7 @@ void Copter::ModeFlowHold::update_height_estimate(void)
// apply a simple filter
height_offset = 0.8 * height_offset + 0.2 * new_offset;
if (ins_height + height_offset < height_min) {
// height estimate is never allowed below the minimum
height_offset = height_min - ins_height;
@ -501,7 +499,7 @@ void Copter::ModeFlowHold::update_height_estimate(void)
// new height estimate for logging
height_estimate = ins_height + height_offset;
DataFlash_Class::instance()->Log_Write("FXY", "TimeUS,DFx,DFy,DVx,DVy,Hest,DH,Hofs,InsH,LastInsH,DTms", "QfffffffffI",
AP_HAL::micros64(),
(double)delta_flowrate.x,
@ -520,4 +518,3 @@ void Copter::ModeFlowHold::update_height_estimate(void)
}
#endif // OPTFLOW == ENABLED