From b0cfb6160fa5efdccdcd921348b9c7282c1bc8be Mon Sep 17 00:00:00 2001 From: "k.n.mcguire@tudelft.nl" Date: Mon, 9 Nov 2015 18:39:42 +0100 Subject: [PATCH] [INS] added extra noise_measurement value to abi message VELOCITY_ESTIMATE, which is calculated in optic flow module --- conf/abi.xml | 1 + .../opticflow/inter_thread_data.h | 2 ++ .../opticflow/opticflow_calculator.c | 17 ++++++++++--- .../computer_vision/opticflow_module.c | 7 ++++-- .../guidance_opticflow_hover.c | 4 ++-- sw/airborne/subsystems/ins/ins_int.c | 24 +++++++------------ 6 files changed, 33 insertions(+), 22 deletions(-) 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();