Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-31 20:38:27 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

upgrades to optical flow landing module 2 (#2087)

Browse Source 
* fix cov method 2 array initialisation for optical flow landing
* add extra divide by 0 checks
This commit is contained in:
Kirk Scheper
2017-08-17 08:29:37 +02:00
committed by GitHub
parent bc246a26dc
commit a40b69fe2e
8 changed files with 622 additions and 608 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/firmwares/rotorcraft.makefile
+1 -1
View File
@@ -80,7 +80,7 @@ endif
# Math functions
#
ifneq ($(TARGET), fbw)
$(TARGET).srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c math/pprz_geodetic_double.c math/pprz_trig_int.c math/pprz_orientation_conversion.c math/pprz_algebra_int.c math/pprz_algebra_float.c math/pprz_algebra_double.c
$(TARGET).srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c math/pprz_geodetic_double.c math/pprz_trig_int.c math/pprz_orientation_conversion.c math/pprz_algebra_int.c math/pprz_algebra_float.c math/pprz_algebra_double.c math/pprz_stat.c
$(TARGET).srcs += subsystems/settings.c
$(TARGET).srcs += $(SRC_ARCH)/subsystems/settings_arch.c
conf/firmwares/subsystems/fixedwing/autopilot.makefile
+1 -1
View File
@@ -83,7 +83,7 @@ $(TARGET).srcs += $(SRC_FIXEDWING)/inter_mcu.c
# Math functions
#
ifneq ($(TARGET),fbw)
$(TARGET).srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c math/pprz_geodetic_double.c math/pprz_trig_int.c math/pprz_orientation_conversion.c math/pprz_algebra_int.c math/pprz_algebra_float.c math/pprz_algebra_double.c
$(TARGET).srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c math/pprz_geodetic_double.c math/pprz_trig_int.c math/pprz_orientation_conversion.c math/pprz_algebra_int.c math/pprz_algebra_float.c math/pprz_algebra_double.c math/pprz_stat.c
endif
#
conf/modules/optical_flow_landing.xml
+16 -13
View File
@@ -9,8 +9,8 @@
de Croon, G.C.H.E. (2016). Monocular distance estimation with optical flow maneuvers and efference copies: a stability-based strategy. Bioinspiration and Biomimetics, 11(1), 016004.
</description>
<define name="OPTICAL_FLOW_LANDING_AGL_ID" value="ABI_BROADCAST" description="Sender id of the AGL (sonar) ABI message"/>
<section name="OPTICAL_FLOW_LANDING" prefix="OPTICAL_FLOW_LANDING_">
<define name="OFL_AGL_ID" value="ABI_BROADCAST" description="Sender id of the AGL (sonar) ABI message"/>
<section name="OFL" prefix="OFL_">
<define name="PGAIN" value="0.50" description="P gain on height error"/>
<define name="IGAIN" value="0.10" description="I gain on summed height error"/>
<define name="DGAIN" value="0.0" description="D gain on summed height error"/>
@@ -18,29 +18,32 @@
<define name="CONTROL_METHOD" value="2" description="0 = fixed gain control, 1 = adaptive gain control, 2 = exponential control"/>
<define name="COV_METHOD" value="0" description="0 = cov(uz, div), 1 = cov(div_past, div)"/>
<define name="COV_WINDOW_SIZE" value="30" description="Number of time steps for window size for getting the covariance over time."/>
<define name="COV_LANDING_LIMIT" value="2.2" description="Covariance where the vehicle engages final landing procedure."/>
<define name="COV_SETPOINT" value="-0.0075" description="Target Covariance for adaptive gain increment."/>
<define name="LP_CONST" value="0.75" description="Low pass filter constant for divergence input."/>
<define name="ELC_OSCILLATE" value="true" description="Oscillate to find optimum gain before initiating landing."/>
</section>
</doc>
<settings>
<dl_settings>
<dl_settings NAME="OpticalFlowLanding">
<dl_setting var="of_landing_ctrl.nominal_thrust" min="0" step="0.001" max="1" module="ctrl/optical_flow_landing" shortname="nominal_thrust"/>
<dl_setting var="of_landing_ctrl.lp_factor" min="0" step="0.01" max="1" module="ctrl/optical_flow_landing" shortname="lp_factor"/>
<dl_setting var="of_landing_ctrl.lp_const" min="0.005" step="0.005" max="1" module="ctrl/optical_flow_landing" shortname="lp_factor" param="OFL_LP_CONST"/>
<dl_setting var="of_landing_ctrl.divergence_setpoint" min="-1" step="0.01" max="1" module="ctrl/optical_flow_landing" shortname="div sp"/>
<dl_setting var="of_landing_ctrl.cov_set_point" min="-0.50" step="0.0001" max="0.50" module="ctrl/optical_flow_landing" shortname="cov_set_point"/>
<dl_setting var="of_landing_ctrl.cov_limit" min="0" step="0.0001" max="3" module="ctrl/optical_flow_landing" shortname="cov_limit"/>
<dl_setting var="of_landing_ctrl.pgain" min="0" step="0.001" max="6" module="ctrl/optical_flow_landing" shortname="pgain" param="OPTICAL_FLOW_LANDING_PGAIN"/>
<dl_setting var="of_landing_ctrl.igain" min="0" step="0.001" max="1" module="ctrl/optical_flow_landing" shortname="igain" param="OPTICAL_FLOW_LANDING_IGAIN"/>
<dl_setting var="of_landing_ctrl.dgain" min="0" step="0.1" max="6" module="ctrl/optical_flow_landing" shortname="dgain" param="OPTICAL_FLOW_LANDING_DGAIN"/>
<dl_setting var="of_landing_ctrl.VISION_METHOD" min="0" step="1" max="1" module="ctrl/optical_flow_landing" shortname="VISION_METHOD" param="OPTICAL_FLOW_LANDING_VISION_METHOD"/>
<dl_setting var="of_landing_ctrl.CONTROL_METHOD" min="0" step="1" max="2" module="ctrl/optical_flow_landing" shortname="CONTROL_METHOD" param="OPTICAL_FLOW_LANDING_CONTROL_METHOD"/>
<dl_setting var="of_landing_ctrl.COV_METHOD" min="0" step="1" max="1" module="ctrl/optical_flow_landing" shortname="COV_METHOD" param="OPTICAL_FLOW_LANDING_COV_METHOD"/>
<dl_setting var="of_landing_ctrl.cov_set_point" min="-6" step="0.0001" max="6" module="ctrl/optical_flow_landing" shortname="cov_set_point" param="OFL_COV_SETPOINT"/>
<dl_setting var="of_landing_ctrl.cov_limit" min="0" step="0.0001" max="6" module="ctrl/optical_flow_landing" shortname="cov_limit" param="OFL_COV_LANDING_LIMIT"/>
<dl_setting var="of_landing_ctrl.pgain" min="0" step="0.001" max="6" module="ctrl/optical_flow_landing" shortname="pgain" param="OFL_PGAIN"/>
<dl_setting var="of_landing_ctrl.igain" min="0" step="0.001" max="1" module="ctrl/optical_flow_landing" shortname="igain" param="OFL_IGAIN"/>
<dl_setting var="of_landing_ctrl.dgain" min="0" step="0.1" max="6" module="ctrl/optical_flow_landing" shortname="dgain" param="OFL_DGAIN"/>
<dl_setting var="of_landing_ctrl.VISION_METHOD" min="0" step="1" max="1" values="Ground truth|Online" module="ctrl/optical_flow_landing" shortname="VISION_METHOD" param="OFL_VISION_METHOD"/>
<dl_setting var="of_landing_ctrl.CONTROL_METHOD" min="0" step="1" max="2" values="Simple|Adaptive|Exp" module="ctrl/optical_flow_landing" shortname="CONTROL_METHOD" param="OFL_CONTROL_METHOD"/>
<dl_setting var="of_landing_ctrl.COV_METHOD" min="0" step="1" max="1" values="Div-Thrust|Div-Shift div" module="ctrl/optical_flow_landing" shortname="COV_METHOD" param="OFL_COV_METHOD"/>
<dl_setting var="of_landing_ctrl.delay_steps" min="0" step="1" max="60" module="ctrl/optical_flow_landing" shortname="delay_steps"/>
<dl_setting var="of_landing_ctrl.window_size" min="5" step="5" max="100" module="ctrl/optical_flow_landing" shortname="window_size"/>
<dl_setting var="of_landing_ctrl.pgain_adaptive" min="0" step="0.1" max="20.0" module="ctrl/optical_flow_landing" shortname="pgain_adaptive"/>
<dl_setting var="of_landing_ctrl.igain_adaptive" min="0" step="0.01" max="1.0" module="ctrl/optical_flow_landing" shortname="igain_adaptive"/>
<dl_setting var="of_landing_ctrl.dgain_adaptive" min="0" step="0.01" max="5.0" module="ctrl/optical_flow_landing" shortname="dgain_adaptive"/>
<dl_setting var="of_landing_ctrl.reduction_factor_elc" min="0.1" step="0.01" max="3.0" module="ctrl/optical_flow_landing" shortname="reduction_factor_elc"/>
<dl_setting var="of_landing_ctrl.div_factor" min="-3.0" step="0.01" max="3.0" module="ctrl/optical_flow_landing" shortname="div_factor"/>
<dl_setting var="of_landing_ctrl.elc_oscillate" min="0" step="1" max="1" values="FALSE|TRUE" module="ctrl/optical_flow_landing" shortname="oscillate" param="OFL_ELC_OSCILLATE"/>
</dl_settings>
</dl_settings>
</settings>
sw/airborne/math/pprz_stat.c
+146
View File
@@ -0,0 +1,146 @@
/*
* Copyright (C) 2017 Kirk Scheper
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file pprz_stat.c
* @brief Statistics functions.
*
*/
#include "math/pprz_stat.h"
/*********
* Integer implementations
*********/
/** Compute the mean value of an array
* This is implemented using floats to handle scaling of all variables
* @param[in] *array The array
* @param[in] n_elements Number of elements in the array
* @return mean
*/
int32_t mean_i(int32_t *array, uint32_t n_elements)
{
// determine the mean for the vector:
float sum = 0.f;
uint32_t i;
for (i = 0; i < n_elements; i++) {
sum += (float)array[i];
}
return (int32_t)(sum / n_elements);
}
/** Compute the variance of an array of values (integer).
* The variance is a measure of how far a set of numbers is spread out
* V(X) = E[(X-E[X])^2] = E[X^2] - E[X]^2
* where E[X] is the expected value of X
* This is implemented using floats to handle scaling of all variables
* @param array pointer to an array of integer
* @param n_elements number of elements in the array
* @return variance
*/
int32_t variance_i(int32_t *array, uint32_t n_elements)
{
return (covariance_i(array, array, n_elements));
}
/** Compute the covariance of two arrays
* V(X) = E[(X-E[X])(Y-E[Y])] = E[XY] - E[X]E[Y]
* where E[X] is the expected value of X
* This is implemented using floats to handle scaling of all variables
* @param[in] *array1 The first array
* @param[in] *array2 The second array
* @param[in] n_elements Number of elements in the arrays
* @return covariance
*/
int32_t covariance_i(int32_t *array1, int32_t *array2, uint32_t n_elements)
{
// Determine means for each vector:
float sumX = 0.f, sumY = 0.f, sumXY = 0.f;
// Determine the covariance:
uint32_t i;
for (i = 0; i < n_elements; i++) {
sumX += (float)array1[i];
sumY += (float)array2[i];
sumXY += (float)(array1[i]) * (float)(array2[i]);
}
return (int32_t)(sumXY / n_elements - sumX * sumY / (n_elements * n_elements));
}
/*********
* Float implementations
*********/
/** Compute the mean value of an array (float)
* @param[in] *array The array
* @param[in] n_elements Number of elements in the array
* @return mean
*/
float mean_f(float *array, uint32_t n_elements)
{
// determine the mean for the vector:
float sum = 0.f;
uint32_t i;
for (i = 0; i < n_elements; i++) {
sum += array[i];
}
return (sum / n_elements);
}
/** Compute the variance of an array of values (float).
* The variance is a measure of how far a set of numbers is spread out
* V(X) = E[(X-E[X])^2] = E[X^2] - E[X]^2
* where E[X] is the expected value of X
* @param array Pointer to an array of float
* @param n_elements Number of values in the array
* @return variance
*/
float variance_f(float *array, uint32_t n_elements)
{
return covariance_f(array, array, n_elements);
}
/** Compute the covariance of two arrays
* V(X) = E[(X-E[X])(Y-E[Y])] = E[XY] - E[X]E[Y]
* where E[X] is the expected value of X
* @param[in] *array1 The first array
* @param[in] *array2 The second array
* @param[in] n_elements Number of elements in the arrays
* @return covariance
*/
float covariance_f(float *arr1, float *arr2, uint32_t n_elements)
{
// Determine means for each vector:
float sumX = 0.f, sumY = 0.f, sumXY = 0.f;
// Determine the covariance:
uint32_t i;
for (i = 0; i < n_elements; i++) {
sumX += arr1[i];
sumY += arr2[i];
sumXY += arr1[i] * arr2[i];
}
return (sumXY / n_elements - sumX * sumY / (n_elements * n_elements));
}
sw/airborne/math/pprz_stat.h
+49 -34
View File
@@ -1,5 +1,5 @@
/*
* Copyright (C) 2012 Gautier Hattenberger (ENAC)
* Copyright (C) 2012 Gautier Hattenberger (ENAC), 2017 Kirk Scheper
*
* This file is part of paparazzi.
*
@@ -20,7 +20,7 @@
/**
* @file pprz_stat.h
* @brief Statistics functions like variance.
* @brief Statistics functions.
*
*/
@@ -33,47 +33,62 @@ extern "C" {
#include "std.h"
/** Compute the variance of an array of values (float).
* The variance is a measure of how far a set of numbers is spread out
* V(X) = E[(X-E[X])^2] = E[X^2] - E[X]^2
* where E[X] is the expected value of X
*
* @param array pointer to an array of float
* @param nb numbre of values in the array, must be >0
* @return variance
/** Compute the mean value of an array
* This is implemented using floats to handle scaling of all variables
* @param[in] *array The array
* @param[in] n_elements Number of elements in the array
* @return mean
*/
static inline float variance_float(float *array, int nb)
{
float me = 0.;
float see = 0.;
for (int i = 0; i < nb; i++) {
me += array[i];
see += array[i] * array[i];
}
me /= nb;
return (see / nb - me * me);
}
extern int32_t mean_i(int32_t *array, uint32_t n_elements);
/** Compute the variance of an array of values (integer).
* The variance is a measure of how far a set of numbers is spread out
* V(X) = E[(X-E[X])^2] = E[X^2] - E[X]^2
* where E[X] is the expected value of X
*
* This is implemented using floats to handle scaling of all variables
* @param array pointer to an array of integer
* @param nb numbre of values in the array, must be >0
* @param n_elements number of elements in the array
* @return variance
*/
static inline int32_t variance_int(int32_t *array, int nb)
{
float me = 0;
float see = 0;
for (int i = 0; i < nb; i++) {
me += (float)array[i];
see += (float)(array[i] * array[i]);
}
me /= nb;
return (see / nb - me * me);
}
extern int32_t variance_i(int32_t *array, uint32_t n_elements);
/** Compute the covariance of two arrays
* V(X) = E[(X-E[X])(Y-E[Y])] = E[XY] - E[X]E[Y]
* where E[X] is the expected value of X
* This is implemented using floats to handle scaling of all variables
* @param[in] *array1 The first array
* @param[in] *array2 The second array
* @param[in] n_elements Number of elements in the arrays
* @return covariance
*/
extern int32_t covariance_i(int32_t *array1, int32_t *array2, uint32_t n_elements);
/** Compute the mean value of an array (float)
* @param[in] *array The array
* @param[in] n_elements Number of elements in the array
* @return mean
*/
extern float mean_f(float *arr, uint32_t n_elements);
/** Compute the variance of an array of values (float).
* The variance is a measure of how far a set of numbers is spread out
* V(X) = E[(X-E[X])^2] = E[X^2] - E[X]^2
* where E[X] is the expected value of X
* @param array Pointer to an array of float
* @param n_elements Number of values in the array
* @return variance
*/
extern float variance_f(float *array, uint32_t n_elements);
/** Compute the covariance of two arrays
* V(X) = E[(X-E[X])(Y-E[Y])] = E[XY] - E[X]E[Y]
* where E[X] is the expected value of X
* @param[in] *array1 The first array
* @param[in] *array2 The second array
* @param[in] n_elements Number of elements in the arrays
* @return covariance
*/
extern float covariance_f(float *array1, float *array2, uint32_t n_elements);
#ifdef __cplusplus
} /* extern "C" */
sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c
+19 -19
View File
@@ -253,7 +253,7 @@ void opticflow_calc_init(struct opticflow_t *opticflow)
* @param[in] *img The image frame to calculate the optical flow from
* @param[out] *result The optical flow result
*/
void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_state_t *cam_state, struct image_t *img,
struct opticflow_result_t *result)
{
if (opticflow->just_switched_method) {
@@ -449,13 +449,13 @@ void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_sta
float diff_flow_y = 0;
/*// Flow Derotation TODO:
float diff_flow_x = (state->phi - opticflow->prev_phi) * img->w / OPTICFLOW_FOV_W;
float diff_flow_y = (state->theta - opticflow->prev_theta) * img->h / OPTICFLOW_FOV_H;*/
float diff_flow_x = (cam_state->phi - opticflow->prev_phi) * img->w / OPTICFLOW_FOV_W;
float diff_flow_y = (cam_state->theta - opticflow->prev_theta) * img->h / OPTICFLOW_FOV_H;*/
if (opticflow->derotation && result->tracked_cnt > 5) {
diff_flow_x = (state->rates.p) / result->fps * img->w /
diff_flow_x = (cam_state->rates.p) / result->fps * img->w /
OPTICFLOW_FOV_W;// * img->w / OPTICFLOW_FOV_W;
diff_flow_y = (state->rates.q) / result->fps * img->h /
diff_flow_y = (cam_state->rates.q) / result->fps * img->h /
OPTICFLOW_FOV_H;// * img->h / OPTICFLOW_FOV_H;
}
@@ -463,13 +463,13 @@ void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_sta
opticflow->derotation_correction_factor_x;
result->flow_der_y = result->flow_y - diff_flow_y * opticflow->subpixel_factor *
opticflow->derotation_correction_factor_y;
opticflow->prev_rates = state->rates;
opticflow->prev_rates = cam_state->rates;
// Velocity calculation
// Right now this formula is under assumption that the flow only exist in the center axis of the camera.
// TODO Calculate the velocity more sophisticated, taking into account the drone's angle and the slope of the ground plane.
float vel_x = result->flow_der_x * result->fps * state->agl / opticflow->subpixel_factor / OPTICFLOW_FX;
float vel_y = result->flow_der_y * result->fps * state->agl / opticflow->subpixel_factor / OPTICFLOW_FY;
float vel_x = result->flow_der_x * result->fps * cam_state->agl / opticflow->subpixel_factor / OPTICFLOW_FX;
float vel_y = result->flow_der_y * result->fps * cam_state->agl / opticflow->subpixel_factor / OPTICFLOW_FY;
//Apply a median filter to the velocity if wanted
if (opticflow->median_filter == true) {
@@ -480,8 +480,8 @@ void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_sta
result->vel_y = vel_y;
}
// Velocity calculation: uncomment if focal length of the camera is not known or incorrect.
// result->vel_x = - result->flow_der_x * result->fps * state->agl / opticflow->subpixel_factor * OPTICFLOW_FOV_W / img->w
// result->vel_y = result->flow_der_y * result->fps * state->agl / opticflow->subpixel_factor * OPTICFLOW_FOV_H / img->h
// result->vel_x = - result->flow_der_x * result->fps * cam_state->agl / opticflow->subpixel_factor * OPTICFLOW_FOV_W / img->w
// result->vel_y = result->flow_der_y * result->fps * cam_state->agl / opticflow->subpixel_factor * OPTICFLOW_FOV_H / img->h
// Determine quality of noise measurement for state filter
@@ -516,7 +516,7 @@ void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_sta
* @param[in] *img The image frame to calculate the optical flow from
* @param[out] *result The optical flow result
*/
void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t *cam_state, struct image_t *img,
struct opticflow_result_t *result)
{
// Define Static Variables
@@ -569,7 +569,7 @@ void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t *
// Copy frame time and angles of image to calculated edge histogram
edge_hist[current_frame_nr].frame_time = img->ts;
edge_hist[current_frame_nr].rates = state->rates;
edge_hist[current_frame_nr].rates = cam_state->rates;
// Calculate which previous edge_hist to compare with the current
uint8_t previous_frame_nr[2];
@@ -651,8 +651,8 @@ void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t *
result->fps = fps_x;
// Calculate velocity
float vel_x = edgeflow.flow_x * fps_x * state->agl * OPTICFLOW_FOV_W / (img->w * RES);
float vel_y = edgeflow.flow_y * fps_y * state->agl * OPTICFLOW_FOV_H / (img->h * RES);
float vel_x = edgeflow.flow_x * fps_x * cam_state->agl * OPTICFLOW_FOV_W / (img->w * RES);
float vel_y = edgeflow.flow_y * fps_y * cam_state->agl * OPTICFLOW_FOV_H / (img->h * RES);
//Apply a median filter to the velocity if wanted
if (opticflow->median_filter == true) {
@@ -686,7 +686,7 @@ void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t *
* @param[in] *img The image frame to calculate the optical flow from
* @param[out] *result The optical flow result
*/
void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *cam_state, struct image_t *img,
struct opticflow_result_t *result)
{
@@ -704,9 +704,9 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_
// Switch between methods (0 = fast9/lukas-kanade, 1 = EdgeFlow)
if (opticflow->method == 0) {
calc_fast9_lukas_kanade(opticflow, state, img, result);
calc_fast9_lukas_kanade(opticflow, cam_state, img, result);
} else if (opticflow->method == 1) {
calc_edgeflow_tot(opticflow, state, img, result);
calc_edgeflow_tot(opticflow, cam_state, img, result);
}
/* Rotate velocities from camera frame coordinates to body coordinates for control
@@ -736,9 +736,9 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_
// Get accelerometer values rotated to body axis
// TODO: use acceleration from the state ?
struct FloatVect3 accel_imu_f;
ACCELS_FLOAT_OF_BFP(accel_imu_f, state->accel_imu_meas);
ACCELS_FLOAT_OF_BFP(accel_imu_f, cam_state->accel_imu_meas);
struct FloatVect3 accel_meas_body;
float_quat_vmult(&accel_meas_body, &state->imu_to_body_quat, &accel_imu_f);
float_quat_vmult(&accel_meas_body, &cam_state->imu_to_body_quat, &accel_imu_f);
float acceleration_measurement[2];
acceleration_measurement[0] = accel_meas_body.x;
sw/airborne/modules/ctrl/optical_flow_landing.c
+380 -532
View File
File diff suppressed because it is too large Load Diff
sw/airborne/modules/ctrl/optical_flow_landing.h
+10 -8
View File
@@ -49,30 +49,32 @@
struct OpticalFlowLanding {
float agl; ///< agl = height from sonar (only used when using "fake" divergence)
float agl_lp; ///< low-pass version of agl
float lp_factor; ///< low-pass factor in [0,1], with 0 purely using the current measurement
float lp_const; ///< low-pass filter constant
float vel; ///< vertical velocity as determined with sonar (only used when using "fake" divergence)
float divergence_setpoint; ///< setpoint for constant divergence approach
float pgain; ///< P-gain for constant divergence control (from divergence error to thrust)
float igain; ///< I-gain for constant divergence control
float dgain; ///< D-gain for constant divergence control
float divergence; ///< Divergence estimate
float previous_err; ///< Previous divergence tracking error
float sum_err; ///< integration of the error for I-gain
float d_err; ///< difference of error for the D-gain
float nominal_thrust; ///< nominal thrust around which the PID-control operates
int VISION_METHOD; ///< whether to use vision (1) or Optitrack / sonar (0)
int CONTROL_METHOD; ///< type of divergence control: 0 = fixed gain, 1 = adaptive gain
uint32_t VISION_METHOD; ///< whether to use vision (1) or Optitrack / sonar (0)
uint32_t CONTROL_METHOD; ///< type of divergence control: 0 = fixed gain, 1 = adaptive gain
float cov_set_point; ///< for adaptive gain control, setpoint of the covariance (oscillations)
float cov_limit; ///< for fixed gain control, what is the cov limit triggering the landing
float pgain_adaptive; ///< P-gain for adaptive gain control
float igain_adaptive; ///< I-gain for adaptive gain control
float dgain_adaptive; ///< D-gain for adaptive gain control
int COV_METHOD; ///< method to calculate the covariance: between thrust and div (0) or div and div past (1)
int delay_steps; ///< number of delay steps for div past
int window_size; ///< number of time steps in "window" used for getting the covariance
uint32_t COV_METHOD; ///< method to calculate the covariance: between thrust and div (0) or div and div past (1)
uint32_t delay_steps; ///< number of delay steps for div past
uint32_t window_size; ///< number of time steps in "window" used for getting the covariance
float reduction_factor_elc; ///< reduction factor - after oscillation, how much to reduce the gain...
float lp_cov_div_factor; ///< low-pass factor for the covariance of divergence in order to trigger the second landing phase in the exponential strategy.
int count_transition; ///< how many time steps the drone has to be oscillating in order to transition to the hover phase with reduced gain
float t_transition; ///< how many seconds the drone has to be oscillating in order to transition to the hover phase with reduced gain
float p_land_threshold; ///< if during the exponential landing the gain reaches this value, the final landing procedure is triggered
float div_factor; ///< Number that transforms the divergence in pixels per frame from the camera to (1 / frame) - taking into account field of view, etc.
bool elc_oscillate; ///< Whether or not to oscillate at beginning of elc to find optimum gain
};
extern struct OpticalFlowLanding of_landing_ctrl;