diff --git a/conf/airframes/examples/bebop2_opticflow.xml b/conf/airframes/examples/bebop2_opticflow.xml index db2fabcc13..4e79bf24ee 100644 --- a/conf/airframes/examples/bebop2_opticflow.xml +++ b/conf/airframes/examples/bebop2_opticflow.xml @@ -1,18 +1,28 @@ + + BEBOP2 airframe: + - requires fixed dampers + - uses bottom camera for hovering (no GPS and no Optitrack) + - uses the flow from the front camera to navigate + + - + + - + @@ -20,70 +30,31 @@ - + - + --> - - + + - - - - - - - - - - - - + - - - - - - + - - - - - - - - - - - - - - - @@ -94,31 +65,12 @@ - - - - - - - - - - - - - - - - - - - - - + + @@ -153,7 +105,14 @@ +
+ + +
+
+ + @@ -161,8 +120,51 @@ - + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + +
+ +
+ + +
@@ -222,10 +224,12 @@
- + +
+
@@ -253,8 +257,6 @@ - - @@ -270,15 +272,7 @@ - - - - + @@ -297,12 +291,6 @@ -
@@ -318,42 +306,11 @@
-
- - -
-
- - - - - - - - - - -
@@ -368,5 +325,6 @@
+ diff --git a/conf/airframes/tudelft/bebop2_opticflow_sim.xml b/conf/airframes/tudelft/bebop2_opticflow_sim.xml deleted file mode 100644 index 844460349b..0000000000 --- a/conf/airframes/tudelft/bebop2_opticflow_sim.xml +++ /dev/null @@ -1,293 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
- - - - - -
- - - - - - - - - -
- - - -
- - -
- - - - - - -
- - - - - -
- - - - - - - -
- -
- - - - -
- - -
- - - - - - - - - - - - - - - -
- - - -
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
- -
- - - - - - -
- -
- - - - - -
- -
- - -
- -
- - - - - - - - - - - -
- -
- - - - - -
- - diff --git a/conf/airframes/tudelft/tudelft_bebop2_opticflow_sim.xml b/conf/airframes/tudelft/tudelft_bebop2_opticflow_sim.xml new file mode 100644 index 0000000000..e69de29bb2 diff --git a/conf/flight_plans/rotorcraft_guido_optitrack.xml b/conf/flight_plans/rotorcraft_guido_optitrack.xml index 7ee822195d..effe5e3aea 100644 --- a/conf/flight_plans/rotorcraft_guido_optitrack.xml +++ b/conf/flight_plans/rotorcraft_guido_optitrack.xml @@ -1,6 +1,6 @@ - +
#include "autopilot.h"
diff --git a/conf/flight_plans/tudelft/rotorcraft_cyberzoo_no_gps.xml b/conf/flight_plans/tudelft/rotorcraft_cyberzoo_no_gps.xml index 5b5222b303..db3c521852 100644 --- a/conf/flight_plans/tudelft/rotorcraft_cyberzoo_no_gps.xml +++ b/conf/flight_plans/tudelft/rotorcraft_cyberzoo_no_gps.xml @@ -1,6 +1,6 @@ - +
#include "autopilot.h"
diff --git a/conf/modules/optical_flow_landing.xml b/conf/modules/optical_flow_landing.xml index 6e08563f10..35ec927f8f 100644 --- a/conf/modules/optical_flow_landing.xml +++ b/conf/modules/optical_flow_landing.xml @@ -31,6 +31,7 @@ + @@ -72,6 +73,7 @@ + diff --git a/conf/userconf/tudelft/conf.xml b/conf/userconf/tudelft/conf.xml index 5eb5ed24ed..b9973c6df4 100644 --- a/conf/userconf/tudelft/conf.xml +++ b/conf/userconf/tudelft/conf.xml @@ -161,7 +161,7 @@ telemetry="telemetry/tudelft/outback.xml" flight_plan="flight_plans/rotorcraft_basic.xml" settings="settings/rotorcraft_basic.xml" - settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/geo_mag.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_indi.xml modules/guidance_rotorcraft.xml modules/heli_throttle_curve.xml modules/imu_common.xml modules/logger_sd_spi_direct.xml modules/nav_basic_rotorcraft.xml modules/opa_controller.xml modules/stabilization_int_quat.xml modules/stabilization_rate.xml modules/temp_adc.xml" + settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/electrical.xml modules/geo_mag.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_indi.xml modules/guidance_rotorcraft.xml modules/heli_throttle_curve.xml modules/imu_common.xml modules/logger_sd_spi_direct.xml modules/nav_rotorcraft.xml modules/opa_controller.xml modules/stabilization_int_quat.xml modules/stabilization_rate.xml modules/temp_adc.xml" gui_color="#ffffdffac31f" /> - - + + + + + diff --git a/sw/airborne/boards/bebop/mt9v117.c b/sw/airborne/boards/bebop/mt9v117.c index 4603b97d89..0d2d8a5a3a 100644 --- a/sw/airborne/boards/bebop/mt9v117.c +++ b/sw/airborne/boards/bebop/mt9v117.c @@ -37,7 +37,6 @@ #include #include -#include "generated/airframe.h" #ifdef BOARD_DISCO #include "boards/disco.h" #else diff --git a/sw/airborne/boards/bebop/mt9v117.h b/sw/airborne/boards/bebop/mt9v117.h index 3305eb6e1a..e1b68640b4 100644 --- a/sw/airborne/boards/bebop/mt9v117.h +++ b/sw/airborne/boards/bebop/mt9v117.h @@ -29,7 +29,8 @@ #include "std.h" #include "mcu_periph/i2c.h" -//#include "generated/airframe.h" +// Camera parameters are defined in sections +#include "generated/airframe.h" #ifndef MT9V117_TARGET_FPS #define MT9V117_TARGET_FPS 0 diff --git a/sw/airborne/modules/computer_vision/lib/vision/edge_flow.h b/sw/airborne/modules/computer_vision/lib/vision/edge_flow.h index 827b0d93c3..9d572422f4 100644 --- a/sw/airborne/modules/computer_vision/lib/vision/edge_flow.h +++ b/sw/airborne/modules/computer_vision/lib/vision/edge_flow.h @@ -36,6 +36,7 @@ #include "lib/vision/image.h" #include "lib/v4l/v4l2.h" #include "opticflow/opticflow_calculator.h" +#include "generated/airframe.h" #include #include #include diff --git a/sw/airborne/modules/computer_vision/textons.c b/sw/airborne/modules/computer_vision/textons.c index 242ea2a1c0..7f991ed3d3 100644 --- a/sw/airborne/modules/computer_vision/textons.c +++ b/sw/airborne/modules/computer_vision/textons.c @@ -34,6 +34,7 @@ #include "modules/computer_vision/cv.h" #include "modules/computer_vision/textons.h" #include "mcu_periph/sys_time.h" +#include "generated/airframe.h" float ** **dictionary; uint32_t learned_samples = 0; diff --git a/sw/airborne/modules/ctrl/optical_flow_landing.c b/sw/airborne/modules/ctrl/optical_flow_landing.c index 40c75262ae..0f0a73da74 100644 --- a/sw/airborne/modules/ctrl/optical_flow_landing.c +++ b/sw/airborne/modules/ctrl/optical_flow_landing.c @@ -200,6 +200,10 @@ PRINT_CONFIG_VAR(OFL_OPTICAL_FLOW_ID) #define OFL_ACTIVE_MOTION 0 #endif +#ifndef OFL_FRONT_DIV_THRESHOLD +#define OFL_FRONT_DIV_THRESHOLD 0.3 +#endif + // Normally, horizontal control is done via sending angle commands to INDI, so 0 (false) // When this is 1 (true),a change in angle will be commanded instead. #define HORIZONTAL_RATE_CONTROL 0 @@ -218,11 +222,15 @@ PRINT_CONFIG_VAR(OFL_OPTICAL_FLOW_ID) // variables retained between module calls + +float old_flow_time; + // horizontal loop: float optical_flow_x; float optical_flow_y; float lp_flow_x; float lp_flow_y; +float lp_divergence_front; float sum_roll_error; float sum_pitch_error; @@ -240,6 +248,8 @@ float previous_cov_err; int32_t thrust_set; float divergence_setpoint; +float divergence_front; + // ********************************* // include and define stuff for SSL: // ********************************* @@ -290,7 +300,7 @@ static void send_divergence(struct transport_tx *trans, struct link_device *dev) { pprz_msg_send_DIVERGENCE(trans, dev, AC_ID, &(of_landing_ctrl.divergence), &divergence_vision_dt, &normalized_thrust, - &cov_div, &pstate, &pused, &(of_landing_ctrl.agl)); + &cov_div, &pstate, &pused, &(of_landing_ctrl.agl), &lp_divergence_front); } /// Function definitions @@ -371,6 +381,8 @@ void vertical_ctrl_module_init(void) of_landing_ctrl.omega_LR = OFL_OMEGA_LR; of_landing_ctrl.active_motion = OFL_ACTIVE_MOTION; + of_landing_ctrl.front_div_threshold = OFL_FRONT_DIV_THRESHOLD; + int i; if (of_landing_ctrl.use_bias) { weights = (float *)calloc(n_textons + 1, sizeof(float)); @@ -423,6 +435,11 @@ void vertical_ctrl_module_init(void) lp_flow_x = 0.0f; lp_flow_y = 0.0f; + lp_divergence_front = 0.0f; + + old_flow_time = get_sys_time_float(); + + divergence_front = 0.0f; } /** @@ -484,6 +501,7 @@ static void reset_all_vars(void) lp_flow_x = 0.0f; lp_flow_y = 0.0f; + lp_divergence_front = 0.0f; } /** @@ -899,8 +917,16 @@ void vertical_ctrl_module_run(bool in_flight) optical_flow_x; lp_flow_y = of_landing_ctrl.lp_factor_prediction * lp_flow_y + (1.0f - of_landing_ctrl.lp_factor_prediction) * optical_flow_y; + lp_divergence_front = of_landing_ctrl.lp_factor_prediction * lp_divergence_front + (1.0f - of_landing_ctrl.lp_factor_prediction) * + divergence_front; + + if(lp_divergence_front > of_landing_ctrl.front_div_threshold) { + // Stop moving in the longitudinal direction: + of_landing_ctrl.omega_FB = 0.0f; + } + if (of_landing_ctrl.active_motion == 1) { // Active motion through varying ventral flow commands float period_factor = 0.2; @@ -1109,6 +1135,28 @@ void vertical_ctrl_optical_flow_cb(uint8_t sender_id, uint32_t stamp, divergence_vision = size_divergence; //printf("Reading %f, %f, %f\n", optical_flow_x, optical_flow_y, divergence_vision); vision_time = ((float)stamp) / 1e6; + + + // checking fps and newness of images: + /*float new_flow_time = get_sys_time_float(); + float dt = (new_flow_time - old_flow_time); + if (dt > 0) { + float fps_flow = 1.0f / dt; + printf("FPS flow bottom cam in OF landing = %f, optical_flow_x = %f\n", fps_flow, optical_flow_x); + } + old_flow_time = new_flow_time; */ + } + else { + + float new_flow_time = get_sys_time_float(); + float dt_flow_front = new_flow_time - old_flow_time; + if (dt_flow_front > 0) { + //float fps_flow = 1.0f / dt_flow_front; + //printf("FPS flow front cam in OF landing = %f\n", fps_flow); + old_flow_time = new_flow_time; + + divergence_front = size_divergence / dt_flow_front; + } } } diff --git a/sw/airborne/modules/ctrl/optical_flow_landing.h b/sw/airborne/modules/ctrl/optical_flow_landing.h index 1ceb89e44d..dc1986dd11 100644 --- a/sw/airborne/modules/ctrl/optical_flow_landing.h +++ b/sw/airborne/modules/ctrl/optical_flow_landing.h @@ -94,6 +94,7 @@ struct OpticalFlowLanding { float omega_LR; ///< Set point for the left-right ventral flow float omega_FB; ///< Set point for the front-back ventral flow uint32_t active_motion; ///< Method for actively inducing motion + float front_div_threshold; ///< Threshold when the front div gets above this value, it will command a horizontal stop. }; extern struct OpticalFlowLanding of_landing_ctrl; diff --git a/sw/airborne/modules/ins/ins_flow.c b/sw/airborne/modules/ins/ins_flow.c index 8fc271d840..e3523db314 100644 --- a/sw/airborne/modules/ins/ins_flow.c +++ b/sw/airborne/modules/ins/ins_flow.c @@ -947,21 +947,28 @@ void ins_flow_update(void) F[i][i] = 1.0f; } - if(CONSTANT_ALT_FILTER) { - F[OF_V_IND][OF_ANGLE_IND] = dt*(g/(cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND]))); - F[OF_ANGLE_IND][OF_ANGLE_DOT_IND] = dt*1.0f; - if(OF_TWO_DIM) { - F[OF_VX_IND][OF_THETA_IND] = dt*(g/(cosf(OF_X[OF_THETA_IND])*cosf(OF_X[OF_THETA_IND]))); + if (CONSTANT_ALT_FILTER) { + F[OF_V_IND][OF_ANGLE_IND] = dt * (g / (cosf(OF_X[OF_ANGLE_IND]) * cosf(OF_X[OF_ANGLE_IND]))); + F[OF_ANGLE_IND][OF_ANGLE_DOT_IND] = dt * 1.0f; + if (OF_TWO_DIM) { + F[OF_VX_IND][OF_THETA_IND] = dt * (g / (cosf(OF_X[OF_THETA_IND]) * cosf(OF_X[OF_THETA_IND]))); + } + } else { + F[OF_V_IND][OF_ANGLE_IND] = dt * (thrust * cosf(OF_X[OF_ANGLE_IND]) / mass); + F[OF_ANGLE_IND][OF_ANGLE_DOT_IND] = dt * 1.0f; + F[OF_Z_IND][OF_Z_DOT_IND] = dt * 1.0f; + F[OF_Z_DOT_IND][OF_ANGLE_IND] = dt * (-thrust * sinf(OF_X[OF_ANGLE_IND]) / mass); + if (OF_THRUST_BIAS) { + F[OF_V_IND][OF_THRUST_BIAS_IND] = -dt * sinf(OF_X[OF_ANGLE_IND]) / mass; + F[OF_Z_DOT_IND][OF_THRUST_BIAS_IND] = -dt * cosf(OF_X[OF_ANGLE_IND]) / mass; } } - else { - F[OF_V_IND][OF_ANGLE_IND] = dt*(thrust*cosf(OF_X[OF_ANGLE_IND])/mass); - F[OF_ANGLE_IND][OF_ANGLE_DOT_IND] = dt*1.0f; - F[OF_Z_IND][OF_Z_DOT_IND] = dt*1.0f; - F[OF_Z_DOT_IND][OF_ANGLE_IND] = dt*(-thrust*sinf(OF_X[OF_ANGLE_IND])/mass); - if(OF_THRUST_BIAS) { - F[OF_V_IND][OF_THRUST_BIAS_IND] = -dt*sinf(OF_X[OF_ANGLE_IND]) / mass; - F[OF_Z_DOT_IND][OF_THRUST_BIAS_IND] = -dt*cosf(OF_X[OF_ANGLE_IND]) / mass; + + if (OF_DRAG) { + // In MATLAB: -sign(v)*2*kd*v/m (always minus, whether v is positive or negative): + F[OF_V_IND][OF_V_IND] -= dt * 2 * kd * fabs(OF_X[OF_V_IND]) / mass; + if (OF_TWO_DIM) { + F[OF_VX_IND][OF_VX_IND] -= dt * 2 * kd * fabs(OF_X[OF_VX_IND]) / mass; } } @@ -975,46 +982,49 @@ void ins_flow_update(void) // Jacobian observation matrix H: float H[N_MEAS_OF_KF][N_STATES_OF_KF] = {{0.}}; - if(CONSTANT_ALT_FILTER) { - // Hx = [-cosf(theta)^2/z, (v*sinf(theta))/ z, (v* cosf(theta)^2)/z^2]; + if (CONSTANT_ALT_FILTER) { + // Hx = [-cosf(theta)^2/z, (v*sinf(theta))/ z, (v* cosf(theta)^2)/z^2]; // lateral flow: - H[OF_LAT_FLOW_IND][OF_V_IND] = -cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/ OF_X[OF_Z_IND]; - H[OF_LAT_FLOW_IND][OF_ANGLE_IND] = OF_X[OF_V_IND]*sinf(2*OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND]; - H[OF_LAT_FLOW_IND][OF_Z_IND] = OF_X[OF_V_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/(OF_X[OF_Z_IND]*OF_X[OF_Z_IND]); + H[OF_LAT_FLOW_IND][OF_V_IND] = -cosf(OF_X[OF_ANGLE_IND]) * cosf(OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND]; + H[OF_LAT_FLOW_IND][OF_ANGLE_IND] = OF_X[OF_V_IND] * sinf(2 * OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND]; + H[OF_LAT_FLOW_IND][OF_Z_IND] = OF_X[OF_V_IND] * cosf(OF_X[OF_ANGLE_IND]) * cosf(OF_X[OF_ANGLE_IND]) / + (OF_X[OF_Z_IND] * OF_X[OF_Z_IND]); H[OF_LAT_FLOW_IND][OF_ANGLE_DOT_IND] = 1.0f; // divergence: - H[OF_DIV_FLOW_IND][OF_V_IND] = -sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]); - H[OF_DIV_FLOW_IND][OF_ANGLE_IND] = -OF_X[OF_V_IND]*cosf(2*OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND]; - H[OF_DIV_FLOW_IND][OF_Z_IND] = OF_X[OF_V_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]*OF_X[OF_Z_IND]); + H[OF_DIV_FLOW_IND][OF_V_IND] = -sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND]); + H[OF_DIV_FLOW_IND][OF_ANGLE_IND] = -OF_X[OF_V_IND] * cosf(2 * OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND]; + H[OF_DIV_FLOW_IND][OF_Z_IND] = OF_X[OF_V_IND] * sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND] * OF_X[OF_Z_IND]); H[OF_DIV_FLOW_IND][OF_ANGLE_DOT_IND] = 0.0f; - if(OF_TWO_DIM) { - // divergence measurement couples the two axes actually...: - H[OF_DIV_FLOW_IND][OF_VX_IND] = -sinf(2*OF_X[OF_THETA_IND])/(2*OF_X[OF_Z_IND]); - H[OF_DIV_FLOW_IND][OF_THETA_IND] = -OF_X[OF_VX_IND]*cosf(2*OF_X[OF_THETA_IND])/OF_X[OF_Z_IND]; + if (OF_TWO_DIM) { + // divergence measurement couples the two axes actually...: + H[OF_DIV_FLOW_IND][OF_VX_IND] = -sinf(2 * OF_X[OF_THETA_IND]) / (2 * OF_X[OF_Z_IND]); + H[OF_DIV_FLOW_IND][OF_THETA_IND] = -OF_X[OF_VX_IND] * cosf(2 * OF_X[OF_THETA_IND]) / OF_X[OF_Z_IND]; - // lateral flow in x direction: - H[OF_LAT_FLOW_X_IND][OF_VX_IND] = -cosf(OF_X[OF_THETA_IND])*cosf(OF_X[OF_THETA_IND])/ OF_X[OF_Z_IND]; - H[OF_LAT_FLOW_X_IND][OF_THETA_IND] = OF_X[OF_VX_IND]*sinf(2*OF_X[OF_THETA_IND])/OF_X[OF_Z_IND]; - H[OF_LAT_FLOW_X_IND][OF_Z_IND] = OF_X[OF_VX_IND]*cosf(OF_X[OF_THETA_IND])*cosf(OF_X[OF_THETA_IND])/(OF_X[OF_Z_IND]*OF_X[OF_Z_IND]); + // lateral flow in x direction: + H[OF_LAT_FLOW_X_IND][OF_VX_IND] = -cosf(OF_X[OF_THETA_IND]) * cosf(OF_X[OF_THETA_IND]) / OF_X[OF_Z_IND]; + H[OF_LAT_FLOW_X_IND][OF_THETA_IND] = OF_X[OF_VX_IND] * sinf(2 * OF_X[OF_THETA_IND]) / OF_X[OF_Z_IND]; + H[OF_LAT_FLOW_X_IND][OF_Z_IND] = OF_X[OF_VX_IND] * cosf(OF_X[OF_THETA_IND]) * cosf(OF_X[OF_THETA_IND]) / + (OF_X[OF_Z_IND] * OF_X[OF_Z_IND]); } } else { // lateral flow: - H[OF_LAT_FLOW_IND][OF_V_IND] = -cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/ OF_X[OF_Z_IND]; - H[OF_LAT_FLOW_IND][OF_ANGLE_IND] = OF_X[OF_V_IND]*sinf(2*OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND] - + OF_X[OF_Z_DOT_IND]*cosf(2*OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND]; + H[OF_LAT_FLOW_IND][OF_V_IND] = -cosf(OF_X[OF_ANGLE_IND]) * cosf(OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND]; + H[OF_LAT_FLOW_IND][OF_ANGLE_IND] = OF_X[OF_V_IND] * sinf(2 * OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND] + + OF_X[OF_Z_DOT_IND] * cosf(2 * OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND]; H[OF_LAT_FLOW_IND][OF_ANGLE_DOT_IND] = 1.0f; - H[OF_LAT_FLOW_IND][OF_Z_IND] = OF_X[OF_V_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/(OF_X[OF_Z_IND]*OF_X[OF_Z_IND]) - - OF_X[OF_Z_DOT_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]*OF_X[OF_Z_IND]); - H[OF_LAT_FLOW_IND][OF_Z_DOT_IND] = sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]); + H[OF_LAT_FLOW_IND][OF_Z_IND] = OF_X[OF_V_IND] * cosf(OF_X[OF_ANGLE_IND]) * cosf(OF_X[OF_ANGLE_IND]) / + (OF_X[OF_Z_IND] * OF_X[OF_Z_IND]) + - OF_X[OF_Z_DOT_IND] * sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND] * OF_X[OF_Z_IND]); + H[OF_LAT_FLOW_IND][OF_Z_DOT_IND] = sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND]); // divergence: - H[OF_DIV_FLOW_IND][OF_V_IND] = -sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]); - H[OF_DIV_FLOW_IND][OF_ANGLE_IND] = -OF_X[OF_V_IND]*cosf(2*OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND] - + OF_X[OF_Z_DOT_IND]*sinf(2*OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND]; + H[OF_DIV_FLOW_IND][OF_V_IND] = -sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND]); + H[OF_DIV_FLOW_IND][OF_ANGLE_IND] = -OF_X[OF_V_IND] * cosf(2 * OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND] + + OF_X[OF_Z_DOT_IND] * sinf(2 * OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND]; H[OF_DIV_FLOW_IND][OF_ANGLE_DOT_IND] = 0.0f; - H[OF_DIV_FLOW_IND][OF_Z_IND] = OF_X[OF_V_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]*OF_X[OF_Z_IND]) - + OF_X[OF_Z_DOT_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/(OF_X[OF_Z_IND]*OF_X[OF_Z_IND]); - H[OF_DIV_FLOW_IND][OF_Z_DOT_IND] = -cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND]; + H[OF_DIV_FLOW_IND][OF_Z_IND] = OF_X[OF_V_IND] * sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND] * OF_X[OF_Z_IND]) + + OF_X[OF_Z_DOT_IND] * cosf(OF_X[OF_ANGLE_IND]) * cosf(OF_X[OF_ANGLE_IND]) / (OF_X[OF_Z_IND] * OF_X[OF_Z_IND]); + H[OF_DIV_FLOW_IND][OF_Z_DOT_IND] = -cosf(OF_X[OF_ANGLE_IND]) * cosf(OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND]; } // rate measurement: @@ -1144,16 +1154,16 @@ void ins_flow_update(void) // Correct the state: // MATLAB: // Z_expected = [-v*cosf(theta)*cosf(theta)/z + zd*sinf(2*theta)/(2*z) + thetad; - // (-v*sinf(2*theta)/(2*z)) - zd*cosf(theta)*cosf(theta)/z]; + // (-v*sinf(2*theta)/(2*z)) - zd*cosf(theta)*cosf(theta)/z]; float Z_expected[N_MEAS_OF_KF]; // TODO: take this var out? It was meant for debugging... //float Z_expect_GT_angle; - if(CONSTANT_ALT_FILTER) { - Z_expected[OF_LAT_FLOW_IND] = -OF_X[OF_V_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND] - +OF_X[OF_ANGLE_DOT_IND]; // TODO: Currently, no p works better than using p here. Analyze! + if (CONSTANT_ALT_FILTER) { + Z_expected[OF_LAT_FLOW_IND] = -OF_X[OF_V_IND] * cosf(OF_X[OF_ANGLE_IND]) * cosf(OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND] + + OF_X[OF_ANGLE_DOT_IND]; // TODO: Currently, no p works better than using p here. Analyze! /* TODO: remove later, just for debugging: @@ -1166,10 +1176,11 @@ void ins_flow_update(void) } printf("\n");*/ - Z_expected[OF_DIV_FLOW_IND] = -OF_X[OF_V_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]); + Z_expected[OF_DIV_FLOW_IND] = -OF_X[OF_V_IND] * sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND]); - if(OF_TWO_DIM) { - Z_expected[OF_LAT_FLOW_X_IND] = -OF_X[OF_VX_IND]*cosf(OF_X[OF_THETA_IND])*cosf(OF_X[OF_THETA_IND])/OF_X[OF_Z_IND]; // TODO: no q? + if (OF_TWO_DIM) { + Z_expected[OF_LAT_FLOW_X_IND] = -OF_X[OF_VX_IND] * cosf(OF_X[OF_THETA_IND]) * cosf(OF_X[OF_THETA_IND]) / + OF_X[OF_Z_IND]; // TODO: no q? } //Z_expect_GT_angle = -OF_X[OF_V_IND]*cosf(eulers->phi)*cosf(eulers->phi)/OF_X[OF_Z_IND]; @@ -1177,30 +1188,29 @@ void ins_flow_update(void) if (OF_USE_GYROS) { Z_expected[OF_RATE_IND] = OF_X[OF_ANGLE_DOT_IND]; // TODO: is this even in the right direction? } - } - else { - Z_expected[OF_LAT_FLOW_IND] = -OF_X[OF_V_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND] - + OF_X[OF_Z_DOT_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]) - + OF_X[OF_ANGLE_DOT_IND]; // TODO: We first had this rate term but not for the constant alt filter. - // Simulation and data analysis from real flights shows that including it is better. CHECK IN REALITY! + } else { + Z_expected[OF_LAT_FLOW_IND] = -OF_X[OF_V_IND] * cosf(OF_X[OF_ANGLE_IND]) * cosf(OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND] + + OF_X[OF_Z_DOT_IND] * sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND]) + + OF_X[OF_ANGLE_DOT_IND]; // TODO: We first had this rate term but not for the constant alt filter. + // Simulation and data analysis from real flights shows that including it is better. CHECK IN REALITY! - Z_expected[OF_DIV_FLOW_IND] = -OF_X[OF_V_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]) - -OF_X[OF_Z_DOT_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND]; + Z_expected[OF_DIV_FLOW_IND] = -OF_X[OF_V_IND] * sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND]) + - OF_X[OF_Z_DOT_IND] * cosf(OF_X[OF_ANGLE_IND]) * cosf(OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND]; //Z_expect_GT_angle = -OF_X[OF_V_IND]*cosf(eulers->phi)*cosf(eulers->phi)/OF_X[OF_Z_IND] - // + OF_X[OF_Z_DOT_IND]*sinf(2*eulers->phi)/(2*OF_X[OF_Z_IND]); - //+ OF_X[OF_ANGLE_DOT_IND]; - if(N_MEAS_OF_KF == 3) { - Z_expected[OF_RATE_IND] = OF_X[OF_ANGLE_DOT_IND]; // TODO: is this even in the right direction? + // + OF_X[OF_Z_DOT_IND]*sinf(2*eulers->phi)/(2*OF_X[OF_Z_IND]); + //+ OF_X[OF_ANGLE_DOT_IND]; + if (N_MEAS_OF_KF == 3) { + Z_expected[OF_RATE_IND] = OF_X[OF_ANGLE_DOT_IND]; // TODO: is this even in the right direction? } -/* - float Z_exp_no_rate = -OF_X[OF_V_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND] - + OF_X[OF_Z_DOT_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]); - float Z_exp_with_rate = -OF_X[OF_V_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND] - + OF_X[OF_Z_DOT_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]) - + OF_X[OF_ANGLE_DOT_IND]; -*/ + /* + float Z_exp_no_rate = -OF_X[OF_V_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND] + + OF_X[OF_Z_DOT_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]); + float Z_exp_with_rate = -OF_X[OF_V_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND] + + OF_X[OF_Z_DOT_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]) + + OF_X[OF_ANGLE_DOT_IND]; + */ /* printf("Z_exp_no_rate = %f, Z_exp_with_rate = %f, measured = %f, angle dot = %f, p = %f: ", Z_exp_no_rate, Z_exp_with_rate, ins_flow.optical_flow_x, OF_X[OF_ANGLE_DOT_IND], dt * (ins_flow.lp_gyro_roll - ins_flow.lp_gyro_bias_roll) * (M_PI/180.0f) / 74.0f); @@ -1472,8 +1482,9 @@ static void ins_act_feedback_cb(uint8_t sender_id UNUSED, struct act_feedback_t { ins_flow.RPM_num_act = num_act; for (int i = 0; i < num_act; i++) { - if(feedback[i].set.rpm) + if (feedback[i].set.rpm) { ins_flow.RPM[i] = feedback[i].rpm; + } } } diff --git a/sw/ext/pprzlink b/sw/ext/pprzlink index cbb8ad25b1..8755be55b2 160000 --- a/sw/ext/pprzlink +++ b/sw/ext/pprzlink @@ -1 +1 @@ -Subproject commit cbb8ad25b1a2a185dce3e231b2c1338be14bd86f +Subproject commit 8755be55b23496bd8962c6b1691b9b02b266fc2d diff --git a/sw/tools/rtp_viewer/play_video_ffmpeg.sh b/sw/tools/rtp_viewer/play_video_ffmpeg.sh new file mode 100755 index 0000000000..571c1a7707 --- /dev/null +++ b/sw/tools/rtp_viewer/play_video_ffmpeg.sh @@ -0,0 +1 @@ +ffplay rtp_6000.sdp -protocol_whitelist file,udp,rtp -fflags nobuffer diff --git a/sw/tools/rtp_viewer/rtp_viewer.py b/sw/tools/rtp_viewer/rtp_viewer.py index e88801dccc..bc81b5e3e3 100755 --- a/sw/tools/rtp_viewer/rtp_viewer.py +++ b/sw/tools/rtp_viewer/rtp_viewer.py @@ -17,6 +17,9 @@ os.environ['OPENCV_FFMPEG_CAPTURE_OPTIONS'] = 'protocol_whitelist;file,rtp,udp' from pprzlink.ivy import IvyMessagesInterface from pprzlink.message import PprzMessage +# See the issue and solution here: https://github.com/opencv/opencv/issues/10328 +os.environ['OPENCV_FFMPEG_CAPTURE_OPTIONS'] = 'protocol_whitelist;file,rtp,udp' + class RtpViewer: running = False scale = 1