Sylvain Munaut | 0f91413 | 2009-12-22 21:53:22 +0100 | [diff] [blame] | 1 | /* |
| 2 | * gps.c |
| 3 | * |
| 4 | * A few utility functions to deal with low level GPS data |
| 5 | * |
| 6 | * |
| 7 | * Copyright (C) 2009 Sylvain Munaut <tnt@246tNt.com> |
| 8 | * |
| 9 | * This program is free software: you can redistribute it and/or modify |
| 10 | * it under the terms of the GNU General Public License as published by |
| 11 | * the Free Software Foundation, either version 2 of the License, or |
| 12 | * (at your option) any later version. |
| 13 | * |
| 14 | * This program is distributed in the hope that it will be useful, |
| 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| 17 | * GNU General Public License for more details. |
| 18 | * |
| 19 | * You should have received a copy of the GNU General Public License |
| 20 | * along with this program. If not, see <http://www.gnu.org/licenses/>. |
| 21 | */ |
| 22 | |
| 23 | #include "gps.h" |
| 24 | |
| 25 | |
| 26 | #define GET_FIELD_U(w, nb, pos) (((w) >> (pos)) & ((1<<(nb))-1)) |
| 27 | #define GET_FIELD_S(w, nb, pos) (((int)((w) << (32-(nb)-(pos)))) >> (32-(nb))) |
| 28 | |
| 29 | /* |
| 30 | * Unpacks GPS Subframe 1,2,3 payloads (3 * 8 words) |
| 31 | * |
| 32 | * Note: eph->sv_id is not filled here since not present in those subframes |
| 33 | * |
| 34 | * (no parity bit checking is done, only the lower 24 bits of each word |
| 35 | * are used) |
| 36 | */ |
| 37 | int |
| 38 | gps_unpack_sf123(uint32_t *sf, struct gps_ephemeris_sv *eph) |
| 39 | { |
| 40 | uint32_t *sf1 = &sf[0]; |
| 41 | uint32_t *sf2 = &sf[8]; |
| 42 | uint32_t *sf3 = &sf[16]; |
| 43 | |
| 44 | int iode1, iode2; |
| 45 | |
| 46 | eph->week_no = GET_FIELD_U(sf1[0], 10, 14); |
| 47 | eph->code_on_l2 = GET_FIELD_U(sf1[0], 2, 12); |
| 48 | eph->sv_ura = GET_FIELD_U(sf1[0], 4, 8); |
| 49 | eph->sv_health = GET_FIELD_U(sf1[0], 6, 2); |
| 50 | eph->l2_p_flag = GET_FIELD_U(sf1[1], 1, 23); |
| 51 | eph->t_gd = GET_FIELD_S(sf1[4], 8, 0); |
| 52 | eph->iodc = (GET_FIELD_U(sf1[0], 2, 0) << 8) | \ |
| 53 | GET_FIELD_U(sf1[5], 8, 16); |
| 54 | eph->t_oc = GET_FIELD_U(sf1[5], 16, 0); |
| 55 | eph->a_f2 = GET_FIELD_S(sf1[6], 8, 16); |
| 56 | eph->a_f1 = GET_FIELD_S(sf1[6], 16, 0); |
| 57 | eph->a_f0 = GET_FIELD_S(sf1[7], 22, 2); |
| 58 | |
| 59 | iode1 = GET_FIELD_U(sf2[0], 8, 16); |
| 60 | eph->c_rs = GET_FIELD_S(sf2[0], 16, 0); |
| 61 | eph->delta_n = GET_FIELD_S(sf2[1], 16, 8); |
| 62 | eph->m_0 = (GET_FIELD_S(sf2[1], 8, 0) << 24) | \ |
| 63 | GET_FIELD_U(sf2[2], 24, 0); |
| 64 | eph->c_uc = GET_FIELD_S(sf2[3], 16, 8); |
| 65 | eph->e = (GET_FIELD_U(sf2[3], 8, 0) << 24) | \ |
| 66 | GET_FIELD_U(sf2[4], 24, 0); |
| 67 | eph->c_us = GET_FIELD_S(sf2[5], 16, 8); |
| 68 | eph->a_powhalf = (GET_FIELD_U(sf2[5], 8, 0) << 24) | \ |
| 69 | GET_FIELD_U(sf2[6], 24, 0); |
| 70 | eph->t_oe = GET_FIELD_U(sf2[7], 16, 8); |
| 71 | eph->fit_flag = GET_FIELD_U(sf2[7], 1, 7); |
| 72 | |
| 73 | eph->c_ic = GET_FIELD_S(sf3[0], 16, 8); |
| 74 | eph->omega_0 = (GET_FIELD_S(sf3[0], 8, 0) << 24) | \ |
| 75 | GET_FIELD_U(sf3[1], 24, 0); |
| 76 | eph->c_is = GET_FIELD_S(sf3[2], 16, 8); |
| 77 | eph->i_0 = (GET_FIELD_S(sf3[2], 8, 0) << 24) | \ |
| 78 | GET_FIELD_U(sf3[3], 24, 0); |
| 79 | eph->c_rc = GET_FIELD_S(sf3[4], 16, 8); |
| 80 | eph->w = (GET_FIELD_S(sf3[4], 8, 0) << 24) | \ |
| 81 | GET_FIELD_U(sf3[5], 24, 0); |
| 82 | eph->omega_dot = GET_FIELD_S(sf3[6], 24, 0); |
| 83 | iode2 = GET_FIELD_U(sf3[7], 8, 16); |
| 84 | eph->idot = GET_FIELD_S(sf3[7], 14, 2); |
| 85 | |
| 86 | eph->_rsvd1 = GET_FIELD_U(sf1[1], 23, 0); |
| 87 | eph->_rsvd2 = GET_FIELD_U(sf1[2], 24, 0); |
| 88 | eph->_rsvd3 = GET_FIELD_U(sf1[3], 24, 0); |
| 89 | eph->_rsvd4 = GET_FIELD_U(sf1[4], 16, 8); |
| 90 | eph->aodo = GET_FIELD_U(sf2[7], 5, 2); |
| 91 | |
| 92 | /* Check & cross-validate iodc[7:0], iode1, iode2 */ |
| 93 | if ((iode1 != iode2) || (iode1 != (eph->iodc & 0xff))) |
| 94 | return -1; |
| 95 | |
| 96 | return 0; |
| 97 | } |
| 98 | |
| 99 | |
| 100 | /* |
| 101 | * Unpacks GPS Subframe 4 or 5 Almanac pages payload (8 words) |
| 102 | * |
| 103 | * (no parity bit checking is done, only the lower 24 bits of each word |
| 104 | * are used) |
| 105 | */ |
| 106 | int |
| 107 | gps_unpack_sf45_almanac(uint32_t *sf, struct gps_almanac_sv *alm) |
| 108 | { |
| 109 | alm->sv_id = GET_FIELD_U(sf[0], 6, 16); |
| 110 | |
| 111 | alm->e = GET_FIELD_U(sf[0], 16, 0); |
| 112 | alm->t_oa = GET_FIELD_U(sf[1], 8, 16); |
| 113 | alm->ksii = GET_FIELD_S(sf[1], 16, 0); |
| 114 | alm->omega_dot = GET_FIELD_S(sf[2], 16, 8); |
| 115 | alm->sv_health = GET_FIELD_U(sf[2], 8, 0); |
| 116 | alm->a_powhalf = GET_FIELD_U(sf[3], 24, 0); |
| 117 | alm->omega_0 = GET_FIELD_S(sf[4], 24, 0); |
| 118 | alm->w = GET_FIELD_S(sf[5], 24, 0); |
| 119 | alm->m_0 = GET_FIELD_S(sf[6], 24, 0); |
| 120 | alm->a_f0 = (GET_FIELD_S(sf[7], 8, 16) << 3) | \ |
| 121 | GET_FIELD_U(sf[7], 3, 2); |
| 122 | alm->a_f1 = GET_FIELD_S(sf[7], 11, 5); |
| 123 | |
| 124 | return 0; |
| 125 | } |
| 126 | |