diff --git a/conf/abi.xml b/conf/abi.xml
index 8cdb3da892..f358de58f4 100644
--- a/conf/abi.xml
+++ b/conf/abi.xml
@@ -70,7 +70,9 @@
-
+ Set negative noise to disable parameter axis
+
+
@@ -103,7 +105,9 @@
-
+ Set negative noise to disable parameter axis
+
+
diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c
index 5d3f939bad..86acf4548f 100644
--- a/sw/airborne/modules/computer_vision/opticflow_module.c
+++ b/sw/airborne/modules/computer_vision/opticflow_module.c
@@ -45,11 +45,6 @@
#endif
PRINT_CONFIG_VAR(OPTICFLOW_AGL_ID)
-#ifndef OPTICFLOW_SEND_ABI_ID
-#define OPTICFLOW_SEND_ABI_ID 1 ///< Default ID to send abi messages
-#endif
-PRINT_CONFIG_VAR(OPTICFLOW_SEND_ABI_ID)
-
#ifndef OPTICFLOW_FPS
#define OPTICFLOW_FPS 0 ///< Default FPS (zero means run at camera fps)
#endif
@@ -116,7 +111,7 @@ void opticflow_module_run(void)
// Update the stabilization loops on the current calculation
if (opticflow_got_result) {
uint32_t now_ts = get_sys_time_usec();
- AbiSendMsgOPTICAL_FLOW(OPTICFLOW_SEND_ABI_ID, now_ts,
+ AbiSendMsgOPTICAL_FLOW(FLOW_OPTICFLOW_ID, now_ts,
opticflow_result.flow_x,
opticflow_result.flow_y,
opticflow_result.flow_der_x,
@@ -125,10 +120,12 @@ void opticflow_module_run(void)
opticflow_result.div_size);
//TODO Find an appropriate quality measure for the noise model in the state filter, for now it is tracked_cnt
if (opticflow_result.noise_measurement < 0.8) {
- AbiSendMsgVELOCITY_ESTIMATE(OPTICFLOW_SEND_ABI_ID, now_ts,
+ AbiSendMsgVELOCITY_ESTIMATE(VEL_OPTICFLOW_ID, now_ts,
opticflow_result.vel_body.x,
opticflow_result.vel_body.y,
opticflow_result.vel_body.z,
+ opticflow_result.noise_measurement,
+ opticflow_result.noise_measurement,
opticflow_result.noise_measurement
);
}
diff --git a/sw/airborne/modules/dragspeed/dragspeed.c b/sw/airborne/modules/dragspeed/dragspeed.c
index ef84e1c416..1eac19563e 100644
--- a/sw/airborne/modules/dragspeed/dragspeed.c
+++ b/sw/airborne/modules/dragspeed/dragspeed.c
@@ -133,9 +133,8 @@ static void accel_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp,
// Send as ABI VELOCITY_ESTIMATE message
#if DRAGSPEED_SEND_ABI_MESSAGE
if (!dragspeed.calibrate_coeff && !dragspeed.calibrate_zero) {
- float vel_z = stateGetSpeedNed_f()->z; // Workaround until VELOCITY_ESTIMATE contains flags to specify which axes are estimated.
AbiSendMsgVELOCITY_ESTIMATE(VEL_DRAGSPEED_ID, stamp, dragspeed.vel.x,
- dragspeed.vel.y, vel_z, DRAGSPEED_R);
+ dragspeed.vel.y, 0.f, DRAGSPEED_R, DRAGSPEED_R, -1.f);
}
#endif
// Perform calibration if required
diff --git a/sw/airborne/modules/guidance_opticflow/guidance_opticflow_hover.c b/sw/airborne/modules/guidance_opticflow/guidance_opticflow_hover.c
index 6163d35050..9e230e6758 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, float noise);
+ uint32_t stamp, float vel_x, float vel_y, float vel_z, float noise_x, float noise_y, float noise_z);
/**
* Initialization of horizontal guidance module.
*/
@@ -149,28 +149,42 @@ void guidance_h_module_run(bool 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, float noise)
+ uint32_t stamp, float vel_x, float vel_y, float vel_z, float noise_x, float noise_y, float noise_z)
{
/* Check if we are in the correct AP_MODE before setting commands */
if (autopilot_get_mode() != AP_MODE_MODULE) {
return;
}
- /* Calculate the error */
- float err_vx = opticflow_stab.desired_vx - vel_x;
- float err_vy = opticflow_stab.desired_vy - vel_y;
+ if (noise_x >= 0.f)
+ {
+ /* Calculate the error */
+ float err_vx = opticflow_stab.desired_vx - vel_x;
- /* Calculate the integrated errors (TODO: bound??) */
- opticflow_stab.err_vx_int += err_vx / 512;
- opticflow_stab.err_vy_int += err_vy / 512;
+ /* Calculate the integrated errors (TODO: bound??) */
+ opticflow_stab.err_vx_int += err_vx / 512;
- /* Calculate the commands */
- opticflow_stab.cmd.phi = opticflow_stab.phi_pgain * err_vy
- + opticflow_stab.phi_igain * opticflow_stab.err_vy_int;
- opticflow_stab.cmd.theta = -(opticflow_stab.theta_pgain * err_vx
+ /* Calculate the commands */
+ opticflow_stab.cmd.theta = -(opticflow_stab.theta_pgain * err_vx
+ opticflow_stab.theta_igain * opticflow_stab.err_vx_int);
- /* Bound the roll and pitch commands */
- BoundAbs(opticflow_stab.cmd.phi, CMD_OF_SAT);
- BoundAbs(opticflow_stab.cmd.theta, CMD_OF_SAT);
+ /* Bound the roll and pitch commands */
+ BoundAbs(opticflow_stab.cmd.theta, CMD_OF_SAT);
+ }
+
+ if (noise_y >= 0.f)
+ {
+ /* Calculate the error */
+ float err_vy = opticflow_stab.desired_vy - vel_y;
+
+ /* Calculate the integrated errors (TODO: bound??) */
+ opticflow_stab.err_vy_int += err_vy / 512;
+
+ /* Calculate the commands */
+ opticflow_stab.cmd.phi = opticflow_stab.phi_pgain * err_vy
+ + opticflow_stab.phi_igain * opticflow_stab.err_vy_int;
+
+ /* Bound the roll and pitch commands */
+ BoundAbs(opticflow_stab.cmd.phi, CMD_OF_SAT);
+ }
}
diff --git a/sw/airborne/modules/optical_flow/px4flow.c b/sw/airborne/modules/optical_flow/px4flow.c
index 611f5abdae..3611e56160 100644
--- a/sw/airborne/modules/optical_flow/px4flow.c
+++ b/sw/airborne/modules/optical_flow/px4flow.c
@@ -86,12 +86,14 @@ static void decode_optical_flow_msg(struct mavlink_message *msg __attribute__((u
if (quality > PX4FLOW_QUALITY_THRESHOLD) {
// flip the axis (if the PX4FLOW is mounted as shown in
// https://pixhawk.org/modules/px4flow
- AbiSendMsgVELOCITY_ESTIMATE(PX4FLOW_VELOCITY_ID,
+ AbiSendMsgVELOCITY_ESTIMATE(VEL_PX4FLOW_ID,
optical_flow.time_usec,
optical_flow.flow_comp_m_y,
optical_flow.flow_comp_m_x,
0.0f,
- noise);
+ noise,
+ noise,
+ -1.f);
}
// compensate AGL measurement for body rotation
diff --git a/sw/airborne/modules/optical_flow/px4flow_i2c.c b/sw/airborne/modules/optical_flow/px4flow_i2c.c
index c47dfa4483..3cdfeb88e7 100644
--- a/sw/airborne/modules/optical_flow/px4flow_i2c.c
+++ b/sw/airborne/modules/optical_flow/px4flow_i2c.c
@@ -85,12 +85,14 @@ static inline void px4flow_i2c_frame_cb(void)
// flip the axis (if the PX4FLOW is mounted as shown in
// https://pixhawk.org/modules/px4flow
- AbiSendMsgVELOCITY_ESTIMATE(PX4FLOW_VELOCITY_ID,
+ AbiSendMsgVELOCITY_ESTIMATE(VEL_PX4FLOW_ID,
time_usec,
flow_comp_m_y,
flow_comp_m_x,
0.0f,
- noise);
+ noise,
+ noise,
+ -1.f);
}
// distance is always positive - use median filter to remove outliers
diff --git a/sw/airborne/modules/stereocam/stereocam.c b/sw/airborne/modules/stereocam/stereocam.c
index 0a4f9e29de..54e30c209a 100644
--- a/sw/airborne/modules/stereocam/stereocam.c
+++ b/sw/airborne/modules/stereocam/stereocam.c
@@ -85,11 +85,6 @@ struct stereocam_t stereocam = {
};
static uint8_t stereocam_msg_buf[256] __attribute__((aligned)); ///< The message buffer for the stereocamera
-// incoming messages definitions
-#ifndef STEREOCAM2STATE_SENDER_ID
-#define STEREOCAM2STATE_SENDER_ID ABI_BROADCAST
-#endif
-
#ifndef STEREOCAM_USE_MEDIAN_FILTER
#define STEREOCAM_USE_MEDIAN_FILTER 0
#endif
@@ -139,10 +134,12 @@ static void stereocam_parse_msg(void)
}
//Send velocities to state
- AbiSendMsgVELOCITY_ESTIMATE(STEREOCAM2STATE_SENDER_ID, now_ts,
+ AbiSendMsgVELOCITY_ESTIMATE(VEL_STEREOCAM_ID, now_ts,
body_vel.x,
body_vel.y,
body_vel.z,
+ noise,
+ noise,
noise
);
diff --git a/sw/airborne/subsystems/abi_sender_ids.h b/sw/airborne/subsystems/abi_sender_ids.h
index d13cae3258..0dc94f1b93 100644
--- a/sw/airborne/subsystems/abi_sender_ids.h
+++ b/sw/airborne/subsystems/abi_sender_ids.h
@@ -34,7 +34,7 @@
#endif
/*
- * IDs of baro modules that can be loaded
+ * IDs of baro modules that can be loaded (message 0)
*/
#ifndef BARO_MS5611_SENDER_ID
#define BARO_MS5611_SENDER_ID 10
@@ -81,7 +81,7 @@
#endif
/*
- * IDs of differential pressure sensors
+ * IDs of differential pressure sensors (message 1)
* can usually also publish temperature like baro sensors
*/
#ifndef MS45XX_SENDER_ID
@@ -89,7 +89,7 @@
#endif
/*
- * IDs of AGL measurment modules that can be loaded (sonars,...)
+ * IDs of AGL measurment modules that can be loaded (sonars,...) (message 2)
*/
#ifndef AGL_SONAR_ADC_ID
#define AGL_SONAR_ADC_ID 1
@@ -143,7 +143,7 @@
#endif
/*
- * IDs of GPS sensors
+ * IDs of GPS sensors (message 10)
*/
#ifndef GPS_UBX_ID
#define GPS_UBX_ID 1
@@ -272,16 +272,19 @@
#define IMU_MPU60X0_ID 16
#endif
-#ifndef PX4FLOW_VELOCITY_ID
-#define PX4FLOW_VELOCITY_ID 17
-#endif
-
#ifndef IMU_PX4_ID
-#define IMU_PX4_ID 18
+#define IMU_PX4_ID 17
#endif
#ifndef IMU_VECTORNAV_ID
-#define IMU_VECTORNAV_ID 19
+#define IMU_VECTORNAV_ID 18
+#endif
+
+/*
+ * IDs of OPTICFLOW estimates (message 12)
+ */
+#ifndef FLOW_OPTICFLOW_ID
+#define FLOW_OPTICFLOW_ID 1
#endif
/*
@@ -291,6 +294,18 @@
#define VEL_DRAGSPEED_ID 1
#endif
+#ifndef VEL_PX4FLOW_ID
+#define VEL_PX4FLOW_ID 2
+#endif
+
+#ifndef VEL_OPTICFLOW_ID
+#define VEL_OPTICFLOW_ID 3
+#endif
+
+#ifndef VEL_STEREOCAM_ID
+#define VEL_STEREOCAM_ID 4
+#endif
+
/*
* IDs of RSSI measurements (message 13)
*/
@@ -347,5 +362,4 @@
#define RANGE_FORCEFIELD_ID 1
#endif
-
#endif /* ABI_SENDER_IDS_H */
diff --git a/sw/airborne/subsystems/ins/hf_float.c b/sw/airborne/subsystems/ins/hf_float.c
index 0df5886b39..28646dd45b 100644
--- a/sw/airborne/subsystems/ins/hf_float.c
+++ b/sw/airborne/subsystems/ins/hf_float.c
@@ -805,9 +805,16 @@ static void hff_update_y(struct HfilterFloat *filt, float y_meas, float Rpos)
*/
void hff_update_vel(struct FloatVect2 vel, struct FloatVect2 Rvel)
{
- hff_speed_lost_counter = 0;
- hff_update_xdot(&hff, vel.x, Rvel.x);
- hff_update_ydot(&hff, vel.y, Rvel.y);
+ if (Rvel.x >= 0.f) {
+ hff_update_xdot(&hff, vel.x, Rvel.x);
+ }
+ if (Rvel.y >= 0.f) {
+ hff_update_ydot(&hff, vel.y, Rvel.y);
+ }
+
+ if (Rvel.x >= 0.f || Rvel.y >= 0.f) {
+ hff_speed_lost_counter = 0;
+ }
}
static void hff_update_xdot(struct HfilterFloat *filt, float vel, float Rvel)
diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c
index 7f841651fa..7b18e867ac 100644
--- a/sw/airborne/subsystems/ins/ins_int.c
+++ b/sw/airborne/subsystems/ins/ins_int.c
@@ -146,7 +146,7 @@ static abi_event vel_est_ev;
static void vel_est_cb(uint8_t sender_id,
uint32_t stamp,
float x, float y, float z,
- float noise);
+ float noise_x, float noise_y, float noise_z);
#ifndef INS_INT_POS_ID
#define INS_INT_POS_ID ABI_BROADCAST
#endif
@@ -154,7 +154,7 @@ static abi_event pos_est_ev;
static void pos_est_cb(uint8_t sender_id,
uint32_t stamp,
float x, float y, float z,
- float noise);
+ float noise_x, float noise_y, float noise_z);
struct InsInt ins_int;
@@ -539,7 +539,7 @@ 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 noise)
+ float noise_x, float noise_y, float noise_z)
{
struct FloatVect3 vel_body = {x, y, z};
@@ -549,26 +549,46 @@ static void vel_est_cb(uint8_t sender_id __attribute__((unused)),
struct FloatVect3 vel_ned;
float_quat_vmult(&vel_ned, &q_b2n, &vel_body);
+ // abi message contains an update to the horizontal velocity estimate
#if USE_HFF
struct FloatVect2 vel = {vel_ned.x, vel_ned.y};
- struct FloatVect2 Rvel = {noise, noise};
+ struct FloatVect2 Rvel = {noise_x, noise_y};
hff_update_vel(vel, Rvel);
ins_update_from_hff();
#else
- ins_int.ltp_speed.x = SPEED_BFP_OF_REAL(vel_ned.x);
- ins_int.ltp_speed.y = SPEED_BFP_OF_REAL(vel_ned.y);
-
- static uint32_t last_stamp = 0;
- if (last_stamp > 0) {
- float dt = (float)(stamp - last_stamp) * 1e-6;
- ins_int.ltp_pos.x = ins_int.ltp_pos.x + POS_BFP_OF_REAL(dt * vel_ned.x);
- ins_int.ltp_pos.y = ins_int.ltp_pos.y + POS_BFP_OF_REAL(dt * vel_ned.y);
+ if (noise_x >= 0.f)
+ {
+ ins_int.ltp_speed.x = SPEED_BFP_OF_REAL(vel_ned.x);
+ }
+ if (noise_y >= 0.f)
+ {
+ ins_int.ltp_speed.y = SPEED_BFP_OF_REAL(vel_ned.y);
+ }
+
+ static uint32_t last_stamp_x = 0, last_stamp_y = 0;
+ if (noise_x >= 0.f) {
+ if (last_stamp_x > 0)
+ {
+ float dt = (float)(stamp - last_stamp_x) * 1e-6;
+ ins_int.ltp_pos.x += POS_BFP_OF_REAL(dt * vel_ned.x);
+ }
+ last_stamp_x = stamp;
+ }
+
+ if (noise_y >= 0.f)
+ {
+ if (last_stamp_y > 0)
+ {
+ float dt = (float)(stamp - last_stamp_y) * 1e-6;
+ ins_int.ltp_pos.y += POS_BFP_OF_REAL(dt * vel_ned.y);
+ }
+ last_stamp_y = stamp;
}
- last_stamp = stamp;
#endif
- vff_update_vz_conf(vel_ned.z, noise);
+ // abi message contains an update to the vertical velocity estimate
+ vff_update_vz_conf(vel_ned.z, noise_z);
ins_ned_to_state();
@@ -581,20 +601,27 @@ static void vel_est_cb(uint8_t sender_id __attribute__((unused)),
static void pos_est_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp __attribute__((unused)),
float x, float y, float z,
- float noise)
+ float noise_x, float noise_y, float noise_z)
{
+
#if USE_HFF
struct FloatVect2 pos = {x, y};
- struct FloatVect2 Rpos = {noise, noise};
+ struct FloatVect2 Rpos = {noise_x, noise_y};
hff_update_pos(pos, Rpos);
ins_update_from_hff();
#else
- ins_int.ltp_pos.x = POS_BFP_OF_REAL(x);
- ins_int.ltp_pos.y = POS_BFP_OF_REAL(y);
+ if (noise_x >= 0.f)
+ {
+ ins_int.ltp_pos.x = POS_BFP_OF_REAL(x);
+ }
+ if (noise_y >= 0.f)
+ {
+ ins_int.ltp_pos.y = POS_BFP_OF_REAL(y);
+ }
#endif
- vff_update_z_conf(z, noise);
+ vff_update_z_conf(z, noise_z);
ins_ned_to_state();
diff --git a/sw/airborne/subsystems/ins/vf_extended_float.c b/sw/airborne/subsystems/ins/vf_extended_float.c
index 20e418108a..69ad96c2e4 100644
--- a/sw/airborne/subsystems/ins/vf_extended_float.c
+++ b/sw/airborne/subsystems/ins/vf_extended_float.c
@@ -225,6 +225,8 @@ void vff_update_baro(float z_meas)
void vff_update_baro_conf(float z_meas, float conf)
{
+ if (conf < 0.f) { return; }
+
update_baro_conf(z_meas, conf);
}
@@ -288,6 +290,8 @@ void vff_update_z(float z_meas)
void vff_update_z_conf(float z_meas, float conf)
{
+ if (conf < 0.f) { return; }
+
update_alt_conf(z_meas, conf);
}
@@ -410,5 +414,7 @@ static inline void update_vz_conf(float vz, float conf)
void vff_update_vz_conf(float vz_meas, float conf)
{
+ if (conf < 0.f) { return; }
+
update_vz_conf(vz_meas, conf);
}
diff --git a/sw/airborne/subsystems/ins/vf_float.c b/sw/airborne/subsystems/ins/vf_float.c
index ff26114941..3e6be5140e 100644
--- a/sw/airborne/subsystems/ins/vf_float.c
+++ b/sw/airborne/subsystems/ins/vf_float.c
@@ -193,6 +193,8 @@ void vff_update(float z_meas)
void vff_update_z_conf(float z_meas, float conf)
{
+ if (conf < 0.f) { return; }
+
update_z_conf(z_meas, conf);
}
@@ -246,6 +248,8 @@ static inline void update_vz_conf(float vz, float conf)
void vff_update_vz_conf(float vz_meas, float conf)
{
+ if (conf < 0.f) { return; }
+
update_vz_conf(vz_meas, conf);
}