From 01c48f6c7690ad968eaf05aecc31a909f3b2396a Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 26 Oct 2018 15:58:42 +0900 Subject: [PATCH] Copter: flowhold formatting fixes --- ArduCopter/mode_flowhold.cpp | 41 +++++++++++++++++------------------- 1 file changed, 19 insertions(+), 22 deletions(-) diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp index e45c8d6d37..791b74b532 100644 --- a/ArduCopter/mode_flowhold.cpp +++ b/ArduCopter/mode_flowhold.cpp @@ -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 -