diff --git a/conf/abi.xml b/conf/abi.xml
index 1e451f9dd6..ac16c25163 100644
--- a/conf/abi.xml
+++ b/conf/abi.xml
@@ -70,6 +70,7 @@
+
diff --git a/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h b/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h
index bd3dafcd22..c6f034c684 100644
--- a/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h
+++ b/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h
@@ -47,6 +47,8 @@ struct opticflow_result_t {
float surface_roughness; ///< Surface roughness as determined with a linear optical flow fit
float divergence; ///< Divergence as determined with a linear flow fit
+
+ float noise_measurement; ///< noise of measurement, for state filter
};
/* The state of the drone when it took an image */
diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c
index 20bc64f3c2..29cfbf4a1b 100644
--- a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c
+++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c
@@ -152,7 +152,8 @@ void opticflow_calc_init(struct opticflow_t *opticflow, uint16_t w, uint16_t h)
* @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, struct opticflow_result_t *result)
+void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
+ struct opticflow_result_t *result)
{
// variables for size_divergence:
float size_divergence; int n_samples;
@@ -230,7 +231,8 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_
error_threshold = 10.0f;
n_iterations_RANSAC = 20;
n_samples_RANSAC = 5;
- success_fit = analyze_linear_flow_field(vectors, result->tracked_cnt, error_threshold, n_iterations_RANSAC, n_samples_RANSAC, img->w, img->h, &fit_info);
+ success_fit = analyze_linear_flow_field(vectors, result->tracked_cnt, error_threshold, n_iterations_RANSAC,
+ n_samples_RANSAC, img->w, img->h, &fit_info);
if (!success_fit) {
fit_info.divergence = 0.0f;
@@ -279,6 +281,14 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_
result->vel_x = -result->flow_der_x * result->fps * state->agl / opticflow->subpixel_factor * img->w / OPTICFLOW_FX;
result->vel_y = result->flow_der_y * result->fps * state->agl / opticflow->subpixel_factor * img->h / OPTICFLOW_FY;
+ // Determine quality of noise measurement for state filter
+ //TODO Experiment with multiple noise measurement models
+ if (result->tracked_cnt < 15) {
+ result->noise_measurement = (float)result->tracked_cnt / (float)opticflow->max_track_corners;
+ } else {
+ result->noise_measurement = 1.0;
+ }
+
// *************************************************************************************
// Next Loop Preparation
// *************************************************************************************
@@ -312,7 +322,8 @@ static int cmp_flow(const void *a, const void *b)
{
const struct flow_t *a_p = (const struct flow_t *)a;
const struct flow_t *b_p = (const struct flow_t *)b;
- return (a_p->flow_x * a_p->flow_x + a_p->flow_y * a_p->flow_y) - (b_p->flow_x * b_p->flow_x + b_p->flow_y * b_p->flow_y);
+ return (a_p->flow_x * a_p->flow_x + a_p->flow_y * a_p->flow_y) - (b_p->flow_x * b_p->flow_x + b_p->flow_y *
+ b_p->flow_y);
}
diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c
index 269cc81723..cc5f374bcc 100644
--- a/sw/airborne/modules/computer_vision/opticflow_module.c
+++ b/sw/airborne/modules/computer_vision/opticflow_module.c
@@ -131,7 +131,8 @@ void opticflow_module_init(void)
#endif
/* Try to initialize the video device */
- opticflow_dev = v4l2_init(STRINGIFY(OPTICFLOW_DEVICE), OPTICFLOW_DEVICE_SIZE, OPTICFLOW_DEVICE_BUFFERS, V4L2_PIX_FMT_UYVY);
+ opticflow_dev = v4l2_init(STRINGIFY(OPTICFLOW_DEVICE), OPTICFLOW_DEVICE_SIZE, OPTICFLOW_DEVICE_BUFFERS,
+ V4L2_PIX_FMT_UYVY);
if (opticflow_dev == NULL) {
printf("[opticflow_module] Could not initialize the video device\n");
}
@@ -168,7 +169,9 @@ void opticflow_module_run(void)
AbiSendMsgVELOCITY_ESTIMATE(OPTICFLOW_SENDER_ID, now_ts,
opticflow_result.vel_x,
opticflow_result.vel_y,
- opticflow_result.tracked_cnt);
+ 0.0f,
+ opticflow_result.noise_measurement
+ );
}
opticflow_got_result = FALSE;
}
diff --git a/sw/airborne/modules/guidance_opticflow/guidance_opticflow_hover.c b/sw/airborne/modules/guidance_opticflow/guidance_opticflow_hover.c
index 122493dd85..eb90064f31 100644
--- a/sw/airborne/modules/guidance_opticflow/guidance_opticflow_hover.c
+++ b/sw/airborne/modules/guidance_opticflow/guidance_opticflow_hover.c
@@ -98,7 +98,7 @@ struct opticflow_stab_t opticflow_stab = {
static void stabilization_opticflow_vel_cb(uint8_t sender_id __attribute__((unused)),
- uint32_t stamp, float vel_x, float vel_y, float vel_z);
+ uint32_t stamp, float vel_x, float vel_y, float vel_z, float noise);
/**
* Initialization of horizontal guidance module.
*/
@@ -149,7 +149,7 @@ void guidance_h_module_run(bool_t in_flight)
* Update the controls on a new VELOCITY_ESTIMATE ABI message.
*/
static void stabilization_opticflow_vel_cb(uint8_t sender_id __attribute__((unused)),
- uint32_t stamp, float vel_x, float vel_y, float vel_z)
+ uint32_t stamp, float vel_x, float vel_y, float vel_z, float noise)
{
/* Check if we are in the correct AP_MODE before setting commands */
if (autopilot_mode != AP_MODE_MODULE) {
diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c
index 5e5bdf4111..70056bc402 100644
--- a/sw/airborne/subsystems/ins/ins_int.c
+++ b/sw/airborne/subsystems/ins/ins_int.c
@@ -141,7 +141,7 @@ static abi_event gps_ev;
#define INS_INT_VEL_ID ABI_BROADCAST
#endif
static abi_event vel_est_ev;
-static void vel_est_cb(uint8_t sender_id, uint32_t stamp, float x, float y, float z);
+static void vel_est_cb(uint8_t sender_id, uint32_t stamp, float x, float y, float z, float noise);
/** measurement noise for velocity estimate */
#ifndef INS_VFF_R_VEL
@@ -238,8 +238,7 @@ void ins_reset_local_origin(void)
ins_int.ltp_def.hmsl = gps.hmsl;
ins_int.ltp_initialized = TRUE;
stateSetLocalOrigin_i(&ins_int.ltp_def);
- }
- else {
+ } else {
ins_int.ltp_initialized = FALSE;
}
#else
@@ -306,7 +305,7 @@ void ins_int_propagate(struct Int32Vect3 *accel, float dt)
ins_ned_to_state();
/* increment the propagation counter, while making sure it doesn't overflow */
- if (ins_int.propagation_cnt < 100*INS_MAX_PROPAGATION_STEPS) {
+ if (ins_int.propagation_cnt < 100 * INS_MAX_PROPAGATION_STEPS) {
ins_int.propagation_cnt++;
}
}
@@ -523,17 +522,17 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)),
static void vel_est_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp __attribute__((unused)),
- float x, float y, float z)
+ float x, float y, float z, float noise)
{
- struct FloatVect3 vel_body = {x, y, 0};
+ struct FloatVect3 vel_body = {x, y, z};
/* rotate velocity estimate to nav/ltp frame */
// from frame coordinates to body coordinates and convert cm/s-> m/s
- //TODO Do this in the optical flow module already
- vel_body.x=y/100;
- vel_body.y=x/100;
+ //TODO Do this in the optical flow module already, but after the guidance opticflow module has disapeared
+ vel_body.x = y / 100;
+ vel_body.y = x / 100;
struct FloatQuat q_b2n = *stateGetNedToBodyQuat_f();
QUAT_INVERT(q_b2n, q_b2n);
@@ -542,12 +541,7 @@ static void vel_est_cb(uint8_t sender_id __attribute__((unused)),
#if USE_HFF
struct FloatVect2 vel = {vel_ned.x, vel_ned.y};
- //struct FloatVect2 Rvel = {INS_VFF_R_VEL, INS_VFF_R_VEL};
- //FIXME this should come out of optical flow module
- if(z<17)
- struct FloatVect2 Rvel = {z/25,z/25};
- else
- struct FloatVect2 Rvel = {1.0,1.0};
+ struct FloatVect2 Rvel = {noise, noise};
b2_hff_update_vel(vel, Rvel);
ins_update_from_hff();