diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index 120919395e..d50ef0a528 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -108,191 +108,187 @@ static uavcan::equipment::ahrs::Solution _att_state; void AP_UAVCAN::UAVCAN_AHRS_update(const AP_AHRS_NavEKF &ahrs) { - bool sem_ret = _fix_out_sem->take(1); + if (_fix_out_sem->take(1) && + (fix_out_array[_uavcan_i] != nullptr || fix2_out_array[_uavcan_i] != nullptr)) { + const AP_GPS &cgps = AP::gps(); + Location loc; + Vector3f vel_NED; - if (sem_ret) { - if (fix_out_array[_uavcan_i] != nullptr || fix2_out_array[_uavcan_i] != nullptr) { - const AP_GPS &cgps = AP::gps(); - Location loc; - Vector3f vel_NED; + if (ahrs.healthy() && ahrs.get_location(loc) + && ahrs.get_velocity_NED(vel_NED)) { + float velVar, posVar, hgtVar; + Vector3f magVar; + float tasVar; + Vector2f offset; + ahrs.get_variances(velVar, posVar, hgtVar, magVar, tasVar, offset); - if (ahrs.healthy() && ahrs.get_location(loc) - && ahrs.get_velocity_NED(vel_NED)) { - float velVar, posVar, hgtVar; - Vector3f magVar; - float tasVar; - Vector2f offset; - ahrs.get_variances(velVar, posVar, hgtVar, magVar, tasVar, offset); + // Manual resize to diagonal form + _fix_state.position_covariance.resize(3); + _fix_state.position_covariance[0] = posVar; + _fix_state.position_covariance[1] = posVar; + _fix_state.position_covariance[2] = hgtVar; - // Manual resize to diagonal form - _fix_state.position_covariance.resize(3); - _fix_state.position_covariance[0] = posVar; - _fix_state.position_covariance[1] = posVar; - _fix_state.position_covariance[2] = hgtVar; + // Manual resize to diagonal form + _fix_state.velocity_covariance.resize(3); + _fix_state.velocity_covariance[0] = velVar; + _fix_state.velocity_covariance[1] = velVar; + _fix_state.velocity_covariance[2] = velVar; - // Manual resize to diagonal form - _fix_state.velocity_covariance.resize(3); - _fix_state.velocity_covariance[0] = velVar; - _fix_state.velocity_covariance[1] = velVar; - _fix_state.velocity_covariance[2] = velVar; + // Manual resize to diagonal form + _fix2_state.covariance.resize(6); + _fix2_state.covariance[0] = posVar; + _fix2_state.covariance[1] = posVar; + _fix2_state.covariance[2] = hgtVar; + _fix2_state.covariance[3] = velVar; + _fix2_state.covariance[4] = velVar; + _fix2_state.covariance[5] = velVar; + } else { + loc = cgps.location(); + vel_NED = cgps.velocity(); - // Manual resize to diagonal form - _fix2_state.covariance.resize(6); - _fix2_state.covariance[0] = posVar; - _fix2_state.covariance[1] = posVar; - _fix2_state.covariance[2] = hgtVar; - _fix2_state.covariance[3] = velVar; - _fix2_state.covariance[4] = velVar; - _fix2_state.covariance[5] = velVar; - } else { - loc = cgps.location(); - vel_NED = cgps.velocity(); + float vel_acc, hacc, vert_acc; + cgps.horizontal_accuracy(hacc); + cgps.vertical_accuracy(vert_acc); + cgps.speed_accuracy(vel_acc); - float vel_acc, hacc, vert_acc; - cgps.horizontal_accuracy(hacc); - cgps.vertical_accuracy(vert_acc); - cgps.speed_accuracy(vel_acc); + // Manual resize to diagonal form + _fix_state.position_covariance.resize(3); + _fix_state.position_covariance[0] = hacc * hacc; + _fix_state.position_covariance[1] = _fix_state.position_covariance[0]; + _fix_state.position_covariance[2] = vert_acc * vert_acc; - // Manual resize to diagonal form - _fix_state.position_covariance.resize(3); - _fix_state.position_covariance[0] = hacc * hacc; - _fix_state.position_covariance[1] = _fix_state.position_covariance[0]; - _fix_state.position_covariance[2] = vert_acc * vert_acc; + // Manual resize to diagonal form + _fix_state.velocity_covariance.resize(3); + _fix_state.velocity_covariance[0] = vel_acc * vel_acc; + _fix_state.velocity_covariance[1] = _fix_state.velocity_covariance[0]; + _fix_state.velocity_covariance[2] = _fix_state.velocity_covariance[0]; - // Manual resize to diagonal form - _fix_state.velocity_covariance.resize(3); - _fix_state.velocity_covariance[0] = vel_acc * vel_acc; - _fix_state.velocity_covariance[1] = _fix_state.velocity_covariance[0]; - _fix_state.velocity_covariance[2] = _fix_state.velocity_covariance[0]; - - // Manual resize to diagonal form - _fix2_state.covariance.resize(6); - _fix2_state.covariance[0] = _fix_state.position_covariance[0]; - _fix2_state.covariance[1] = _fix_state.position_covariance[1]; - _fix2_state.covariance[2] = _fix_state.position_covariance[2]; - _fix2_state.covariance[3] = _fix_state.velocity_covariance[0]; - _fix2_state.covariance[4] = _fix_state.velocity_covariance[1]; - _fix2_state.covariance[5] = _fix_state.velocity_covariance[2]; - } - - _fix_state.height_msl_mm = loc.alt * 10; - _fix_state.latitude_deg_1e8 = ((uint64_t) loc.lat) * 10; - _fix_state.longitude_deg_1e8 = ((uint64_t) loc.lng) * 10; - - _fix2_state.height_msl_mm = loc.alt * 10; - _fix2_state.latitude_deg_1e8 = ((uint64_t) loc.lat) * 10; - _fix2_state.longitude_deg_1e8 = ((uint64_t) loc.lng) * 10; - - // Not saved in AP - //_fix_state.height_ellipsoid_mm - - _fix_state.ned_velocity[0] = vel_NED.x; - _fix_state.ned_velocity[1] = vel_NED.y; - _fix_state.ned_velocity[2] = vel_NED.z; - - _fix2_state.ned_velocity[0] = vel_NED.x; - _fix2_state.ned_velocity[1] = vel_NED.y; - _fix2_state.ned_velocity[2] = vel_NED.z; - - _fix_state.sats_used = cgps.num_sats(); - _fix_state.pdop = cgps.get_hdop(); - - _fix2_state.sats_used = _fix_state.sats_used; - _fix2_state.pdop = _fix_state.pdop; - - switch(cgps.status()) { - case AP_GPS::GPS_Status::NO_GPS: - _fix_state.status = uavcan::equipment::gnss::Fix::STATUS_NO_FIX; - break; - case AP_GPS::GPS_Status::NO_FIX: - _fix_state.status = uavcan::equipment::gnss::Fix::STATUS_NO_FIX; - break; - case AP_GPS::GPS_Status::GPS_OK_FIX_2D: - _fix_state.status = uavcan::equipment::gnss::Fix::STATUS_2D_FIX; - break; - case AP_GPS::GPS_Status::GPS_OK_FIX_3D: - case AP_GPS::GPS_Status::GPS_OK_FIX_3D_DGPS: - case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FLOAT: - case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FIXED: - _fix_state.status = uavcan::equipment::gnss::Fix::STATUS_3D_FIX; - break; - } - - switch(cgps.status()) { - case AP_GPS::GPS_Status::NO_GPS: - case AP_GPS::GPS_Status::NO_FIX: - case AP_GPS::GPS_Status::GPS_OK_FIX_2D: - case AP_GPS::GPS_Status::GPS_OK_FIX_3D: - _fix2_state.mode = uavcan::equipment::gnss::Fix2::MODE_SINGLE; - _fix2_state.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_DGPS_OTHER; - break; - case AP_GPS::GPS_Status::GPS_OK_FIX_3D_DGPS: - _fix2_state.mode = uavcan::equipment::gnss::Fix2::MODE_DGPS; - _fix2_state.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_DGPS_OTHER; - break; - case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FLOAT: - _fix2_state.mode = uavcan::equipment::gnss::Fix2::MODE_RTK; - _fix2_state.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_RTK_FLOAT; - break; - case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FIXED: - _fix2_state.mode = uavcan::equipment::gnss::Fix2::MODE_RTK; - _fix2_state.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_RTK_FIXED; - break; - } - - _fix_state.gnss_time_standard = uavcan::equipment::gnss::Fix::GNSS_TIME_STANDARD_UTC; - _fix2_state.gnss_time_standard = _fix_state.gnss_time_standard; - uavcan::Timestamp ts; - ts.usec = 1000ULL * ((cgps.time_week() * AP_MSEC_PER_WEEK + cgps.time_week_ms()) + UNIX_OFFSET_MSEC); - _fix_state.gnss_timestamp = ts; - _fix2_state.gnss_timestamp = ts; - - _fix_state.num_leap_seconds = 0; - _fix2_state.num_leap_seconds = 0; - ts.usec = 0; - _fix_state.timestamp = ts; - _fix2_state.timestamp = ts; + // Manual resize to diagonal form + _fix2_state.covariance.resize(6); + _fix2_state.covariance[0] = _fix_state.position_covariance[0]; + _fix2_state.covariance[1] = _fix_state.position_covariance[1]; + _fix2_state.covariance[2] = _fix_state.position_covariance[2]; + _fix2_state.covariance[3] = _fix_state.velocity_covariance[0]; + _fix2_state.covariance[4] = _fix_state.velocity_covariance[1]; + _fix2_state.covariance[5] = _fix_state.velocity_covariance[2]; } - if (attitude_out_array[_uavcan_i] != nullptr) { - uavcan::Timestamp ts; - ts.usec = AP_HAL::micros64(); - _att_state.timestamp = ts; + _fix_state.height_msl_mm = loc.alt * 10; + _fix_state.latitude_deg_1e8 = ((uint64_t) loc.lat) * 10; + _fix_state.longitude_deg_1e8 = ((uint64_t) loc.lng) * 10; - Quaternion qt; - Matrix3f rot = ahrs.get_rotation_body_to_ned(); - qt.from_rotation_matrix(rot); - _att_state.orientation_xyzw[0] = qt.q1; - _att_state.orientation_xyzw[1] = qt.q2; - _att_state.orientation_xyzw[2] = qt.q3; - _att_state.orientation_xyzw[3] = qt.q4; + _fix2_state.height_msl_mm = loc.alt * 10; + _fix2_state.latitude_deg_1e8 = ((uint64_t) loc.lat) * 10; + _fix2_state.longitude_deg_1e8 = ((uint64_t) loc.lng) * 10; - // TODO: extract from EKF - //_att_state.orientation_covariance + // Not saved in AP + //_fix_state.height_ellipsoid_mm - Vector3f av = ahrs.get_gyro(); - _att_state.angular_velocity[0] = av.x; - _att_state.angular_velocity[1] = av.y; - _att_state.angular_velocity[2] = av.z; + _fix_state.ned_velocity[0] = vel_NED.x; + _fix_state.ned_velocity[1] = vel_NED.y; + _fix_state.ned_velocity[2] = vel_NED.z; - // TODO: extract from EKF - //_att_state.angular_velocity_covariance + _fix2_state.ned_velocity[0] = vel_NED.x; + _fix2_state.ned_velocity[1] = vel_NED.y; + _fix2_state.ned_velocity[2] = vel_NED.z; - Vector3f la = ahrs.get_accel_ef(); - _att_state.linear_acceleration[0] = la.x; - _att_state.linear_acceleration[1] = la.y; - _att_state.linear_acceleration[2] = la.z; + _fix_state.sats_used = cgps.num_sats(); + _fix_state.pdop = cgps.get_hdop(); - // TODO: extract from EKF - //_att_state.linear_acceleration_covariance + _fix2_state.sats_used = _fix_state.sats_used; + _fix2_state.pdop = _fix_state.pdop; + + switch(cgps.status()) { + case AP_GPS::GPS_Status::NO_GPS: + _fix_state.status = uavcan::equipment::gnss::Fix::STATUS_NO_FIX; + break; + case AP_GPS::GPS_Status::NO_FIX: + _fix_state.status = uavcan::equipment::gnss::Fix::STATUS_NO_FIX; + break; + case AP_GPS::GPS_Status::GPS_OK_FIX_2D: + _fix_state.status = uavcan::equipment::gnss::Fix::STATUS_2D_FIX; + break; + case AP_GPS::GPS_Status::GPS_OK_FIX_3D: + case AP_GPS::GPS_Status::GPS_OK_FIX_3D_DGPS: + case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FLOAT: + case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FIXED: + _fix_state.status = uavcan::equipment::gnss::Fix::STATUS_3D_FIX; + break; } + switch(cgps.status()) { + case AP_GPS::GPS_Status::NO_GPS: + case AP_GPS::GPS_Status::NO_FIX: + case AP_GPS::GPS_Status::GPS_OK_FIX_2D: + case AP_GPS::GPS_Status::GPS_OK_FIX_3D: + _fix2_state.mode = uavcan::equipment::gnss::Fix2::MODE_SINGLE; + _fix2_state.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_DGPS_OTHER; + break; + case AP_GPS::GPS_Status::GPS_OK_FIX_3D_DGPS: + _fix2_state.mode = uavcan::equipment::gnss::Fix2::MODE_DGPS; + _fix2_state.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_DGPS_OTHER; + break; + case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FLOAT: + _fix2_state.mode = uavcan::equipment::gnss::Fix2::MODE_RTK; + _fix2_state.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_RTK_FLOAT; + break; + case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FIXED: + _fix2_state.mode = uavcan::equipment::gnss::Fix2::MODE_RTK; + _fix2_state.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_RTK_FIXED; + break; + } + + _fix_state.gnss_time_standard = uavcan::equipment::gnss::Fix::GNSS_TIME_STANDARD_UTC; + _fix2_state.gnss_time_standard = _fix_state.gnss_time_standard; + uavcan::Timestamp ts; + ts.usec = 1000ULL * ((cgps.time_week() * AP_MSEC_PER_WEEK + cgps.time_week_ms()) + UNIX_OFFSET_MSEC); + _fix_state.gnss_timestamp = ts; + _fix2_state.gnss_timestamp = ts; + + _fix_state.num_leap_seconds = 0; + _fix2_state.num_leap_seconds = 0; + ts.usec = 0; + _fix_state.timestamp = ts; + _fix2_state.timestamp = ts; + _fix_out_sem->give(); - } else { - if (sem_ret) { - _fix_out_sem->give(); - } } + + if (_att_out_sem->take(1) && (attitude_out_array[_uavcan_i] != nullptr)) { + uavcan::Timestamp ts; + ts.usec = AP_HAL::micros64(); + _att_state.timestamp = ts; + + Quaternion qt; + Matrix3f rot = ahrs.get_rotation_body_to_ned(); + qt.from_rotation_matrix(rot); + _att_state.orientation_xyzw[0] = qt.q1; + _att_state.orientation_xyzw[1] = qt.q2; + _att_state.orientation_xyzw[2] = qt.q3; + _att_state.orientation_xyzw[3] = qt.q4; + + // TODO: extract from EKF + //_att_state.orientation_covariance + + Vector3f av = ahrs.get_gyro(); + _att_state.angular_velocity[0] = av.x; + _att_state.angular_velocity[1] = av.y; + _att_state.angular_velocity[2] = av.z; + + // TODO: extract from EKF + //_att_state.angular_velocity_covariance + + Vector3f la = ahrs.get_accel_ef(); + _att_state.linear_acceleration[0] = la.x; + _att_state.linear_acceleration[1] = la.y; + _att_state.linear_acceleration[2] = la.z; + + // TODO: extract from EKF + //_att_state.linear_acceleration_covariance + + _att_out_sem->give(); + } + } static void gnss_fix_cb(const uavcan::ReceivedDataStructure& msg, uint8_t mgr)