47 struct uavcan_equipment_gnss_Fix2 msg;
48 if(uavcan_equipment_gnss_Fix2_decode(
transfer, &msg)) {
55#if GPS_UAVCAN_USE_NODE_ID
69 case UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_NO_FIX:
72 case UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_TIME_ONLY:
75 case UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_2D_FIX:
78 case UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX:
79 if(msg.mode == UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_DGPS) {
81 }
else if(msg.mode == UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_RTK) {
83 }
else if(msg.mode == UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_PPP) {
98 if(msg.ecef_position_velocity.len > 0) {
100 state.ecef_pos.x = msg.ecef_position_velocity.data[0].position_xyz_mm[0] / 10;
101 state.ecef_pos.y = msg.ecef_position_velocity.data[0].position_xyz_mm[1] / 10;
102 state.ecef_pos.z = msg.ecef_position_velocity.data[0].position_xyz_mm[2] / 10;
106 state.ecef_vel.x = msg.ecef_position_velocity.data[0].velocity_xyz[0] * 100;
107 state.ecef_vel.y = msg.ecef_position_velocity.data[0].velocity_xyz[1] * 100;
108 state.ecef_vel.z = msg.ecef_position_velocity.data[0].velocity_xyz[2] * 100;
115 state.lla_pos.lat = msg.latitude_deg_1e8 / 10;
116 state.lla_pos.lon = msg.longitude_deg_1e8 / 10;
117 state.lla_pos.alt = msg.height_ellipsoid_mm;
123 state.hmsl = msg.height_msl_mm;
127 state.ned_vel.x = msg.ned_velocity[0] * 100;
128 state.ned_vel.y = msg.ned_velocity[1] * 100;
129 state.ned_vel.z = msg.ned_velocity[2] * 100;
144 if (msg.covariance.len == 6) {
146 if (!isnan(msg.covariance.data[0])) {
147 state.hacc = sqrtf(msg.covariance.data[0]) * 100;
150 if (!isnan(msg.covariance.data[2])) {
151 state.vacc = sqrtf(msg.covariance.data[2]) * 100;
154 if(!isnan(msg.covariance.data[0]) && !isnan(msg.covariance.data[2])) {
158 if (!isnan(msg.covariance.data[3]) &&
159 !isnan(msg.covariance.data[4]) &&
160 !isnan(msg.covariance.data[5])) {
161 state.sacc = sqrtf((msg.covariance.data[3] + msg.covariance.data[4] + msg.covariance.data[5])/3) * 100;
163 }
else if(msg.covariance.len != 0){
168 state.pdop = msg.pdop * 100;
170 state.num_sv = msg.sats_used;
174 const uint32_t unixToGpsEpoch = 315964800;
175 uint64_t gnss_ts_msec = msg.gnss_timestamp.usec / 1000 - unixToGpsEpoch*1000;
176 switch (msg.gnss_time_standard)
178 case UAVCAN_EQUIPMENT_GNSS_FIX2_GNSS_TIME_STANDARD_NONE:
180 case UAVCAN_EQUIPMENT_GNSS_FIX2_GNSS_TIME_STANDARD_TAI:
181 gnss_ts_msec -= 19000;
183 case UAVCAN_EQUIPMENT_GNSS_FIX2_GNSS_TIME_STANDARD_UTC:
184 gnss_ts_msec += (msg.num_leap_seconds*1000 - 9000);
186 case UAVCAN_EQUIPMENT_GNSS_FIX2_GNSS_TIME_STANDARD_GPS:
199 state.nb_channels = 0;
void uavcan_bind(uint16_t data_type_id, uint64_t data_type_signature, uavcan_event *ev, uavcan_callback cb)
Bind to a receiving message from uavcan.