|
|
|
@ -4,7 +4,7 @@
@@ -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 |
|
|
|
|
*/ |
|
|
|
|
|
|
|
|
@ -190,7 +190,6 @@ void Copter::ModeFlowHold::flowhold_flow_to_angle(Vector2f &bf_angles, bool stic
@@ -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; |
|
|
|
|
|
|
|
|
@ -366,7 +365,6 @@ void Copter::ModeFlowHold::run()
@@ -366,7 +365,6 @@ void Copter::ModeFlowHold::run()
|
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|
update height estimate using integrated accelerometer ratio with optical flow |
|
|
|
|
*/ |
|
|
|
@ -458,7 +456,7 @@ void Copter::ModeFlowHold::update_height_estimate(void)
@@ -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++) { |
|
|
|
@ -520,4 +518,3 @@ void Copter::ModeFlowHold::update_height_estimate(void)
@@ -520,4 +518,3 @@ void Copter::ModeFlowHold::update_height_estimate(void)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
#endif // OPTFLOW == ENABLED
|
|
|
|
|
|
|
|
|
|