diff --git a/msg/VehicleOpticalFlowVel.msg b/msg/VehicleOpticalFlowVel.msg index c89207f4ab..d92806803a 100644 --- a/msg/VehicleOpticalFlowVel.msg +++ b/msg/VehicleOpticalFlowVel.msg @@ -10,4 +10,8 @@ float32[2] flow_compensated_integral # integrated optical flow measurement com float32[3] gyro_rate # gyro measurement synchronized with flow measurements (rad/s) float32[3] gyro_rate_integral # gyro measurement integrated to flow rate and synchronized with flow measurements (rad) +float32[3] gyro_bias +float32[3] ref_gyro +float32[3] meas_gyro + # TOPICS estimator_optical_flow_vel vehicle_optical_flow_vel diff --git a/src/drivers/uavcan/sensors/flow.cpp b/src/drivers/uavcan/sensors/flow.cpp index 099de615f0..f084cd98b3 100644 --- a/src/drivers/uavcan/sensors/flow.cpp +++ b/src/drivers/uavcan/sensors/flow.cpp @@ -79,7 +79,7 @@ void UavcanFlowBridge::flow_sub_cb(const uavcan::ReceivedDataStructure FLT_EPSILON) + && (_flow_sample_delayed.dt > FLT_EPSILON)) { + const Vector3f reference_body_rate = -_imu_del_ang_of / _delta_time_of; // flow gyro has opposite sign convention + _ref_body_rate = reference_body_rate; - // if accumulation time differences are not excessive and accumulation time is adequate - // compare the optical flow and and navigation rate data and calculate a bias error - if ((_delta_time_of > FLT_EPSILON) - && (_flow_sample_delayed.dt > FLT_EPSILON) - && (fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.1f)) { + if (!PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(0)) || !PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(1))) { + _flow_sample_delayed.gyro_xyz = reference_body_rate * _flow_sample_delayed.dt; - const Vector3f reference_body_rate(_imu_del_ang_of * (1.0f / _delta_time_of)); + } else if (!PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(2))) { + // Some flow modules only provide X ind Y angular rates. If this is the case, complete the vector with our own Z gyro + _flow_sample_delayed.gyro_xyz(2) = reference_body_rate(2) * _flow_sample_delayed.dt; + } - const Vector3f measured_body_rate(_flow_sample_delayed.gyro_xyz * (1.0f / _flow_sample_delayed.dt)); + const Vector3f measured_body_rate = _flow_sample_delayed.gyro_xyz / _flow_sample_delayed.dt; + _measured_body_rate = measured_body_rate; + if (fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.1f) { // calculate the bias estimate using a combined LPF and spike filter _flow_gyro_bias = _flow_gyro_bias * 0.99f + matrix::constrain(measured_body_rate - reference_body_rate, -0.1f, 0.1f) * 0.01f; - - // apply gyro bias - _flow_sample_delayed.gyro_xyz -= (_flow_gyro_bias * _flow_sample_delayed.dt); - - is_body_rate_comp_available = true; } - } else { - // Use the EKF gyro data if optical flow sensor gyro data is not available - // for clarification of the sign see definition of flowSample and imuSample in common.h - if ((_delta_time_of > FLT_EPSILON) - && (_flow_sample_delayed.dt > FLT_EPSILON)) { - - _flow_sample_delayed.gyro_xyz = -_imu_del_ang_of / _delta_time_of * _flow_sample_delayed.dt; - _flow_gyro_bias.zero(); - - is_body_rate_comp_available = true; - } + is_body_rate_comp_available = true; } // reset the accumulators diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 426707abb9..e2aa9507b2 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -2001,6 +2001,10 @@ void EKF2::PublishOpticalFlowVel(const hrt_abstime ×tamp) _ekf.getFlowGyro().copyTo(flow_vel.gyro_rate); _ekf.getFlowGyroIntegral().copyTo(flow_vel.gyro_rate_integral); + _ekf.getFlowGyroBias().copyTo(flow_vel.gyro_bias); + _ekf.getRefBodyRate().copyTo(flow_vel.ref_gyro); + _ekf.getMeasuredBodyRate().copyTo(flow_vel.meas_gyro); + flow_vel.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _estimator_optical_flow_vel_pub.publish(flow_vel); diff --git a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp index fbebe07ad1..2e36e7cc46 100644 --- a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp +++ b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp @@ -126,9 +126,20 @@ void VehicleOpticalFlow::Run() // delta angle // - from sensor_optical_flow if available, otherwise use synchronized sensor_gyro if available - if (sensor_optical_flow.delta_angle_available && Vector3f(sensor_optical_flow.delta_angle).isAllFinite()) { + if (sensor_optical_flow.delta_angle_available && Vector2f(sensor_optical_flow.delta_angle).isAllFinite()) { // passthrough integrated gyro if available - _delta_angle += _flow_rotation * Vector3f{sensor_optical_flow.delta_angle}; + Vector3f delta_angle(sensor_optical_flow.delta_angle); + + if (!PX4_ISFINITE(delta_angle(2))) { + // Some sensors only provide X and Y angular rates, rotate them but place back the NAN on the Z axis + delta_angle(2) = 0.f; + _delta_angle += _flow_rotation * delta_angle; + _delta_angle(2) = NAN; + + } else { + _delta_angle += _flow_rotation * delta_angle; + } + _delta_angle_available = true; } else { @@ -320,10 +331,12 @@ void VehicleOpticalFlow::Run() // gyro_rate flow_vel.gyro_rate[0] = measured_body_rate(0); flow_vel.gyro_rate[1] = measured_body_rate(1); + flow_vel.gyro_rate[2] = measured_body_rate(2); // gyro_rate_integral flow_vel.gyro_rate_integral[0] = gyro_xyz(0); flow_vel.gyro_rate_integral[1] = gyro_xyz(1); + flow_vel.gyro_rate_integral[2] = gyro_xyz(2); flow_vel.timestamp = hrt_absolute_time();